diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-08-16 11:16:27 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-08-16 11:16:27 +0200 |
commit | ee254be845f16531990f4574ca7505ed359c4a61 (patch) | |
tree | 6ed2ff79415d6b60690f1616cf537d33f6e8db4a /src/modules | |
parent | e9b0ee75015cf3544a3dab2015d7af77f53bd23e (diff) | |
download | px4-firmware-ee254be845f16531990f4574ca7505ed359c4a61.tar.gz px4-firmware-ee254be845f16531990f4574ca7505ed359c4a61.tar.bz2 px4-firmware-ee254be845f16531990f4574ca7505ed359c4a61.zip |
navigator: skip takeoff if already above takeoff altitude
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/navigator/mission.cpp | 67 | ||||
-rw-r--r-- | src/modules/navigator/mission.h | 4 |
2 files changed, 41 insertions, 30 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3ed..98a4340e1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -107,6 +107,7 @@ Mission::on_inactive() update_offboard_mission(); } + /* require takeoff after non-loiter or landing */ if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { _need_takeoff = true; } @@ -399,7 +400,7 @@ Mission::set_mission_items() takeoff_alt += _navigator->get_home_position()->alt; } - /* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); @@ -407,43 +408,53 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); + /* check if we already above takeoff altitude */ + if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - } else { - /* set current position setpoint from mission item */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); + return; - /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { - _need_takeoff = true; + } else { + /* skip takeoff */ + _takeoff = false; } + } - _navigator->set_can_loiter_at_sp(false); - reset_mission_item_reached(); + /* set current position setpoint from mission item */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); - } - // TODO: report onboard mission item somehow + /* require takeoff after landing or idle */ + if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + _need_takeoff = true; + } - /* try to read next mission item */ - struct mission_item_s mission_item_next; + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow - } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; - } + /* try to read next mission item */ + struct mission_item_s mission_item_next; + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; } _navigator->set_position_setpoint_triplet_updated(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a1155..1edaa5c8a 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -142,8 +142,8 @@ private: int _current_onboard_mission_index; int _current_offboard_mission_index; - bool _need_takeoff; - bool _takeoff; + bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + bool _takeoff; /**< takeoff state flag */ orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; |