diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 36325a270f..2fbcb736ee 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -138,6 +138,19 @@ enum NAV_FRAME { #pragma GCC diagnostic error "-Wpadded" #endif // GCC >= 5 || Clang +/** + * @brief Landing position. + * Defines the global 3D position and landing yaw. + * + */ +struct LandingPosition_s { + double lat; /**< latitude in WGS84 [rad].*/ + double lon; /**< longitude in WGS84 [rad].*/ + float alt; /**< altitude in MSL [m].*/ + float yaw; /**< final yaw when landed [rad].*/ +}; + + /** * Mission Item structure * diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 4a74da44d3..9b118034a2 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -210,7 +210,7 @@ void RTL::setRtlTypeAndDestination() } else { // check the closest allowed destination. DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME}; - RtlDirect::RtlPosition rtl_position; + LandingPosition_s rtl_position; float rtl_alt; findRtlDestination(destination_type, rtl_position, rtl_alt); @@ -221,16 +221,13 @@ void RTL::setRtlTypeAndDestination() break; case DestinationType::DESTINATION_TYPE_SAFE_POINT: - _rtl_direct.setRtlAlt(rtl_alt); - _rtl_direct.setRtlPosition(rtl_position); - _rtl_type = RtlType::RTL_DIRECT; - break; - + // Fall through case DestinationType::DESTINATION_TYPE_HOME: { // check if we can apply vtol land. + _rtl_vtol_land.setLandPosition(rtl_position); bool vtol_land{_vehicle_status_sub.get().is_vtol && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && - _rtl_vtol_land.hasVtolHomeLandApproach()}; + _rtl_vtol_land.hasVtolLandApproach()}; if (vtol_land) { _rtl_type = RtlType::RTL_DIRECT_SAFE_VTOL; @@ -253,7 +250,7 @@ void RTL::setRtlTypeAndDestination() } } -void RTL::findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPosition &rtl_position, float &rtl_alt) +void RTL::findRtlDestination(DestinationType &destination_type, LandingPosition_s &rtl_position, float &rtl_alt) { // set destination to home per default, then check if other valid landing spot is closer rtl_position.alt = _home_pos_sub.get().alt; @@ -332,7 +329,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPo } } -void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position) +void RTL::setLandPosAsDestination(LandingPosition_s &rtl_position) { rtl_position.alt = _land_pos.alt; rtl_position.lat = _land_pos.lat; @@ -340,7 +337,7 @@ void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position) rtl_position.yaw = _home_pos_sub.get().yaw; } -void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, +void RTL::setSafepointAsDestination(LandingPosition_s &rtl_position, const mission_item_s &mission_safe_point) { // There is a safe point closer than home/mission landing @@ -368,7 +365,7 @@ void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, } } -float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, +float RTL::calculate_return_alt_from_cone_half_angle(const LandingPosition_s &rtl_position, float cone_half_angle_deg) { // horizontal distance to destination diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 60225b1c23..54014dffc7 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -45,6 +45,7 @@ #include #include "navigator_mode.h" +#include "navigation.h" #include "lib/mission/planned_mission_interface.h" #include "rtl_direct.h" #include "rtl_mission_fast.h" @@ -97,20 +98,20 @@ private: * @brief Find RTL destination. * */ - void findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPosition &rtl_position, float &rtl_alt); + void findRtlDestination(DestinationType &destination_type, LandingPosition_s &rtl_position, float &rtl_alt); /** * @brief Set the position of the land start marker in the planned mission as destination. * */ - void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position); + void setLandPosAsDestination(LandingPosition_s &rtl_position); /** * @brief Set the safepoint as destination. * * @param mission_safe_point is the mission safe point/rally point to set as destination. */ - void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_item_s &mission_safe_point); + void setSafepointAsDestination(LandingPosition_s &rtl_position, const mission_item_s &mission_safe_point); /** * @brief @@ -118,7 +119,7 @@ private: * @param cone_half_angle_deg * @return float */ - float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg); + float calculate_return_alt_from_cone_half_angle(const LandingPosition_s &rtl_position, float cone_half_angle_deg); hrt_abstime _destination_check_time{0}; diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index d698e3252a..2d931a2734 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -45,6 +45,7 @@ #include "mission_block.h" #include "lib/mission/planned_mission_interface.h" +#include "navigation.h" #include #include @@ -61,18 +62,6 @@ class Navigator; class RtlDirect : public MissionBlock, public ModuleParams { public: - /** - * @brief Return to launch position. - * Defines the position and landing yaw for the return to launch destination. - * - */ - struct RtlPosition { - double lat; /**< latitude in WGS84 [rad].*/ - double lon; /**< longitude in WGS84 [rad].*/ - float alt; /**< altitude in MSL [m].*/ - float yaw; /**< final yaw when landed [rad].*/ - }; - RtlDirect(Navigator *navigator); ~RtlDirect() = default; @@ -101,7 +90,7 @@ public: void setRtlAlt(float alt) {_rtl_alt = alt;}; - void setRtlPosition(RtlPosition position) {_destination = position;}; + void setRtlPosition(LandingPosition_s position) {_destination = position;}; private: /** @@ -189,7 +178,7 @@ private: /** Current state in the state machine.*/ RTLState _rtl_state{RTL_STATE_NONE}; - RtlPosition _destination{}; ///< the RTL position to fly to + LandingPosition_s _destination{}; ///< the RTL position to fly to float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position diff --git a/src/modules/navigator/vtol_land.cpp b/src/modules/navigator/vtol_land.cpp index 62daa43cbf..edf2d206c4 100644 --- a/src/modules/navigator/vtol_land.cpp +++ b/src/modules/navigator/vtol_land.cpp @@ -55,11 +55,9 @@ void VtolLand::on_activation() { _global_pos_sub.update(); - _home_pos_sub.update(); _wind_sub.update(); - _land_pos_lat_lon = matrix::Vector2(_home_pos_sub.get().lat, _home_pos_sub.get().lon); - readVtolHomeLandApproachesFromStorage(); + readVtolLandApproachesFromStorage(); set_loiter_position(); _land_state = vtol_land_state::MOVE_TO_LOITER; @@ -68,7 +66,6 @@ VtolLand::on_activation() void VtolLand::on_inactive() { _global_pos_sub.update(); - _home_pos_sub.update(); _wind_sub.update(); } @@ -76,13 +73,12 @@ void VtolLand::on_active() { _global_pos_sub.update(); - _home_pos_sub.update(); _wind_sub.update(); if (is_mission_item_reached_or_completed()) { switch (_land_state) { case vtol_land_state::MOVE_TO_LOITER: { - _mission_item.altitude = _home_pos_sub.get().alt + _land_approach.height_m; + _mission_item.altitude = _destination.alt + _land_approach.height_m; _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); @@ -179,9 +175,9 @@ VtolLand::on_active() } } -bool VtolLand::hasVtolHomeLandApproach() +bool VtolLand::hasVtolLandApproach() { - readVtolHomeLandApproachesFromStorage(); + readVtolLandApproachesFromStorage(); return _vtol_home_land_approaches.isAnyApproachValid(); } @@ -196,14 +192,14 @@ VtolLand::set_loiter_position() _loiter_pos_lat_lon(1) = _land_approach.lon; } else { - _loiter_pos_lat_lon(0) = _home_pos_sub.get().lat; - _loiter_pos_lat_lon(1) = _home_pos_sub.get().lon; + _loiter_pos_lat_lon(0) = _destination.lat; + _loiter_pos_lat_lon(1) = _destination.lon; } _mission_item.lat = _loiter_pos_lat_lon(0); _mission_item.lon = _loiter_pos_lat_lon(1); - _mission_item.altitude = math::max(_home_pos_sub.get().alt + _param_return_alt_rel_m.get(), + _mission_item.altitude = math::max(_destination.alt + _param_return_alt_rel_m.get(), _global_pos_sub.get().alt); _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.force_heading = true; @@ -240,8 +236,8 @@ loiter_point_s VtolLand::chooseBestLandingApproach() for (int i = 0; i < _vtol_home_land_approaches.num_approaches_max; i++) { if (_vtol_home_land_approaches.approaches[i].isValid()) { - const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(_home_pos_sub.get().lat, - _home_pos_sub.get().lon, _vtol_home_land_approaches.approaches[i].lat, + const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(_destination.lat, + _destination.lon, _vtol_home_land_approaches.approaches[i].lat, _vtol_home_land_approaches.approaches[i].lon) - wind_direction); if (fabsf(wind_angle) < wind_angle_prev) { @@ -261,22 +257,18 @@ loiter_point_s VtolLand::chooseBestLandingApproach() } } -void VtolLand::readVtolHomeLandApproachesFromStorage() +void VtolLand::readVtolLandApproachesFromStorage() { // go through all mission items in the rally point storage. If we find a mission item of type NAV_CMD_RALLY_POINT - // which is within MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES of our current home position then treat ALL following mission items of type NAV_CMD_LOITER_TO_ALT which come - // BEFORE the next mission item of type NAV_CMD_RALLY_POINT as land approaches for the home position + // which is within MAX_DIST_FROM_LAND_FOR_APPROACHES of our current pestination position then treat ALL following mission items of type NAV_CMD_LOITER_TO_ALT which come + // BEFORE the next mission item of type NAV_CMD_RALLY_POINT as land approaches for the destination _vtol_home_land_approaches.resetAllApproaches(); - if (!(_home_pos_sub.get().valid_alt && _home_pos_sub.get().valid_hpos)) { - return; - } - mission_stats_entry_s stats; int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); int num_mission_items = 0; - bool foundHomeLandApproaches = false; + bool foundLandApproaches = false; uint8_t sector_counter = 0; if (ret == sizeof(mission_stats_entry_s)) { @@ -294,22 +286,22 @@ void VtolLand::readVtolHomeLandApproachesFromStorage() if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) { - if (foundHomeLandApproaches) { + if (foundLandApproaches) { break; } - const float dist_to_home = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, _home_pos_sub.get().lat, - _home_pos_sub.get().lon); + const float dist_to_land = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, _destination.lat, + _destination.lon); - if (!mission_item.is_mission_rally_point && dist_to_home < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { - foundHomeLandApproaches = true; + if (!mission_item.is_mission_rally_point && dist_to_land < MAX_DIST_FROM_LAND_FOR_APPROACHES) { + foundLandApproaches = true; _vtol_home_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.lat, mission_item.lon); } sector_counter = 0; } - if (foundHomeLandApproaches && mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + if (foundLandApproaches && mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { _vtol_home_land_approaches.approaches[sector_counter].lat = mission_item.lat; _vtol_home_land_approaches.approaches[sector_counter].lon = mission_item.lon; _vtol_home_land_approaches.approaches[sector_counter].height_m = mission_item.altitude; diff --git a/src/modules/navigator/vtol_land.h b/src/modules/navigator/vtol_land.h index d053c71e0f..a1b2a40797 100644 --- a/src/modules/navigator/vtol_land.h +++ b/src/modules/navigator/vtol_land.h @@ -40,6 +40,7 @@ #pragma once #include "navigator_mode.h" +#include "navigation.h" #include "mission_block.h" #include "safe_point_land.hpp" @@ -63,7 +64,9 @@ public: void on_active() override; void on_inactive() override; - bool hasVtolHomeLandApproach(); + bool hasVtolLandApproach(); + + void setLandPosition(LandingPosition_s position){_destination = position;}; rtl_time_estimate_s calc_rtl_time_estimate(); private: @@ -88,19 +91,20 @@ private: (ParamFloat) _param_rtl_loiter_rad ) + LandingPosition_s _destination; + void set_loiter_position(); loiter_point_s chooseBestLandingApproach(); land_approaches_s _vtol_home_land_approaches{}; - void readVtolHomeLandApproachesFromStorage(); + void readVtolLandApproachesFromStorage(); - static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES = + static constexpr float MAX_DIST_FROM_LAND_FOR_APPROACHES = 10.0; // [m] We don't consider home land approaches valid if the distance from the current home to the land location is greater than this distance uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ - uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; };