DEBUG: AP_AHRS: validate that we can generate eulers from backend qua…#42
Open
peterbarker wants to merge 125 commits into
Open
DEBUG: AP_AHRS: validate that we can generate eulers from backend qua…#42peterbarker wants to merge 125 commits into
peterbarker wants to merge 125 commits into
Conversation
peterbarker
force-pushed
the
pr-test/eulers-from-quat-test
branch
from
dbc3888 to
58622b1
Compare
this allows for operation (only just!) on a 1MBit link
this allows much higher logging rates for the mavlink logging backend
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.
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 #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.
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.
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.
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.
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).
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.
Pair with the new RISJ DAL message that carries per-instance gyro bias metadata, so replay feeds it back into the DAL.
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.
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.
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 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.
Add AP_AHRS_EKF_RESET_ENABLED to the custom build server so the EKF bootstrap reset aux function can be compiled out on flash-constrained boards. Enabled by default, depends on EKF3.
Save validOrigin and EKF_origin before InitialiseVariables() in InitialiseFilterBootstrap() and restore them after, so an externally commanded reset does not shift the local NED frame. Without this the per-core EKF_origin gets re-set from the next GPS fix, diverging from the shared frontend common_EKF_origin. On first boot validOrigin is false and the restore is a no-op.
Capture vehicle position before triggering the bootstrap reset and assert horizontal drift stays under 5m after a 10s GUIDED hover. If the EKF origin shifts during the reset, the position controller would command a correction and the vehicle would fly away from the takeoff point.
Resetting the EKF while armed would discard the in-flight state estimate. Check the armed flag first so reset_configured_backend() is not called when armed, and emit a distinct STATUSTEXT for the refused-while-armed case.
The aux switch now only resets the EKF when disarmed. Exercise both halves of that rule: the disarmed reset succeeds and the origin survives across a subsequent GUIDED takeoff (position hold proves the EKF origin did not move), and an armed reset is refused with a STATUSTEXT. Uses self.takeoff(), wait_statustext, and wait_location(minimum_duration=) in place of the open-coded change_mode/arm/user_takeoff + delay_sim_time + recv_match sequence.
EKF3 is a static member of the AP_AHRS_NavEKF3 backend wrapper, not a direct member of AP_AHRS. Route the bootstrap reset call through ekf3.EKF3 and gate on AP_AHRS_NAVEKF3_ENABLED to match the surrounding code.
Drives a motorboat-skid frame between two GUIDED waypoints 50m apart, pausing 10s at each, while tracking accumulated yaw change to detect spinning behaviour. Reproduces the bug fixed in "SITL: fixed skid steering boat" where the boat would spin between waypoints because the is_zero(speed) skid-steering check failed when speed had drifted slightly off zero. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
peterbarker
force-pushed
the
pr-test/eulers-from-quat-test
branch
from
aa1aff6 to
996b118
Compare
Adds @increment: 0.0001 to the SHUNT parameter documentation for both the INA2xx and INA239 backends. Without an Increment (or DecimalPlaces) hint, ground stations default to 3 decimal places when displaying the value, which rounds typical shunt values like 0.0005 Ohm to "0.001" on screen even though the stored float is correct. Surfacing the increment lets GCS code derive the right precision automatically.
…ate_alt linearly_interpolate_alt() interpolates altitude between two Location endpoints and only produces meaningful results when both endpoints share an alt frame; mixing frames produces nonsense altitudes. Add a SITL-only panic at the top of the function so the contract is checked for every caller (BendyRulerHorizontal and Dijkstras path handling in AC_WPNav_OA, plus any future callers), as suggested by IamPete1 in code review. Fixes ArduPilot#32926
never called
On Cygwin, xterm requires X11 server and bitmap fonts to be installed. mintty is the native Cygwin terminal emulator and works without X11. Add mintty as a fallback terminal option before the log-to-file fallback, allowing SITL terminal windows to open natively on Cygwin without requiring an X server.
Adds SITL::MAVLinkCamV2, a protocol mixin for MAVLink Camera Protocol v2 peripherals, modelled on the MAVLinkGimbalv2/Gimbal pattern. New classes: - SITL::Camera — simple camera state holder (shot count) - SITL::MAVLinkCamV2 — sends MAV_TYPE_CAMERA heartbeats, responds to MAV_CMD_REQUEST_MESSAGE for CAMERA_INFORMATION (with CAMERA_CAP_FLAGS_CAPTURE_IMAGE), and handles MAV_CMD_IMAGE_START_CAPTURE AVT_CM62 gains multiple inheritance from MAVLinkCamV2 and overrides handle_message/update/set_instance to dispatch to both base classes. The camera component ID is MAV_COMP_ID_CAMERA + instance, allowing AP_Camera_MAVLinkCamV2 (CAM_TYPE=6) to discover and drive it. MAVLinkGimbalv2::handle_message is made protected+virtual so AVT_CM62 can override it; send_mavlink_message and transport accessors move to protected so the camera mixin can reach them via the combined class. Autotest: MountAVTCM62Dual gains a 12 s delay before triggering to allow AP_Camera_MAVLinkCamV2::find_camera() to complete its 10 s boot wait and exchange CAMERA_INFORMATION. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Verify that no "Takeoff blocked" messages appear during normal spoolup with in-range RPMs in both Copter and QuadPlane TakeoffCheck tests.
the values in results are rotated, here they are subtracted. Fix them for comparison purposes
peterbarker
force-pushed
the
pr-test/eulers-from-quat-test
branch
from
996b118 to
fa7ee4d
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
20 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
…ternions
Summary
Classification & Testing (check all that apply and add your own)
Description