RTL: clean up naming of function arguments

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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.lat = pos_yaw_sp.lat;
-	item.lon = dest.lon;
+	item.lon = pos_yaw_sp.lon;
-	item.altitude = dest.alt;
+	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.lat = pos_yaw_sp.lat;
-	item.lon = dest.lon;
+	item.lon = pos_yaw_sp.lon;
-	item.altitude = dest.alt;
+	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.lat = pos_yaw_sp.lat;
-	item.lon = dest.lon;
+	item.lon = pos_yaw_sp.lon;
-	item.altitude = dest.alt;
+	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.lat = pos_yaw_sp.lat;
-	item.lon = dest.lon;
+	item.lon = pos_yaw_sp.lon;
-	item.altitude = dest.alt;
+	item.altitude = pos_yaw_sp.alt;
-	item.yaw = dest.yaw;
+	item.yaw = pos_yaw_sp.yaw;
 	item.acceptance_radius = _navigator->get_acceptance_radius();
 	item.time_inside = 0.0f;
 	item.autocontinue = true;

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

src/modules/navigator/navigation.h

@@ -230,15 +230,15 @@ struct mission_fence_point_s {
 };
 
 /**
- * @brief Return to launch position.
+ * @brief Position and yaw setpoint struct.
- * Defines the position and landing yaw for the return to launch destination.
+ * Used in RTL state machine.
  *
  */
-struct DestinationPosition {
+struct PositionYawSetpoint {
-	double lat;	/**< latitude in WGS84 [rad].*/
+	double lat;	/**< latitude setpoint in WGS84 [rad].*/
-	double lon;	/**< longitude in WGS84 [rad].*/
+	double lon;	/**< longitude setpoint in WGS84 [rad].*/
-	float alt;	/**< altitude in MSL [m].*/
+	float alt;	/**< altitude setpoint in MSL [m].*/
-	float yaw;	/**< final yaw when landed [rad].*/
+	float yaw;	/**< yaw setpoint [rad].*/
 };
 
 

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

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

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;
+				pos_yaw_sp.yaw = NAN;
-				setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m);
+				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;
+				pos_yaw_sp.yaw = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN;
-				setMoveToPositionMissionItem(_mission_item, dest);
+				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};
+			PositionYawSetpoint pos_yaw_sp{_destination};
-			dest.alt = loiter_altitude;
+			pos_yaw_sp.alt = loiter_altitude;
-			dest.yaw = NAN;
+			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};
+			PositionYawSetpoint pos_yaw_sp{_destination};
-			dest.alt = loiter_altitude;
+			pos_yaw_sp.alt = loiter_altitude;
-			dest.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled
+			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};
+			PositionYawSetpoint pos_yaw_sp{_destination};
-			dest.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled
+			pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled
-			setLandMissionItem(_mission_item, dest);
+			setLandMissionItem(_mission_item, pos_yaw_sp);
 
 			_mission_item.land_precision = _param_rtl_pld_md.get();
 

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