From 695781860393653328a60a3bdd4dfa60041b0174 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 12 Feb 2024 15:58:06 +0100 Subject: [PATCH] RTL: clean up naming of function arguments Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission_block.cpp | 39 +++++++++++---------- src/modules/navigator/mission_block.h | 8 ++--- src/modules/navigator/navigation.h | 14 ++++---- src/modules/navigator/rtl.cpp | 16 ++++----- src/modules/navigator/rtl.h | 12 +++---- src/modules/navigator/rtl_direct.cpp | 46 ++++++++++++------------- src/modules/navigator/rtl_direct.h | 4 +-- 7 files changed, 70 insertions(+), 69 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index b33cc6bbcd..f9d3e76173 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -928,15 +928,15 @@ MissionBlock::initialize() _mission_item.origin = ORIGIN_ONBOARD; } -void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, +void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, float loiter_radius) const { item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - item.lat = dest.lat; - item.lon = dest.lon; - item.altitude = dest.alt; + item.lat = pos_yaw_sp.lat; + item.lon = pos_yaw_sp.lon; + item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; - item.yaw = dest.yaw; + item.yaw = pos_yaw_sp.yaw; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; @@ -945,7 +945,8 @@ void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const Destina item.loiter_radius = loiter_radius; } -void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, +void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, + float loiter_time, float loiter_radius) const { const bool autocontinue = (loiter_time > -FLT_EPSILON); @@ -957,9 +958,9 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; } - item.lat = dest.lat; - item.lon = dest.lon; - item.altitude = dest.alt; + item.lat = pos_yaw_sp.lat; + item.lon = pos_yaw_sp.lon; + item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; item.yaw = NAN; @@ -971,12 +972,12 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat item.loiter_radius = loiter_radius; } -void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest) const +void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const { item.nav_cmd = NAV_CMD_WAYPOINT; - item.lat = dest.lat; - item.lon = dest.lon; - item.altitude = dest.alt; + item.lat = pos_yaw_sp.lat; + item.lon = pos_yaw_sp.lon; + item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; item.autocontinue = true; @@ -984,16 +985,16 @@ void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const Dest item.time_inside = 0.f; item.origin = ORIGIN_ONBOARD; - item.yaw = dest.yaw; + item.yaw = pos_yaw_sp.yaw; } -void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest) const +void MissionBlock::setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const { item.nav_cmd = NAV_CMD_LAND; - item.lat = dest.lat; - item.lon = dest.lon; - item.altitude = dest.alt; - item.yaw = dest.yaw; + item.lat = pos_yaw_sp.lat; + item.lon = pos_yaw_sp.lon; + item.altitude = pos_yaw_sp.alt; + item.yaw = pos_yaw_sp.yaw; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; item.autocontinue = true; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index eea8a2cb3d..b97731567a 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -205,14 +205,14 @@ protected: */ void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode); - void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius) const; + void setLoiterToAltMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, float loiter_radius) const; - void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, + void setLoiterHoldMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, float loiter_time, float loiter_radius) const; - void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest) const; + void setMoveToPositionMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const; - void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest) const; + void setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const; void startPrecLand(uint16_t land_precision); diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 77d24f46dd..87dd05d166 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -230,15 +230,15 @@ struct mission_fence_point_s { }; /** - * @brief Return to launch position. - * Defines the position and landing yaw for the return to launch destination. + * @brief Position and yaw setpoint struct. + * Used in RTL state machine. * */ -struct DestinationPosition { - double lat; /**< latitude in WGS84 [rad].*/ - double lon; /**< longitude in WGS84 [rad].*/ - float alt; /**< altitude in MSL [m].*/ - float yaw; /**< final yaw when landed [rad].*/ +struct PositionYawSetpoint { + double lat; /**< latitude setpoint in WGS84 [rad].*/ + double lon; /**< longitude setpoint in WGS84 [rad].*/ + float alt; /**< altitude setpoint in MSL [m].*/ + float yaw; /**< yaw setpoint [rad].*/ }; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 043a03721f..af30abab1e 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -297,7 +297,7 @@ void RTL::setRtlTypeAndDestination() if (_param_rtl_type.get() != 2) { // check the closest allowed destination. DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME}; - DestinationPosition rtl_position; + PositionYawSetpoint rtl_position; float rtl_alt; findRtlDestination(destination_type, rtl_position, rtl_alt); @@ -333,7 +333,7 @@ void RTL::setRtlTypeAndDestination() } } -void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt) +void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt) { // set destination to home per default, then check if other valid landing spot is closer rtl_position.alt = _home_pos_sub.get().alt; @@ -413,7 +413,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; - DestinationPosition safepoint_position; + PositionYawSetpoint safepoint_position; setSafepointAsDestination(safepoint_position, mission_safe_point); if (((dist + MIN_DIST_THRESHOLD) < min_dist) && (!vtol_in_fw_mode || (_param_rtl_approach_force.get() == 0) @@ -435,7 +435,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit } } -void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const +void RTL::setLandPosAsDestination(PositionYawSetpoint &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; @@ -444,7 +444,7 @@ void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_ite rtl_position.yaw = _home_pos_sub.get().yaw; } -void RTL::setSafepointAsDestination(DestinationPosition &rtl_position, +void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const { // There is a safe point closer than home/mission landing @@ -472,7 +472,7 @@ void RTL::setSafepointAsDestination(DestinationPosition &rtl_position, } } -float RTL::calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position, +float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const { // horizontal distance to destination @@ -576,7 +576,7 @@ bool RTL::hasMissionLandStart() const return _mission_sub.get().land_start_index > 0; } -bool RTL::hasVtolLandApproach(const DestinationPosition &rtl_position) const +bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const { return readVtolLandApproaches(rtl_position).isAnyApproachValid(); } @@ -611,7 +611,7 @@ loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land } } -land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position) const +land_approaches_s RTL::readVtolLandApproaches(PositionYawSetpoint 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 diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9ebc7ab4e3..525a4583a7 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -109,20 +109,20 @@ private: * @brief Find RTL destination. * */ - void findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt); + void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt); /** * @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) const; + void setLandPosAsDestination(PositionYawSetpoint &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) const; + void setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const; /** * @brief calculate return altitude from cone half angle @@ -131,7 +131,7 @@ private: * @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 calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const; /** @@ -152,7 +152,7 @@ private: * @param[in] rtl_position landing position of the rtl * */ - land_approaches_s readVtolLandApproaches(DestinationPosition rtl_position) const; + land_approaches_s readVtolLandApproaches(PositionYawSetpoint rtl_position) const; /** * @brief Has VTOL land approach @@ -162,7 +162,7 @@ private: * @return true if home land approaches are defined for home position * @return false otherwise */ - bool hasVtolLandApproach(const DestinationPosition &rtl_position) const; + bool hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const; /** * @brief Choose best landing approach diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 3164626dd4..2eb7977fe8 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -122,7 +122,7 @@ void RtlDirect::on_active() } } -void RtlDirect::setRtlPosition(DestinationPosition rtl_position, loiter_point_s loiter_pos) +void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s loiter_pos) { _home_pos_sub.update(); @@ -182,20 +182,20 @@ void RtlDirect::set_rtl_item() switch (_rtl_state) { case RTLState::CLIMBING: { - DestinationPosition dest { + PositionYawSetpoint pos_yaw_sp { .lat = _global_pos_sub.get().lat, .lon = _global_pos_sub.get().lon, .alt = _rtl_alt, .yaw = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading, }; - setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius()); + setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _navigator->get_loiter_radius()); _rtl_state = RTLState::MOVE_TO_LOITER; break; } case RTLState::MOVE_TO_LOITER: { - DestinationPosition dest { + PositionYawSetpoint pos_yaw_sp { .lat = _land_approach.lat, .lon = _land_approach.lon, .alt = _rtl_alt, @@ -204,13 +204,13 @@ void RtlDirect::set_rtl_item() // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status // can be displayed on groundstation and the WP is accepted once within loiter radius if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - dest.yaw = NAN; - setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m); + pos_yaw_sp.yaw = NAN; + setLoiterHoldMissionItem(_mission_item, pos_yaw_sp, 0.f, _land_approach.loiter_radius_m); } else { // already set final yaw if close to destination and WV is disabled - dest.yaw = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN; - setMoveToPositionMissionItem(_mission_item, dest); + pos_yaw_sp.yaw = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN; + setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); } _rtl_state = RTLState::LOITER_DOWN; @@ -219,14 +219,14 @@ void RtlDirect::set_rtl_item() } case RTLState::LOITER_DOWN: { - DestinationPosition dest{ + PositionYawSetpoint pos_yaw_sp{ .lat = _land_approach.lat, .lon = _land_approach.lon, .alt = loiter_altitude, .yaw = !_param_wv_en.get() ? _destination.yaw : NAN, // set final yaw if WV is disabled }; - setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m); + setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _land_approach.loiter_radius_m); pos_sp_triplet->next.valid = true; pos_sp_triplet->next.lat = _destination.lat; @@ -246,7 +246,7 @@ void RtlDirect::set_rtl_item() } case RTLState::LOITER_HOLD: { - DestinationPosition dest { + PositionYawSetpoint pos_yaw_sp { .lat = _land_approach.lat, .lon = _land_approach.lon, .alt = loiter_altitude, @@ -254,7 +254,7 @@ void RtlDirect::set_rtl_item() }; // set final yaw if WV is disabled - setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m); + setLoiterHoldMissionItem(_mission_item, pos_yaw_sp, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m); if (_param_rtl_land_delay.get() < -FLT_EPSILON) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); @@ -274,11 +274,11 @@ void RtlDirect::set_rtl_item() case RTLState::MOVE_TO_LAND: { - DestinationPosition dest{_destination}; - dest.alt = loiter_altitude; - dest.yaw = NAN; + PositionYawSetpoint pos_yaw_sp{_destination}; + pos_yaw_sp.alt = loiter_altitude; + pos_yaw_sp.yaw = NAN; - setMoveToPositionMissionItem(_mission_item, dest); + setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); // Prepare for transition _mission_item.vtol_back_transition = true; @@ -305,11 +305,11 @@ void RtlDirect::set_rtl_item() } case RTLState::MOVE_TO_LAND_HOVER: { - DestinationPosition dest{_destination}; - dest.alt = loiter_altitude; - dest.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled + PositionYawSetpoint pos_yaw_sp{_destination}; + pos_yaw_sp.alt = loiter_altitude; + pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled - setMoveToPositionMissionItem(_mission_item, dest); + setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); _navigator->reset_position_setpoint(pos_sp_triplet->previous); _rtl_state = RTLState::LAND; @@ -318,9 +318,9 @@ void RtlDirect::set_rtl_item() } case RTLState::LAND: { - DestinationPosition dest{_destination}; - dest.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled - setLandMissionItem(_mission_item, dest); + PositionYawSetpoint pos_yaw_sp{_destination}; + pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled + setLandMissionItem(_mission_item, pos_yaw_sp); _mission_item.land_precision = _param_rtl_pld_md.get(); diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 04883dcdc6..9dc89e42fc 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -99,7 +99,7 @@ public: void setReturnAltMin(bool min) { _enforce_rtl_alt = min; } void setRtlAlt(float alt) {_rtl_alt = alt;}; - void setRtlPosition(DestinationPosition position, loiter_point_s loiter_pos); + void setRtlPosition(PositionYawSetpoint position, loiter_point_s loiter_pos); private: /** @@ -179,7 +179,7 @@ private: bool _enforce_rtl_alt{false}; bool _force_heading{false}; - DestinationPosition _destination; ///< the RTL position to fly to + PositionYawSetpoint _destination; ///< the RTL position to fly to loiter_point_s _land_approach; float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position