Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: disable turning at low speeds when simple avoidance is active

This commit is contained in:
Randy Mackay 2021-05-06 20:15:32 +09:00
parent b4fd9848df
commit 4f77f7545a
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Rover/mode.cpp
View File

@ -433,8 +433,16 @@ void Mode::navigate_to_waypoint()
const float turn_rate = g2.sailboat.tacking() ? g2.wp_nav.get_pivot_rate() : 0.0f;
calc_steering_to_heading(desired_heading_cd, turn_rate);
} else {
// retrieve turn rate from waypoint controller
float desired_turn_rate_rads = g2.wp_nav.get_turn_rate_rads();
// if simple avoidance is active at very low speed do not attempt to turn
if (g2.avoid.limits_active() && (fabsf(attitude_control.get_desired_speed()) <= attitude_control.get_stop_speed())) {
desired_turn_rate_rads = 0.0f;
}
// call turn rate steering controller
calc_steering_from_turn_rate(g2.wp_nav.get_turn_rate_rads());
calc_steering_from_turn_rate(desired_turn_rate_rads);
}
}