Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Rover: steering mode direction fix when reversing

Browse Source 
this corrects the rotation direction when moving backwards in steering mode so that it's consistent with manual mode
This commit is contained in:
Randy Mackay 2017-07-21 13:17:19 +09:00
parent f9186de0ca
commit 3d13d98410
1 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
APMrover2/mode_steering.cpp
View File

@ -3,6 +3,11 @@
void ModeSteering::update() void ModeSteering::update()
{ {
// convert pilot throttle input to desired speed
// speed in proportion to cruise speed, up to 50% throttle, then uses nudging above that.
float target_speed = channel_throttle->get_control_in() * 0.01f * 2.0f * g.speed_cruise;
target_speed = constrain_float(target_speed, -g.speed_cruise, g.speed_cruise);
// in steering mode we control lateral acceleration directly. We first calculate the maximum lateral // in steering mode we control lateral acceleration directly. We first calculate the maximum lateral
// acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn. // acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn.
// We get the radius of turn from half the STEER2SRV_P. // We get the radius of turn from half the STEER2SRV_P.
@ -15,17 +20,17 @@ void ModeSteering::update()
// convert pilot steering input to desired lateral acceleration // convert pilot steering input to desired lateral acceleration
lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f); lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f);
// run steering controller // reverse target lateral acceleration if backing up
calc_nav_steer(); if (is_negative(target_speed)) {
lateral_acceleration = -lateral_acceleration;
// convert pilot throttle input to desired speed }
// speed in proportion to cruise speed, up to 50% throttle, then uses nudging above that.
float target_speed = channel_throttle->get_control_in() * 0.01f * 2.0f * g.speed_cruise;
target_speed = constrain_float(target_speed, -g.speed_cruise, g.speed_cruise);
// mark us as in_reverse when using a negative throttle to stop AHRS getting off // mark us as in_reverse when using a negative throttle to stop AHRS getting off
rover.set_reverse(is_negative(target_speed)); rover.set_reverse(is_negative(target_speed));
// run steering controller
calc_nav_steer();
// run speed to throttle output controller // run speed to throttle output controller
calc_throttle(target_speed); calc_throttle(target_speed);
} }