From 7bae7795834b3a43af4a558ad81db1d6a9065c83 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 11 Nov 2019 11:56:48 +0100 Subject: [PATCH] RoverPositionContro: Enable driving backwards in attitude control --- .../rover_pos_control/RoverPositionControl.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 738162f4233e..5d8bd0335216 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -335,11 +335,17 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi float control_effort = euler_sp(2) / _param_max_turn_angle.get(); control_effort = math::constrain(control_effort, -1.0f, 1.0f); - _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + const float control_throttle = math::constrain(att_sp.thrust_body[0], -1.0f, 1.0f); - const float control_throttle = att_sp.thrust_body[0]; + if (control_throttle >= 0.0f) { + _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + + } else { + // reverse steering, if driving backwards + _act_controls.control[actuator_controls_s::INDEX_YAW] = -control_effort; + } - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, 1.0f); + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = control_throttle; }