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.
PX4 BuildBot
Marco Hauswirth
Michael Fritsche
Wang Jiayue
Hamish Willee
G. Amponis
Nick
Apoorv Thapliyal
Samuel Toledano
Francesco P. Carmone
Claudio Chies
av-remco
Matthias Grob
DrinkingHook
Himanshu
bresch
Stolniceanu Iustin
Jacob Dahl
msl-dev