RTL: change when to set a heading setpoint, generally leave it up to the executer

-remove RTL_HDG_MD
-only set heading setpoint in Navigator::RTL once above landing point,
or when RTL is triggered close to it
-never set a heading during RTL if weather vane is enabled

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2024-02-08 13:46:56 +01:00 committed by Matthias Grob
parent dce53a626e
commit b19e35ec7c
5 changed files with 36 additions and 65 deletions

View File

@ -929,7 +929,7 @@ MissionBlock::initialize()
} }
void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius,
HeadingMode heading_mode) const float heading_sp) const
{ {
item.nav_cmd = NAV_CMD_LOITER_TO_ALT; item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
item.lat = dest.lat; item.lat = dest.lat;
@ -937,7 +937,7 @@ void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const Destina
item.altitude = dest.alt; item.altitude = dest.alt;
item.altitude_is_relative = false; item.altitude_is_relative = false;
item. yaw = setYawFromHeadingMode(dest, heading_mode); item.yaw = heading_sp;
item.acceptance_radius = _navigator->get_acceptance_radius(); item.acceptance_radius = _navigator->get_acceptance_radius();
item.time_inside = 0.0f; item.time_inside = 0.0f;
@ -947,7 +947,7 @@ void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const Destina
} }
void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time,
float loiter_radius, HeadingMode heading_mode) const float loiter_radius, float heading_sp) const
{ {
const bool autocontinue = (loiter_time > -FLT_EPSILON); const bool autocontinue = (loiter_time > -FLT_EPSILON);
@ -963,7 +963,7 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat
item.altitude = dest.alt; item.altitude = dest.alt;
item.altitude_is_relative = false; item.altitude_is_relative = false;
item. yaw = setYawFromHeadingMode(dest, heading_mode); item.yaw = NAN;
item.acceptance_radius = _navigator->get_acceptance_radius(); item.acceptance_radius = _navigator->get_acceptance_radius();
item.time_inside = math::max(loiter_time, 0.0f); item.time_inside = math::max(loiter_time, 0.0f);
@ -973,7 +973,7 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat
} }
void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest,
HeadingMode heading_mode) const float heading_sp) const
{ {
item.nav_cmd = NAV_CMD_WAYPOINT; item.nav_cmd = NAV_CMD_WAYPOINT;
item.lat = dest.lat; item.lat = dest.lat;
@ -986,46 +986,23 @@ void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const Dest
item.time_inside = 0.f; item.time_inside = 0.f;
item.origin = ORIGIN_ONBOARD; item.origin = ORIGIN_ONBOARD;
item. yaw = setYawFromHeadingMode(dest, heading_mode); item.yaw = heading_sp;
} }
void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest,
HeadingMode heading_mode) const const float heading_sp) const
{ {
item.nav_cmd = NAV_CMD_LAND; item.nav_cmd = NAV_CMD_LAND;
item.lat = dest.lat; item.lat = dest.lat;
item.lon = dest.lon; item.lon = dest.lon;
item.altitude = dest.alt; item.altitude = dest.alt;
item.yaw = heading_sp;
if (heading_mode == HeadingMode::CURRENT_HEADING) {
item.yaw = _navigator->get_local_position()->heading;
} else {
item.yaw = dest.yaw;
}
item.acceptance_radius = _navigator->get_acceptance_radius(); item.acceptance_radius = _navigator->get_acceptance_radius();
item.time_inside = 0.0f; item.time_inside = 0.0f;
item.autocontinue = true; item.autocontinue = true;
item.origin = ORIGIN_ONBOARD; item.origin = ORIGIN_ONBOARD;
} }
float MissionBlock::setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const
{
float desired_yaw(_navigator->get_local_position()->heading);
if (heading_mode == HeadingMode::NAVIGATION_HEADING) {
desired_yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon, dest.lat, dest.lon);
} else if (heading_mode == HeadingMode::DESTINATION_HEADING) {
desired_yaw = dest.yaw;
}
return desired_yaw;
}
void MissionBlock::startPrecLand(uint16_t land_precision) void MissionBlock::startPrecLand(uint16_t land_precision)
{ {
if (_mission_item.land_precision == 1) { if (_mission_item.land_precision == 1) {

View File

@ -206,17 +206,15 @@ protected:
void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode); 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, void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius,
HeadingMode heading_mode) const; float heading_sp) const;
void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time,
float loiter_radius, HeadingMode heading_mode) const; float loiter_radius, float heading_sp) const;
void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest,
HeadingMode heading_mode) const; float heading_sp) const;
void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, HeadingMode heading_mode) const; void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, const float heading_sp) const;
float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const;
void startPrecLand(uint16_t land_precision); void startPrecLand(uint16_t land_precision);

View File

@ -178,11 +178,7 @@ void RtlDirect::set_rtl_item()
_global_pos_sub.get().lat, _global_pos_sub.get().lon); _global_pos_sub.get().lat, _global_pos_sub.get().lon);
const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt); const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt);
HeadingMode rtl_heading_mode = static_cast<HeadingMode>(_param_rtl_hdg_md.get()); const bool is_close_to_destination = destination_dist < _param_rtl_min_dist.get();
if ((rtl_heading_mode == HeadingMode::NAVIGATION_HEADING) && (destination_dist < _param_rtl_min_dist.get())) {
rtl_heading_mode = HeadingMode::DESTINATION_HEADING;
}
switch (_rtl_state) { switch (_rtl_state) {
case RTLState::CLIMBING: { case RTLState::CLIMBING: {
@ -191,8 +187,8 @@ void RtlDirect::set_rtl_item()
.lon = _global_pos_sub.get().lon, .lon = _global_pos_sub.get().lon,
.alt = _rtl_alt, .alt = _rtl_alt,
}; };
const float heading_sp = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading;
setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), HeadingMode::CURRENT_HEADING); setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), heading_sp);
_rtl_state = RTLState::MOVE_TO_LOITER; _rtl_state = RTLState::MOVE_TO_LOITER;
break; break;
@ -209,10 +205,12 @@ void RtlDirect::set_rtl_item()
// For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status // 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 // 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) { if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m, rtl_heading_mode); setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m, NAN);
} else { } else {
setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); // already set final yaw if close to destination and WV is disabled
const float heading_sp = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN;
setMoveToPositionMissionItem(_mission_item, dest, heading_sp);
} }
_rtl_state = RTLState::LOITER_DOWN; _rtl_state = RTLState::LOITER_DOWN;
@ -228,7 +226,9 @@ void RtlDirect::set_rtl_item()
.yaw = _destination.yaw, .yaw = _destination.yaw,
}; };
setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m, rtl_heading_mode); // set final yaw if WV is disabled
const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN;
setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m, heading_sp);
pos_sp_triplet->next.valid = true; pos_sp_triplet->next.valid = true;
pos_sp_triplet->next.lat = _destination.lat; pos_sp_triplet->next.lat = _destination.lat;
@ -255,8 +255,10 @@ void RtlDirect::set_rtl_item()
.yaw = _destination.yaw, .yaw = _destination.yaw,
}; };
// set final yaw if WV is disabled
const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN;
setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m, setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m,
rtl_heading_mode); heading_sp);
if (_param_rtl_land_delay.get() < -FLT_EPSILON) { if (_param_rtl_land_delay.get() < -FLT_EPSILON) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t");
@ -279,7 +281,7 @@ void RtlDirect::set_rtl_item()
DestinationPosition dest{_destination}; DestinationPosition dest{_destination};
dest.alt = loiter_altitude; dest.alt = loiter_altitude;
setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); setMoveToPositionMissionItem(_mission_item, dest, NAN);
// Prepare for transition // Prepare for transition
_mission_item.vtol_back_transition = true; _mission_item.vtol_back_transition = true;
@ -309,7 +311,9 @@ void RtlDirect::set_rtl_item()
DestinationPosition dest{_destination}; DestinationPosition dest{_destination};
dest.alt = loiter_altitude; dest.alt = loiter_altitude;
setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); // set final yaw if WV is disabled
const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN;
setMoveToPositionMissionItem(_mission_item, dest, heading_sp);
_navigator->reset_position_setpoint(pos_sp_triplet->previous); _navigator->reset_position_setpoint(pos_sp_triplet->previous);
_rtl_state = RTLState::LAND; _rtl_state = RTLState::LAND;
@ -319,7 +323,9 @@ void RtlDirect::set_rtl_item()
case RTLState::LAND: { case RTLState::LAND: {
setLandMissionItem(_mission_item, _destination, rtl_heading_mode); // set final yaw if WV is disabled
const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN;
setLandMissionItem(_mission_item, _destination, heading_sp);
_mission_item.land_precision = _param_rtl_pld_md.get(); _mission_item.land_precision = _param_rtl_pld_md.get();

View File

@ -190,9 +190,11 @@ private:
(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist, (ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md, (ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md,
(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad, (ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad,
(ParamInt<px4::params::RTL_HDG_MD>) _param_rtl_hdg_md,
(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor, (ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,
(ParamInt<px4::params::RTL_TIME_MARGIN>) _param_rtl_time_margin (ParamInt<px4::params::RTL_TIME_MARGIN>) _param_rtl_time_margin,
// external params
(ParamBool<px4::params::WV_EN>) _param_wv_en
) )
param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; param_t _param_mpc_z_v_auto_up{PARAM_INVALID};

View File

@ -160,18 +160,6 @@ PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
*/ */
PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 80.0f); PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 80.0f);
/**
* RTL heading mode
*
* Defines the heading behavior during RTL
*
* @value 0 Towards next waypoint.
* @value 1 Heading matches destination.
* @value 2 Use current heading.
* @group Return Mode
*/
PARAM_DEFINE_INT32(RTL_HDG_MD, 0);
/** /**
* RTL time estimate safety margin factor * RTL time estimate safety margin factor
* *