diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-02 22:00:56 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-02 22:00:56 +0400 |
| commit | dae5c838422a6250e1a7e4920d59cb8976a16a8c (patch) | |
| tree | 1ace844e36016efde9e3324c36bd82faf5b90ba3 /src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | |
| parent | 2dc2c2d28f6f56936a09df2ef61a488c990e6379 (diff) | |
| download | px4-firmware-dae5c838422a6250e1a7e4920d59cb8976a16a8c.tar.gz px4-firmware-dae5c838422a6250e1a7e4920d59cb8976a16a8c.tar.bz2 px4-firmware-dae5c838422a6250e1a7e4920d59cb8976a16a8c.zip | |
mc_att_control_vector: support for disabled rate controller flag to handle AUTO_READY mode
Diffstat (limited to 'src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp')
| -rw-r--r-- | src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | 20 |
1 files changed, 14 insertions, 6 deletions
diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index f13c04e45..239bd5570 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -643,11 +643,20 @@ MulticopterAttitudeControl::task_main() } /* publish the attitude controls */ - _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; - _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; - _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; - _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; - _actuators.timestamp = hrt_absolute_time(); + if (_control_mode.flag_control_rates_enabled) { + _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; + _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; + _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; + _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + } else { + /* controller disabled, publish zero attitude controls */ + _actuators.control[0] = 0.0f; + _actuators.control[1] = 0.0f; + _actuators.control[2] = 0.0f; + _actuators.control[3] = 0.0f; + _actuators.timestamp = hrt_absolute_time(); + } if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ @@ -657,7 +666,6 @@ MulticopterAttitudeControl::task_main() /* advertise and publish */ _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } - } perf_end(_loop_perf); |