diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-01 13:31:27 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-01 13:31:27 +0400 |
| commit | 3bc94f1fe6edba3a48bcb799f97c51168a16bf76 (patch) | |
| tree | 07a0e7d227d252e1abd8b11d448c0cf2ebba267c /src/modules/navigator | |
| parent | 7f9a7ffe82372d311a7947284871d794a8934493 (diff) | |
| download | px4-firmware-3bc94f1fe6edba3a48bcb799f97c51168a16bf76.tar.gz px4-firmware-3bc94f1fe6edba3a48bcb799f97c51168a16bf76.tar.bz2 px4-firmware-3bc94f1fe6edba3a48bcb799f97c51168a16bf76.zip | |
navigator: takeoff fix
Diffstat (limited to 'src/modules/navigator')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 16 |
1 files changed, 13 insertions, 3 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3380c232b..982ebefcc 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1173,8 +1173,12 @@ Navigator::mission_item_reached() float dist_z = -1.0f; // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { + + // current relative or AMSL altitude depending on _mission_item_triplet.current.altitude_is_relative + float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt; + dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, current_alt, &dist_xy, &dist_z); // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); @@ -1193,8 +1197,14 @@ Navigator::mission_item_reached() // // XXX TODO // } - if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { - _waypoint_position_reached = true; + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + /* require only altitude for takeoff */ + if (current_alt > _mission_item_triplet.current.altitude) + _waypoint_position_reached = true; + } else { + if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { + _waypoint_position_reached = true; + } } /* check if required yaw reached */ |