to be determined if this breaks anything substantial as hinted at in the
comment in BlockRandGauss.hpp (comment from 2013, only moved once since):
// XXX currently in nuttx if you seed to 0, rand breaks
Seven simulation modules (sensor_{airspeed,mag,agp,baro,gps}_sim,
gz_bridge, simulator_sih) each carried a byte-identical copy of
generate_wgn() -- a Marsaglia polar method white Gaussian noise sampler
originally copy-pasted from BlockRandGauss.hpp. Four of them also
duplicated a noiseGauss3f() helper.
Consolidate both into a new header-only library src/lib/noise/ exposing
math::generate_wgn() and math::noiseGauss3f().
Also add math::generate_wgn_boxmuller() (basic Box-Muller) for callers
who need stateless / thread-safe / constant-time sampling; doc-comment
explains when to prefer each. This was previously only in a comment.
BlockRandGauss::update() now uses math::generate_wgn(), so the entire
tree has exactly one copy of the algorithm.
srand(1234) seeds in SensorBaroSim and Sih are preserved at the caller
-- seeding the process-global rand() is the caller's responsibility.
Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
Once a serial telem channel hit 10 consecutive timeouts it was added
to a skip mask and never polled again. Channels that came online late
(e.g. ESC power-cycled after the autopilot boots) stayed permanently
offline, and with both serial and bdshot enabled the motor was
reported offline overall because the combined health check requires
both sources.
Periodically clear the skip mask while disarmed so recovered channels
get another chance. If still broken they are re-skipped after the
normal timeout threshold. Disarmed-only to avoid timeout blips on
healthy channels during flight.
* forwarding offboard setpoints in modes using the external mode registration
* requesting offboard setpoint in RAPTOR
* adding old message versions
* bumping vehicle status
* adding message translations
* updating reference to versioned old message on previous translations
Detects when a rotary-wing vehicle drops more than FD_ALT_LOSS metres
below a NED-z reference while altitude control is active,
and immediately triggers flight termination (parachute deployment).
Detection (FailureDetector):
- FD_ALT_LOSS: drop threshold in metres (0 = disabled, default)
- FD_ALT_LOSS_T: hysteresis time
- Guards: rotary-wing only, altitude control active, z_valid, setpoint
fresh (<1 s). Manual, Acro and FW/VTOL-FW modes are excluded.
- Ratcheting reference: initialises to lpos.z on first sample below
setpoint, preventing false triggers on new waypoints
Failsafe action (commander):
- New fd_alt_loss flag in FailsafeFlags.msg
- COM_ALT_LOSS_ACT: -1=Disabled (default), 0=Terminate
- Terminate fires immediately, cannot be overridden, and never clears
until disarm (parachute deployment is irreversible)
* fix(mavlink): align signing with MAVLink spec and fix performance regression
Remove the non-standard MAV_SIGN_CFG parameter and align the signing
implementation with the MAVLink specification.
Key changes:
- Remove MAV_SIGN_CFG parameter that conflicted with GCS implementations
- Only enable signing when a valid key is present on the SD card
- Accept SETUP_SIGNING on any link, not just USB
- Reject SETUP_SIGNING while the vehicle is armed
- Allow disabling signing via signed all-zero key SETUP_SIGNING message
- Propagate key changes to all mavlink instances
- Zero CPU/bandwidth overhead when signing is not active
Fixes#26893
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
* Signing minor subedit
---------
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
* refactor(battery_simulator): remove SIM_BAT_ENABLE
disable instead with SIM_BAT_DRAIN <= 0
* fix(battery_simulator): disable battery sim only if SIM_BAT_DRAIN strictly < 0
* fix(battery_simulator): disable if 0, adjust limit to 0
* fix(battery_simulator): remove constraining again
now that SIM_BAT_DRAIN=0 means the module is not started we are safe
from division by zero again (param compare has a tolerance of 1e-7)
* fix(battery_simulator): constrain param to min of 1
to avoid division by zero.
This reverts commit 6380c4fdee.
* fix(battery_simulator): remove arbitrary param max
* fix(battery_simulator): reword long param description
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
* fix(battery_simulator): reword short param description
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
---------
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
* offboard: report specific failures
Figuring out offboard failures is quite difficult because the user currently
gets a single, very generic error message that does not identify the actual
missing requirement.
This change aims to improve the user experience by:
- moving offboard failure reporting into OffboardChecks, where the exact cause is known
- reporting specific arming failures for missing local position, local velocity and attitude estimates
- keeping the generic offboard signal error only as a fallback for true signal-loss cases
- removing the duplicate offboard check from ModeChecks (as already invoked by HealthAndArmingChecks)
Signed-off-by: Onur Özkan <work@onurozkan.dev>
* offboard: handle attitude mode in offboard check
Signed-off-by: Onur Özkan <work@onurozkan.dev>
---------
Signed-off-by: Onur Özkan <work@onurozkan.dev>
Extract the repeated `offboard_control_mode_s` population logic into a shared
`fill_offboard_control_mode()` helper in MavlinkReceiver and, similar to
`fill_thrust()`, reuse it in both local and global position target handlers.
Reduces the code duplication without changing any behavior.
Signed-off-by: Onur Özkan <work@onurozkan.dev>
* Remove unused parameters from function signature and make the parameter accessors consistent
* Update the caller function signature
* Update src/modules/navigator/rtl.cpp
---------
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
Use timestamp_sample instead of time_now_us for the rate limiter check
to sync to the sensor clock rather than the wall clock.
Switch from direct timestamp assignment to epoch-advance
(_last_publication_timestamp += interval_us) with a catch-up guard to
prevent aliasing artifacts when the sensor sample rate is close to the
configured publication rate.
getPreviousPositionItems() already decrements the start index
internally before searching. The call in on_activation() at line 227
passed _inactivation_index - 1, causing a double-decrement that made
the vehicle resume at waypoint n-2 instead of n-1.
All other call sites (rtl_mission_fast_reverse.cpp:81,
rtl_mission_fast_reverse.cpp:133, mission_base.cpp:1149) pass the
index directly without pre-decrementing.
The bug has been present since commit 007ed11bbe (June 2023).
Closes#26795
Signed-off-by: Pavel Guzenfeld <pavelgu@gmail.com>
The FC-side DroneCAN sensor bridges (accel, gyro, rangefinder) used
hrt_absolute_time() in the receive callback as timestamp_sample,
adding ~3-16ms of systematic CAN transport delay.
For messages with a uavcan.Timestamp field, the cannode can publish
the actual sample time via UAVCAN GlobalTimeSync. The RawIMU publisher
already did this for IMU data; apply the same pattern to the range
sensor publisher, and update all three FC bridges to prefer the
message timestamp with a fallback to hrt_absolute_time() for nodes
that don't set it.
* fix: added comment explaining why dev id address can only be 3 or 4
* fix: change link to point to main px4 repo
* fix: typo
* formatted
* chore: formatting
When EKF2_HGT_REF=2 (range sensor) with no GPS, optical flow could
never start. The starting condition required isTerrainEstimateValid()
or isHorizontalAidingActive(), but terrain is never "estimated" when
range is the height reference (ground is the datum, terrain state is
fixed at 0), and there's no horizontal aiding without GPS.
HAGL is directly known from the range measurement in this case, so
optical flow has everything it needs to fuse. Add the range height
reference check to the optical flow starting conditions.
Fixes: https://github.com/PX4/PX4-Autopilot/issues/25248
Add cmake/cpack infrastructure for building .deb packages from
px4_sitl_sih and px4_sitl_default targets. Includes install rules,
package scripts, Gazebo wrapper, and CI workflow.
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
Add MAVLink stream that maps EstimatorFusionControl uORB message to
ESTIMATOR_SENSOR_FUSION_STATUS, exposing per-sensor intended/active
bitmasks to the GCS.
Split FusionSensor into available (CTRL param != disabled) and enabled
(runtime-toggleable). intended() = enabled && available. EKF core aid
sources now set available themselves and use intended() or _params
directly for CTRL-level checks. Remove drag/imu from FusionControl,
add aspd/rngbcn. Add AGP sourceFusingBitmask() for active-status.
The MPL3115A2 ADC conversion at OSR 2 (ratio 4) takes ~18ms. The
driver polls until the conversion completes, so the read time is at
the end of the integration window. Correct timestamp_sample to the
midpoint by subtracting CONVERSION_TIME / 2.