From 2bc4c0fb28ddaf946d6f6a5dd89496d05ef0a91c Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 16 Oct 2023 16:54:47 +0300 Subject: [PATCH] make some functions constant Signed-off-by: RomanBapst --- src/modules/navigator/rtl.cpp | 49 +++++++++++++++++++---------------- src/modules/navigator/rtl.h | 21 +++++++-------- 2 files changed, 36 insertions(+), 34 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d3278bedad8a..2a313d99bbe7 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -310,8 +310,10 @@ void RTL::setRtlTypeAndDestination() landing_loiter.lon = rtl_position.lon; landing_loiter.height_m = rtl_position.alt; - if (hasVtolLandApproach(rtl_position)) { - landing_loiter = chooseBestLandingApproach(); + land_approaches_s rtl_land_approaches{readVtolLandApproaches(rtl_position)}; + + if (rtl_land_approaches.isAnyApproachValid()) { + landing_loiter = chooseBestLandingApproach(rtl_land_approaches); } _rtl_type = RtlType::RTL_DIRECT; @@ -418,7 +420,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit } } -void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) +void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const { rtl_position.alt = land_mission_item.altitude_is_relative ? land_mission_item.altitude + _home_pos_sub.get().alt : land_mission_item.altitude; @@ -428,7 +430,7 @@ void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_ite } void RTL::setSafepointAsDestination(DestinationPosition &rtl_position, - const mission_item_s &mission_safe_point) + const mission_item_s &mission_safe_point) const { // There is a safe point closer than home/mission landing // TODO: handle all possible mission_safe_point.frame cases @@ -456,7 +458,7 @@ void RTL::setSafepointAsDestination(DestinationPosition &rtl_position, } float RTL::calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position, - float cone_half_angle_deg) + float cone_half_angle_deg) const { // horizontal distance to destination const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, @@ -554,29 +556,28 @@ void RTL::parameters_update() } } -bool RTL::hasMissionLandStart() +bool RTL::hasMissionLandStart() const { return _mission_sub.get().land_start_index > 0; } -bool RTL::hasVtolLandApproach(DestinationPosition rtl_position) +bool RTL::hasVtolLandApproach(const DestinationPosition &rtl_position) const { - readVtolLandApproaches(rtl_position); - return _vtol_land_approaches.isAnyApproachValid(); + return readVtolLandApproaches(rtl_position).isAnyApproachValid(); } -loiter_point_s RTL::chooseBestLandingApproach() +loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches) { const float wind_direction = atan2f(_wind_sub.get().windspeed_east, _wind_sub.get().windspeed_north); int8_t min_index = -1; float wind_angle_prev = INFINITY; - for (int i = 0; i < _vtol_land_approaches.num_approaches_max; i++) { + for (int i = 0; i < vtol_land_approaches.num_approaches_max; i++) { - if (_vtol_land_approaches.approaches[i].isValid()) { + if (vtol_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_land_approaches.approaches[i].lat, - _vtol_land_approaches.approaches[i].lon) - wind_direction); + _home_pos_sub.get().lon, vtol_land_approaches.approaches[i].lat, + vtol_land_approaches.approaches[i].lon) - wind_direction); if (fabsf(wind_angle) < wind_angle_prev) { min_index = i; @@ -587,7 +588,7 @@ loiter_point_s RTL::chooseBestLandingApproach() } if (min_index >= 0) { - return _vtol_land_approaches.approaches[min_index]; + return vtol_land_approaches.approaches[min_index]; } else { @@ -595,16 +596,16 @@ loiter_point_s RTL::chooseBestLandingApproach() } } -void RTL::readVtolLandApproaches(DestinationPosition rtl_position) +land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position) const { // 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 - _vtol_land_approaches.resetAllApproaches(); + land_approaches_s vtol_land_approaches{}; if (!_safe_points_updated) { - return; + return vtol_land_approaches; } bool foundHomeLandApproaches = false; @@ -633,18 +634,20 @@ void RTL::readVtolLandApproaches(DestinationPosition rtl_position) if (dist_to_safepoint < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { foundHomeLandApproaches = true; - _vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.lat, mission_item.lon); + vtol_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) { - _vtol_land_approaches.approaches[sector_counter].lat = mission_item.lat; - _vtol_land_approaches.approaches[sector_counter].lon = mission_item.lon; - _vtol_land_approaches.approaches[sector_counter].height_m = mission_item.altitude; - _vtol_land_approaches.approaches[sector_counter].loiter_radius_m = mission_item.loiter_radius; + vtol_land_approaches.approaches[sector_counter].lat = mission_item.lat; + vtol_land_approaches.approaches[sector_counter].lon = mission_item.lon; + vtol_land_approaches.approaches[sector_counter].height_m = mission_item.altitude; + vtol_land_approaches.approaches[sector_counter].loiter_radius_m = mission_item.loiter_radius; sector_counter++; } } + + return vtol_land_approaches; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 70105344ac89..53f6f2057ca9 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -96,7 +96,7 @@ class RTL : public NavigatorMode, public ModuleParams }; private: - bool hasMissionLandStart(); + bool hasMissionLandStart() const; /** * @brief function to call regularly to do background work @@ -115,14 +115,14 @@ class RTL : public NavigatorMode, public ModuleParams * @brief Set the position of the land start marker in the planned mission as destination. * */ - void setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item); + void setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const; /** * @brief Set the safepoint as destination. * * @param mission_safe_point is the mission safe point/rally point to set as destination. */ - void setSafepointAsDestination(DestinationPosition &rtl_position, const mission_item_s &mission_safe_point); + void setSafepointAsDestination(DestinationPosition &rtl_position, const mission_item_s &mission_safe_point) const; /** * @brief calculate return altitude from cone half angle @@ -131,7 +131,8 @@ class RTL : public NavigatorMode, public ModuleParams * @param[in] cone_half_angle_deg half angle of the cone [deg] * @return return altitude */ - float calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position, float cone_half_angle_deg); + float calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position, + float cone_half_angle_deg) const; /** * @brief initialize RTL mission type @@ -151,7 +152,7 @@ class RTL : public NavigatorMode, public ModuleParams * @param[in] rtl_position landing position of the rtl * */ - void readVtolLandApproaches(DestinationPosition rtl_position); + land_approaches_s readVtolLandApproaches(DestinationPosition rtl_position) const; /** * @brief Has VTOL land approach @@ -161,7 +162,7 @@ class RTL : public NavigatorMode, public ModuleParams * @return true if home land approaches are defined for home position * @return false otherwise */ - bool hasVtolLandApproach(DestinationPosition rtl_position); + bool hasVtolLandApproach(const DestinationPosition &rtl_position) const; /** * @brief Choose best landing approach @@ -170,7 +171,7 @@ class RTL : public NavigatorMode, public ModuleParams * * @return loiter_point_s best landing approach */ - loiter_point_s chooseBestLandingApproach(); + loiter_point_s chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches); enum class DatamanState { UpdateRequestWait, @@ -191,18 +192,16 @@ class RTL : public NavigatorMode, public ModuleParams DatamanState _error_state{DatamanState::UpdateRequestWait}; uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache - DatamanCache _dataman_cache_safepoint{"rtl_dm_cache_miss_geo", 4}; + mutable DatamanCache _dataman_cache_safepoint{"rtl_dm_cache_miss_geo", 4}; DatamanClient &_dataman_client_safepoint = _dataman_cache_safepoint.client(); bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed - DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; + mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; int16_t _mission_counter = -1; mission_stats_entry_s _stats; RtlDirect _rtl_direct; - land_approaches_s _vtol_land_approaches{}; - bool _enforce_rtl_alt{false}; DEFINE_PARAMETERS(