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 <[email protected]>
PX4 · Feb 8, 2024 · f6945d7 · f6945d7
1 parent 97cb933
commit f6945d7
Show file tree

Hide file tree

Showing 5 changed files with 36 additions and 65 deletions.
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
@@ -929,15 +929,15 @@ MissionBlock::initialize()
 }
 
 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.lat = dest.lat;
 	item.lon = dest.lon;
 	item.altitude = dest.alt;
 	item.altitude_is_relative = false;
 
-	item. yaw = setYawFromHeadingMode(dest, heading_mode);
+	item.yaw = heading_sp;
 
 	item.acceptance_radius = _navigator->get_acceptance_radius();
 	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,
-		float loiter_radius, HeadingMode heading_mode) const
+		float loiter_radius, float heading_sp) const
 {
 	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_is_relative = false;
 
-	item. yaw = setYawFromHeadingMode(dest, heading_mode);
+	item.yaw = NAN;
 
 	item.acceptance_radius = _navigator->get_acceptance_radius();
 	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,
-		HeadingMode heading_mode) const
+		float heading_sp) const
 {
 	item.nav_cmd = NAV_CMD_WAYPOINT;
 	item.lat = dest.lat;
@@ -986,46 +986,23 @@ void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const Dest
 	item.time_inside = 0.f;
 	item.origin = ORIGIN_ONBOARD;
 
-	item. yaw = setYawFromHeadingMode(dest, heading_mode);
+	item.yaw = heading_sp;
 }
 
 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.lat = dest.lat;
 	item.lon = dest.lon;
 	item.altitude = dest.alt;
-
-	if (heading_mode == HeadingMode::CURRENT_HEADING) {
-		item.yaw = _navigator->get_local_position()->heading;
-
-	} else {
-		item.yaw = dest.yaw;
-	}
-
+	item.yaw = heading_sp;
 	item.acceptance_radius = _navigator->get_acceptance_radius();
 	item.time_inside = 0.0f;
 	item.autocontinue = true;
 	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)
 {
 	if (_mission_item.land_precision == 1) {

diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
@@ -206,17 +206,15 @@ class MissionBlock : public NavigatorMode
 	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,
-				       HeadingMode heading_mode) const;
+				       float heading_sp) const;
 
 	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,
-					  HeadingMode heading_mode) const;
+					  float heading_sp) const;
 
-	void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, HeadingMode heading_mode) const;
-
-	float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const;
+	void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, const float heading_sp) const;
 
 	void startPrecLand(uint16_t land_precision);
 

diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp
@@ -178,11 +178,7 @@ void RtlDirect::set_rtl_item()
 				       _global_pos_sub.get().lat, _global_pos_sub.get().lon);
 	const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt);
 
-	HeadingMode rtl_heading_mode = static_cast<HeadingMode>(_param_rtl_hdg_md.get());
-
-	if ((rtl_heading_mode == HeadingMode::NAVIGATION_HEADING) && (destination_dist < _param_rtl_min_dist.get())) {
-		rtl_heading_mode = HeadingMode::DESTINATION_HEADING;
-	}
+	const bool is_close_to_destination = destination_dist < _param_rtl_min_dist.get();
 
 	switch (_rtl_state) {
 	case RTLState::CLIMBING: {
@@ -191,8 +187,8 @@ void RtlDirect::set_rtl_item()
 				.lon = _global_pos_sub.get().lon,
 				.alt = _rtl_alt,
 			};
-
-			setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), HeadingMode::CURRENT_HEADING);
+			const float heading_sp = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading;
+			setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), heading_sp);
 
 			_rtl_state = RTLState::MOVE_TO_LOITER;
 			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
 			// 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) {
-				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 {
-				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;
@@ -228,7 +226,9 @@ void RtlDirect::set_rtl_item()
 				.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.lat = _destination.lat;
@@ -255,8 +255,10 @@ void RtlDirect::set_rtl_item()
 				.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,
-						 rtl_heading_mode);
+						 heading_sp);
 
 			if (_param_rtl_land_delay.get() < -FLT_EPSILON) {
 				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t");
@@ -279,7 +281,7 @@ void RtlDirect::set_rtl_item()
 			DestinationPosition dest{_destination};
 			dest.alt = loiter_altitude;
 
-			setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode);
+			setMoveToPositionMissionItem(_mission_item, dest, NAN);
 
 			// Prepare for transition
 			_mission_item.vtol_back_transition = true;
@@ -309,7 +311,9 @@ void RtlDirect::set_rtl_item()
 			DestinationPosition dest{_destination};
 			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);
 
 			_rtl_state = RTLState::LAND;
@@ -319,7 +323,9 @@ void RtlDirect::set_rtl_item()
 
 	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();
 

diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h
@@ -190,9 +190,11 @@ class RtlDirect : public MissionBlock, public ModuleParams
 		(ParamFloat<px4::params::RTL_MIN_DIST>)    _param_rtl_min_dist,
 		(ParamInt<px4::params::RTL_PLD_MD>)        _param_rtl_pld_md,
 		(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,
-		(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};

diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
@@ -160,18 +160,6 @@ PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
  */
 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
  *