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

Rover: avoid divide by zero in desired speed at waypoint calcs

Browse Source
This commit is contained in:
Randy Mackay 2018-03-14 11:58:54 +09:00
parent e31f19a9e1
commit 5662aa1b75
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
APMrover2/mode.cpp
View File

@ -115,13 +115,13 @@ void Mode::set_desired_location(const struct Location& destination, float next_l
// set final desired speed
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
// if not turning can continue at full speed
if (is_zero(next_leg_bearing_cd)) {
const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
if (is_zero(turn_angle_cd)) {
// if not turning can continue at full speed
_desired_speed_final = _desired_speed;
} else {
// calculate maximum speed that keeps overshoot within bounds
const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
}