fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:58:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator mission_block check range for loiter exit

Browse Source 
- fixes #7510
This commit is contained in:
Daniel Agar
2017-06-29 21:28:26 +02:00
committed by Lorenz Meier
parent 6fcd7d9529
commit c8189b9c23
1 changed files with 12 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+12 -11
View File
@@ -363,31 +363,32 @@ MissionBlock::is_mission_item_reached()
if ((get_time_inside(_mission_item) < FLT_EPSILON) ||
(now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current;
const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
const float range = get_distance_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon);
// exit xtrack location
if (_mission_item.loiter_exit_xtrack &&
// reset lat/lon of loiter waypoint so vehicle follows a tangent
if (_mission_item.loiter_exit_xtrack && next_sp.valid && PX4_ISFINITE(range) &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
// reset lat/lon of loiter waypoint so vehicle follows a tangent
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
const struct position_setpoint_s *next_sp = &_navigator->get_position_setpoint_triplet()->next;
float range = get_distance_to_next_waypoint(curr_sp->lat, curr_sp->lon, next_sp->lat, next_sp->lon);
float bearing = get_bearing_to_next_waypoint(curr_sp->lat, curr_sp->lon, next_sp->lat, next_sp->lon);
float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon);
float inner_angle = M_PI_2_F - asinf(_mission_item.loiter_radius / range);
// Compute "ideal" tangent origin
if (curr_sp->loiter_direction > 0) {
if (curr_sp.loiter_direction > 0) {
bearing -= inner_angle;
} else {
bearing += inner_angle;
}
// Replace current setpoint lat/lon with tangent coordinate
waypoint_from_heading_and_distance(curr_sp->lat, curr_sp->lon,
bearing, curr_sp->loiter_radius,
&curr_sp->lat, &curr_sp->lon);
waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon,
bearing, curr_sp.loiter_radius,
&curr_sp.lat, &curr_sp.lon);
}
return true;