Plane: rangefinder sanity checking

ArduPilot / ardupilot

ArduPlane, ArduCopter, ArduRover, ArduSub source

http://ardupilot.org/

GNU General Public License v3.0

11.08k stars 17.64k forks source link

Plane: rangefinder sanity checking #4876

Open magicrub opened 8 years ago

magicrub commented 8 years ago

Issue details

To support landing locations at a different altitude than launch, the baro.alt - lidar.alt delta sanity check was removed. This allows noisy sensors to control the aircraft with invalid ranges (such as flaring at 50m). Not all sensors are created equal. We need more configuration options to sanity check some sensors more than others. Suggestion: have the sonr.cnt value threshold be a param. Soem sensors need it to be large, such as 30 (3 seconds of continuous good data).

Version

3.6

Platform

[ ] All [ ] AntennaTracker [ ] Copter [x] Plane [ ] Rover

Airframe type

Fixed wing

Hardware type

any

Logs

https://drive.google.com/open?id=0B2WywYPDbzkfa3l1ZHkxZ3k5REk (see second land approach flares up at 50m and lidar data is very very lousy on an SF02.

magicrub commented 8 years ago

suraqis commented 8 years ago

May we have quick fix for single erroneous rangefinder altitude value? I saw early flare at about 50-80 m after single false rangefinder signal. Just check for correlation between EKF altitude and rangefinder altitude at least for 1-2 seconds.

magicrub commented 8 years ago

There's actually a lot of sanity check going on, and simply checking the ekf doesn't help when landing and a different attitude than where you took off from. I think this is a duplicate issue. I'll search later when I'm not mobile

suraqis commented 8 years ago

I think Simpliest way to quick fix it is just check rangefinder values for continuity and consistency regarding to vertical speed from EKF. The most annoying bug is flare after single false rangefinder reading.

magicrub commented 8 years ago

@suraqis you are incorrectly assuming there is no current sanity checking. Here is the current sanity checking: https://github.com/ArduPilot/ardupilot/blob/master/ArduPlane/altitude.cpp#L601

rangefinder must be healthy (allows feature for power-down (unhealthy state) above certain altitudes)
data range is adjusted to compensate for aircraft title, assuming sensor is always pointing directly face down.
grab first ever sample, and then we must see 10 more samples that are at least 5% different (helps against Lidar Lite hardware bugs)
apply terrain offsets if available
compute and low-pass altitude correction (this is applied to baro data to give accurate slope/flare values)
if the correction changes by more than 30m then reset whole system (re-require 10 good samples)

possible room for improvement areas:

do not allow step-input of initial correction at line 659.

suraqis commented 8 years ago

I think it's rather about https://github.com/ArduPilot/ardupilot/blob/2d2ed7b06e8df3631d5d5ce92183cc8ba4fc2784/ArduPlane/altitude.cpp#L576 But I can't find where rangefinder_state.last_correction_time_ms is updated.

magicrub commented 8 years ago

happens on any successful correction update: https://github.com/ArduPilot/ardupilot/blob/master/ArduPlane/altitude.cpp#L672

Although, I see that it should definitely always be before the memset 0 at line 669.

magicrub commented 8 years ago

fixed :) https://github.com/ArduPilot/ardupilot/commit/350ed204600a93892c31fcd9715a72d2c65b341c