diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-02 16:44:31 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-02 16:44:31 +0400 |
| commit | 911c2bdeeeb624c111c680b228e49d27391af484 (patch) | |
| tree | e6a4f01d01d6d954229f16ba126813e8d169663a /src/modules/navigator | |
| parent | 6a5d5f7136c8f317594bcc768131fe1779eabf2c (diff) | |
| download | px4-firmware-911c2bdeeeb624c111c680b228e49d27391af484.tar.gz px4-firmware-911c2bdeeeb624c111c680b228e49d27391af484.tar.bz2 px4-firmware-911c2bdeeeb624c111c680b228e49d27391af484.zip | |
navigator: takeoff fixes
Diffstat (limited to 'src/modules/navigator')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 20 |
1 files changed, 14 insertions, 6 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a6f33b5e7..ca807df6f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1053,13 +1053,19 @@ Navigator::advance_mission() /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; - /* check if we really need takeoff */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; + /* calculate desired takeoff altitude AMSL */ + float takeoff_alt_amsl = _mission_item_triplet.current.altitude; if (_mission_item_triplet.current.altitude_is_relative) - wp_alt_amsl += _home_pos.altitude; - if (_vstatus.condition_landed || _global_pos.alt - _home_pos.altitude < _parameters.takeoff_alt || - (_mission_item_triplet.next.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && _global_pos.alt > wp_alt_amsl)) { - /* force TAKEOFF if (landed or near ground) and waypoint altitude is more than current */ + takeoff_alt_amsl += _home_pos.altitude; + + if (_vstatus.condition_landed) { + /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ + takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt); + } + + /* check if we really need takeoff */ + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) { + /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; /* move current mission item to next */ @@ -1071,6 +1077,8 @@ Navigator::advance_mission() _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.altitude = takeoff_alt_amsl; + _mission_item_triplet.current.altitude_is_relative = false; } } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ |