From bc7cf97b4ed8311df5ec140a04256b923428ec10 Mon Sep 17 00:00:00 2001 From: Daniel Widmann Date: Wed, 8 Nov 2017 14:37:50 +0900 Subject: [PATCH] Rover: don't use GPS speed to detect reversing instead pass reverse to attitude controller based on desired speed --- APMrover2/mode.cpp | 2 +- APMrover2/mode_auto.cpp | 2 +- APMrover2/mode_guided.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index c08a0a8549..6ee583dddc 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -228,6 +228,6 @@ void Mode::calc_nav_steer(bool reversed) lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS); // send final steering command to motor library - float steering_out = attitude_control.get_steering_out_lat_accel(lateral_acceleration, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right); + float steering_out = attitude_control.get_steering_out_lat_accel(lateral_acceleration, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); g2.motors.set_steering(steering_out * 4500.0f); } diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index 5289a3eda8..b3351104c0 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -65,7 +65,7 @@ void ModeAuto::update() if (!_reached_heading) { // run steering and throttle controllers const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f)); - const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right); + const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true); // check if we have reached target diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index 8bacd3a634..d6ea771168 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -50,7 +50,7 @@ void ModeGuided::update() if (have_attitude_target) { // run steering and throttle controllers const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f)); - const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right); + const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true); } else { @@ -69,7 +69,7 @@ void ModeGuided::update() } if (have_attitude_target) { // run steering and throttle controllers - float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right); + float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true); } else {