fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 00:01:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw_att_ctrl: Code cleanup

Browse Source 
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
CarlOlsson
2016-10-31 15:17:14 +01:00
committed by Lorenz Meier
parent 00cd2902c7
commit 93fb02bfa3
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
@@ -1045,7 +1045,8 @@ FixedwingAttitudeControl::task_main()
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */ /* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(control_input); float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll :
_parameters.trim_roll;
if (!PX4_ISFINITE(roll_u)) { if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator(); _roll_ctrl.reset_integrator();
@@ -1057,7 +1058,8 @@ FixedwingAttitudeControl::task_main()
} }
float pitch_u = _pitch_ctrl.control_bodyrate(control_input); float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch :
_parameters.trim_pitch;
if (!PX4_ISFINITE(pitch_u)) { if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator();
@@ -1089,10 +1091,11 @@ FixedwingAttitudeControl::task_main()
yaw_u = _yaw_ctrl.control_bodyrate(control_input); yaw_u = _yaw_ctrl.control_bodyrate(control_input);
} }
_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw :
_parameters.trim_yaw;
/* add in manual rudder control */ /* add in manual rudder control */
_actuators.control[2] += yaw_manual; _actuators.control[actuator_controls_s::INDEX_YAW] += yaw_manual;
if (!PX4_ISFINITE(yaw_u)) { if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator();
@@ -1106,10 +1109,10 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through if it is finite and if no engine failure was /* throttle passed through if it is finite and if no engine failure was
* detected */ * detected */
_actuators.control[3] = (PX4_ISFINITE(throttle_sp) && _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure || !(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ? _vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f; throttle_sp : 0.0f;
if (!PX4_ISFINITE(throttle_sp)) { if (!PX4_ISFINITE(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) { if (_debug && loop_counter % 10 == 0) {