Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: fix calc_steering_to_heading rate_max handling

Browse Source 
units mix up led to deg/s parameter value being applies as rad/s
This commit is contained in:
Randy Mackay 2018-09-10 16:25:27 +09:00
parent d7d6c6ab04
commit cd64ce45ac
2 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
APMrover2/mode.cpp
View File

@ -446,14 +446,15 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse
} }
// calculate steering output to drive towards desired heading // calculate steering output to drive towards desired heading
void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max, bool reversed) // rate_max is a maximum turn rate in deg/s. set to zero to use default turn rate limits
void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max_degs)
{ {
// calculate yaw error so it can be used for reporting and slowing the vehicle // calculate yaw error so it can be used for reporting and slowing the vehicle
_yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor); _yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor);
// call heading controller // call heading controller
const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f), const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f),
rate_max, radians(rate_max_degs),
g2.motors.limit.steer_left, g2.motors.limit.steer_left,
g2.motors.limit.steer_right, g2.motors.limit.steer_right,
rover.G_Dt); rover.G_Dt);

3
APMrover2/mode.h
View File

@ -141,7 +141,8 @@ protected:
void calc_steering_from_lateral_acceleration(float lat_accel, bool reversed = false); void calc_steering_from_lateral_acceleration(float lat_accel, bool reversed = false);
// calculate steering output to drive towards desired heading // calculate steering output to drive towards desired heading
void calc_steering_to_heading(float desired_heading_cd, float rate_max, bool reversed = false); // rate_max is a maximum turn rate in deg/s. set to zero to use default turn rate limits
void calc_steering_to_heading(float desired_heading_cd, float rate_max_degs = 0.0f);
// calculates the amount of throttle that should be output based // calculates the amount of throttle that should be output based
// on things like proximity to corners and current speed // on things like proximity to corners and current speed