* feat(uxrce_dds_client): add "drain" and "unlimited" rate options for dds_topics bridging
Drain option allows to specify that a queued uORB topic being bridged should
have its queue drained completely at every rate interval, instead of sending
only the latest message. This is useful to prevent jitter and delay being
added on topics subject to burst and irregular publishing patterns.
The "unlimited" rate option allows to specify that a uORB topic being bridged
should be sent as fast as possible, without any rate limiting. This is useful
for reducing latency on topics where no message loss is acceptable and where
the uORB topic is being published at a high rate.
* fix(dds_topics): remove "drain" option and safe guard against inf loop
Drain option is removed to be coupled with rate_limit = unlimited.
The loop is gated to the size of the uorb queue to avoid infinite loops
if publisher is faster that this loop.
* fix(dds_topics): simplification recommended in review
* tecs: Prevent. NaN in pitch integrator
Prevents NaN propagation into _pitch_integ_state when seb_rate.setpoint is non-finite (as seen when exiting FW velocity Offboard). Mirrors the existing throttle integrator finite check. Fixes#25906.
* tecs: Harden the TECS pitch integrator against NaN contamination
* fix(tecs): guard pitch output against non-finite SEB rate
The integrator guard alone does not prevent the reported NaN lockup when
the non-finite value originates on the energy/speed side: the feedforward
and damping terms in _calcPitchControlOutput feed seb_rate straight into
the pitch demand, bypassing the integrator. constrain() does not reject
NaN, so the pitch setpoint latches NaN permanently and the aircraft
becomes uncontrollable.
Fall back to the (now guaranteed finite) integrator state when the
uncorrected pitch setpoint is non-finite, and drop the stray trailing
whitespace introduced by the previous commit.
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
* test(tecs): regression test for NaN pitch integrator and output
Drives TECSControl::update with a non-finite true airspeed setpoint, the
same way seb_rate goes non-finite on the energy/speed side when exiting
offboard velocity mode, and asserts the pitch integrator state and the
pitch setpoint both stay finite, across a single bad frame and sustained
non-finite input. Both assertions fail against unguarded TECS.
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
* test(tecs): cover the corrupted-integrator-state safety reset
Addresses review feedback on #27749: the existing tests inject a
non-finite airspeed setpoint, which the integrator-input guard rejects
before it can reach _pitch_integ_state, so the state-corruption reset
branch in _calcPitchControlUpdate was never actually exercised.
Add a FRIEND_TEST seam so the regression test can pre-corrupt
_pitch_integ_state directly, then confirm a single update() call detects
the non-finite state, resets it, and keeps the pitch setpoint finite.
Verified the test fails against the code with the reset guard removed and
passes with it in place.
---------
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
Co-authored-by: Ryan Johnston <31726584+ryanjAA@users.noreply.github.com>
Co-authored-by: Ryan Johnston <rjohnston@appliedaeronautics.com>
* feat(drivers/dps310): add DPS368 variant support via -8 flag
Register DRV_BARO_DEVTYPE_DPS368 (0x77) and extend the dps310 driver
entry point with a -8 flag to select the DPS368 devtype at startup.
* fix(drivers/dps310): set device type from devid_driver_index after init
A recent SIH log (V6X) was full of:
WARNING: [px4] [load_mon] sih low on stack! (144 bytes left)
Using PublicationData (which holds the esc_status_s in heap rather than
stack) fixes them and adheres to the philosophy from the module
description: "Most of the variables are declared global in the .hpp file
to avoid stack overflow"
The trajectory time-stretch is skipped when the max-error param is at or below
FLT_EPSILON, so zero is a valid 'disabled' setting. Lower the minimum of
MPC_XY_ERR_MAX and MPC_Z_ERR_MAX from 0.1 to 0 so users can actually select it,
and document it in both params.
Also fix the setMaxAllowedVerticalError() doc comment, which incorrectly
claimed zero was the default: the MPC_Z_ERR_MAX param default is 1 m.
Signed-off-by: Julian Oes <julian@oes.ch>
These helpers are no longer called from production code after the smoother
was reworked to clamp XY and Z velocity components independently. Their only
remaining references were in TrajectoryConstraintsTest.cpp, which is not
registered in CMake and therefore never compiled. Drop both the functions and
the orphaned tests.
Signed-off-by: Julian Oes <julian@oes.ch>
PositionSmoothing already has a horizontal-error gate (MPC_XY_ERR_MAX)
that slows trajectory integration when the smoother's virtual position
runs ahead of the drone in XY. The same time-stretch was applied to
all three axes, but it was only ever computed from XY error. So:
- Z lag never slowed Z down: a noisy altitude reference (e.g. mission
setpoint jittering as home altitude is refined in flight) could
walk the commanded z 5+ m above the mission target while horizontal
tracking stayed fine. The jerk-limited smoother integrates each
small step into a velocity ramp, and successive ramps superpose
into multi-meter altitude excursions.
- Z lag dragging XY: when a Z stretch eventually was added, applying
the same scalar to all axes meant a ±1 m altitude jitter could
cap forward flight speed during a straight mission leg. That's
almost never what we want.
Fix: compute time_stretch_xy and time_stretch_z independently using
their own _max_allowed_*_error thresholds and apply per-axis. Default
the new MPC_Z_ERR_MAX to 1 m (tighter than XY's 2 m because altitude
holds are usually crisper). Other PositionSmoothing users (Orbit,
GotoControl) keep zero as the vertical error cap, which is the
explicit "disable Z stretch" sentinel — so behaviour there is
unchanged.
ScheduleOnInterval and registerCallback can both invoke Run() on
another thread (the work queue / publisher thread). configure_gripper
sets state Run() reads, so it must complete before the callbacks are
armed — otherwise a fast first iteration can race with the gripper
initialization.
set_interval_us() and the initial _time_stamp_last_loop write must
happen before the uORB subscription callback is armed — otherwise a
publisher on another thread can invoke Run() concurrently with init().
Run() reads _time_stamp_last_loop and depends on the interval being
set; observing zero-init values briefly leads to either a giant first
dt or an over-rate first iteration.
PositionSmoothing::_generateVelocitySetpoint computed a single
constrained 3D velocity vector and then ran clampToXYNorm /
clampToZNorm in series. The XY clamp could shorten the Z component
(and vice versa), so a Z speed limit ended up capping forward speed
and a tight altitude error capped horizontal motion. Compute XY and
Z velocity components independently using their own speed limits so
they don't mutually clamp.
Companion to the per-axis time-stretch fix (commit added later in
the chain). Together they decouple the horizontal and vertical
trajectory dynamics in the smoother.
When a mission is finished we need to make sure to send the mission
current message and not drop it. Otherwise, MAVSDK tests might not get
the update that the mission has been finished.
Before calling _tecs.update in tecs_update_pitch_throttle, all pitch
values are corrected by subtracting trim pitch, so TECS internal pitch
is always relative to trim.
To stay correct we have to add it back when passing the TECS internal
pitch setpoint to the rest of the system.
Document the Official PX4 Developer Kit program: a new top-level
Developer Kits section listing the certified kits (ModalAI Starling 2
Max, DroneBlocks DEXI 5, Holybro X500 v2), a new DEXI 5 page, and a
manufacturer-facing page describing the program requirements, benefits,
and eligibility.
Restructure the hardware navigation so all certified kits live in one
place instead of being split across Complete Vehicles and Kits, drop
discontinued build guides from the sidebar (still linked from the kit
build guides page), and group the manufacturer guides under a new
Manufacturers section in Development > Hardware.
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
The per-channel send mutex array was defined without static, giving it external
linkage in the single-binary firmware even though it is only used within
mavlink_main.cpp (its status/buffer siblings are already static). Make it static
to match and avoid a latent cross-TU name clash.
Signed-off-by: Julian Oes <julian@oes.ch>
get_status()/get_buffer() returned per-instance _mavlink_status_early/
_mavlink_buffer_early fallbacks because the constructor touches the channel
status (via mavlink_update_parameters() -> setProtocolVersion()) before
set_instance_id() has assigned a channel, and set_instance_id() then copied
those into the static per-channel arrays.
Instead, don't touch channel state before we own a channel: setProtocolVersion()
now only caches _protocol_version until _instance_id is valid, and set_instance_id()
applies the cached version to the freshly-claimed channel's status flags. This lets
get_status()/get_buffer() index the per-channel arrays directly and removes the two
early buffers. The end state is identical (the buffer copy always copied a
zero-initialized buffer, since nothing writes it before assignment).
Signed-off-by: Julian Oes <julian@oes.ch>
Replace the terse "triggers TSAN" note with a TODO explaining that 'sensors
status' reads sensor module state from the shell thread without synchronization,
so it should be re-enabled once that race is fixed.
Signed-off-by: Julian Oes <julian@oes.ch>
NuttX tasks are task_create() children, not pthreads, so px4_task_join has no
portable join and instead polls whether the task still exists. Document that it
returns no exit status, can wait on the wrong task if the PID is reused, and is
only safe because its sole on-target caller is WorkQueueManager shutdown (and
NuttX shutdown is a full reboot).
Signed-off-by: Julian Oes <julian@oes.ch>
px4_task_t is a plain int and callers hold -1 for "no task" (e.g. dataman's
g_task_id after stop). The bounds checks only tested id < PX4_MAX_TASKS, which is
true for negative ids, so px4_task_join(-1) and px4_task_delete(-1) would index
taskmap[-1] out of bounds. Check id >= 0 as well.
Signed-off-by: Julian Oes <julian@oes.ch>
process_add_subscription() (CONFIG_ORB_COMMUNICATOR only) read _data_valid and
the topic buffer contents while passing a pointer into _data straight to
send_message(), with no synchronization against a concurrent publisher. A
publisher's write() memcpy's into _data and sets _data_valid under ATOMIC_ENTER,
so the remote-init send can read a torn/partially-updated sample - most clearly
for o_queue == 1, where the read slot ((gen - 1) % 1) is exactly the slot being
written.
This is a pre-existing latent race (the previous px4::atomic<uint8_t*> _data only
guarded the pointer, never the bytes) that neither ASan nor TSan can catch because
CONFIG_ORB_COMMUNICATOR is not built in those configs. On VOXL2 it runs on the
muorb RX thread concurrently with publishers.
Snapshot the most recent sample into a temporary buffer under ATOMIC_ENTER, then
send it outside the critical section - mirroring copy(). The buffer is allocated
before the critical section (it cannot be allocated under ATOMIC_ENTER on NuttX)
and send_message() must run outside it anyway, since it can be slow and may call
back into DeviceNode. This only affects the rare subscription-add path, not the
publish hot path.
Signed-off-by: Julian Oes <julian@oes.ch>
The daemon server's client-handler thread spawns module tasks (e.g. dataman).
Start px4::init_once()/init() before creating px4_daemon::Server so platform init
- uORB, work queues, logging - completes with a happens-before edge to those
tasks. Otherwise a module task racing the still-running init reads globals like
uORB::Manager's instance pointer or the log message advertisement without
synchronization (ThreadSanitizer flags it) - and an atomic would not make it
correct, since the publication could simply be skipped.
tx_message_count (every send) and tx_buffer_overruns took _tstatus_mutex on the
hot path just to increment a counter. Make them px4::atomic, like the existing
_bytes_* counters, bumped lock-free from either thread's send path and folded
into _tstatus (cumulative) under the mutex at the ~1 Hz publish. The mutex is
kept for the genuinely shared low-frequency fields. (Suggested in review.)
The param access mutex must be recursive because a layered param lookup
re-enters the lock on the same thread (Layer::get() -> _parent->get()).
Initialize it in param_init() - which runs once before any thread accesses
parameters - via pthread_mutexattr_settype(PTHREAD_MUTEX_RECURSIVE), replacing
the previous pthread_once lazy init. This avoids the non-portable glibc static
initializer PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP, which is unavailable on QURT
and gated behind _GNU_SOURCE elsewhere (broke the voxl2 and SITL builds).
_data (the lazily-allocated topic buffer) was a px4::atomic only to make the
double-checked-locking publish safe against concurrent readers. It is allocated
under the node lock() and written exactly once, so reading it under ATOMIC_ENTER
- which is that same lock() on POSIX - serializes the subscriber side against the
allocation without needing an atomic. On NuttX it is a set-once, word-aligned
pointer. The copy() guard is moved inside ATOMIC_ENTER accordingly. (Suggested in
review.)
DeviceNode::open() took the cdev lock only to call mark_as_advertised(), but
_advertised is a px4::atomic, so the single store needs no lock. (Suggested in
review.)
Add a test_and_ci page covering the PX4_ASAN / PX4_TSAN / PX4_MSAN / PX4_UBSAN
build variables, the Linux `sudo sysctl vm.mmap_rnd_bits=28` workaround for the
sanitizer runtime's startup memory mapping, and the TSan suppressions file
(test/sitl_tsan.supp) for the Gazebo simulation transport. Linked from SUMMARY.md
and the test_and_ci index.
Running SITL with the Gazebo bridge under ThreadSanitizer reports many data races
inside libzmq and gz-transport (and the GZBridge glue) - third-party simulation
transport that is not in the flight path or the SIH-based CI and that we cannot
fix. Add a suppressions file so the TSan signal stays usable for finding real
races. Use via TSAN_OPTIONS="suppressions=test/sitl_tsan.supp".
init() registered the vehicle_local_position callback before calling
set_interval_us() and initialising _time_stamp_last_loop. The moment the callback
is registered it fires (and Run() is scheduled) on the publisher's thread, racing
those writes - ThreadSanitizer flags both. Register the callback last, once the
object is fully initialised.
_interval_us and _last_update are read in SubscriptionCallbackWorkItem::call()
(and SubscriptionBlocking::call()) on the publishing thread, while the subscriber
sets them - e.g. from mixer_module's setMaxTopicUpdateRate() on a reconfigure.
That is a genuine cross-thread access that ThreadSanitizer flags.
Instead of making these fields atomic in the base SubscriptionInterval - which is
used single-threaded by 100+ subscriptions - parameterize the class on whether the
interval / last-update storage is atomic and expose two aliases:
SubscriptionInterval plain, single-threaded (the common case)
SubscriptionIntervalAtomic atomic, for callback subscriptions
SubscriptionCallback - and hence all callback subscriptions
(SubscriptionCallbackWorkItem, SubscriptionBlocking, CallbackHandler) - uses the
atomic variant; every other subscription stays plain with no synchronization
overhead.
The updated()/update()/copy() bodies stay out-of-line with explicit instantiation
of both variants, so they are not inlined at every call site (flash size).
The publisher fires callbacks via SubscriptionCallbackWorkItem::call(), which read
the subscriber's _last_generation (and called updated(), which reads it again) to
decide whether to schedule the work item. That made the subscriber's read cursor a
cross-thread field - publisher reads it while the subscriber writes it in copy() -
which TSan flagged and which we had wrapped in an atomic.
Instead, hand the publishing node's generation into call() and let the callback keep
its own _last_scheduled_generation cursor for the count gate (_required_updates).
call() runs under the node lock, so that cursor is only ever touched there and is
serialized - no atomic needed, and using the real generation (not a plain counter)
means a coalesced/missed call() does not drift the batch. The redundant updated()
generation check is dropped (call() only runs right after a publish, so it is always
true); the interval gate (_last_update / _interval_us) is kept unchanged.
With the publisher no longer reading it, _last_generation reverts to a plain unsigned
(subscriber-thread only). Behaviour is unchanged: the count and interval throttles
fire at the same rates (verified via work_queue status - gyro_fft 62.5 Hz,
flight_mode_manager 50 Hz) and TSan stays clean.
Times perf_count and perf_begin/perf_end in tight loops with hrt, plus a
plain non-atomic counter loop as reference, to quantify the overhead of
the atomic perf_counter change. Run with: microbench microbench_perf
MavlinkMissionManager keeps several pieces of mission state as static
class members so all instances share one view of the active mission:
_mission_dataman_id, _fence_dataman_id, _safepoint_dataman_id,
_current_seq, _count[], _crc32[]. Multiple instances' receiver
threads (and their send() ticks) read and write these concurrently,
which TSan flags.
Add a static pthread_mutex_t _shared_state_mutex (recursive, so the
nested call inside update_geofence_count -> update_active_mission is
fine) initialized once via pthread_once, and take it around every
read-modify-write or read-then-publish path:
- update_active_mission
- update_geofence_count
- update_safepoint_count
- send() (covers _current_seq / _count / _crc32 access)
- check_active_mission (the publisher of the dataman bump)
Mutex declaration is in the public section so the pthread_once
initializer can reach it; it remains a class-level static.
Three closely-related races flagged by TSan with multi-instance
mavlink under SIH:
- _protocol_version is touched by both threads (task_main writes it
in setProtocolVersion via mavlink_update_parameters; receiver
reads it via getProtocolVersion and itself writes it on auto-
upgrade to v2). Make it px4::atomic<uint8_t>.
- setProtocolVersion's get_status()->flags RMW touches the per-
channel mavlink_channel_statuses[] global, which the receiver
thread also reads/writes from mavlink_frame_char_buffer. Take
lock_send() around the body, matching the existing pattern around
mavlink_parse_char in the receiver.
- get_use_hil_gps() read _param_mav_usehilgps directly from the
receiver thread while task_main was writing it via updateParams.
Add an atomic snapshot _use_hil_gps_atomic, refresh it in
mavlink_update_parameters(), and return the atomic load from the
getter. Same pattern as _boot_complete and the other cross-thread
state in this class.
Drive-by: the receiver's "upgrade to v2" check at the top of
MavlinkReceiver::run was reading _mavlink.get_status()->flags
outside lock_send(). mavlink_parse_char() copies the channel-
global flags into the receiver-local _status, so read _status.flags
instead — no lock needed and no race.
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.
