This fixes the same class of bug as #27232. The external position
reset-by-fusion path explicitly intends to strongly correct correlated
states, including heading, as documented by the existing comment next to
the artificial Kalman gain increase.
However, when VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE is handled in
EKF2::Run(), it runs before _ekf.update(), so heading_observable can
still reflect the previous controlFusionModes() result. If it is false,
clearInhibitedStateKalmanGains() inhibits the intended heading
correction.
Set heading_observable before the external position fusion so the reset
can preserve the documented correction of correlated states. The normal
EKF update path recomputes the flag afterwards.
* fix(ekf2,commander): split "no heading source" from heading-innovation-failure
The pre_flt_fail_innov_heading flag conflated two distinct conditions: bad
heading innovations and the absence of any heading source. Optical-flow-only
quad setups (no magnetometer, no GNSS yaw, no external-vision yaw) never set
yaw_align, so the flag latched true and produced a misleading
"Preflight Fail: heading estimate invalid" message in modes that don't
actually depend on heading.
EKF2: pre_flt_fail_innov_heading now reports innovation failures only.
Commander: a new branch consumes estimator_status_flags.cs_yaw_align (when
fresh) to block heading-required modes (POSCTL, AUTO_*, ORBIT, AUTO_LAND,
FW AUTO_TAKEOFF) when no source is present, with a new "no heading
reference" message. The branch only fires for fresh estimator_status_flags
data so external-INS drivers (MicroStrain, VectorNav, ILabs, sbgecom) that
don't publish the flags topic retain prior behaviour. PR #26778's
mode-scoping and PR #26596's silent-pass protection are both preserved.
Resolves#27107.
* style: trim source comments per review
* Apply suggestions from code review
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
* refactor(commander): move no-heading-source check into checkEstimatorStatusFlags
* Update src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
---------
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
There's a check to disallow arming in modes that are not suite for takeoff. The allowed autonmous ones are:
Mission - desired
Loiter - does not make sense but needs to be kept for **** MAVLink MAVSDK workflow of first arming then switching mode
Offboard - acceptable advanced case even for RC
Takeoff - desired
VTOL Takeoff - desired
* fix(flight_mode_manager): fix terrain following position setpoint
In MPC_ALT_MODE=1 terrain following, the smoothing block was
overwriting the parent class's terrain-adjusted position setpoint.
Unify terrain hold and terrain following into a single terrain_position
flag that syncs the smoothing block to the parent's position, preventing
divergence and simplifying transition handling.
* refactor(flight_mode_manager): lift terrain-driven Z flag into base class
FlightTaskManualAltitudeSmoothVel re-derived the parent's terrain gate
in _setOutputState() to decide whether to keep the parent's position
setpoint or overwrite it with the smoothing block. Two problems with
that:
1. The condition was duplicated verbatim from the parent's gate in
_updateAltitudeLock(), so any future change there would silently
break the child.
2. The parent also invokes _terrainFollowing() from the min-altitude
safety branch (MPC_ALT_MODE != 1 && !_terrain_hold but below
hagl_min while braking). The child's open-coded condition did not
cover that case, so the smoothing block still overwrote a valid
terrain-adjusted setpoint there.
Move ownership of the decision into the parent: _terrainFollowing()
sets _z_setpoint_from_terrain iff it produced a finite setpoint, and
_updateAltitudeLock() resets the flag at the top of each iteration.
The child now reads the flag instead of re-deriving the condition,
which also fixes the min-altitude safety branch above.
_z_setpoint_from_terrain is named to stay semantically distinct from
_terrain_hold (an MPC_ALT_MODE=2 sub-state that gates the terrain
following logic).
NaN signals "unset" per the MAVLink spec. Without a guard,
math::radians(NaN) propagates through the Eulerf-to-Quatf conversion
and produces q=[nan,nan,nan,nan], freezing the gimbal. Substitute 0
for any non-finite axis with PX4_ISFINITE, matching the manual-control
pattern at input_mavlink.cpp:951.
Closes#26640.
* feat(zenoh): add support for configuring zenoh publisher options
Add zenoh parameters for common default options for all publishers. Options exposed are reliability, priority, congestion control and is_express
Allow override of common publisher options for specific publisher through its config: config fille supports additional row where multiple options can be specified with csv string
Expose config options through zenoh config add publisher.
Allow options per publisher to be specified for default config in zenoh/dds_topics.yaml
* fix(zenoh): Put individual zenoh publisher config override feature under config option ZENOH_PUB_OPTION_OVERRIDE
Enabled ZENOH_PUB_OPTION_OVERRIDE on targets with enough flash memory
* fix(zenoh): added publication options for default publications in dds_topics.yaml
Rare messages that are important to arive: set as reliable, enabled express and with blocking congesiton control
Fast vehcile position messages that can impact control decision making: set as best enabled express and with drop congestion control
* docs(zenoh) : added documentation for zenoh publisher options usage
* docs(docs): Link to params
---------
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Michael Fritsche
PX4BuildBot
Wang Jiayue
Hamish Willee
G. Amponis
Marco Hauswirth
Nick
Apoorv Thapliyal
Samuel Toledano
Francesco P. Carmone
Claudio Chies
av-remco
Matthias Grob
DrinkingHook
Himanshu
bresch
Stolniceanu Iustin
Jacob Dahl
msl-dev
Vuk-SFL