From 9ce22b19dd38f4b7d686c9ceee1f8ef9fad074d1 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 16 Feb 2023 13:28:49 +0300 Subject: [PATCH] Revert "use legacy parameter system and cleaned up vehicle type" This reverts commit 11fd3ef71a3e5160692fceaa2965f750fc4ca517. --- .../MissionFeasibility/FeasibilityChecker.cpp | 81 +++++-------------- .../MissionFeasibility/FeasibilityChecker.hpp | 23 +++--- 2 files changed, 30 insertions(+), 74 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index 800fedf0ff..61dcc12605 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -50,7 +50,8 @@ void FeasibilityChecker::reset() _is_landed = false; _home_alt_msl = NAN; _home_lat_lon = matrix::Vector2d(NAN, NAN); - _vehicle_type = VehicleType::RotaryWing; + _vehicle_type = 0; + _is_vtol = false; _mission_validity_failed = false; _takeoff_failed = false; @@ -97,19 +98,8 @@ void FeasibilityChecker::updateData() if (_status_sub.updated()) { _status_sub.copy(&status); - - if (status.is_vtol) { - _vehicle_type = VehicleType::Vtol; - - } else if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _vehicle_type = VehicleType::RotaryWing; - - } else if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _vehicle_type = VehicleType::FixedWing; - - } else { - _vehicle_type = VehicleType::Other; - } + _vehicle_type = status.vehicle_type; + _is_vtol = status.is_vtol; } vehicle_land_detected_s land_detected = {}; @@ -119,35 +109,6 @@ void FeasibilityChecker::updateData() _is_landed = land_detected.landed; } - param_t handle = param_find("FW_LND_ANG"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_param_fw_lnd_ang); - } - - handle = param_find("MIS_DIST_1WP"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_param_mis_dist_1wp); - } - - handle = param_find("MIS_DIST_WPS"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_param_mis_dist_wps); - } - - handle = param_find("NAV_ACC_RAD"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_param_nav_acc_rad); - } - - handle = param_find("MIS_TKO_LAND_REQ"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_param_mis_takeoff_land_req); - } } void FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int current_index, const int total_count) @@ -159,13 +120,13 @@ void FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int doCommonChecks(mission_item, current_index); - if (_vehicle_type == VehicleType::Vtol) { + if (_is_vtol) { doVtolChecks(mission_item, current_index); - } else if (_vehicle_type == VehicleType::FixedWing) { + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { doFixedWingChecks(mission_item, current_index); - } else if (_vehicle_type == VehicleType::RotaryWing) { + } else { doMulticopterChecks(mission_item, current_index); } @@ -323,7 +284,7 @@ bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item) ? mission_item.altitude : mission_item.altitude - _home_alt_msl; - float acceptance_radius = _param_nav_acc_rad; + float acceptance_radius = _param_nav_acc_rad.get(); if (mission_item.acceptance_radius > NAV_EPSILON_POSITION) { acceptance_radius = mission_item.acceptance_radius; @@ -451,13 +412,13 @@ bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item // 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 + 0.1f)); + const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get() + 0.1f)); if (glide_slope > max_glide_slope) { - 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); + 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); mavlink_log_critical(_mavlink_log_pub, "Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t", @@ -544,7 +505,7 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() { bool result = true; - switch (_param_mis_takeoff_land_req) { + switch (_param_mis_takeoff_land_req.get()) { case 0: result = true; break; @@ -610,7 +571,7 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item) { - if (_param_mis_dist_1wp <= 0.0f || !_home_lat_lon.isAllFinite()) { + if (_param_mis_dist_1wp.get() <= 0.0f || !_home_lat_lon.isAllFinite()) { /* param not set, check is ok */ return true; } @@ -624,7 +585,7 @@ bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_it mission_item.lat, mission_item.lon, _home_lat_lon(0), _home_lat_lon(1)); - if (dist_to_1wp < _param_mis_dist_1wp) { + if (dist_to_1wp < _param_mis_dist_1wp.get()) { return true; @@ -632,9 +593,9 @@ bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_it /* item is too far from home */ mavlink_log_critical(_mavlink_log_pub, "First waypoint too far away: %dm, %d max\t", - (int)dist_to_1wp, (int)_param_mis_dist_1wp); + (int)dist_to_1wp, (int)_param_mis_dist_1wp.get()); events::send(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info}, - "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp, (uint32_t)_param_mis_dist_1wp); + "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp, (uint32_t)_param_mis_dist_1wp.get()); return false; } @@ -646,7 +607,7 @@ bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_it bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mission_item) { - if (_param_mis_dist_wps <= 0.0f) { + if (_param_mis_dist_wps.get() <= 0.0f) { /* param not set, check is ok */ return true; } @@ -665,14 +626,14 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi _last_lat, _last_lon); - if (dist_between_waypoints > _param_mis_dist_wps) { + if (dist_between_waypoints > _param_mis_dist_wps.get()) { /* distance between waypoints is too high */ mavlink_log_critical(_mavlink_log_pub, "Distance between waypoints too far: %d meters, %d max.\t", - (int)dist_between_waypoints, (int)_param_mis_dist_wps); + (int)dist_between_waypoints, (int)_param_mis_dist_wps.get()); events::send(events::ID("navigator_mis_wp_dist_too_far"), {events::Log::Error, events::LogInternal::Info}, "Distance between waypoints too far: {1m}, (maximum: {2m})", (uint32_t)dist_between_waypoints, - (uint32_t)_param_mis_dist_wps); + (uint32_t)_param_mis_dist_wps.get()); return false; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index fde9d8cdbf..aae5ea301c 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -47,13 +47,6 @@ public: FeasibilityChecker(); - enum class VehicleType { - RotaryWing, - FixedWing, - Vtol, - Other - }; - void processNextItem(mission_item_s &mission_item, const int current_index, const int total_count); void setMavlinkLogPub(orb_advert_t *mavlink_log_pub) @@ -82,18 +75,20 @@ private: uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; - // parameters - float _param_fw_lnd_ang{0.f}; - float _param_mis_dist_1wp{0.f}; - float _param_mis_dist_wps{0.f}; - float _param_nav_acc_rad{0.f}; - int _param_mis_takeoff_land_req{0}; + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_lnd_ang, + (ParamFloat) _param_mis_dist_1wp, + (ParamFloat) _param_mis_dist_wps, + (ParamFloat) _param_nav_acc_rad, + (ParamInt) _param_mis_takeoff_land_req + ) + // parameter that need to be set from the outside bool _is_landed{false}; float _home_alt_msl{NAN}; matrix::Vector2d _home_lat_lon = matrix::Vector2d(NAN, NAN); + int _vehicle_type{0}; bool _is_vtol{false}; - VehicleType _vehicle_type{VehicleType::RotaryWing}; // internal flags to keep track of which checks failed bool _mission_validity_failed{false};