forked from Archive/PX4-Autopilot
mission block: fix incorrectly calculated ccw loiter exit (#19487)
* mission block: fix incorrectly calculated ccw loiter exit * mission block: update comment on orbit exit location
This commit is contained in:
parent
5e05d98fe2
commit
d5a6174e7f
|
@ -466,9 +466,10 @@ MissionBlock::is_mission_item_reached()
|
|||
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
|
||||
|
||||
float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon);
|
||||
// We should not use asinf outside of [-1..1].
|
||||
const float ratio = math::constrain(_mission_item.loiter_radius / range, -1.0f, 1.0f);
|
||||
float inner_angle = M_PI_2_F - asinf(ratio);
|
||||
|
||||
// calculate (positive) angle between current bearing vector (orbit center to next waypoint) and vector pointing to tangent exit location
|
||||
const float ratio = math::min(fabsf(_mission_item.loiter_radius / range), 1.0f);
|
||||
float inner_angle = acosf(ratio);
|
||||
|
||||
// Compute "ideal" tangent origin
|
||||
if (curr_sp.loiter_direction > 0) {
|
||||
|
|
Loading…
Reference in New Issue