Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: don't use GPS speed to detect reversing

Browse Source 
instead pass reverse to attitude controller based on desired speed
This commit is contained in:
Daniel Widmann 2017-11-08 14:37:50 +09:00 committed by Randy Mackay
parent 49887235df
commit bc7cf97b4e
3 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/mode.cpp
View File

@ -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); 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 // 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); g2.motors.set_steering(steering_out * 4500.0f);
} }

2
APMrover2/mode_auto.cpp
View File

@ -65,7 +65,7 @@ void ModeAuto::update()
if (!_reached_heading) { if (!_reached_heading) {
// run steering and throttle controllers // run steering and throttle controllers
const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f)); 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); g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true); calc_throttle(_desired_speed, true);
// check if we have reached target // check if we have reached target

4
APMrover2/mode_guided.cpp
View File

@ -50,7 +50,7 @@ void ModeGuided::update()
if (have_attitude_target) { if (have_attitude_target) {
// run steering and throttle controllers // run steering and throttle controllers
const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f)); 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); g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true); calc_throttle(_desired_speed, true);
} else { } else {
@ -69,7 +69,7 @@ void ModeGuided::update()
} }
if (have_attitude_target) { if (have_attitude_target) {
// run steering and throttle controllers // 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); g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true); calc_throttle(_desired_speed, true);
} else { } else {