diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 330fd429de..dfb328c7f6 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/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(¤t_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(&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(&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); + } +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b4bb8bccaa..2617f0f571 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -46,6 +46,7 @@ #pragma once #include +#include #include #include #include @@ -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) _param_rally_approach_clear + ) }; diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index 8a793f5e5f..797080fac5 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/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);