ArduPlane: altitude: rangefinder correction should be relative to the altitude source being used for navigation. This avoid applying it twice when there is an existing correction saved.

This commit is contained in:
Samuel Tabor 2021-07-06 21:34:14 +01:00 committed by Randy Mackay
parent 541435cb83
commit 4e86cc8b41
1 changed files with 1 additions and 1 deletions

View File

@ -687,7 +687,7 @@ void Plane::rangefinder_height_update(void)
if (rangefinder_state.in_range) {
// base correction is the difference between baro altitude and
// rangefinder estimate
float correction = relative_altitude - rangefinder_state.height_estimate;
float correction = adjusted_relative_altitude_cm()*0.01 - rangefinder_state.height_estimate;
#if AP_TERRAIN_AVAILABLE
// if we are terrain following then correction is based on terrain data