fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtl: never set arbitrary yaw, initialize _destination and _last_position_before_link_loss with NAN

Browse Source
This commit is contained in:
Matthias Grob 2026-01-15 23:01:09 +01:00 committed by Silvan Fuhrer
parent 18c3d889fe
commit 8117fce790
4 changed files with 18 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -312,7 +312,7 @@ void RTL::setRtlTypeAndDestination()
{
init_rtl_mission_type();
uint8_t safe_point_index{UINT8_MAX};
uint8_t safe_point_index = UINT8_MAX;
if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) {
// check the closest allowed destination.
@ -322,18 +322,18 @@ void RTL::setRtlTypeAndDestination()
float rtl_alt = computeReturnAltitude(destination, (float)_param_rtl_cone_ang.get());
if (destination_type == DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION) {
rtl_alt = destination.alt;
}
switch (destination_type) {
case DestinationType::DESTINATION_TYPE_MISSION_LAND:
_rtl_type = RtlType::RTL_DIRECT_MISSION_LAND;
_rtl_mission_type_handle->setRtlAlt(rtl_alt);
break;
case DestinationType::DESTINATION_TYPE_SAFE_POINT:
case DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION:
rtl_alt = destination.alt; // Override return altitude with last known link altitude
// FALLTHROUGH
case DestinationType::DESTINATION_TYPE_SAFE_POINT:
default:
loiter_point_s landing_loiter;
landing_loiter.lat = destination.lat;
@ -422,10 +422,13 @@ PositionYawSetpoint RTL::findRtlDestination(DestinationType &destination_type, u
PositionYawSetpoint destination{NAN, NAN, NAN, NAN};
if (_param_rtl_type.get() == 5) {
destination = findClosestSafePoint(FLT_MAX, safe_point_index);
destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
PositionYawSetpoint safe_point = findClosestSafePoint(FLT_MAX, safe_point_index);
if (safe_point_index == UINT8_MAX) {
if (safe_point_index != UINT8_MAX) {
destination = safe_point;
destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
} else {
// no safe point found, set destination to last position with valid data link
for (auto &telemetry_status : _telemetry_status_subs) {
telemetry_status_s telemetry;
@ -441,14 +444,14 @@ PositionYawSetpoint RTL::findRtlDestination(DestinationType &destination_type, u
}
}
if (!PX4_ISFINITE(_last_position_before_link_loss.lat) || !PX4_ISFINITE(_last_position_before_link_loss.lon)) {
if (PX4_ISFINITE(_last_position_before_link_loss.lat) && PX4_ISFINITE(_last_position_before_link_loss.lon)) {
destination = _last_position_before_link_loss;
} else {
// if we never had a valid data link position, fallback to current position
destination.alt = _global_pos_sub.get().alt;
destination.lat = _global_pos_sub.get().lat;
destination.lon = _global_pos_sub.get().lon;
} else {
destination = _last_position_before_link_loss;
}
destination_type = DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION;
@ -532,7 +535,6 @@ void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_ite
_home_pos_sub.get().alt : land_mission_item.altitude;
rtl_position.lat = land_mission_item.lat;
rtl_position.lon = land_mission_item.lon;
rtl_position.yaw = _home_pos_sub.get().yaw;
}
void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const
@ -544,14 +546,12 @@ void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mis
rtl_position.lat = mission_safe_point.lat;
rtl_position.lon = mission_safe_point.lon;
rtl_position.alt = mission_safe_point.altitude;
rtl_position.yaw = _home_pos_sub.get().yaw;;
break;
case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT
rtl_position.lat = mission_safe_point.lat;
rtl_position.lon = mission_safe_point.lon;
rtl_position.alt = mission_safe_point.altitude + _home_pos_sub.get().alt; // alt of safe point is rel to home
rtl_position.yaw = _home_pos_sub.get().yaw;;
break;
default:

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

@ -230,7 +230,7 @@ private:
mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
uint32_t _mission_id = 0u;
uint32_t _safe_points_id = 0u;
PositionYawSetpoint _last_position_before_link_loss;
PositionYawSetpoint _last_position_before_link_loss{NAN, NAN, NAN, NAN};
mission_stats_entry_s _stats;

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

@ -54,8 +54,6 @@ RtlDirect::RtlDirect(Navigator *navigator) :
MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL),
ModuleParams(navigator)
{
_destination.lat = static_cast<double>(NAN);
_destination.lon = static_cast<double>(NAN);
_land_approach.lat = static_cast<double>(NAN);
_land_approach.lon = static_cast<double>(NAN);
_land_approach.height_m = NAN;

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

@ -167,7 +167,7 @@ private:
bool _force_heading{false};
RtlTimeEstimator _rtl_time_estimator;
PositionYawSetpoint _destination; ///< the RTL position to fly to
PositionYawSetpoint _destination{NAN, NAN, NAN, NAN}; ///< 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