forked from Archive/PX4-Autopilot
navigator: takeoff fix, always reach takeoff altitude, even if first waypoint is lower
This commit is contained in:
parent
1ce1ece0bb
commit
4ee647015a
|
@ -1477,27 +1477,27 @@ Navigator::check_mission_item_reached()
|
|||
acceptance_radius = _parameters.acceptance_radius;
|
||||
}
|
||||
|
||||
float dist = -1.0f;
|
||||
float dist_xy = -1.0f;
|
||||
float dist_z = -1.0f;
|
||||
|
||||
/* calculate AMSL altitude for this waypoint */
|
||||
float wp_alt_amsl = _mission_item.altitude;
|
||||
|
||||
if (_mission_item.altitude_is_relative)
|
||||
wp_alt_amsl += _home_pos.alt;
|
||||
|
||||
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
|
||||
(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
|
||||
&dist_xy, &dist_z);
|
||||
|
||||
if (_do_takeoff) {
|
||||
if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
|
||||
/* require only altitude for takeoff */
|
||||
/* require only altitude for takeoff */
|
||||
if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
float dist = -1.0f;
|
||||
float dist_xy = -1.0f;
|
||||
float dist_z = -1.0f;
|
||||
|
||||
/* calculate AMSL altitude for this waypoint */
|
||||
float wp_alt_amsl = _mission_item.altitude;
|
||||
|
||||
if (_mission_item.altitude_is_relative)
|
||||
wp_alt_amsl += _home_pos.alt;
|
||||
|
||||
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
|
||||
(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
|
||||
&dist_xy, &dist_z);
|
||||
|
||||
if (dist >= 0.0f && dist <= acceptance_radius) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue