Browse Source

fw_att_ctrl: Code cleanup

Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
sbg
CarlOlsson 8 years ago committed by Lorenz Meier
parent
commit
93fb02bfa3
  1. 19
      src/modules/fw_att_control/fw_att_control_main.cpp

19
src/modules/fw_att_control/fw_att_control_main.cpp

@ -1045,7 +1045,8 @@ FixedwingAttitudeControl::task_main() @@ -1045,7 +1045,8 @@ FixedwingAttitudeControl::task_main()
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
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)) {
_roll_ctrl.reset_integrator();
@ -1057,7 +1058,8 @@ FixedwingAttitudeControl::task_main() @@ -1057,7 +1058,8 @@ FixedwingAttitudeControl::task_main()
}
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)) {
_pitch_ctrl.reset_integrator();
@ -1089,10 +1091,11 @@ FixedwingAttitudeControl::task_main() @@ -1089,10 +1091,11 @@ FixedwingAttitudeControl::task_main()
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 */
_actuators.control[2] += yaw_manual;
_actuators.control[actuator_controls_s::INDEX_YAW] += yaw_manual;
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
@ -1106,10 +1109,10 @@ FixedwingAttitudeControl::task_main() @@ -1106,10 +1109,10 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!PX4_ISFINITE(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {

Loading…
Cancel
Save