diff options
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 91 |
1 files changed, 48 insertions, 43 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 73514185b..46e7c2e18 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -237,8 +237,7 @@ private: RTL_STATE_NONE = 0, RTL_STATE_CLIMB, RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LAND + RTL_STATE_DESCEND }; enum RTLState _rtl_state; @@ -304,6 +303,7 @@ private: void start_mission(); void start_rtl(); void start_land(); + void start_land_home(); /** * Guards offboard mission @@ -688,7 +688,8 @@ Navigator::task_main() /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ - if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { + if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -745,7 +746,8 @@ Navigator::task_main() break; case NAV_STATE_RTL: - if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { + if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -994,7 +996,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, @@ -1040,8 +1042,8 @@ Navigator::start_ready() _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state != RTL_STATE_LAND) { - /* allow RTL if landed not at home */ + if (_rtl_state != RTL_STATE_DESCEND) { + /* reset RTL state if landed not at home */ _rtl_state = RTL_STATE_NONE; } @@ -1074,11 +1076,6 @@ Navigator::start_loiter() } _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; - - if (_rtl_state == RTL_STATE_LAND) { - /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ - _rtl_state = RTL_STATE_DESCEND; - } } _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; @@ -1261,6 +1258,36 @@ Navigator::start_land() } void +Navigator::start_land_home() +{ + /* land to home position, should be called when hovering above home, from RTL state */ + _do_takeoff = false; + _reset_loiter_pos = true; + + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; +} + +void Navigator::set_rtl_item() { switch (_rtl_state) { @@ -1350,33 +1377,6 @@ Navigator::set_rtl_item() break; } - case RTL_STATE_LAND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } - default: { mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); start_loiter(); @@ -1563,10 +1563,15 @@ Navigator::on_mission_item_reached() } else if (myState == NAV_STATE_RTL) { /* RTL completed */ - if (_rtl_state == RTL_STATE_LAND) { - /* landed at home position */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); - dispatch(EVENT_READY_REQUESTED); + if (_rtl_state == RTL_STATE_DESCEND) { + /* hovering above home position, land if needed or loiter */ + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + if (_mission_item.autocontinue) { + dispatch(EVENT_LAND_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } } else { /* next RTL step */ |