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

Rover: fix calc-steering-to-waypoint go use real heading without reverse

Browse Source
This commit is contained in:
Randy Mackay 2018-05-06 13:41:37 +09:00
parent 3645e18243
commit 0a29e4eac5
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
APMrover2/mode.cpp
View File

@ -356,10 +356,10 @@ void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct
float desired_lat_accel = rover.nav_controller->lateral_acceleration();
float desired_heading = rover.nav_controller->target_bearing_cd();
if (reversed) {
_yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor + 18000);
} else {
_yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);
desired_heading = wrap_360_cd(desired_heading + 18000);
desired_lat_accel *= -1.0f;
}
_yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);
if (rover.use_pivot_steering(_yaw_error_cd)) {
// for pivot turns use heading controller
@ -376,6 +376,7 @@ void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct
void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reversed)
{
// add obstacle avoidance response to lateral acceleration target
// ToDo: replace this type of object avoidance with path planning
if (!reversed) {
lat_accel += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g;
}