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

Merge pull request #1312 from PX4/airspeed_fix

Browse Source 
Deal with zero airspeed measurements
This commit is contained in:
Thomas Gubler
2014-08-25 09:01:45 +02:00
parent 04ccf5d8c9 2f5c0cbd13
commit ca06c7b4e7
1 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+11 -8
View File
@@ -145,6 +145,7 @@ private:
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */
bool _debug; /**< if set to true, print debug output */
struct { struct {
float tconst; float tconst;
@@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
/* states */ /* states */
_setpoint_valid(false) _setpoint_valid(false),
_debug(false)
{ {
/* safely initialize structs */ /* safely initialize structs */
_att = {}; _att = {};
@@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main()
perf_count(_nonfinite_input_perf); perf_count(_nonfinite_input_perf);
} }
} else { } else {
airspeed = _airspeed.true_airspeed_m_s; /* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
} }
/* /*
@@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main()
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else { } else {
if (loop_counter % 10 == 0) { if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n"); warnx("Did not get a valid R\n");
} }
} }
@@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.reset_integrator(); _roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf); perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) { if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u); warnx("roll_u %.4f", (double)roll_u);
} }
} }
@@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(pitch_u)) { if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf); perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) { if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f," " airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f," " roll_sp %.4f, pitch_sp %.4f,"
@@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(yaw_u)) { if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf); perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) { if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u); warnx("yaw_u %.4f", (double)yaw_u);
} }
} }
@@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through */ /* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) { if (!isfinite(throttle_sp)) {
if (loop_counter % 10 == 0) { if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp); warnx("throttle_sp %.4f", (double)throttle_sp);
} }
} }
} else { } else {
perf_count(_nonfinite_input_perf); perf_count(_nonfinite_input_perf);
if (loop_counter % 10 == 0) { if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
} }
} }