diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9bec297659..de9e2568ae 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -123,6 +123,9 @@ Mission::on_inactive() _offboard_mission.dataman_id = mission_state.dataman_id; _offboard_mission.count = mission_state.count; _current_offboard_mission_index = mission_state.current_seq; + + // find and store landing start marker (if available) + find_offboard_land_start(); } /* On init let's check the mission, maybe there is already one available. */ @@ -251,18 +254,19 @@ Mission::on_active() } bool -Mission::set_current_offboard_mission_index(unsigned index) +Mission::set_current_offboard_mission_index(uint16_t index) { - if (index < _offboard_mission.count) { + if (_navigator->get_mission_result()->valid && + (index != _current_offboard_mission_index) && (index < _offboard_mission.count)) { _current_offboard_mission_index = index; - set_current_offboard_mission_item(); // update mission items if already in active mission if (_navigator->is_planned_mission()) { // prevent following "previous - current" line _navigator->get_position_setpoint_triplet()->previous.valid = false; _navigator->get_position_setpoint_triplet()->current.valid = false; + _navigator->get_position_setpoint_triplet()->next.valid = false; set_mission_items(); } @@ -272,11 +276,11 @@ Mission::set_current_offboard_mission_index(unsigned index) return false; } -int +bool Mission::find_offboard_land_start() { - /* find the first MAV_CMD_DO_LAND_START and return the index - * return -1 if not found + /* return true if a MAV_CMD_DO_LAND_START is found and internally save the index + * return false if not found * * TODO: implement full spec and find closest landing point geographically */ @@ -289,15 +293,48 @@ Mission::find_offboard_land_start() if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return -1; + PX4_ERR("dataman read failure"); + break; } if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { - return i; + _land_start_available = true; + _land_start_index = i; + return true; } } - return -1; + _land_start_available = false; + return false; +} + +bool +Mission::land_start() +{ + // if not currently landing, jump to do_land_start + if (_land_start_available) { + if (landing()) { + return true; + + } else { + set_current_offboard_mission_index(get_land_start_index()); + return landing(); + } + } + + return false; +} + +bool +Mission::landing() +{ + // vehicle is currently landing if + // mission valid, still flying, and in the landing portion of mission + + const bool mission_valid = _navigator->get_mission_result()->valid; + const bool on_landing_stage = _land_start_available && (_current_offboard_mission_index >= get_land_start_index()); + + return mission_valid && on_landing_stage; } void @@ -405,6 +442,9 @@ Mission::update_offboard_mission() PX4_ERR("mission check failed"); } + // find and store landing start marker (if available) + find_offboard_land_start(); + set_current_offboard_mission_item(); } @@ -1128,10 +1168,8 @@ Mission::altitude_sp_foh_update() pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; } - // we set altitude directly so we can run this in parallel to the heading update _navigator->set_position_setpoint_triplet_updated(); - } void @@ -1192,10 +1230,8 @@ Mission::do_abort_landing() (int)(alt_sp - alt_landing)); // reset mission index to start of landing - int land_start_index = find_offboard_land_start(); - - if (land_start_index != -1) { - _current_offboard_mission_index = land_start_index; + if (_land_start_available) { + _current_offboard_mission_index = get_land_start_index(); } else { // move mission index back (landing approach point) @@ -1437,12 +1473,15 @@ Mission::check_mission_valid(bool force) _missionFeasibilityChecker.checkMissionFeasible(_offboard_mission, _param_dist_1wp.get(), _param_dist_between_wps.get(), - false); + _navigator->mission_landing_required()); _navigator->get_mission_result()->seq_total = _offboard_mission.count; _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); _home_inited = _navigator->home_position_valid(); + + // find and store landing start marker (if available) + find_offboard_land_start(); } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 1652e98965..671c43a72e 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -91,9 +91,12 @@ public: MISSION_YAWMODE_MAX = 5 }; - bool set_current_offboard_mission_index(unsigned index); + bool set_current_offboard_mission_index(uint16_t index); - int find_offboard_land_start(); + bool land_start(); + bool landing(); + + uint16_t get_land_start_index() const { return _land_start_index; } private: /** @@ -236,6 +239,11 @@ private: */ void generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw); + /** + * Find and store the index of the landing sequence (DO_LAND_START) + */ + bool find_offboard_land_start(); + control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; @@ -247,8 +255,13 @@ private: struct mission_s _onboard_mission {}; struct mission_s _offboard_mission {}; - int _current_onboard_mission_index{-1}; - int _current_offboard_mission_index{-1}; + int32_t _current_onboard_mission_index{-1}; + int32_t _current_offboard_mission_index{-1}; + + // track location of planned mission landing + bool _land_start_available{false}; + uint16_t _land_start_index{UINT16_MAX}; /**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */ + bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ enum { diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 85b51dbfe0..722a65f493 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -664,22 +664,6 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio item->origin = ORIGIN_ONBOARD; } -void -MissionBlock::set_current_position_item(struct mission_item_s *item) -{ - item->nav_cmd = NAV_CMD_WAYPOINT; - item->lat = _navigator->get_global_position()->lat; - item->lon = _navigator->get_global_position()->lon; - item->altitude_is_relative = false; - item->altitude = _navigator->get_global_position()->alt; - item->yaw = NAN; - item->loiter_radius = _navigator->get_loiter_radius(); - item->acceptance_radius = _navigator->get_acceptance_radius(); - item->time_inside = 0.0f; - item->autocontinue = true; - item->origin = ORIGIN_ONBOARD; -} - void MissionBlock::set_idle_item(struct mission_item_s *item) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 8d621ad34c..ad5023f5c6 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -106,8 +106,6 @@ protected: */ void set_land_item(struct mission_item_s *item, bool at_current_location); - void set_current_position_item(struct mission_item_s *item); - /** * Set idle mission item */ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index e627a4fe0f..ba1efbd1ed 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -238,12 +238,18 @@ public: void set_mission_failure(const char *reason); - bool is_planned_mission() { return _navigation_mode == &_mission; } + // MISSION + bool is_planned_mission() const { return _navigation_mode == &_mission; } + bool on_mission_landing() { return _mission.landing(); } + bool start_mission_landing() { return _mission.land_start(); } + + // RTL + bool mission_landing_required() { return _rtl.mission_landing_required(); } bool abort_landing(); + // Param access float get_loiter_min_alt() const { return _param_loiter_min_alt.get(); } - bool force_vtol() const { return _vstatus.is_vtol && !_vstatus.is_rotary_wing && _param_force_vtol.get(); } private: @@ -261,9 +267,9 @@ private: int _onboard_mission_sub{-1}; /**< onboard mission subscription */ int _param_update_sub{-1}; /**< param update subscription */ int _sensor_combined_sub{-1}; /**< sensor combined subscription */ + int _traffic_sub{-1}; /**< traffic subscription */ int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */ - int _vstatus_sub{-1}; /**< vehicle status subscription */ - int _traffic_sub{-1}; /**< traffic subscription */ + int _vstatus_sub{-1}; /**< vehicle status subscription */ orb_advert_t _geofence_result_pub{nullptr}; orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 107c184a4d..abbafc1b7b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -476,25 +476,23 @@ Navigator::task_main() /* find NAV_CMD_DO_LAND_START in the mission and * use MAV_CMD_MISSION_START to start the mission there */ - int land_start = _mission.find_offboard_land_start(); - - if (land_start != -1) { + if (_mission.land_start()) { vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; - vcmd.param1 = land_start; + vcmd.param1 = _mission.get_land_start_index(); publish_vehicle_cmd(&vcmd); } else { - PX4_WARN("planned landing not available"); + PX4_WARN("planned mission landing not available"); } publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { - if (_mission_result.valid && - PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) { - - _mission.set_current_offboard_mission_index(cmd.param1); + if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) { + if (!_mission.set_current_offboard_mission_index(cmd.param1)) { + PX4_WARN("CMD_MISSION_START failed"); + } } // CMD_MISSION_START is acknowledged by commander @@ -625,7 +623,15 @@ Navigator::task_main() case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; - navigation_mode_new = &_rtl; + + // if RTL is set to use a mission landing and mission has a planned landing, then use MISSION + if (mission_landing_required() && on_mission_landing()) { + navigation_mode_new = &_mission; + + } else { + navigation_mode_new = &_rtl; + } + break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index eb213ddd7f..f35f8f3d87 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -55,7 +55,8 @@ RTL::RTL(Navigator *navigator, const char *name) : _param_return_alt(this, "RTL_RETURN_ALT", false), _param_descend_alt(this, "RTL_DESCEND_ALT", false), _param_land_delay(this, "RTL_LAND_DELAY", false), - _param_rtl_min_dist(this, "RTL_MIN_DIST", false) + _param_rtl_min_dist(this, "RTL_MIN_DIST", false), + _param_rtl_land_type(this, "RTL_LAND_TYPE", false) { } @@ -66,6 +67,13 @@ RTL::on_inactive() _rtl_state = RTL_STATE_NONE; } +bool +RTL::mission_landing_required() +{ + // returns true if navigator should use planned mission landing + return (_param_rtl_land_type.get() == 1); +} + void RTL::on_activation() { @@ -73,6 +81,10 @@ RTL::on_activation() // for safety reasons don't go into RTL if landed _rtl_state = RTL_STATE_LANDED; + } else if (mission_landing_required() && _navigator->on_mission_landing()) { + // RTL straight to RETURN state, but mission will takeover for landing + _rtl_state = RTL_STATE_RETURN; + } else if ((_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) || _rtl_alt_min) { @@ -106,6 +118,22 @@ RTL::set_return_alt_min(bool min) void RTL::set_rtl_item() { + // RTL_TYPE: mission landing + // landing using planned mission landing, fly to DO_LAND_START instead of returning HOME + // do nothing, let navigator takeover with mission landing + if (mission_landing_required()) { + if (_rtl_state > RTL_STATE_CLIMB) { + if (_navigator->start_mission_landing()) { + mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing"); + return; + + } else { + // otherwise use regular RTL + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unable to use mission landing"); + } + } + } + _navigator->set_can_loiter_at_sp(false); const home_position_s &home = *_navigator->get_home_position(); diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index b33d3ef037..ebac5f54a4 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -58,6 +58,8 @@ public: void set_return_alt_min(bool min); + bool mission_landing_required(); + private: /** * Set the RTL item @@ -86,6 +88,7 @@ private: control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; control::BlockParamFloat _param_rtl_min_dist; + control::BlockParamInt _param_rtl_land_type; }; #endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index ea9bb1c1ad..1cf279e0b2 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -103,3 +103,14 @@ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); * @group Return To Land */ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f); + +/** + * RTL land location + * + * Land at the home location or planned mission landing + * + * @value 0 Home Position + * @value 1 Planned Landing (Mission) + * @group Return To Land + */ +PARAM_DEFINE_INT32(RTL_LAND_TYPE, 0);