test_multiple_semaphores_waiting() spawns a worker thread per TestCase, then
advances virtual time in a loop and completes each case in check(). Since
check() early-returns while !_thread_ready, a slow-to-start worker thread can
miss the whole loop: the loop advances past the case's timeout before the thread
registers, so the case is never completed. ~TestCase() then fails
EXPECT_TRUE(_is_done) and destroys a still-joinable std::thread, which calls
std::terminate() and aborts the test. This was flaky (~50% locally, worse under
CI load).
Wait for every worker thread to be ready before advancing virtual time. Also
re-comment the per-iteration std::cout that was left enabled.
Signed-off-by: Julian Oes <julian@oes.ch>
When a multicopter enters Hold (AUTO.LOITER) below NAV_MIN_LTR_ALT the
documented behavior is to first climb to that minimum altitude. The
clamp was added in 2022 to setLoiterItemFromCurrentPosition (the
fixed-wing branch) but never applied to the sibling
setLoiterItemFromCurrentPositionWithBraking, which is the rotary-wing
Hold path selected at loiter.cpp:131. As a result, multicopters
silently held at the entry altitude.
Apply the same min-altitude clamp in the braking variant so the
rotary-wing path matches the fixed-wing one.
Verified in SITL (jMAVSim x500): with NAV_MIN_LTR_ALT=20 and Hold
engaged from a 5m hover, the vehicle now climbs to 20m as documented.
Fixes#27578
Signed-off-by: saad1551 <saad.ashraf.9094@gmail.com>
Switches from using VehicleControlMode to a specific setpoint type message
with reply.
Reasons:
- in case a VehicleControlMode was dropped (e.g. when the mode switched
setpoint type), no confirmation was returned, and resulting in wrong
controller flags.
- cleaner interface separation: external modes do not need to configure
(or know) which controller to run for a certain setpoint
- allows for external modes to check for compatibility: e.g. a mode using
fixed-wing setpoint types can now be rejected on a multicopter.
- allows for further extensions, like a setpoint timeout
This makes it a bit more effort to add a new setpoint type. Specifically,
setpoint_types.cpp needs to be extended when adding a new setpoint message.
PX4 internal modes also make use of the setpoint types. The information
flow is:
nav_state -> setpoint type -> vehicle control mode flags
It also adds a timeout to the setpoint config, but is not implemented
yet.
This changes the interface for external modes and thus the compatibility
version is increased.
* 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.
