these undocumented bits in register 0x4d control the "adaptive full
scale range" mode of the ICM42688. The feature is enabled by default
but has a bug where it gives "stuck" gyro values for short periods
(between 1ms and 2ms):, leading to a significant gyro bias at longer
time scales, enough to in some cases cause a vehicle to crash if it is
unable to switch to an alternative IMU
this fixes https://github.com/ArduPilot/ardupilot/issues/25025
an incorrectly configured ESC telemetry source can lead to a notch
running at very low frequencies. A simple example is a lua script like
this:
function update()
esc_telem:update_rpm(12, 0, 0)
return update, 10
end
return update()
where motor 12 is unused.
with that script in place we get a 1.0078 Hz filter which leads to
massive phase lag and a crashed aircraft
this is a safety protection. We should also try to find out why the
INS_HNTCH_FREQ lower limit is not working
users with busy CAN bus often get significiantly lower GPS rates on a
moving baseline rover, preventing arming. This PR relaxes the required
frame rate as the EKF is quite happy with 3Hz yaw and the yaw is the
only data consumed from a moving baseline rover
this fixes a bug where if the terrain database cache does not have the
tile for the location of a rally point then RTL to the rally point
with TERRAIN_FOLLOW=1 will not track terrain
The underlying issue is that Location::loc.change_alt_frame() will
return false if the location is not in the terrain memory cache. We
can't just extrapolate as the rally point could be in a totally
different terrain area to the current location. So instead we set it
as terrain_following_pending and fix it as soon as the terrain cache
is filled.
fixes https://github.com/ArduPilot/ardupilot/issues/25157
the bad value is -32768 not 0xffff (which is -1)
-32768 badly corrupts the low-pass filter, and is what we see in logs
(a large negative spike on all 3 axes)
update to bug fix from:
https://github.com/ArduPilot/ardupilot/pull/23033
update_pitch_throttle can be called when update_50hz hasn't run in a very long time, or ever. This requires a main loop rate >50Hz, and for the mode change to occur in the same loop that update_50Hz doesn't run but update_pitch_throttle does.
this fixes a state where we either cannot climb or descend in an
uncontrolled manner in a TECS controlled mode
the conditions under which this happened were:
- _use_synthetic_airspeed_once was true due to quadplane takeoff
- we left _thr_clip_status as MAX from previous use of synthetic airspeed
- then run without airspeed
note that this can also impact users with an airspeed sensor if they
disable it or it fails in flight, particularly during a climb
... or at least closer to it.
We were hard-resetting the yaw to zero when the vehicle was upright. That makes for huge simulated gyro rates, and that means the differences between the gyros can be huge sample-to-sample, so we can get gyros-inconsistent errors.
Fix things so we don't reset yaw at the same time as pitch, and also twist the vehicle to point North again when disarmed.
Backport of PR https://github.com/ArduPilot/ardupilot/pull/25046
The hard-reset of all state variables means we feed the EKF inconsistent gyro data. Attempt to upright the vehicle with a simple p-gain when it is disarmed, as if someone is pushing the thing back upright
Backport of PR https://github.com/ArduPilot/ardupilot/pull/24314