diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index df7a4c886e..0163ee4fb0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1612,10 +1612,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTL print_reject_mode(status, "POSCTL"); } + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1626,7 +1626,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin print_reject_mode(status, "ALTCTL"); } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1638,28 +1638,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_RTL"); + + // fallback to LOITER if home position not set + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_LOITER"); + } else { res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_MISSION"); } - // else fallback to ALTCTL (POSCTL likely will not work too) + // fallback to POSCTL + res = main_state_transition(status, MAIN_STATE_POSCTL); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 8bbb79d5ee..f5eb1e7a5a 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -70,11 +70,10 @@ bool Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { /* set loiter item, don't reuse an existing position setpoint */ - return set_loiter_item(false, pos_sp_triplet);; + return set_loiter_item(pos_sp_triplet); } void Loiter::on_inactive() { } - diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9244063b19..c09f617446 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -223,7 +223,7 @@ Mission::advance_mission() void Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) { - set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); + set_previous_pos_setpoint(pos_sp_triplet); /* try setting onboard mission item */ if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { @@ -233,7 +233,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: onboard mission running"); } _mission_type = MISSION_TYPE_ONBOARD; - _navigator->set_is_in_loiter(false); + _navigator->set_can_loiter_at_sp(false); /* try setting offboard mission item */ } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { @@ -243,7 +243,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: offboard mission running"); } _mission_type = MISSION_TYPE_OFFBOARD; - _navigator->set_is_in_loiter(false); + _navigator->set_can_loiter_at_sp(false); } else { if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_info(_navigator->get_mavlink_fd(), @@ -253,24 +253,14 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) "#audio: no mission available"); } _mission_type = MISSION_TYPE_NONE; + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); - bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; - set_loiter_item(use_current_pos_sp, pos_sp_triplet); + set_loiter_item(pos_sp_triplet); reset_mission_item_reached(); report_mission_finished(); } } -void -Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, - struct position_setpoint_s *previous_pos_sp) -{ - /* reuse current setpoint as previous setpoint */ - if (current_pos_sp->valid) { - memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s)); - } -} - bool Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 322aaf96af..842c8c9ee8 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -106,12 +106,6 @@ private: */ void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); - /** - * Set previous position setpoint - */ - void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, - struct position_setpoint_s *previous_pos_sp); - /** * Try to set the current position setpoint from an onboard mission item * @return true if mission item successfully set diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 08576750c9..8ef7be4496 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -69,20 +69,15 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { - /* don't check landed WPs */ if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return false; + return _navigator_priv->get_vstatus()->condition_landed; } - /* TODO: count turns */ -#if 0 - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { + /* TODO: count turns */ + if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { return false; } -#endif hrt_abstime now = hrt_absolute_time(); @@ -178,48 +173,70 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; - if (item->nav_cmd == NAV_CMD_TAKEOFF) { + switch (item->nav_cmd) { + case NAV_CMD_IDLE: + sp->type = SETPOINT_TYPE_IDLE; + break; + + case NAV_CMD_TAKEOFF: sp->type = SETPOINT_TYPE_TAKEOFF; + break; - } else if (item->nav_cmd == NAV_CMD_LAND) { + case NAV_CMD_LAND: sp->type = SETPOINT_TYPE_LAND; + break; - } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + case NAV_CMD_LOITER_TIME_LIMIT: + case NAV_CMD_LOITER_TURN_COUNT: + case NAV_CMD_LOITER_UNLIMITED: sp->type = SETPOINT_TYPE_LOITER; + break; - } else { + default: sp->type = SETPOINT_TYPE_POSITION; + break; } } -bool -MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) +void +MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet) { - if (_navigator_priv->get_is_in_loiter()) { - /* already loitering, bail out */ - return false; - } + /* reuse current setpoint as previous setpoint */ + if (pos_sp_triplet->current.valid) { + memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s)); + } +} - if (reuse_current_pos_sp && pos_sp_triplet->current.valid) { - /* leave position setpoint as is */ - } else { +bool +MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* don't change setpoint if 'can_loiter_at_sp' flag set */ + if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { /* use current position */ pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon; pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt; pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ + + _navigator_priv->set_can_loiter_at_sp(true); } - pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius(); - pos_sp_triplet->current.loiter_direction = 1; - pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; + if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER + || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius() + || pos_sp_triplet->current.loiter_direction != 1 + || pos_sp_triplet->previous.valid + || !pos_sp_triplet->current.valid + || pos_sp_triplet->next.valid) { + /* position setpoint triplet should be updated */ + pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; + pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius(); + pos_sp_triplet->current.loiter_direction = 1; - _navigator_priv->set_is_in_loiter(true); - return true; + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; + return true; + } + + return false; } - diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 47e6fe81fe..8726964fa3 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -82,14 +82,18 @@ public: */ void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); + /** + * Set previous position setpoint to current setpoint + */ + void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); + /** * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position * - * @param true if the current position setpoint should be re-used * @param the position setpoint triplet to set * @return true if setpoint has changed */ - bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet); + bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet); bool _waypoint_position_reached; bool _waypoint_yaw_reached; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index dadd155271..4709f71965 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -102,7 +102,7 @@ public: /** * Setters */ - void set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; } + void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } /** * Getters @@ -113,7 +113,7 @@ public: int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } Geofence& get_geofence() { return _geofence; } - bool get_is_in_loiter() { return _is_in_loiter; } + bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); } int get_mavlink_fd() { return _mavlink_fd; } @@ -161,7 +161,7 @@ private: NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ - bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ + bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _update_triplet; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d762b8a9dc..e22c4d72df 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -347,7 +347,7 @@ Navigator::task_main() case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: _navigation_mode = nullptr; - _is_in_loiter = false; + _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: _navigation_mode = &_mission; @@ -365,7 +365,7 @@ Navigator::task_main() case NAVIGATION_STATE_TERMINATION: default: _navigation_mode = nullptr; - _is_in_loiter = false; + _can_loiter_at_sp = false; break; } @@ -386,7 +386,7 @@ Navigator::task_main() _update_triplet = true; } - if (_update_triplet ) { + if (_update_triplet) { publish_position_setpoint_triplet(); _update_triplet = false; } @@ -446,329 +446,7 @@ Navigator::status() warnx("Geofence not set"); } } -#if 0 -bool -Navigator::start_none_on_ground() -{ - reset_reached(); - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - - _update_triplet = true; - return true; -} - -bool -Navigator::start_none_in_air() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - - _update_triplet = true; - return true; -} - -bool -Navigator::start_auto_on_ground() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = false; - - _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - - _update_triplet = true; - return true; -} - -bool -Navigator::start_loiter() -{ - /* if no existing item available, use current position */ - if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) { - - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - _pos_sp_triplet.current.alt = _global_pos.alt; - } - _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; - _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; - _pos_sp_triplet.current.loiter_direction = 1; - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); - - _update_triplet = true; - return true; -} - -bool -Navigator::start_mission() -{ - /* start fresh */ - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - - return set_mission_items(); -} - -bool -Navigator::advance_mission() -{ - /* tell mission to move by one */ - _mission.move_to_next(); - - /* now try to set the new mission items, if it fails, it will dispatch loiter */ - return set_mission_items(); -} - -bool -Navigator::set_mission_items() -{ - if (_pos_sp_triplet.current.valid) { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _pos_sp_triplet.previous.valid = true; - } - - bool onboard; - int index; - - /* if we fail to set the current mission, continue to loiter */ - if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - - return false; - } - - /* if we got an RTL mission item, switch to RTL mode and give up */ - if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - return false; - } - - _mission_item_valid = true; - - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - _pos_sp_triplet.current.valid = true; - - - mission_item_s next_mission_item; - - bool last_wp = false; - /* now try to set the next mission item as well, if there is no more next - * this means we're heading to the last waypoint */ - if (_mission.get_next_mission_item(&next_mission_item)) { - /* convert the next mission item and set it valid */ - mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next); - _pos_sp_triplet.next.valid = true; - } else { - last_wp = true; - } - - /* notify user about what happened */ - mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d", - (last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index); - - _update_triplet = true; - - reset_reached(); - - return true; -} - -bool -Navigator::start_rtl() -{ - if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - - _mission_item_valid = true; - - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - _pos_sp_triplet.current.valid = true; - - reset_reached(); - - _update_triplet = true; - return true; - } - - /* if RTL doesn't work, fallback to loiter */ - return false; -} - -bool -Navigator::advance_rtl() -{ - /* tell mission to move by one */ - _rtl.move_to_next(); - - /* now try to set the new mission items, if it fails, it will dispatch loiter */ - if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - - _mission_item_valid = true; - - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - _pos_sp_triplet.current.valid = true; - - reset_reached(); - - _update_triplet = true; - return true; - } - - return false; -} - -bool -Navigator::start_land() -{ - /* TODO: verify/test */ - - reset_reached(); - - /* this state can be requested by commander even if no global position available, - * in his case controller must perform landing without position control */ - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_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; - - _mission_item_valid = true; - - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - - _pos_sp_triplet.next.valid = false; - - _update_triplet = true; - return true; -} -bool -Navigator::check_mission_item_reached() -{ - /* only check if there is actually a mission item to check */ - if (!_mission_item_valid) { - return false; - } - - if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return _vstatus.condition_landed; - } - - /* XXX TODO count turns */ - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { - - // return false; - // } - - uint64_t now = hrt_absolute_time(); - - if (!_waypoint_position_reached) { - float acceptance_radius; - - if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item.acceptance_radius; - - } else { - acceptance_radius = _parameters.acceptance_radius; - } - - if (_do_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; - } - } - } - - if (_waypoint_position_reached && !_waypoint_yaw_reached) { - - /* TODO: removed takeoff, why? */ - if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) { - - /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - - if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ - _waypoint_yaw_reached = true; - } - - } else { - _waypoint_yaw_reached = true; - } - } - - /* check if the current waypoint was reached */ - if (_waypoint_position_reached && _waypoint_yaw_reached) { - - if (_time_first_inside_orbit == 0) { - _time_first_inside_orbit = now; - - if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", - (double)_mission_item.time_inside); - } - } - - /* check if the MAV was long enough inside the waypoint orbit */ - if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) - || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - return true; - } - } - return false; -} - -void -Navigator::reset_reached() -{ - _time_first_inside_orbit = 0; - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - -} -#endif void Navigator::publish_position_setpoint_triplet() { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c1b1d3f097..b7961a260c 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -44,6 +44,7 @@ #include #include +#include #include #include @@ -52,6 +53,8 @@ #include "navigator.h" #include "rtl.h" +#define DELAY_SIGMA 0.01f + RTL::RTL(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), MissionBlock(navigator), @@ -75,7 +78,11 @@ void RTL::on_inactive() { _first_run = true; - _rtl_state = RTL_STATE_NONE; + + /* reset RTL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rtl_state = RTL_STATE_NONE; + } } bool @@ -84,14 +91,33 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool updated = false; if (_first_run) { + _first_run = false; + + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_LANDED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + } + set_rtl_item(pos_sp_triplet); updated = true; - _first_run = false; - } - if ((_rtl_state == RTL_STATE_CLIMB - || _rtl_state == RTL_STATE_RETURN) - && is_mission_item_reached()) { + } else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { advance_rtl(); set_rtl_item(pos_sp_triplet); updated = true; @@ -106,31 +132,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* make sure we have the latest params */ updateParams(); - /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_FINISHED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; - /* otherwise go straight to return */ - } else { - _rtl_state = RTL_STATE_RETURN; - } - } - - /* if switching directly to return state, set altitude setpoint to current altitude */ - if (_rtl_state == RTL_STATE_RETURN) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } + set_previous_pos_setpoint(pos_sp_triplet); + _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { - float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); _mission_item.lat = _navigator->get_global_position()->lat; @@ -147,27 +153,28 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - _navigator->set_is_in_loiter(false); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", (int)(climb_alt - _navigator->get_home_position()->alt)); break; } - case RTL_STATE_RETURN: { + case RTL_STATE_RETURN: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; + // don't change altitude - /* TODO: add this again */ - // don't change altitude - // if (_pos_sp_triplet.previous.valid) { - // /* if previous setpoint is valid then use it to calculate heading to home */ - // _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); + if (pos_sp_triplet->previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint( + pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, + _mission_item.lat, _mission_item.lon); - // } else { - // /* else use current position */ - // _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); - // } + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + } _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -177,14 +184,12 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - _navigator->set_is_in_loiter(false); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } - case RTL_STATE_DESCEND: { + case RTL_STATE_DESCEND: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; @@ -194,20 +199,45 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _param_acceptance_radius.get(); - _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = _param_land_delay.get() > -0.001f; + _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; - _navigator->set_is_in_loiter(true); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } - case RTL_STATE_LAND: { + case RTL_STATE_LOITER: { + bool autoland = _param_land_delay.get() > -DELAY_SIGMA; + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + + if (autoland) { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside); + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); + } + break; + } + + case RTL_STATE_LAND: { _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; @@ -222,30 +252,38 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - _navigator->set_is_in_loiter(false); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); break; } - case RTL_STATE_FINISHED: { - /* nothing to do, report fail */ + case RTL_STATE_LANDED: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_IDLE; + _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + break; } default: break; } - if (_rtl_state == RTL_STATE_FINISHED) { - pos_sp_triplet->current.valid = false; - pos_sp_triplet->next.valid = false; - } else { - /* if not finished, convert mission item to current position setpoint and make it valid */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - reset_mission_item_reached(); - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; - } + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + reset_mission_item_reached(); + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; } void @@ -262,18 +300,20 @@ RTL::advance_rtl() case RTL_STATE_DESCEND: /* only go to land if autoland is enabled */ - if (_param_land_delay.get() < 0) { - _rtl_state = RTL_STATE_FINISHED; + if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { + _rtl_state = RTL_STATE_LOITER; + } else { _rtl_state = RTL_STATE_LAND; } break; - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_FINISHED; + case RTL_STATE_LOITER: + _rtl_state = RTL_STATE_LAND; break; - case RTL_STATE_FINISHED: + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; break; default: diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index bcccf74548..c4286b391f 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -97,8 +97,9 @@ private: RTL_STATE_CLIMB, RTL_STATE_RETURN, RTL_STATE_DESCEND, + RTL_STATE_LOITER, RTL_STATE_LAND, - RTL_STATE_FINISHED, + RTL_STATE_LANDED, } _rtl_state; control::BlockParamFloat _param_return_alt; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index d25db3a772..d9dd61df12 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -50,6 +50,7 @@ /* compatible to mavlink MAV_CMD */ enum NAV_CMD { + NAV_CMD_IDLE=0, NAV_CMD_WAYPOINT=16, NAV_CMD_LOITER_UNLIMITED=17, NAV_CMD_LOITER_TURN_COUNT=18,