From 81ab99c2a926bd908610545db814478a417de53a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 6 Jul 2026 10:53:36 +1200 Subject: [PATCH] fix(mavlink): make mission shared-state mutex non-recursive The mission manager's _shared_state_mutex was recursive only because the count updaters (update_geofence_count / update_safepoint_count) called the public update_active_mission(), which re-took the same lock. That is the "public method calls another public locking method" antipattern, not a genuine need for recursion. Split the state update into an update_active_mission_locked() helper that assumes the lock is held and returns the mission_s to publish. The count updaters call it while already holding the lock and publish after unlocking; the public update_active_mission() locks around the helper. The mutex is now a plain, statically-initialized (PTHREAD_MUTEX_INITIALIZER) mutex, dropping the pthread_once runtime init and the non-portable recursive attribute. This removes the dependency on CONFIG_PTHREAD_MUTEX_TYPES for this mutex, which is not enabled on most NuttX boards. Signed-off-by: Julian Oes --- src/modules/mavlink/mavlink_mission.cpp | 50 ++++++++++++------------- src/modules/mavlink/mavlink_mission.h | 8 ++-- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 47f6beb3242..5b8df3fd9b4 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -67,7 +67,9 @@ uint32_t MavlinkMissionManager::_crc32[3] = { 0, 0, 0 }; int32_t MavlinkMissionManager::_current_seq = 0; bool MavlinkMissionManager::_transfer_in_progress = false; -pthread_mutex_t MavlinkMissionManager::_shared_state_mutex; +// Non-recursive: the count updaters that already hold it use update_active_mission_locked(). +// Statically initialized so no runtime init (and no non-portable recursive initializer) is needed. +pthread_mutex_t MavlinkMissionManager::_shared_state_mutex = PTHREAD_MUTEX_INITIALIZER; constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; #define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ @@ -75,22 +77,9 @@ constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ (_msg.target_component == MAV_COMP_ID_ALL))) -static pthread_once_t s_mission_mutex_once = PTHREAD_ONCE_INIT; - -static void init_mission_shared_mutex() -{ - pthread_mutexattr_t attr; - pthread_mutexattr_init(&attr); - pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); - pthread_mutex_init(&MavlinkMissionManager::_shared_state_mutex, &attr); - pthread_mutexattr_destroy(&attr); -} - MavlinkMissionManager::MavlinkMissionManager(Mavlink &mavlink) : _mavlink(mavlink) { - pthread_once(&s_mission_mutex_once, init_mission_shared_mutex); - if (!_dataman_init) { _dataman_init = true; @@ -167,12 +156,11 @@ MavlinkMissionManager::load_safepoint_stats() /** * Publish mission topic to notify navigator about changes. */ -void -MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint16_t count, int32_t seq, uint32_t crc32, - bool write_to_dataman) +mission_s +MavlinkMissionManager::update_active_mission_locked(dm_item_t mission_dataman_id, uint16_t count, int32_t seq, + uint32_t crc32) { - /* update active mission state */ - pthread_mutex_lock(&_shared_state_mutex); + /* update active mission state (caller holds _shared_state_mutex) */ _mission_dataman_id = mission_dataman_id; _my_mission_dataman_id = _mission_dataman_id; _count[MAV_MISSION_TYPE_MISSION] = count; @@ -191,6 +179,15 @@ MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint1 mission.safe_points_id = _crc32[MAV_MISSION_TYPE_RALLY]; mission.land_start_index = _land_start_marker; mission.land_index = _land_marker; + return mission; +} + +void +MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint16_t count, int32_t seq, uint32_t crc32, + bool write_to_dataman) +{ + pthread_mutex_lock(&_shared_state_mutex); + mission_s mission = update_active_mission_locked(mission_dataman_id, count, seq, crc32); pthread_mutex_unlock(&_shared_state_mutex); if (write_to_dataman) { @@ -237,11 +234,11 @@ MavlinkMissionManager::update_geofence_count(dm_item_t fence_dataman_id, unsigne return PX4_ERROR; } - // update_active_mission takes the same (recursive) mutex - update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, - _crc32[MAV_MISSION_TYPE_MISSION], - false); + mission_s mission = update_active_mission_locked(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, + _crc32[MAV_MISSION_TYPE_MISSION]); pthread_mutex_unlock(&_shared_state_mutex); + + _offboard_mission_pub.publish(mission); return PX4_OK; } @@ -277,10 +274,11 @@ MavlinkMissionManager::update_safepoint_count(dm_item_t safepoint_dataman_id, un return PX4_ERROR; } - update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, - _crc32[MAV_MISSION_TYPE_MISSION], - false); + mission_s mission = update_active_mission_locked(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, + _crc32[MAV_MISSION_TYPE_MISSION]); pthread_mutex_unlock(&_shared_state_mutex); + + _offboard_mission_pub.publish(mission); return PX4_OK; } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 4eead22cd21..76debb373d9 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -131,13 +131,11 @@ private: static uint32_t _crc32[3]; ///< Checksum of items in (active) mission for each MAV_MISSION_TYPE static int32_t _current_seq; ///< Current item sequence in active mission -public: // Serializes access to the shared static state above. Multiple Mavlink // instances' receiver threads concurrently detect mission changes and // write these statics; without this lock they race on writes. - // Public so that the pthread_once initializer can reach it. + // Non-recursive: callers that already hold it use the _locked() helpers. static pthread_mutex_t _shared_state_mutex; -private: int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached) bool _last_finished{false}; ///< Last mission finished state @@ -198,6 +196,10 @@ private: void update_active_mission(dm_item_t mission_dataman_id, uint16_t count, int32_t seq, uint32_t crc32, bool write_to_dataman = true); + // Updates the shared active-mission statics and returns the mission_s to publish. + // The caller must hold _shared_state_mutex; does not lock, write to dataman, or publish. + mission_s update_active_mission_locked(dm_item_t mission_dataman_id, uint16_t count, int32_t seq, uint32_t crc32); + /** store the geofence count to dataman */ int update_geofence_count(dm_item_t fence_dataman_id, unsigned count, uint32_t crc32);