mavlink_mission: clear rally points with approaches on startup

src/modules/mavlink/mavlink_mission.cpp

@@ -73,8 +73,11 @@ uint16_t MavlinkMissionManager::_safepoint_update_counter = 0;
 		 (_msg.target_component == MAV_COMP_ID_ALL)))
 
 MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
+	ModuleParams(nullptr),
 	_mavlink(mavlink)
 {
+	updateParams();
+
 	if (!_dataman_init) {
 		_dataman_init = true;
 
@@ -85,7 +88,10 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
 		if (success) {
 			init_offboard_mission(mission_state);
 			load_geofence_stats();
-			load_safepoint_stats();
+
+			if (load_safepoint_stats()) {
+				clear_rally_points_with_approaches();
+			}
 
 		} else {
 			PX4_WARN("offboard mission init failed");
@@ -1779,3 +1785,59 @@ void MavlinkMissionManager::check_active_mission()
 		}
 	}
 }
+
+void MavlinkMissionManager::clear_rally_points_with_approaches()
+{
+	if (_param_rally_approach_clear.get()) {
+
+		size_t new_rally_points_count{0U};
+		mission_item_s last_rally_point{};
+		bool last_item_was_rally_point{false};
+
+		// Go through all rally points and clear all rally points with approaches
+		for (size_t current_seq{1U}; current_seq <= _count[MAV_MISSION_TYPE_RALLY]; ++current_seq) {
+			mission_item_s current_mission_item{};
+
+			const bool success_read = _dataman_client.readSync(DM_KEY_SAFE_POINTS, current_seq,
+						  reinterpret_cast<uint8_t *>(&current_mission_item),
+						  sizeof(mission_item_s));
+
+			if (success_read && current_mission_item.nav_cmd == NAV_CMD_RALLY_POINT) {
+				if (last_item_was_rally_point) {
+					// Last rally point did not have approaches. Save again.
+					const bool success_write = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, new_rally_points_count + 1U,
+								   reinterpret_cast<uint8_t *>(&last_rally_point),
+								   sizeof(mission_item_s));
+
+					if (success_write) {
+						new_rally_points_count++;
+
+					} else {
+						PX4_ERR("Lost valid rally point while clearing approaches.");
+					}
+				}
+
+				last_item_was_rally_point = true;
+				last_rally_point = current_mission_item;
+
+			} else {
+				last_item_was_rally_point = false;
+			}
+		}
+
+		if (last_item_was_rally_point) {
+			const bool success_write = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, new_rally_points_count + 1U,
+						   reinterpret_cast<uint8_t *>(&last_rally_point),
+						   sizeof(mission_item_s));
+
+			if (success_write) {
+				new_rally_points_count++;
+
+			} else {
+				PX4_ERR("Lost valid rally point while clearing approaches.");
+			}
+		}
+
+		update_safepoint_count(new_rally_points_count);
+	}
+}

src/modules/mavlink/mavlink_mission.h

@@ -46,6 +46,7 @@
 #pragma once
 
 #include <dataman_client/DatamanClient.hpp>
+#include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/mission_result.h>
@@ -74,7 +75,7 @@ static constexpr uint64_t MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT = 250000; ///< P
 
 class Mavlink;
 
-class MavlinkMissionManager
+class MavlinkMissionManager : public ModuleParams
 {
 public:
 	explicit MavlinkMissionManager(Mavlink *mavlink);
@@ -267,4 +268,10 @@ private:
 	 */
 	void copy_params_from_mavlink_to_mission_item(struct mission_item_s *mission_item,
 			const mavlink_mission_item_t *mavlink_mission_item, int8_t start_idx = 1, int8_t end_idx = 7);
+
+	void clear_rally_points_with_approaches();
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::RP_APPR_CLEAR>)   _param_rally_approach_clear
+	)
 };

src/modules/mavlink/mavlink_params.c

@@ -155,3 +155,13 @@ PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
  * @max 250
  */
 PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 5);
+
+/**
+ * Clear rally points with approaches on startup
+ *
+ * If set to true, all rally points with associated approaches are cleared on startup.
+ *
+ * @boolean
+ * @group Return To Land
+ */
+PARAM_DEFINE_INT32(RP_APPR_CLEAR, 0);