fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 00:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'private_swissfang/stable' into obcfailsafe

Browse Source 
Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
This commit is contained in:
Thomas Gubler
2014-09-13 15:50:51 +02:00
parent e8503ab3f9 dd5627726d
commit 06ad49a690
8 changed files with 119 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/bottle_drop/bottle_drop.cpp
+40 -44
View File
@@ -463,10 +463,10 @@ BottleDrop::task_main()
continue;
}
const unsigned sleeptime_us = 50000;
const unsigned sleeptime_us = 9500;
hrt_abstime last_run = hrt_absolute_time();
float dt_runs = 1e6f / sleeptime_us;
float dt_runs = sleeptime_us / 1e6f;
// switch to faster updates during the drop
while (_drop_state > DROP_STATE_INIT) {
@@ -517,53 +517,49 @@ BottleDrop::task_main()
case DROP_STATE_TARGET_VALID:
{
// Update drop point at 10 Hz
if (counter % 10 == 0) {
az = g; // acceleration in z direction[m/s^2]
vz = 0; // velocity in z direction [m/s]
z = 0; // fallen distance [m]
h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m]
h = h_0; // height over target [m]
ax = 0; // acceleration in x direction [m/s^2]
vx = groundspeed_body;// XXX project // ground speed in x direction [m/s]
x = 0; // traveled distance in x direction [m]
vw = 0; // wind speed [m/s]
vrx = 0; // relative velocity in x direction [m/s]
v = groundspeed_body; // relative speed vector [m/s]
Fd = 0; // Drag force [N]
Fdx = 0; // Drag force in x direction [N]
Fdz = 0; // Drag force in z direction [N]
az = g; // acceleration in z direction[m/s^2]
vz = 0; // velocity in z direction [m/s]
z = 0; // fallen distance [m]
h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m]
h = h_0; // height over target [m]
ax = 0; // acceleration in x direction [m/s^2]
vx = groundspeed_body;// XXX project // ground speed in x direction [m/s]
x = 0; // traveled distance in x direction [m]
vw = 0; // wind speed [m/s]
vrx = 0; // relative velocity in x direction [m/s]
v = groundspeed_body; // relative speed vector [m/s]
Fd = 0; // Drag force [N]
Fdx = 0; // Drag force in x direction [N]
Fdz = 0; // Drag force in z direction [N]
// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
while (h > 0.05f) {
// z-direction
vz = vz + az * dt_freefall_prediction;
z = z + vz * dt_freefall_prediction;
h = h_0 - z;
// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
while (h > 0.05f) {
// z-direction
vz = vz + az * dt_freefall_prediction;
z = z + vz * dt_freefall_prediction;
h = h_0 - z;
// x-direction
vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
vx = vx + ax * dt_freefall_prediction;
x = x + vx * dt_freefall_prediction;
vrx = vx + vw;
// x-direction
vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
vx = vx + ax * dt_freefall_prediction;
x = x + vx * dt_freefall_prediction;
vrx = vx + vw;
// drag force
v = sqrtf(vz * vz + vrx * vrx);
Fd = 0.5f * rho * A * cd * (v * v);
Fdx = Fd * vrx / v;
Fdz = Fd * vz / v;
// drag force
v = sqrtf(vz * vz + vrx * vrx);
Fd = 0.5f * rho * A * cd * (v * v);
Fdx = Fd * vrx / v;
Fdz = Fd * vz / v;
// acceleration
az = g - Fdz / m;
ax = -Fdx / m;
}
// compute drop vector
x = groundspeed_body * t_signal + x;
// acceleration
az = g - Fdz / m;
ax = -Fdx / m;
}
// compute drop vector
x = groundspeed_body * t_signal + x;
x_t = 0.0f;
y_t = 0.0f;
@@ -688,10 +684,10 @@ BottleDrop::task_main()
// update_actuators();
// run at roughly 20 Hz
// run at roughly 100 Hz
usleep(sleeptime_us);
dt_runs = 1e6f / hrt_elapsed_time(&last_run);
dt_runs = hrt_elapsed_time(&last_run) / 1e6f;
last_run = hrt_absolute_time();
}
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+22 -6
View File
@@ -390,7 +390,8 @@ private:
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
tecs_mode mode = TECS_MODE_NORMAL);
tecs_mode mode = TECS_MODE_NORMAL,
bool pitch_max_special = false);
};
@@ -1140,7 +1141,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
/* select maximum pitch: the launchdetector may impose another limit for the pitch
* depending on the state of the launch */
float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
* meters */
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
@@ -1148,7 +1155,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
takeoff_pitch_max_rad,
_parameters.throttle_min, takeoff_throttle,
_parameters.throttle_cruise,
true,
@@ -1156,7 +1163,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::radians(10.0f)),
_global_pos.alt,
ground_speed,
TECS_MODE_TAKEOFF);
TECS_MODE_TAKEOFF,
takeoff_pitch_max_deg != _parameters.pitch_limit_max);
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
@@ -1228,7 +1236,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* making sure again that the correct thrust is used,
* without depending on library calls */
* without depending on library calls for safety reasons */
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
} else {
/* Copy thrust and pitch values from tecs */
@@ -1420,7 +1428,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
tecs_mode mode)
tecs_mode mode, bool pitch_max_special)
{
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
@@ -1440,6 +1448,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
} else {
limitOverride.disablePitchMinOverride();
}
if (pitch_max_special) {
/* Use the maximum pitch from the argument */
limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad);
} else {
/* use pitch max set by MT param */
limitOverride.disablePitchMaxOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {