mirror of https://github.com/ArduPilot/ardupilot
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:
parent
8f5264b3e7
commit
cadd0b0232
|
@ -687,7 +687,7 @@ void Plane::rangefinder_height_update(void)
|
||||||
if (rangefinder_state.in_range) {
|
if (rangefinder_state.in_range) {
|
||||||
// base correction is the difference between baro altitude and
|
// base correction is the difference between baro altitude and
|
||||||
// rangefinder estimate
|
// 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 AP_TERRAIN_AVAILABLE
|
||||||
// if we are terrain following then correction is based on terrain data
|
// if we are terrain following then correction is based on terrain data
|
||||||
|
|
Loading…
Reference in New Issue