Lidar in AltHold

Hi everybody,
I want to use the BENEWAKE TFMINI plus as an altitude sensor for indoor flights.
Right now I have the drone on my workbench so I am not sure if this will effect my problem.

I set up the rangefinder parameters and i can see the correctly measured distance.
However, the altitude in ALTHOLD mode does not refer to this reading ( picture attached)
Basically the questions comes down to this:
a) The Altitude in the HUD does not equal the altitude the controller uses for its calculation
b) For some reason the controller refuses to use the Lidar Reading for the altitude.

Besides the ones specific to the TFmini i set the following parameter:
EK2_ALT_SOURCE: 1
EK2_RNG_USE_HGT: 70
EK2_GPS_TYPE: 3

Thank you all very much!

@Frankendrone,

I don’t have a lot of experience with indoor flight but it looks like the EKF is unhappy for some reason. Maybe try clicking on the EKF label to see why. I suspect if it gets very unhappy with the rangefinder readings it may stop using them.

By the way, in general we strongly recommend against changing the EK2_ALT_SOURCE to 1 (RangeFinder). We have the following warnings on the terrain following wiki page for non-autonomous modes:

Do not set the EK2_ALT_SOURCE or EK3_ALT_SOURCE parameters. These parameters should be left at “0” (barometer).

Do not set the EK2_RNG_USE_HGT or EK3_RNG_USE_HGT parameters. These parameters should be left at “-1”.

Your case (indoor flight) is the one place where this may be OK though. Just make sure that the ground below the copter really is flat.

1 Like

Hi, thanks for your help!

Attached you will find a picture of the unhappy EKF. The rise of “position (vert)” is induced by shaking my lidar and this is the highest spike that i could create. When the drone is just standing on the bench and the lidar is just aiming on a surface there is hardly any rise.
I can definitely see that position (vert) depends on the movement of the Lidar.
I think that the EKF is unhappy as I am not using GPS and I am not offering a different position system.

I would have questions regarding this warning as I might be misunderstanding the documentation:
If I would not set those parameters and leave them on default, would my rangefinder actually do anything?

I really want to use this drone only indoors in a lab with a bright and very flat ground surface. The barometer however is absolutely useless as a big AC and ceiling fans create a very mixed atmosphere.

Which parameters would have to be set for this usecase? The purpose of the lidar is to create a stable ALTHOLD for trimming and tuning and to make sure that the drone does not rise for the ceiling (8m height)

Thank you all very much and cheers!EKF_unhappy

1 Like

@Frankendrone,

OK, this sounds like the one use case where setting EK2_ALT_SOURCE = 1 is the correct thing to do. The EKF definitely won’t have a horizontal position estimate so those two red lines are fine.

The altitude that you see on the HUD is the height above home which is different from the EKF’s altitude estimate so I wouldn’t worry too much about the discrepancy.

I think it might be good to walk around with the drone (or at least lift it up and down) and make sure the altitude estimate climbs and descends as expected.

Great! I will set the parameter EK2_ALT_SOURCE = 1 and try out how it works.
Yesterday did try the drone outside with the recommended settings for terrain following
over a flat surface but added a small table in the flightzone to verify the usage of the rangefinder and it worked fine.
I will try out both settings inside now and give an update.

Thanks for your help!

PS: Should EK2_RNG_USE_HGT be set to -1 or 70? if EK2_ALT_SOURCE is set to 1?

@Frankendrone,

Sorry for the slow reply, I think that EK2_RNG_USE_HGT should be set to 70.

Please note that the detection distance will be shorter in a bright place even indoors. In that case, you need to adjust RNGFND1_MAX_CM and EK2_RNG_USE_HGT. In particular, it is a case of flying near the ceiling 7-8m.

Hi guys,
thanks for the input!
I am using the Benewake TFmini plus. So, theoretically the max reading is 10m but my goal is it to keep the drone always below 2m. I tested the reading and I had good readings up until more than 8m.

I will do a testflight soon now and I will leave a note here, how it worked out!

@rmackay9 I am trying to understand the use of EK2_RNG_USE_HGT looking at source but it seems to me that it is never used. The condition in the following if is never false and so the code in the else case is never executed, that means EK2_RNG_USE_HGT (frontend->_useRngSwHgt) is never used. Correct me if I am wrong.

@anbello,

I certainly agree that there is some dead code there introduced by a PR I added a few months back. I’m pretty sure though that the EKF will use the lidar for it’s main altitude estimate. What it won’t do though is switch between baro and lidar when it cross above/below the EK2_RNG_USE_HGT threshold.

In this way the value of EK2_RNG_USE_HGT is not used, it is only tested to be > 0.
Another question: in which condition happens the switch between baro and lidar? When alt becomes > RNGFND_MAX_CM? I don’t find it in the code.

@anbello,

I thought the EKF would switch back to use the barometer (instead of Lidar) if the lidar range becomes longer than >EK2_RNG_USE_HGT or >RNGFNDx_MAX_CM (whichever comes first) but you may be right that this logic is broken. It would be worth testing (a SITL test might be easiest).

For other reading this thread, I’d like to clarify once again that we are discussing a very special case of indoor flight in a room with a flat floor and an air conditioner that upsets the barometer. For terrain following the EKF parameters should not be modified. Terrain following setup advice is here for non-autonomous modes and here for autonomous mode.

@rmackay9
Thanks for answers, I see that there is also an open issue on this stuff: https://github.com/ArduPilot/ardupilot/issues/12991
Also I have seen that in this page it is recommended to use the parameters EK2_ALT_SOURCE and EK2_RNG_USE_HGT in a different way:
https://ardupilot.org/dev/docs/extended-kalman-filter.html#ekf-alt-source
“To use range finder at lower altitudes and barometer for up and away flight, set EK2_ALT_SOURCE = 0 and use the RNG_USE_HGT parameter.”
It seems something that could be right pre your PR.

Anyway I did some SITL testing with latest (as of today) master and I see weird things but maybe I did something wrong.
I used this commands to have a simulated rangefinder in SITL:

param set SIM_SONAR_SCALE 10
param set RNGFND_TYPE 1
param set RNGFND_SCALING 10
param set RNGFND_PIN 0
param set RNGFND_MAX_CM 2000
param set RNGFND_MIN_CM 0

then takeoff at 40 m

mode guided
arm throttle
takeoff 40

and land (after reaching 40 meters)

mode land

All this in two cases:

  1. EK2_ALT_SOURCE = 0 EK2_RNG_USE_HGT = -1

dataflash log

  1. EK2_ALT_SOURCE = 1 EK2_RNG_USE_HGT = 70

dataflash log

1 Like

Hi,
I can give a first feedback now:
The drone could hover stabily indoors however there were problems with the reflectivity of the ground.
I took off from the ground and reached around 3ft altitude. The drone was somehow stable but eventually started to raise. I had limited the maximal vertical velocity in altitude mode to 0.5m so the raise was very slow. I landed the drone.
As I suspected the reading of the Lidar to be noisy over the reflective ground I put wooden pannels on the ground and took off from there. I hovered a couple of seconds over the wood and flew over the ground. The altitude remained stable and I did not see a raise. I returned to the wooden pannels and hovered for a couple of seconds and landed.
In the log I can see that the Lidar reading is extremely noisy over the reflective ground but useful over the wood.
However, the good thing about it is that the drone did not go crazy even when the reading was useless. On the bad side, we can see small spikes in the vibration logs during the over above the reflective ground. Attached you will find the log, in case you are interested!

Best,

1 Like

@anbello,

Thanks for the info and testing. Here is the parameter file for using the rangefinder is SITL and it seems to have some slightly different values (the scaling in particular). I don’t know if the parameter values are relevant but I thought I should point this out.

The wiki page linked above (here it is again) is for the EKF1 which we don’t use anymore. Perhaps we should update the page but for now I recommend users look at this EKF wiki page which I know is maintained although it has much less information on it.

By the way, my PR removed the ability to use the rangefinder at low altitudes and the barometer at higher altitudes. This wasn’t completely intentional (at the time I didn’t know the EKF2 even had this feature) but the change was the result of countless support calls from users who had set one of the two parameters (EK2_ALT_SOURCE and/or EK2_RNG_USE_HGT) and had horrible experiences often resulting in crashes. I don’t remember ever seeing a positive report regarding these parameters. Perhaps there are more issues in the EKF in this area, I’m not sure… but disabling the rangefinder + baro mixing has definitely reduced the support calls. I guess my point is I’m not willing to re-enable this behaviour unless it’s proven to be reliable in a wide variety of situations (baro or rangefinder failures, uneven surfaces under the vehicle, rapid climbs and descents, flights right at the threshold of the switch, etc).

@Frankendrone,

Thanks for the feedback on testing and good to hear that it’s basically working.

One thing that seems odd is the EKF appears to be consuming the rangefinder altitude even when it’s clearly bad. The EKF’s altitude is in red, the sonar’s range is in green. The EKF should really only be consuming the altitude when the rangefinder status is “Good” (4).

The rangefinder’s actual status can be seen in the RFND message’s Stat field

I’ve created an issue in the issues list and hopefully we can find someone to fix it. I suspect it’s actually not that hard to make the EKF only consume the rangefinder when it’s status is Good.

1 Like

@rmackay9,
I took the parameter for rangefinder in SITL from this page, anyway as you said differ only in scaling. I did another test with the params you suggest but the result is the same.

It seems to me that the problems we see on my second test above (with plot and dataflash log) has the same root of the problem on @Frankendrone test: the EKF continue to use rangefinder altitude even when status is not 4 (Good), in my case it becomes 3 (OutOfRangeHigh) over 20m (RNGFND_MAX_CM 2000) and then we see all the weird things.

Does it still keep climbing? I know I had the same the same recently during indoorflight, ended up by setting the Wind_PSCALE parameter to 0 or some other Wind parameter.
I only knows it calculated in wind during flight, so it keeps ascending. Turned that off and it was gone. This happened to me in V3.6…

@anbello,

I’ve created an issue here re the EKF apparently consuming the range finder data even when it’s not “Good”. @priseborough has commented but I’m unsure if his reply means that he thinks it is all working properly or not.

@Sander,

Can you point me at that log or discussion? I’m very surprised if the wind speed estimate is causing an issue with the EKF’s altitude estimate especially on a multicopter.