dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

RoverPositionContro: Enable driving backwards in attitude control

This commit is contained in:
Jannik Beyerstedt 2019-11-11 11:56:48 +01:00 committed by Lorenz Meier
parent ba99ef0d3b
commit 38103654e9
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -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;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, 1.0f);
} else {
// reverse steering, if driving backwards
_act_controls.control[actuator_controls_s::INDEX_YAW] = -control_effort;
}
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = control_throttle;
}