Filter for rangefinder signal


I’m using the Benewake TF03 for measuring the distance to the ground. I got some issues with the signal when the drone flies over corn fields. The signal gets a lot of small spikes. In some cases arducopter triggers a failsafe due to the spikes.

I had this problem this summer with arducopter 3.6.11. But I thought there will be new parameters in copter 4.0. But I miss them:
RNGFND1_WSP_MAVG: Moving Average Range
RNGFND1_WSP_MEDF: Moving Median Filter
RNGFND1_WSP_FRQ: Frequency

Are they not implemented yet?

Many thanks in advance


Those RNGFND_WSP parameters are specifically for the Wasp LRF 200 rangefinder to configure some filtering options onboard the rangefinder itself. Many rangefinders have similar onboard settings, although most need to be configured outside of Ardupilot.

The EKF should be rejecting those spikes anyways. Have you checked NKF5.rng and NKF5.RI? The rangefinder innovation RI should jump every time those spikes appear.

And are you sure that’s what’s causing the failsafes? It really shouldn’t.

I got a few times the message “missing terrain data”. The copter stops the mission and return to launch.

No, I didn’t have a look on NKF5.rng and NK5.RI. Where can I check them?

Unfortunately the TF03 has no option for onboard filtering. So I thought maybe it’s possible with arducopter.

What is your rngfnd_max_cm set at. Looks like the spikes start only over 20 meters.
And whats the max range for your lidar device? Are you over max range when the craft is at a 45 degree angle?
with my lidar lite2, I see such spikes, and as long as the primary altitude controller is set to use baro, the bad data doesn’t create a problem. If EK2_alt_source is set to rangfinder then bad things will happen.
I made sure the rngfnd_max is set well below the altitude that causes the spikes, and have no trouble.

“Missing terrain data” is a failsafe that engages when you are using TERRAIN_ENABLE for terrain following, which is different than using a rangefinder. This type of terrain following uses the GCS to download terrain data from the internet and upload it to the FC. If you lose internet connection or the drone loses connection to the GCS for a few seconds, the “missing terrain data” failsafe will engage.

If you want to use the rangefinder instead of the online terrain database, then set the parameter TERRAIN_ENABLE = 0. Then, when you set waypoint altitudes to “terrain,” the copter will use the rangefinder.

If you want to use the terrain database, you just need to make sure you keep a good telemetry connection to the GCS and a stable internet connection.

The rngfnd_max_cm is set to 12000 (120m) It’s the official value from the ardupilot website:

NO, the drone’s max angle isn’t over 45°.

My theory about the spikes is the following: The leafs of the corn plants reflect the laser from the lidar sometimes in a strange way, like a mirror or something similar. At this moment the lidar have no feedback and spikes occur. If I fly over grass fields I haven’t any issues.

@Anubis I only use the rangefinder for terrain data

I’m pretty sure that’s the only situation where a terrain data failsafe is possible (rangefinders do not collect terrain data). If you can find a flight log where your drone failsafed, upload it here and I can take a look at it.

Hi Rick

I finally find the logfile with the missing terrain data message.

Many thanks for having a look at the file.


On this flight, you did have TERRAIN_ENABLE = 1. Setting this parameter to 0 will stop it from using the terrain database and prevent the terrain failsafe from triggering.

I’m not entirely sure why it triggered, though. From the graph below, the error that caused the failsafe is cleared almost immediately after it appears (FAILSAFE_TERRAIN-0 is the error being cleared), so it might have just been some lost packets in an imperfect telemetry connection. But the log shows that there’s no pending terrain data to be downloaded, so I’m not sure if that’s the case.

Anyways, regarding the rangefinder:

The green line is your rangefinder readings. The orange line is the value that is actually being used by the navigation filter. As you can see, the filter is ignoring the spiky readings already. So you should be good to go once you disable the terrain database - you really don’t need it if you’re not flying over mountains, anyways.

Hi Rick

Thank you very much for checking the data log file. If I set TERRAIN_ENBABLE to 0, does the drone still following the ground with the rangefinder?


Yes, as long as you set the altitude type to Terrain in the mission waypoints.

edit: and WPNAV_RFND_USE = 1

Thanks a lot, I didn’t know that.

No, I don’t plan to fly over mountains :slight_smile:

I think with your help It should be possible to get rid of the most issues.

BTW, I first plan this summer to fly only 5meters over the corn fields. I didn’t work. So I adjusted the flight height to 20meters. Maybe you can have a look at this data log:
To my data log understanding, the TF03 Lidar worked only with the 20meters flight hight. Below that, the data is so wrong, that even the filter can’t make it right.


Yeah, it looks like you’re right. From what I can tell, Auto mode is using the unfiltered rangefinder reading instead of the filtered one for maintaining altitude while following terrain:

Every time your rangefinder spikes, the desired climb rate goes down, even though the EKF rangefinder value ignores the spike. You can see that this doesn’t happen in Loiter mode.

So I think there’s bad news and good news.
Bad news: This is a firmware-level feature/bug. We can’t change the behavior with parameters.
Good news: Looks like we can use parameters to force the flight controller to ignore the spikes.

This problem was still happening when you were flying at 20 meters, but the rangefinder was spiking to >50 meters instead of ~40 meters. >50 meters is above your configured max rangefinder range, so the spikes were ignored. So, if you want to try flying at 5 meters again, set RNGFND_MAX_CM to something lower, like 2000. That way, any spikes above 20 meters are ignored, and the spikes were consistently 40+ meters, so I think it will work.

Here’s you flying at 20 meters, showing that there is no change in desired climb rate when the rangefinder spikes:

1 Like

By the way, it looks like the terrain failsafe you were experiencing earlier has been fixed in 4.0:

But using TERRAIN_ENABLE = 0 is still best in your case.

Hi Rick

Many thanks for your analysis.

Why does Auto mode not use the filtered signal? I can’t see any reason for this. Maybe it’s worth to modify the code to have the auto mode with the filtered signal?

Anyway, I will reduce the max rangefinder distance and see how it performs.

I don’t know, maybe this feature was written before the filtered EKF signal was. I opened an issue in the Ardupilot repo for this:

perfect, thanks a lot. I’m curious for the update :slight_smile:

TF03 consumption goes up to 180 mA. Not too much, but you can try an independent supply or a low ESR capacitor.

On last versions RNGFND1_TYPE is 27 for Benewake TF03.

ST VL53L1X is a good sonar alternative, up to 4m. As I understand, you can try both at the time and compare logs. From its documentation (corn vs grass):
Unlike conventional IR sensors, the VL53L1X usesST’s latest generation ToF technology which allows
absolute distance measurement whatever the target color and reflectance.