Two fixes for the singleton MavlinkCommandSender used across all
Mavlink instances:
1. initialize() races between instances. Multiple Mavlink::start()
calls hit it concurrently; the existing nullptr check on _instance
is not atomic, and re-running px4_sem_init on a semaphore another
thread is already using is a TSan-flagged data race. Move the
actual init into do_initialize() and call it through pthread_once,
which is portable across POSIX and NuttX.
2. check_timeout() is invoked every Mavlink main loop tick on every
instance. Under load they queue up on the singleton's lock and
form a convoy that delays heartbeat sending. Switch to try_lock:
if another instance is already inside, skip this cycle and retry
on the next tick. The wait was never useful here — the work is
"scan for timed-out commands", which is fine to defer one tick.
Per-channel mavlink_status_t and the per-instance telemetry_status_s
are touched by both the main mavlink thread (sending streams,
publishing status) and the receiver thread (mavlink_parse_char,
counting RX bytes). Until the library is refactored so per-channel
state is owned by the instance and TX seq becomes atomic, take the
existing recursive lock_send() / lock_telemetry_status() helpers in
all the spots that needed them but didn't have them:
mavlink_main.cpp:
- Wrap the per-iteration streams / ulog / events / forwarding loop
with one lock_send() so a stream's send() never races with the
receiver's parse_char on the same channel status. The mutex is
recursive, so nested helpers that take it again are fine.
- lock_send() around the individual sends in handleMavlinkShellOutput,
handleCommands and handleAndGetCurrentCommandAck (these run after
the streams loop, but with the lock dropped between them).
mavlink_receiver.cpp:
- lock_send() around mavlink_parse_char() only — NOT around
handle_message(), because some handlers (e.g. param requests
flowing into configure_stream_threadsafe) busy-wait for the main
mavlink thread, which itself wants lock_send. Holding it across
handle_message produces a circular wait.
- lock_send() around request_message() invocations in
handle_request_message_command (synchronous send paths from RX).
- lock_telemetry_status() around the RX-side _tstatus update so it
doesn't race with publish_telemetry_status() on the main thread.
mavlink_parameters.cpp:
- lock_send() in the hash-param value send, send_param() and
send_error() — these run on both the main thread (no outer lock
in some paths) and the RX thread.
mavlink_mission.cpp:
- lock_send() around every mavlink_msg_mission_*_send_struct call.
streams/LINK_NODE_STATUS.hpp:
- lock_telemetry_status() around the tstatus copy.
Mirrors the comments left in code: the proper fix is to refactor the
shared globals out of the MAVLink library; this is the
"add the lock everywhere it was missing" interim.
The receiver thread reads _mavlink_start_time via get_start_time() in
its first iterations, so writing it after _receiver.start() races
with the receiver. Move the assignment up so the value is published
before the receiver can observe it.
Three closely related thread-safety fixes around state shared between
the Mavlink main thread, the receiver thread, and configure_stream
callers from arbitrary threads:
- _boot_complete becomes px4::atomic_bool. set_boot_complete() runs
on the main mavlink thread; boot_complete() is read from receiver
threads to gate forwarding decisions.
- _subscribe_to_stream becomes px4::atomic<char *>. Producers
(configure_stream_threadsafe on any thread) hand off a stream name
to the consumer (main mavlink thread, in check_requested_subscriptions);
the rate field is sequenced by the atomic store/load on the pointer.
Wall-clock usleep replaces px4_usleep here because in lockstep mode
px4_usleep waits in sim time, which can stall startup if the main
thread isn't advancing time yet.
- mavlink_system.sysid / .compid are written exactly once across
all instances using compare_exchange latches. The values are
singletons (every instance reads the same param), but the field is
read concurrently by _mav_finalize_message_chan_send, so two
instances writing the same value at construction is still a race
TSan flags. Latch only on a *valid* write so an early constructor
with params not yet loaded doesn't permanently lock the latch
closed.
GCC does not provide __has_feature, and its preprocessor tokenizes both
operands of && before evaluating the operator, so
#if defined(__SANITIZE_ADDRESS__) || defined(__has_feature) && __has_feature(address_sanitizer)
failed to compile with 'missing binary operator before token "("' on
toolchains such as the voxl2 linaro GCC. Add the standard __has_feature
fallback shim so the check uses __SANITIZE_ADDRESS__ on GCC and the real
__has_feature on Clang. Behavior is unchanged.
Signed-off-by: Julian Oes <julian@oes.ch>
system_exit() (= _exit()) on Linux skips C++ destructors and atexit
hooks, including ASan's leak-and-error report. Under sanitizer builds
we want that report. Switch to plain exit() when __SANITIZE_ADDRESS__
is defined; native posix shutdown still uses _exit().
Two related fixes for HRT callout queue corruption under lockstep SIH:
1. WorkQueue::Run() popped a WorkItem, released the work-queue lock,
then called work->Run(). If another thread called Deinit on the
item in that window, wq->Remove was a no-op (item already popped)
and Deinit returned while Run() was still executing. Run() could
then call ScheduleDelayed → hrt_call_after on an object that the
caller of Deinit has since destructed and reused via placement new
— the hrt_call metadata ends up living inside another object's
fields, which later overwrite the flink pointer and tear the hrt
callout queue.
Fix: WorkItem carries a _run_in_progress atomic flag set by
WorkQueue::Run around work->Run(). Deinit spins on that flag after
wq->Remove so it cannot return while a Run() is still executing.
WorkQueue captures its worker tid in the constructor (the ctor
runs on the worker thread) so Deinit can skip the self-wait when
called from inside Run() itself (e.g. should_exit() paths that
invoke ScheduleClear).
2. _hrt_lock was a px4_sem_t and hrt_call_invoke unlocked around the
callback so callbacks could re-enter hrt_call_*. That also exposed
the queue to other threads mid-invocation. Switch _hrt_lock to a
PTHREAD_MUTEX_RECURSIVE pthread_mutex and hold it across the
callback — matches NuttX's enter_critical_section() nesting
semantics, lets callbacks reschedule themselves safely, and
prevents concurrent queue manipulation.
Together these eliminate the HRT queue tearing observed in the
SIH-at-20x MAVSDK integration soak (torn flink chains with the tail
unreachable from the head; orphan nodes pointing into reused
memory).
Move the helper from the header into WorkQueue.cpp. Drop the
SignalWorkerThread() call from request_stop() — request_stop sets
_should_exit but the worker is woken up via the existing exit path
in Detach already, so the extra signal here is redundant.
fuzztest's coverage instrumentation is incompatible with Thread
Sanitizer. Add px4_setup_gtest_without_fuzztest() macro to
cmake/px4_add_gtest.cmake that fetches GTest standalone and stubs out
fuzztest cmake functions. Guard all fuzztest-specific code on
TARGET fuzztest::fuzztest so it compiles cleanly without fuzztest.
Split set_absolute_time() into two phases: iterate the waiter list under
_timed_waits_mutex, then broadcast under a separate _broadcast_mutex.
This eliminates the lock-order-inversion cycle between cond_timedwait()
(holds passed_lock -> acquires _timed_waits_mutex) and set_absolute_time()
(held _timed_waits_mutex -> acquired passed_lock).
Fix tests to have each thread lock its own mutex before calling
cond_timedwait, as required by POSIX (the calling thread must own the
mutex passed to pthread_cond_wait). The previous cross-thread ownership
caused TSAN's deadlock detector to overflow its 64-entry limit.
Move registerCallback() after all member variable writes in
SensorSelectionUpdate() and reorder Start() to complete initial
sensor selection before registering the sensor_selection callback.
This prevents Run() from reading partially-written members like
_selected_sensor_device_id and _filter_sample_rate_hz on the work
queue thread while Start() is still initializing them.
This fixes TSAN issues in unit tests.
This fixes TSAN issues on destruction of AtomicTransaction happening
during the unit tests.
The static _MutexHolder instance wraps a pthread_mutex_t in a C++ class,
creating a destructor ordering problem during process exit when worker
threads may still hold the lock. Replace with a raw pthread_mutex_t
initialized via pthread_once, eliminating the C++ destructor entirely.
perf counters are written on the hot path (perf_begin / perf_end /
perf_count) and read concurrently by the `perf` command and the logger,
which ThreadSanitizer flags as data races.
Wrap the counter fields in PerfVar: px4::atomic on POSIX (where TSan runs
and native 64-bit atomics are essentially free) and a plain scalar on
flight hardware (NuttX/QURT). The call sites are identical in both
configurations.
The embedded path matters because a 64-bit atomic on Cortex-M has no
native instruction and lowers to a locked, interrupt-disabling
__atomic_* sequence. Measured on fmu-v6x (microbench microbench_perf):
perf_count 73.7 ns -> 171.8 ns
perf_begin+end 982.9 ns -> 1831.7 ns
~2x on instrumentation that wraps many module Run() loops. These are
single-writer diagnostic counters; the only effect of the plain path is
that a concurrent reader may observe an off-by-one torn read of a 64-bit
field, which is benign and self-correcting (the counter list itself is,
and remains, mutex-protected). The conditional version restores embedded
performance (76.6 ns / 1015 ns) while keeping SITL TSan-clean.
Single TSan/ASan-clean implementation that works on both POSIX and NuttX:
- size()/byteSize() take the AtomicTransaction lock so they don't race
with store() updating _next_slot/_n_slots.
- _grow() snapshots _n_slots under the lock for sizing the malloc, so
another thread's growth between the malloc and the memcpy can't make
the memcpy exceed the allocation. Both platforms use the unlock-
around-malloc + CAS-retry pattern: NuttX requires it (malloc can't
be called with IRQs disabled), and on POSIX it's safe because the
AtomicTransaction mutex still serializes everything else and the
brief unlock window doesn't touch shared state.
- ~DynamicSparseLayer() takes the lock, zeros _next_slot / _n_slots,
stores nullptr into _slots, then frees the buffer. This fixes the
heap-use-after-free TSan caught at process exit on POSIX, where the
static DynamicSparseLayer instances in parameters.cpp are torn down
by cxa_atexit while other threads (commander etc.) are still calling
param_get(). Any reader that acquires the lock post-destruction sees
_next_slot == 0 and falls through to the parent layer; the parent
chain in parameters.cpp is in reverse-declaration order, so the
parent is still alive when the child is destroyed.
Add DynamicSparseLayerTest with concurrent stress tests that reproduce
the races. ConcurrentMultipleWriters pre-allocates to TOTAL to avoid
the known ABA limitation in the CAS retry loop with concurrent _grow().
Drive-by: drop the unused void *p member from param_value_u — nothing
in the tree reads or writes it, and on 64-bit POSIX it was silently
doubling the size of every parameter slot.
Originally found while running the new SIH-based CI tests.
- Separate stopped motor mask into separate state variables (one for
stopping due to flight phase, one for stopping due to NaN thrust in
each direction) and combine them on demand in getStoppedMotors. This
removes the need of hard to follow &= and |= operation involving
shared state.
- use the new nanToZero to clean _thrust_sp for CA internal use
- Introduce lateral/longitudinal/vertical motor masks in base
ActuatorEffecctiveness (and remove previous from individual
Effectiveness classes)
- Populate the bitmasks from all relevant effectivenesses by calling
the new _rotors.setMotorDirectionBitmasks()
- isAlignedWithCoordinateAxis checks axis alignment with given
angle tolerance
- motor stopping is the same as before, using the result of
getStoppedMotors
* docs(docs): Add missing uorb topics sidebar
* docs(docs): discontinue cuav v5 plus/nano
* docs(docs): Fix 404 and 303 status HTTP links
* docs(docs): more link fixes
* update(drivers): Docs fix to fix up link
* docs(docs): Fix up all remaining broken links and add exclusions
Currently, this bogus number of vertices is reported:
nsh> navigator status
INFO [navigator] Running
INFO [navigator] Geofence: 1 inclusion, 6 exclusion polygons, 0 inclusion circles, 2 exclusion circles, 83345 total vertices
The code that reports it accumulates _polygons[i].vertex_count. But for
circles, that field is a float (union), so we get a random large integer
from casing it implicitly.
struct PolygonInfo {
uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_*
uint16_t dataman_index;
union {
uint16_t vertex_count;
float circle_radius;
};
};
Solution: only add vertex_count if we are dealing with a polygon.
Also shorten the overall string slightly, and reorder to make it clear
the vertices belong to the polygons rather than the circles.
* fix(mavlink): reject unsupported params in commands and missions
PX4 accepted any non-NaN value in params it does not use for a given
MAV_CMD, silently storing or ignoring them. This made it impossible for
a GCS to detect that a mission item or command was malformed, and risked
unexpected behaviour if the param meaning is assigned in a future update.
Add mavlink_command_params.h: a header-only sorted lookup table mapping
each supported MAV_CMD to uint8 bitmasks (one for mission items, one for
commands) indicating which of params 1-4 are valid. check_params() does
a binary search and returns the 1-based index of the first offending
param, or 0 if all unsupported params are unset.
A param is considered unset when it is NaN (the MAVLink standard) or 0.0
(the conventional GCS default for unused float fields). Any other value
in an unsupported slot is rejected:
- Mission uploads (MISSION_ITEM / MISSION_ITEM_INT): NACK with
MAV_MISSION_INVALID_PARAMn for the offending param index.
- Commands (COMMAND_LONG / COMMAND_INT): ACK with MAV_RESULT_DENIED.
Validation is placed at the MAVLink ingress boundary
(parse_mavlink_mission_item and handle_message_command_both) before any
downstream processing, keeping Commander and Navigator unmodified.
The table covers all 45 MAV_CMDs handled by PX4's mission and command
parsers. The static_assert enforces sort order at compile time.
Fixes#27483
Signed-off-by: Himaghna <pen314paper@gmail.com>
* fix(mavlink): split mission/command p5-7 masks, fix int_mode branch, add vehicle overrides
Signed-off-by: Himaghna <pen314paper@gmail.com>
* fix(mavlink): fix 142-char lines and Python CI type/style errors
Signed-off-by: Himaghna <pen314paper@gmail.com>
* fix(mavlink): pack p5-7 bits into mission/command byte, fix astyle/type-limits/mypy
* fix(mavlink): range-for over VehicleParamOverrides, fix mypy ignore code
* fix(mavlink): wire check_params_for_vehicle, add int variant, drop non-vehicle check_params
Signed-off-by: Himaghna <pen314paper@gmail.com>
* fix(mavlink): suppress clang unused-function on check_params_*_for_vehicle
Signed-off-by: Himaghna <pen314paper@gmail.com>
* fix(mavlink): rename local vs to avoid shadow in handle_message_command_both
Signed-off-by: Himaghna <pen314paper@gmail.com>
* fix(mavlink): cache vehicle_type_bitmask as member, update on vehicle_status change
Signed-off-by: Himaghna <pen314paper@gmail.com>
* fix(mavlink): use named VEHICLE_TYPE_* consts, fix masks, drop WARN spam, fix int_mode cast
Signed-off-by: Himaghna <pen314paper@gmail.com>
* fix(mavlink): normalize INT32_MAX to 0 for MAV_FRAME_MISSION p5/p6 in int_mode
For MAV_FRAME_MISSION items, x/y are generic p5/p6 params (not lat/lon),
so GCS tools send 0 for unused params — not INT32_MAX. Using
check_params_int_for_vehicle here caused int_param_is_unset(0)=false,
which rejected valid DO_LAND_START and similar items with x=y=0.
Fix by normalizing INT32_MAX to 0.0f before calling check_params_for_vehicle,
so both the MISSION_ITEM_INT sentinel and the conventional float zero are
treated as unset. This restores acceptance of zero-param items like
DO_LAND_START while still correctly rejecting items that carry non-zero,
non-sentinel values in unsupported param slots.
Signed-off-by: Himaghna <pen314paper@gmail.com>
* fix(mavlink): address review feedback on VTOL masks, NaN sentinel, dead enum, and RTL test frame
* fix(mavlink): move param validation test out of mavsdk_tests, rename header to .hpp
* fix(mavlink): dedupe param-scan loop shared by check_params_for_vehicle variants
---------
Signed-off-by: Himaghna <pen314paper@gmail.com>
