From e0f40928d95bb6ec908b9576cd462a6b8272b279 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 12 Jul 2024 15:40:10 +0200 Subject: [PATCH] navigator: implemented protection against flying into terrain during landing - introduced NAV_VTL_MIN_HGT to specify min distance to terrain which should be maintained during the landing approach Signed-off-by: RomanBapst --- src/modules/navigator/mission.cpp | 36 --------------------- src/modules/navigator/mission.h | 2 -- src/modules/navigator/mission_base.cpp | 40 ++++++++++++++++++++++++ src/modules/navigator/mission_base.h | 3 ++ src/modules/navigator/mission_block.cpp | 33 +++++++++++++++++++ src/modules/navigator/mission_block.h | 2 ++ src/modules/navigator/navigator.h | 11 +++++-- src/modules/navigator/navigator_params.c | 17 ++++++++++ src/modules/navigator/rtl.cpp | 23 ++++++++++++++ src/modules/navigator/rtl.h | 2 ++ src/modules/navigator/rtl_direct.cpp | 2 ++ src/modules/navigator/rtl_direct.h | 8 +++++ 12 files changed, 139 insertions(+), 40 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 2949c88d62..1f65e436c2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -91,42 +91,6 @@ Mission::on_activation() MissionBase::on_activation(); } -bool -Mission::isLanding() -{ - if (get_land_start_available()) { - static constexpr size_t max_num_next_items{1u}; - int32_t next_mission_items_index[max_num_next_items]; - size_t num_found_items; - - getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items); - - // vehicle is currently landing if - // mission valid, still flying, and in the landing portion of mission (past land start marker) - bool on_landing_stage = (num_found_items > 0U) && _mission.current_seq > next_mission_items_index[0U]; - - // special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the - // distance to the WP is below the loiter radius + acceptance. - if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U] - && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { - const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - - // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. - const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) - && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : - _navigator->get_loiter_radius(); - - on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); - } - - return _navigator->get_mission_result()->valid && on_landing_stage; - - } else { - return false; - } -} - bool Mission::set_current_mission_index(uint16_t index) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 7299e065e5..d073101bae 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -67,8 +67,6 @@ public: uint16_t get_land_start_index() const { return _mission.land_start_index; } bool get_land_start_available() const { return hasMissionLandStart(); } - bool isLanding(); - private: bool setNextMissionItem() override; diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 595f548c83..7b2736e327 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -352,6 +352,8 @@ MissionBase::on_active() } else if (_navigator->get_precland()->is_activated()) { _navigator->get_precland()->on_inactivation(); } + + updateMinAltDuringVtolLandingAndRepublishTriplet(_mission_item); } void MissionBase::update_mission() @@ -1413,3 +1415,41 @@ void MissionBase::updateMissionAltAfterHomeChanged() _home_update_counter = _navigator->get_home_position()->update_count; } } + +bool +MissionBase::isLanding() +{ + if (hasMissionLandStart()) { + static constexpr size_t max_num_next_items{1u}; + int32_t next_mission_items_index[max_num_next_items]; + size_t num_found_items; + + getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items); + + // vehicle is currently landing if + // mission valid, still flying, and in the landing portion of mission (past land start marker) + bool on_landing_stage = (num_found_items > 0U) && _mission.current_seq > next_mission_items_index[0U]; + + // special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the + // distance to the WP is below the loiter radius + acceptance. + if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U] + && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. + const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) + && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : + _navigator->get_loiter_radius(); + + on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); + } + + + + return _navigator->get_mission_result()->valid && on_landing_stage; + + } else { + return false; + } +} diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 2f416f29e2..c7f2127582 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -73,6 +73,8 @@ public: virtual void on_activation() override; virtual void on_active() override; + bool isLanding(); + protected: /** @@ -321,6 +323,7 @@ protected: */ void setMissionIndex(int32_t index); + bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/ bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/ bool _mission_checked{false}; /**< Flag indicating if the mission has been checked by the mission validator*/ diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 17dd3e620c..0256bc2544 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -55,6 +55,9 @@ using matrix::wrap_pi; +static constexpr float max_height_correction = + 30.0f; // maximum altitude we can add on top of setpoint to avoid terrain during landing + MissionBlock::MissionBlock(Navigator *navigator) : NavigatorMode(navigator) { @@ -1011,3 +1014,33 @@ void MissionBlock::startPrecLand(uint16_t land_precision) _navigator->get_precland()->on_activation(); } } + +void MissionBlock::updateMinAltDuringVtolLandingAndRepublishTriplet(mission_item_s mission_item) +{ + // APX4 Custom start: Whenever we get too close to terrain in fixed wing mode, change the altitude setpoint in the triplet to maintain the + // minimum distance to terrain and republish the triplet. We only do this when we are executing a landing. + // We also change the mission item altitude used for acceptance calculations to prevent getting stuck in a loop. + const float fw_min_height = _navigator->get_fw_min_height(); + + float alt_sp_msl = get_absolute_altitude_for_item(mission_item); + + if (fw_min_height > FLT_EPSILON && _navigator->get_vstatus()->is_vtol && _navigator->force_vtol() + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && _navigator->get_local_position()->dist_bottom_valid && _navigator->get_local_position()->dist_bottom < fw_min_height + && _navigator->get_global_position()->alt > alt_sp_msl && _navigator->isLanding()) { + const float alt_corrected = _navigator->get_global_position()->alt + fw_min_height - + _navigator->get_local_position()->dist_bottom; + + alt_sp_msl = math::constrain(alt_corrected, alt_sp_msl, + alt_sp_msl + max_height_correction); + + struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + curr_sp->alt = alt_sp_msl; + _navigator->set_position_setpoint_triplet_updated(); + + _mission_item.altitude = alt_sp_msl; + _mission_item.altitude_is_relative = false; + } + + // APX4 Custom end +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index b97731567a..a07b58da35 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -248,4 +248,6 @@ protected: bool _payload_deploy_ack_successful{false}; // Flag to keep track of whether we received an acknowledgement for a successful payload deployment hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received + + void updateMinAltDuringVtolLandingAndRepublishTriplet(mission_item_s mission_item); }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 03a1ea621e..39689ee374 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -255,6 +255,11 @@ public: bool get_mission_start_land_available() { return _mission.get_land_start_available(); } + bool isLanding() + { + return (_navigation_mode == &_rtl && _rtl.isLanding()) || (_navigation_mode == &_mission && _mission.isLanding()); + } + // RTL bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } @@ -268,6 +273,7 @@ public: float get_param_mis_takeoff_alt() const { return _param_mis_takeoff_alt.get(); } float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); } + float get_fw_min_height() const { return _param_fw_min_height.get(); } float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } @@ -408,6 +414,7 @@ private: (ParamFloat) _param_mis_yaw_tmt, (ParamFloat) _param_mis_yaw_err, (ParamFloat) _param_mis_payload_delivery_timeout, - (ParamInt) _param_mis_lnd_abrt_alt + (ParamInt) _param_mis_lnd_abrt_alt, + (ParamFloat) _param_fw_min_height ) -}; +}; \ No newline at end of file diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index c300c8e3c8..1199cd1694 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -187,3 +187,20 @@ PARAM_DEFINE_INT32(NAV_FORCE_VT, 1); * @group Mission */ PARAM_DEFINE_FLOAT(NAV_MIN_LTR_ALT, -1.f); + + +/** + * Minimum height above ground for vtol landings. + * + * Minimum height above ground in fixed wing mode during a vtol landing (mission landing or rtl with landing pattern). + * When the estimated distance to the ground is below this value, the system will + * increase the current altitude setpoint (by max 30m) to maintain this distance. + * Setting the value to 0 will disable the feature. + * + * @unit m + * @min 0 + * @decimal 1 + * @increment 1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_VTL_MIN_HGT, 0.0f); \ No newline at end of file diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2b6bd55b3f..bef6775475 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -712,3 +712,26 @@ land_approaches_s RTL::readVtolLandApproaches(PositionYawSetpoint rtl_position) return vtol_land_approaches; } + +bool RTL::isLanding() +{ + bool is_landing = false; + + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: // Fall through + case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through + case RtlType::RTL_DIRECT_MISSION_LAND: + is_landing = _rtl_mission_type_handle && _rtl_mission_type_handle->isLanding(); + break; + + case RtlType::RTL_DIRECT: + is_landing = _rtl_direct.isLoiterindDownOrMovingToTransitionPoint(); + break; + + default: + break; + + } + + return is_landing; +} diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index e7b720bd61..4ef13cb787 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -89,6 +89,8 @@ public: void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; } + bool isLanding(); + private: enum class DestinationType { DESTINATION_TYPE_HOME, diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index d2936986db..0ee0f702c4 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -104,6 +104,8 @@ void RtlDirect::on_active() set_rtl_item(); } + updateMinAltDuringVtolLandingAndRepublishTriplet(_mission_item); + if (_rtl_state == RTLState::LAND && _param_rtl_pld_md.get() > 0) { // Need to update the position and type on the current setpoint triplet. _navigator->get_precland()->on_active(); diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 211b2779ea..5a1249b8f2 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -102,6 +102,14 @@ public: void setRtlPosition(PositionYawSetpoint position, loiter_point_s loiter_pos); + bool isLoiterindDownOrMovingToTransitionPoint() const + { + // during the following states the vehicle is loitering down to transition altitude or moving to the + // land waypoint at transition altitude + return _rtl_state == RTLState::LOITER_HOLD || _rtl_state == RTLState::MOVE_TO_LAND + || _rtl_state == RTLState::TRANSITION_TO_MC; + } + private: /** * @brief Return to launch state machine.