dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

RTL: clean up naming of function arguments 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2024-02-12 15:58:06 +01:00 committed by Matthias Grob
parent cb03835124
commit 6957818603
7 changed files with 70 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/modules/navigator/mission_block.cpp
View File

@ -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;

8
src/modules/navigator/mission_block.h
View File

@ -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);

14
src/modules/navigator/navigation.h
View File

@ -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].*/
};

16
src/modules/navigator/rtl.cpp
View File

@ -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

12
src/modules/navigator/rtl.h
View File

@ -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

46
src/modules/navigator/rtl_direct.cpp
View File

@ -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();

4
src/modules/navigator/rtl_direct.h
View File

@ -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