Rangefinder data should not be used in ALTHOLD and other flight modes

[ABLomas]

Wiki says:

Current support of rangefinders in INAV is very limited. They are used only to:
landing detection for multirotors
automated landing support for fixed wings
Experimental terrain following (Surface) flight mode activated with SURFACE and ALTHOLD flight mode

(even inav_w_z_surface_p description says "Setting is used on both airplanes and multirotors when rangefinder is present and Surface mode enabled")

Based on documentation it looks like rangefinder data will be used only on LANDING or SURFACE modes (on flying wing).
But it's not true. I set up plane with TOF10120 rangefinder and did not set up SURFACE or landing modes, used only ALTHOLD. I output rangefinder data on OSD and checked - it was working correctly at ~0...0.5m range (a bit low, yes, but this is in bright daylight). Then run WP mission flight with 120m preset altitude and plane started to fluctuate upwards every few seconds (using motor and pitch up).
I tried to set inav_default_alt_sensor to BARO_ONLY, GPS_ONLY but results were even worse. OSD shows huge altitude fluctuations, sometimes even going negative, while plane flies straight, but at the same point rangefinder triggers showing 0.0m (instead of "--"). Reviewing blackbox data confirms, that GPS and baro data stays at +/- constant altitude, while navPos[2] jumps a lot (and plane tries to correct altitude).

I disabled rangefinder and repeated the same (130km) mission, altitude was spot-on, zero issues.

It's really not possible to prevent false-positives trigger on bright daylight using laser-based altimeters, i tried VL53L0X, TFLuna too, all can show false reading sometimes. I know about rangefinder_median_filter but this does not fix issue.

There are two ways how to fix this:

1. Enable rangefinder data ONLY in related flight modes (SURFACE, LANDING...)
2. Have hard limit on rangefinder data readings. Most rangefinders are useless above 5m anyway and those lucky NRA users can set upper limit higher.

For now there's no way to set-up long range mission using INAV8 and rangefinder with autolanding, as lots of energy (~30% increase) will be consumed by "altitude corrections based on false positives".

[sensei-hacker]

Was the aircraft on on landing approach at the time?

Also can you please attach the black box log and let us know approximately the time stamp where you saw this problem?

You may be running into a known issue.

Also your diff please.

[sensei-hacker]

Have hard limit on rangefinder data readings. Most rangefinders are useless above 5m anyway and those lucky NRA users can set upper limit higher.

There is both a hardcoded maximum limit depending on the type of rangefinder, and a user setting.
The hardcoded max could kick in when the user gets too optimistic even for indoor flat over a white floor.

[sensei-hacker]

You might also be interested in testing #10903

[ABLomas]

Blackbox log of flight with rangefinder attached (this one was not fully decoded by bbtools, i mentioned that in discord, only beginning is decoded for some reason, so reviewing only in blackbox-explorer):
LOG00009.zip
Here's screenshot describing issue:

Each "spike" in throttle graph on bottom is sudden dip on navPos[2] while baro data stays approx the same (GPS alt too).
Diff in next comment.

[ABLomas]

As for "advanced altitude control" - i'm happy that some things is improving, but this is def. not the case here. I disabled rangefinder and altitude control was as expected on 130km route, i do not see alt indicator to go more than 2m from target (which is ok, while with rangefinder it was sometimes going negative).
Speed/throttle control requires improvement (and real reason why some planes switched to AP), but this is offtopic, we can discuss that on discord ;-)

Here's diff (rangefinder disabled, no other changes IIRC):

# version
# INAV/SPEEDYBEEF405WING 8.0.1 Mar 28 2025 / 09:52:26 (ae47bcba) 
# GCC-13.2.1 20231009

# start the command batch
batch start

# resources

# Timer overrides

# Outputs [servo]

# safehome

# Fixed Wing Approach
fwapproach 8 6000 500 0 0 0 0

# geozone

# geozone vertices

# features
feature GPS
feature PWM_OUTPUT_ENABLE

# beeper

# blackbox
blackbox -NAV_ACC
blackbox NAV_POS
blackbox NAV_PID
blackbox MAG
blackbox ACC
blackbox ATTI
blackbox RC_DATA
blackbox RC_COMMAND
blackbox MOTORS
blackbox -GYRO_RAW
blackbox -PEAKS_R
blackbox -PEAKS_P
blackbox -PEAKS_Y
blackbox SERVOS

# Receiver: Channel map
map TAER

# Ports
serial 4 2048 115200 115200 0 115200

# LEDs

# LED color

# LED mode_color

# Modes [aux]
aux 0 0 0 1300 2100
aux 1 1 2 1700 2100
aux 2 12 2 900 1300
aux 3 10 3 1700 2100
aux 4 28 5 1700 2100
aux 5 35 3 1300 1700
aux 6 54 4 1700 2100
aux 7 36 4 1300 1700
aux 8 26 0 1700 2100

# Adjustments [adjrange]
adjrange 0 0 6 900 2100 53 6

# Receiver rxrange

# temp_sensor

# Mission Control Waypoints [wp]
#wp 5 valid
wp 0 1 548796744 255161870 10000 0 0 0 0
wp 1 1 548694087 254679394 10000 0 0 0 0
wp 2 1 548776313 255216050 10000 0 0 0 0
wp 3 1 549129106 255664301 10000 0 0 0 0
wp 4 6 0 0 0 0 10 0 165

# OSD [osd_layout]
osd_layout 0 0 1 13 H
osd_layout 0 1 1 14 H
osd_layout 0 2 0 0 V
osd_layout 0 3 8 6 V
osd_layout 0 7 13 11 V
osd_layout 0 9 24 13 V
osd_layout 0 10 2 1 V
osd_layout 0 11 10 14 V
osd_layout 0 12 16 14 V
osd_layout 0 13 23 8 V
osd_layout 0 14 1 2 V
osd_layout 0 15 2 8 V
osd_layout 0 22 15 3 V
osd_layout 0 23 24 2 V
osd_layout 0 24 24 1 V
osd_layout 0 26 23 9 V
osd_layout 0 27 22 7 V
osd_layout 0 28 23 14 V
osd_layout 0 30 1 12 V
osd_layout 0 32 1 14 V
osd_layout 0 35 3 9 V
osd_layout 0 97 11 1 V
osd_layout 0 105 14 5 H
osd_layout 0 109 3 10 H
osd_layout 0 110 1 13 V
osd_layout 0 120 3 7 V
osd_layout 0 129 11 2 V

# Programming: logic

# Programming: global variables

# Programming: PID controllers

# OSD: custom elements

# master
set gyro_main_lpf_hz = 25
set dynamic_gyro_notch_q = 250
set dynamic_gyro_notch_min_hz = 30
set gyro_zero_x = 6
set gyro_zero_y = -5
set gyro_zero_z = -5
set ins_gravity_cmss =  979.686
set gyro_adaptive_filter_min_hz = 20
set acc_hardware = ICM42605
set acczero_x = 1
set acczero_y = -4
set acczero_z = 27
set accgain_x = 4091
set accgain_y = 4093
set accgain_z = 4100
set align_mag = CW270FLIP
set mag_hardware = NONE
set maggain_x = 1022
set baro_hardware = SPL06
set pitot_hardware = MS4525
set blackbox_rate_denom = 16
set motor_pwm_protocol = DSHOT150
set failsafe_mission_delay = -1
set align_board_roll = 20
set align_board_yaw = 2700
set servo_pwm_rate = 330
set small_angle = 180
set applied_defaults = 4
set gps_sbas_mode = EGNOS
set gps_ublox_use_galileo = ON
set airmode_type = STICK_CENTER_ONCE
set inav_max_surface_altitude = 450
set nav_wp_load_on_boot = ON
set nav_wp_radius = 1000
set nav_wp_max_safe_distance = 1500
set nav_fw_wp_tracking_accuracy = 3
set nav_fw_wp_turn_smoothing = ON
set nav_overrides_motor_stop = OFF
set nav_rth_allow_landing = FS_ONLY
set nav_rth_altitude = 5000
set nav_fw_bank_angle = 40
set nav_fw_dive_angle = 20
set nav_fw_pitch2thr_smoothing = 8
set nav_fw_control_smoothness = 4
set nav_fw_launch_max_angle = 85
set nav_fw_launch_motor_delay = 90
set nav_fw_launch_max_altitude = 5000
set nav_fw_launch_climb_angle = 25
set nav_fw_allow_manual_thr_increase = ON
set osd_video_system = PAL
set osd_alt_alarm = 5000
set osd_dist_alarm = 50000
set osd_crosshairs_style = TYPE7
set osd_crsf_lq_format = TYPE2
set tz_automatic_dst = EU
set vtx_power = 0
set dshot_beeper_enabled = OFF

# control_profile
control_profile 3

set rc_expo = 80
set manual_rc_expo = 55

# mixer_profile
mixer_profile 1

set platform_type = AIRPLANE
set model_preview_type = 8

# Mixer: motor mixer

mmix reset

mmix 0  1.000  0.000  0.000  0.000

# Mixer: servo mixer
smix reset

smix 0 1 0 -75 0 -1
smix 1 1 1 75 0 -1
smix 2 2 0 -75 0 -1
smix 3 2 1 -75 0 -1

# battery_profile
battery_profile 1

set nav_fw_cruise_thr = 1255
set nav_fw_min_thr = 1120
set nav_fw_max_thr = 1500
set nav_fw_launch_thr = 1680

# save configuration
save

# end the command batch
batch end

[breadoven]

Fluctuations in estimated position Z is an issue with the nav position estimator. Poor position Z estimations will screw up altitude control whether using velocity (derived from position error) or just position error directly. Something would appear to be wrong with Nav position estimation when a range finder is used based on the above.

[breadoven]

Had a quick look at this and it's not immediately obvious why having the rangefinder enabled would screw up pos Z. Possibly something to do with function navGetCurrentActualPositionAndVelocity in navigation.c but it's hard to see how. I did also notice that AGL_TRUSTED was randomly showing in the log during the WP mission even though the altitude was 120m which doesn't make sense although it didn't correspond with the the bogus changes in Pos Z.

[sensei-hacker]

AGL_TRUSTED was randomly showing in the log during the WP mission even though the altitude was 120m

I have only looked at it briefly so far, but it occurs to me that reflections from parts of the craft or other issues could cause it to falsely report < 5m AGL, while GPS shows 100+m. Or it could truly be at 2m AGL while at 100m msl. Ie, mountains do exist. We have a setting for the maximum distance reading that is trusted, but I wonder if there's really any way to identify false distance readings.

[breadoven]

I'm thinking it might be related to the following code except this shouldn't affect things if sensor is set to GPS_ONLY :

inav/src/main/navigation/navigation_pos_estimator.c

Line 602 in 0589461

(((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||

.

The logic for isBaroGroundValid looks like it could get stuck on true if Vel Z briefly > 15 cm/s then drops below +15 cm/s setting the Baro correction altitude to effectively takeoff altitude thereby applying a large false baro altitude correction.

[ABLomas]

this shouldn't affect things if sensor is set to GPS_ONLY

I tested with GYRO_ONLY (and BARO_ONLY), it made things even worse (alt drops even bigger).
BB log should be in card i think so can pull if needed.

[Jetrell]

I have only looked at it briefly so far, but it occurs to me that reflections from parts of the craft

@sensei-hacker I've certainly experienced that problem before. Causing the AGL altitude to appear randomly, displaying something like 0.7m. When the plane was more than 50m above the ground. I tracked it down to the Lidar being reflected off a certain part of the aircraft.

But being there are so many cases of late. With both copters and planes. I tend to think its more than that. I personally haven't seen Rangefinders working outside correctly since 7.0.

[breadoven]

Seems the Air Cushion code mentioned above was added under #146 and is related to multirotors so at the very least it should only be used for multirotor not fixed wing. However, looking at it now that code has other issues such as the same problem as with this issue if you fly below takeoff altitude when you wouldn't even need a rangefinder enabled to screw up the Pos Z estimation.