fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 08:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control_vector: support for disabled rate controller flag to handle AUTO_READY mode

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-02 22:00:56 +04:00
parent 2dc2c2d28f
commit dae5c83842
1 changed files with 14 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+14 -6
View File
@@ -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);