From ceb432aacb447c4ae09fd25debcd12369094e88d Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 5 Jul 2022 10:52:37 +0200 Subject: [PATCH] mission feasibility checker: use param_find() to check fixed-wing land angle --- .../navigator/mission_feasibility_checker.cpp | 21 +++++++++++++++---- .../navigator/mission_feasibility_checker.h | 4 ---- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 03f495ac39..d69a3c3dfe 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -487,6 +487,19 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool if (missionitem.nav_cmd == NAV_CMD_LAND) { mission_item_s missionitem_previous {}; + float param_fw_lnd_ang = 0.0f; + const param_t param_handle_fw_lnd_ang = param_find("FW_LND_ANG"); + + if (param_handle_fw_lnd_ang == PARAM_INVALID) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: FW_LND_ANG parameter is missing.\t"); + events::send(events::ID("navigator_mis_land_angle_param_missing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: FW_LND_ANG parameter is missing"); + return false; + + } else { + param_get(param_handle_fw_lnd_ang, ¶m_fw_lnd_ang); + } + if (i > 0) { landing_approach_index = i - 1; @@ -553,13 +566,13 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool // rounding on next check with small (arbitrary) 0.1 deg buffer, as the // landing angle parameter is what is typically used for steepest glide // in landing config - const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get() + 0.1f)); + const float max_glide_slope = tanf(math::radians(param_fw_lnd_ang + 0.1f)); if (glide_slope > max_glide_slope) { - const uint8_t land_angle_left_of_decimal = (uint8_t)_param_fw_lnd_ang.get(); - const uint8_t land_angle_first_after_decimal = (uint8_t)((_param_fw_lnd_ang.get() - floorf( - _param_fw_lnd_ang.get())) * 10.0f); + const uint8_t land_angle_left_of_decimal = (uint8_t)param_fw_lnd_ang; + const uint8_t land_angle_first_after_decimal = (uint8_t)((param_fw_lnd_ang - floorf( + param_fw_lnd_ang)) * 10.0f); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t", diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index f3c90eb770..da05399291 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -90,8 +90,4 @@ public: bool checkMissionFeasible(const mission_s &mission, float max_distance_to_1st_waypoint, float max_distance_between_waypoints, bool land_start_req); - - DEFINE_PARAMETERS( - (ParamFloat) _param_fw_lnd_ang - ) };