Add RCx_OPTION=187 (EKF_RESET) aux function that triggers a full EKF
bootstrap reset on HIGH, gated to fire only on state transition.
Sends GCS status message indicating whether the reset succeeded or
failed. Available for all vehicle types.
Gated by AP_AHRS_EKF_RESET_ENABLED compile-time option.
Expose EKF3 bootstrap reset through the AHRS interface. Returns bool to
indicate success. Calls InitialiseFilterBootstrap() unconditionally when
EKF3 is available, regardless of active EKF type — if we've fallen back
to DCM due to EKF failure, that's exactly when a bootstrap reset is most
needed to force re-convergence.
Gated by AP_AHRS_EKF_RESET_ENABLED
Add InitialiseFilterBootstrap() public method that clears statesInitialised
and re-runs bootstrap alignment on all cores.
Uses statesInitialised to check success rather than the per-core return
value which is false when the IMU delay buffer is not yet full.
The v3 driver covers eight chips with a wide spread in ZRO
temperature coefficient. ArduPilot's startup gyro cal removes the
static ZRO, so the EKF clamp must bound the in-flight drift driven
mostly by temperature swing times the temp coefficient.
ICM-42688-P, ICM-45686: 0.005 deg/s/C -> ~0.3 deg/s over 60C
ICM-40609-D: 0.01 deg/s/C
ICM-42670-P: 0.015 deg/s/C
ICM-42605, IIM-42652: 0.02 deg/s/C -> ~1.2 deg/s over 60C
IIM-42653: 0.04 deg/s/C -> ~2.4 deg/s over 60C
Applying radians(2.0) across the whole family was too tight for
IIM-42653 and marginal for the mid-tier parts. Tier the override:
the two very-low-drift parts keep radians(2.0)/1.0 deg/s, the
mid-tier parts get radians(3.0)/1.5 deg/s, and ICM-40605 (no
datasheet checked) plus IIM-42653 fall back to the base default.
Datasheets consulted: DS-000347, DS-000489, DS-000292, DS-000330,
DS-000451, DS-000440, DS-000529.
These fields are constant per-instance, so logging them in RISI
inflated the replay log on every frame and broke replay of logs
captured before this PR landed (RISI gained two floats).
Restore RISI to its original layout and add RISJ, which carries
gyro_bias_limit and gyro_bias_init_dps plus the instance and is
written only when changed via WRITE_REPLAY_BLOCK_IFCHANGED.
Seed _RISJ[] with the legacy defaults (0.5 rad/s clamp,
2.5 deg/s init) in the DAL constructor so replays of older logs
that have no RISJ messages still feed sane values to the EKF.
Drop the per-instance _gyro_bias_limit_rads / _gyro_bias_init_dps
arrays and the set_gyro_bias_metadata() helper. The values are
constants set by each backend, so the frontend now walks _backends[]
and calls the virtual accessor on the owning backend.
Move gyro_bias_limit_rads(), gyro_bias_init_dps(),
get_gyro_instance() and get_accel_instance() to the public section
of AP_InertialSensor_Backend so the frontend lookup can reach them.
Expand the Invensensev3 comment to name the eight v3 variants the
driver supports and explain why the 2 deg/s clamp and 1 deg/s init
uncertainty are conservative across all of them (they bound the
residual drift after ArduPilot's startup gyro calibration, not the
raw initial bias).
getGyroBiasLimit() and InitialGyroBiasUncertainty() now read the
per-IMU values populated by AP_InertialSensor backends through the
DAL, instead of a hard-coded two-step switch on a low-drift boolean.
Drops the GYRO_BIAS_LIMIT and GYRO_BIAS_LIMIT_LOW_DRIFT defines from
AP_NavEKF3_core.h; the values now live in the backends.
Carry the per-IMU gyro bias state clamp (rad/s) and initial 1-sigma
bias uncertainty (deg/s) through the DAL so the EKF sees the same
per-IMU values during replay as it did in flight. Replaces the
single boolean low-drift bit previously stored in RISI with the two
float values themselves, exposed as get_gyro_bias_limit() and
get_gyro_bias_init_dps() on AP_DAL_InertialSensor.
Some IMU families (e.g. Invensensev3) drift much more slowly than the
legacy default and can support both a tighter EKF gyro bias state clamp
and a smaller initial bias uncertainty. Expose this via two virtuals
on the backend: gyro_bias_limit_rads() (rad/s, default 0.5) and
gyro_bias_init_dps() (deg/s, default 2.5), with Invensensev3 overriding
to radians(2.0) and 1.0 respectively.
Backends publish their values into per-instance frontend arrays via
set_gyro_bias_metadata() at registration time; the EKF reads them
through the DAL (see following commits). The two values are kept
separate because they encode different beliefs: the limit is a
generous hardware-bounded clamp on the bias state, while the init
uncertainty is the realistic 1-sigma prior on the bias at filter
start, and the two are not derivable from each other for legacy IMUs.
Replace compile-time #if guards with runtime queries to
dal.ins().is_low_noise() for gyro bias limit and initial uncertainty.
Each EKF core now uses its own IMU's low noise flag, enabling
per-sensor behaviour and correct EKF replay.
Add low_noise per-instance flag to log_RISI struct and expose
is_low_drift() on the DAL INS interface so the EKF can query it
through the DAL rather than directly from the sensor.
Replace compile-time AP_INERTIALSENSOR_LOW_NOISE define with a runtime
per-instance bitmask set by IMU drivers. Add is_low_noise() accessor
on the frontend and set_low_noise() on the backend. Invensensev3
driver marks its instances as low noise during init.
this makes the estimator backends return the quaternion result in body frame, rather than having the AP_AHRS_Backend base class rotate it for only some of the backends.
Putting the result into vehicle body frame makes sense as the external hardware device may need to apply its own trim to its result if it is mounted at an angle.
Move the common mavlink_msg_camera_information_send call into
AP_Mount_Backend::send_camera_information(), following the same pattern as
send_gimbal_device_attitude_status(). Backends supply their specific data
via virtual getter overrides: get_camera_vendor_name(), get_camera_model_name(),
get_camera_firmware_version(), get_camera_focal_length_mm(), get_camera_lens_id(),
and get_camera_cap_flags(). Backends suppress sending by leaving
has_camera_information() returning false (the base class default); backends
with a known camera override it to return true. Siyi's previous
!_fw_version.received guard is dropped — the message now sends with
fw_version=0 until the version is received.
Also fixes the misaligned parameter comments in Viewpro's send call (focal_length
was labelled as sensor_size_h), since there is now only one copy of those comments.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
The scheduler has a nice diagnostic print for each task it creates. But that is
only needed on the DSP diagnostic display, not the console, so changing output
to only DSP diagnostic display.
These two model parameter files were the only ones in
Tools/autotest/models using the .param extension; every other model
parameter file in the tree uses .parm. Aligning their extension makes
them match the existing *.parm ROMFS embed glob without having to
extend it, and keeps the convention consistent across the directory.
Update the corresponding default_params_filename entries in
vehicleinfo.json to match the new filenames.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
wait_ready_to_arm(require_absolute=False) returns as soon as the EKF
reports relative aiding, before the origin (and thus home) is set.
The GUIDED takeoff that follows needs an ABOVE_HOME -> ABOVE_ORIGIN
alt frame conversion, which silently fails when home is unset, so
MAV_CMD_NAV_TAKEOFF returns MAV_RESULT_FAILED.
going via the static methods threw away the const-correctness so we need a bunch of non-const equivalents of several const methods
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
going via the static methods threw away the const-correctness so we need a bunch of non-const equivalents of several const methods
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
going via the static methods threw away the const-correctness so we need a bunch of non-const equivalents of several const methods
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
going via the static methods threw away the const-correctness so we need a bunch of non-const equivalents of several const methods
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
going via the static methods threw away the const-correctness so we need a bunch of non-const equivalents of several const methods
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
Andy Piper
Paul Riseborough
Andrew Tridgell
Tatsuya Yamaguchi
Peter Barker
rishabsingh3003
Thomas Watson
Leonard Hall
Hunter McClelland
Iampete1
Eric Katzfey