Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Make some functions const #22227

Merged
merged 1 commit into from
Oct 17, 2023
+36 −34
Merged

Make some functions const #22227

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 26 additions & 23 deletions src/modules/navigator/rtl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand All @@ -587,24 +588,24 @@ loiter_point_s RTL::chooseBestLandingApproach()
}

if (min_index >= 0) {
return _vtol_land_approaches.approaches[min_index];
return vtol_land_approaches.approaches[min_index];

} else {

return loiter_point_s();
}
}

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;
Expand Down Expand Up @@ -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;
}
21 changes: 10 additions & 11 deletions src/modules/navigator/rtl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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,
Expand All @@ -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(
Expand Down
Loading