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

Rover: add calc_steering_to_heading

Browse Source 
removes some duplicate code in Guided and Auto
This commit is contained in:
Randy Mackay 2017-12-23 14:54:35 +09:00
parent af7bb7cbd7
commit 49493fe6a2
4 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
APMrover2/mode.cpp
View File

@ -364,3 +364,12 @@ void Mode::calc_steering_from_lateral_acceleration(bool reversed)
float steering_out = attitude_control.get_steering_out_lat_accel(_desired_lat_accel, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); float steering_out = attitude_control.get_steering_out_lat_accel(_desired_lat_accel, 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);
} }
// calculate steering output to drive towards desired heading
void Mode::calc_steering_to_heading(float desired_heading_cd, bool reversed)
{
// calculate yaw error (in radians) and pass to steering angle controller
const float yaw_error = wrap_PI(radians((desired_heading_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, reversed);
g2.motors.set_steering(steering_out * 4500.0f);
}

2
APMrover2/mode.h
View File

@ -113,6 +113,8 @@ protected:
// calculate steering angle given a desired lateral acceleration // calculate steering angle given a desired lateral acceleration
void calc_steering_from_lateral_acceleration(bool reversed = false); void calc_steering_from_lateral_acceleration(bool reversed = false);
// calculate steering output to drive towards desired heading
void calc_steering_to_heading(float desired_heading_cd, bool reversed = false);
// 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

8
APMrover2/mode_auto.cpp
View File

@ -69,12 +69,10 @@ 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)); calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
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); calc_throttle(_desired_speed, true);
// check if we have reached target // check if we have reached within 5 degrees of target
_reached_heading = (fabsf(yaw_error) < radians(5)); _reached_heading = (fabsf(_desired_yaw_cd - ahrs.yaw_sensor) < 500);
} else { } else {
stop_vehicle(); stop_vehicle();
} }

4
APMrover2/mode_guided.cpp
View File

@ -46,9 +46,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)); calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
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); calc_throttle(_desired_speed, true);
} else { } else {
stop_vehicle(); stop_vehicle();