Skip to content

Commit

Permalink
make some functions constant
Browse files Browse the repository at this point in the history 
Signed-off-by: RomanBapst <[email protected]>
  • Loading branch information
@RomanBapst @KonradRudin
RomanBapst authored and KonradRudin committed Oct 17, 2023
1 parent 2a5c60c commit 57d71ff
Show file tree
Hide file tree
Showing 2 changed files with 36 additions and 34 deletions.
49 changes: 26 additions & 23 deletions src/modules/navigator/rtl.cpp
View file
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
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

0 comments on commit 57d71ff

Please sign in to comment.