mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
Andy Piper
ffbecd9403
update() cleared _gyro_healthy/_accel_healthy for every instance and relied on the backends to set them true again a few microseconds later. Those flags are read from other threads - AP_RCTelemetry::check_sensor_status_flags() runs from the CRSF frame handler on the RC input thread - so every main loop iteration left a window in which a perfectly healthy sensor reads unhealthy. At an ELRS telemetry rate of ~200Hz that window is sampled often enough to report "Bad Gyro Health" continuously on a vehicle whose gyro never missed a sample. It is invisible in a log: IMU.GH stays 1 even logged at loop rate, because logging also runs from the main loop and so cannot observe the window. Drop the central clear and have update_gyro()/update_accel() set the flag false only when there is no fresh sample, so a healthy sensor never sees it transiently cleared. The error-count preference later in update() can still demote a sensor in the same cycle; that path is unchanged. A backend whose start() failed to register stays in _backends[] and still has update() and update_filters() called every cycle, with gyro_instance/accel_instance left at their default. Default those to INS_MAX_INSTANCES and bounds-check the frontend helpers that index by them, so the new clear is not applied to instance 0 and nothing indexes one past the end. notify_*_fifo_reset() gets the same check because LSM9DS1 calls it from probe(), before registration. ADIS16607 and NONE redeclared gyro_instance/accel_instance, shadowing the base members; the duplicates are removed so the default applies there too. ADIS16607 also tested enable_fast_sampling(accel_instance) from probe(), before the instance is known. That was only right for instance 0 while the driver had its own zero-initialised copy, and with the new default it disabled fast sampling altogether. Pre-fetch the instance numbers in start() as the Invensense drivers do and move the rate decision and the DEC_RATE write there, after registration, so a failed write leaves an instance that reads unhealthy rather than pre-fetched numbers with nothing registered behind them.