diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index adb1e7f0d7..b6f0558f12 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -376,17 +376,17 @@ Mission::set_execution_mode(const uint8_t mode) bool Mission::find_mission_land_start() { - /* return true if a MAV_CMD_DO_LAND_START is found and internally save the index + /* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index * return false if not found - * - * TODO: implement full spec and find closest landing point geographically */ const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; + struct mission_item_s missionitem = {}; + struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START - for (size_t i = 0; i < _mission.count; i++) { - struct mission_item_s missionitem = {}; + for (size_t i = 1; i < _mission.count; i++) { const ssize_t len = sizeof(missionitem); + missionitem_prev = missionitem; // store the last mission item before reading a new one if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -394,11 +394,27 @@ Mission::find_mission_land_start() break; } - if ((missionitem.nav_cmd == NAV_CMD_DO_LAND_START) || - ((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || - (missionitem.nav_cmd == NAV_CMD_LAND)) { + // first check for DO_LAND_START marker + if ((missionitem.nav_cmd == NAV_CMD_DO_LAND_START) && (missionitem_prev.nav_cmd == NAV_CMD_WAYPOINT)) { + _land_start_available = true; _land_start_index = i; + // the DO_LAND_START marker contains no position sp, so take them from the previous mission item + _landing_lat = missionitem_prev.lat; + _landing_lon = missionitem_prev.lon; + _landing_alt = missionitem_prev.altitude; + return true; + + // if no DO_LAND_START marker available, also check for VTOL_LAND or normal LAND + + } else if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || + (missionitem.nav_cmd == NAV_CMD_LAND)) { + + _land_start_available = true; + _land_start_index = i; + _landing_lat = missionitem.lat; + _landing_lon = missionitem.lon; + _landing_alt = missionitem.altitude; return true; } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 33ccfe6edc..da7852b4f8 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -91,6 +91,9 @@ public: bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; } bool get_mission_changed() const { return _mission_changed ; } bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; } + double get_landing_lat() { return _landing_lat; } + double get_landing_lon() { return _landing_lon; } + float get_landing_alt() { return _landing_alt; } void set_closest_item_as_current(); @@ -251,6 +254,9 @@ private: // 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 */ + double _landing_lat{0.0}; + double _landing_lon{0.0}; + float _landing_alt{0.0f}; bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 501746231d..26d2e7598b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -268,7 +268,11 @@ public: bool is_planned_mission() const { return _navigation_mode == &_mission; } bool on_mission_landing() { return _mission.landing(); } bool start_mission_landing() { return _mission.land_start(); } - bool mission_start_land_available() { return _mission.get_land_start_available(); } + bool get_mission_start_land_available() { return _mission.get_land_start_available(); } + int get_mission_landing_index() { return _mission.get_land_start_index(); } + double get_mission_landing_lat() { return _mission.get_landing_lat(); } + double get_mission_landing_lon() { return _mission.get_landing_lon(); } + float get_mission_landing_alt() { return _mission.get_landing_alt(); } // RTL bool mission_landing_required() { return _rtl.rtl_type() == RTL::RTL_LAND; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a30a813fb7..f36e6a736c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -490,9 +490,16 @@ Navigator::run() const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; switch (rtl_type()) { - case RTL::RTL_LAND: + case RTL::RTL_LAND: // use mission landing + case RTL::RTL_CLOSEST: if (rtl_activated) { - mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL LAND activated"); + if (rtl_type() == RTL::RTL_LAND) { + mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL LAND activated"); + + } else { + mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated"); + } + } // if RTL is set to use a mission landing and mission has a planned landing, then use MISSION to fly there directly @@ -570,6 +577,7 @@ Navigator::run() navigation_mode_new = &_rtl; break; + } break; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d39aa417b4..ea94e7c951 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -57,6 +57,150 @@ RTL::on_inactive() { // Reset RTL state. _rtl_state = RTL_STATE_NONE; + + find_RTL_destination(); + +} + +void +RTL::find_closest_landing_point() +{ + // get home position: + home_position_s &home_landing_position = *_navigator->get_home_position(); + // get global position + const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + + // set destination to home per default, then check if other valid landing spot is closer + _destination.set(home_landing_position); + // get distance to home position + double dlat = home_landing_position.lat - global_position.lat; + double dlon = home_landing_position.lon - global_position.lon; + double min_dist_squared = dlat * dlat + dlon * dlon; + + _destination.type = RTL_DESTINATION_HOME; + + if (_navigator->get_mission_start_land_available()) { + double mission_landing_lat = _navigator->get_mission_landing_lat(); + double mission_landing_lon = _navigator->get_mission_landing_lon(); + + // compare home position to landing position to decide which is closer + dlat = mission_landing_lat - global_position.lat; + dlon = mission_landing_lon - global_position.lon; + double dist_squared = dlat * dlat + dlon * dlon; + + if (dist_squared < min_dist_squared) { + min_dist_squared = dist_squared; + _destination.lat = _navigator->get_mission_landing_lat(); + _destination.lon = _navigator->get_mission_landing_lon(); + _destination.alt = _navigator->get_mission_landing_alt(); + _destination.type = RTL_DESTINATION_MISSION_LANDING; + + } + } + + // compare to safe landing positions + mission_safe_point_s closest_safe_point {} ; + mission_stats_entry_s stats; + int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + int num_safe_points = 0; + + if (ret == sizeof(mission_stats_entry_s)) { + num_safe_points = stats.num_items; + } + + // check if a safe point is closer than home or landing + int closest_index = 0; + + for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) { + mission_safe_point_s mission_safe_point; + + if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) != + sizeof(mission_safe_point_s)) { + PX4_ERR("dm_read failed"); + continue; + } + + // TODO: take altitude into account for distance measurement + dlat = mission_safe_point.lat - global_position.lat; + dlon = mission_safe_point.lon - global_position.lon; + double dist_squared = dlat * dlat + dlon * dlon; + + if (dist_squared < min_dist_squared) { + closest_index = current_seq; + min_dist_squared = dist_squared; + closest_safe_point = mission_safe_point; + } + } + + if (closest_index > 0) { + _destination.type = RTL_DESTINATION_SAFE_POINT; + + // There is a safe point closer than home/mission landing + // TODO: handle all possible mission_safe_point.frame cases + switch (closest_safe_point.frame) { + case 0: // MAV_FRAME_GLOBAL + _destination.lat = closest_safe_point.lat; + _destination.lon = closest_safe_point.lon; + _destination.alt = closest_safe_point.alt; + _destination.yaw = home_landing_position.yaw; + break; + + case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT + _destination.lat = closest_safe_point.lat; + _destination.lon = closest_safe_point.lon; + _destination.alt = closest_safe_point.alt + home_landing_position.alt; // alt of safe point is rel to home + _destination.yaw = home_landing_position.yaw; + break; + + default: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME"); + break; + } + } + +} + +void +RTL::find_RTL_destination() +{ + // get home position: + home_position_s &home_landing_position = *_navigator->get_home_position(); + + switch (rtl_type()) { + case RTL_HOME: + // always take home position as landing destination + _destination.set(home_landing_position); + _destination.type = RTL_DESTINATION_HOME; + break; + + case RTL_LAND: + + // take mission landing as landing destination (if available), otherwise home + case RTL_MISSION: + + // inverse mission + if (_navigator->get_mission_start_land_available()) { + _destination.lat = _navigator->get_mission_landing_lat(); + _destination.lon = _navigator->get_mission_landing_lon(); + _destination.alt = _navigator->get_mission_landing_alt(); + _destination.type = RTL_DESTINATION_MISSION_LANDING; + + } else { + _destination.set(home_landing_position); + _destination.type = RTL_DESTINATION_HOME; + } + + break; + + case RTL_CLOSEST: + // choose closest possible landing point (consider home, mission landing and safe points) + find_closest_landing_point(); + break; + + default: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported RTL_TYPE. Change RTL_TYPE param."); + break; + } } int @@ -69,86 +213,30 @@ void RTL::on_activation() { - // find the RTL destination: go through the safe points & home position - const home_position_s &home_position = *_navigator->get_home_position(); + // output the correct message, depending on where the RTL destination is + switch (_destination.type) { + case RTL_DESTINATION_HOME: + mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position."); + break; + + case RTL_DESTINATION_MISSION_LANDING: + mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing."); + break; + + case RTL_DESTINATION_SAFE_POINT: + mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point."); + break; + } + const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); - int num_safe_points = 0; - - if (ret == sizeof(mission_stats_entry_s)) { - num_safe_points = stats.num_items; - } - - int closest_index = 0; - mission_safe_point_s closest_safe_point; - - // take home position into account - double dlat = home_position.lat - global_position.lat; - double dlon = home_position.lon - global_position.lon; - double min_dist_squared = dlat * dlat + dlon * dlon; - - // find the closest point - for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) { - mission_safe_point_s mission_safe_point; - - if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) != - sizeof(mission_safe_point_s)) { - PX4_ERR("dm_read failed"); - continue; - } - - // TODO: take altitude into account for distance measurement - - dlat = mission_safe_point.lat - global_position.lat; - dlon = mission_safe_point.lon - global_position.lon; - double dist_squared = dlat * dlat + dlon * dlon; - - if (dist_squared < min_dist_squared) { - closest_index = current_seq; - min_dist_squared = dist_squared; - closest_safe_point = mission_safe_point; - } - } - - if (closest_index == 0) { - _destination.set(home_position); - - } else { - - // TODO: handle all possible mission_safe_point.frame cases - switch (closest_safe_point.frame) { - case 0: // MAV_FRAME_GLOBAL - _destination.safe_point_index = closest_index; - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt; - _destination.yaw = home_position.yaw; - break; - - case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT - _destination.safe_point_index = closest_index; - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt + home_position.alt; // alt of safe point is rel to home - _destination.yaw = home_position.yaw; - break; - - default: - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME. Landing at HOME."); - _destination.set(home_position); - break; - } - } - _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); if (_navigator->get_land_detected()->landed) { // For safety reasons don't go into RTL if landed. _rtl_state = RTL_STATE_LANDED; - } else if ((rtl_type() == RTL_LAND) && _navigator->on_mission_landing()) { + } else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->on_mission_landing()) { // RTL straight to RETURN state, but mission will takeover for landing. } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { @@ -185,8 +273,8 @@ RTL::set_rtl_item() { // RTL_TYPE: mission landing. // Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination. - // Do nothing, let navigator takeover with mission landing. - if (rtl_type() == RTL_LAND) { + // After reaching DO_LAND_START, do nothing, let navigator takeover with mission landing. + if (_destination.type == RTL_DESTINATION_MISSION_LANDING) { 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"); diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 7138828ae1..1b71f43c3c 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -57,6 +57,13 @@ public: RTL_HOME = 0, RTL_LAND, RTL_MISSION, + RTL_CLOSEST, + }; + + enum RTLDestinationType { + RTL_DESTINATION_HOME = 0, + RTL_DESTINATION_MISSION_LANDING, + RTL_DESTINATION_SAFE_POINT, }; RTL(Navigator *navigator); @@ -67,10 +74,15 @@ public: void on_activation() override; void on_active() override; + void find_closest_landing_point(); + void find_RTL_destination(); + void set_return_alt_min(bool min); int rtl_type() const; + int rtl_destination(); + private: /** * Set the RTL item @@ -101,7 +113,8 @@ private: double lon; float alt; float yaw; - uint8_t safe_point_index; ///< 0 = home position + uint8_t safe_point_index; ///< 0 = home position, 1 = mission landing, >1 = safe landing points (rally points) + RTLDestinationType type{RTL_DESTINATION_HOME}; void set(const home_position_s &home_position) { @@ -110,10 +123,11 @@ private: alt = home_position.alt; yaw = home_position.yaw; safe_point_index = 0; + type = RTL_DESTINATION_HOME; } }; - RTLPosition _destination; ///< the RTL position to fly to (typically the home position or a safe point) + RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point) float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position bool _rtl_alt_min{false}; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index bb57aab9d3..0307945639 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -113,6 +113,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f); * @value 0 Return home via direct path * @value 1 Return to a planned mission landing, if available, via direct path, else return to home via direct path * @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path + * @value 3 Return via direct way to whatever is closest: home, mission lading or safe point * @group Return Mode */ PARAM_DEFINE_INT32(RTL_TYPE, 0);