I’m certainly not a skilled c++ coder, but maybe there is a coding error in AP_NavEKF2_PosVelFusion.cpp that prevents the rangefinder from being used as primary Alt-Source even if the EK2 parameters are set accordingly (EK2_RNG_USE_HGT > 0; EK2_ALT_SOURCE = 0)
Seems to me: No matter, which value frontend ->_altSource has, all the code of “determine if we are above or below the height switch region” is NEVER executed !?
// select height source
if (extNavUsedForPos) {
// always use external vision as the height source if using for position.
activeHgtSource = HGT_SOURCE_EV;
} else if (((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
if (frontend->_altSource == 1) {
// always use range finder
activeHgtSource = HGT_SOURCE_RNG;
} else {
// determine if we are above or below the height switch region
float rangeMaxUse = 1e-4f * (float)frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;
// If the terrain height is consistent and we are moving slowly, then it can be
// used as a height reference in combination with a range finder
// apply a hysteresis to the speed check to prevent rapid switching
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
bool trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
/*
* Switch between range finder and primary height source using height above ground and speed thresholds with
* hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
* which cannot be assumed if the vehicle is moving horizontally.
*/
if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) {
// cannot trust terrain or range finder so stop using range finder height
if (frontend->_altSource == 0) {
activeHgtSource = HGT_SOURCE_BARO;
} else if (frontend->_altSource == 2) {
activeHgtSource = HGT_SOURCE_GPS;
}
} else if (belowLowerSwHgt && trustTerrain && (activeHgtSource != HGT_SOURCE_RNG)) {
// reliable terrain and range finder so start using range finder height
activeHgtSource = HGT_SOURCE_RNG;
}
}
} else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
activeHgtSource = HGT_SOURCE_GPS;
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
activeHgtSource = HGT_SOURCE_BCN;
} else {
activeHgtSource = HGT_SOURCE_BARO;
}
I don’t have a full answer but I know that a couple of months ago a PR I added created some deadcode. I don’t think that change stops the EKF from using the rangefinder has a height source though. Here’s a link to the PR: https://github.com/ArduPilot/ardupilot/pull/12250
Randy, these bugs have nothing to do with the PR a few months ago, since they exist since Copter 3.4
If you look at the code section i copied, it’s pretty clear that the developer made an unwanted mistake in those if … else if clauses.
This mistake makes Alt-source switching in low heights not working at all (since > 3 years now !), which makes the discussion in your PR a bit funny !
Imho in this discussion it was priseborough who gave te correct answer. Maybe he can review this whole lib…
unfortunatly i dont know anything about coding. Sorry for that.
But lowflying is my daily bread. And fs007, you are right, that was a problem in the past. but these Days, its working fairly good. I can provide you a LOG from some days ago with the 4.0.0_dev Software.
I set the altitude at 1m terrainfollowing 5m/sec with the Lidar. He was doing it really well, but still the Tolerance was about 50cm (70 to 120cm)
But in one point you are right. When I see the RngFnd is measuring 70cm, why is he not straight away correcting to 1m ?
Stefan, you left EK2_RNG_USE_HGT = -1, so you are not affected by this bug at all.
Your quadcopter has a good althold performance even at low altitude, so that’s fine.
But you state that there is still a deviation of half a meter around desired alt,although you are using a high-quality lightware lidar, that should do better.
I can’tgive you a full answer (i’m no EKF expert), but the first reason is: arducopter doesn’t use the lidar as primary alt-source, but still the baro. The noise of your baro is +/- 2 meter and the field you are flying is not perfectly flat,so you can be happy, that you end up with +/- 0,25m.
My bug report refers to something else:
The smart developers of arducopter added a “switch” that should make the rangefinder the primary alt-source at low height. That’s a good idea, because often the baro is not reliable at low heights .
But: Because of that little bug, the code that should perform that switch is NEVER executed, so the whole thing doesn’t work at all.
ah, ok, now i do understand the Problem you are talking about. As i said, im not a coder at all… And unfortunately in this life i think im not gonna learn it. Although I find it extremely interesting.
You are right, the field (my testing ground) is not flat at all. So Im quite happy with the results.
If anyone has got an Idea what I could do to get a better performance, I would appreciate it a lot.
I know, close to the ground is not the best place for a Baro
The best idea is simply to wait until this bug is fixed and after that make use of switching to lidar as primary alt-source allowing an appropriate noise (EK2_RNG_M_NSE) depending on how uneven your field is.
Altough i never understood why i couldn´t make the Lidar to the primary alt source together with the GPS (only with the optical flow) , theire must be a Reason for that.
In PX4 you can do so, but the results are the same.
There are several things i don’t understand too:
The documentation recommends multiple times not to set the rangefinder as primary alt-source even in terrain following.
Why should it be better, to use an inaccurate, drifting baro first instead of an centimeter accurate Lidar ???
One Point is defenetly If you fly e.g. over Water. If the Water is clear the Laserlidar would mesure the Ground and we all could Imagine the Result.So, theire must be another “neutral” instance wich has got also an influence.
A Switch would be good to decide by myself wich altitude mesure i want to usw.
I tested today setting EK2_ALT_SOURCE = 1 and EK2_RNG_USE_HGT = 70 (%) and found that the EKF was correctly using the range finder as it’s altitude source. I could see it was using it because the altitude did not drift around like it normally does when the source is barometer.
I think @fs007’s point is different though, he’s saying that when EK2_RNG_USE_HGT = 70 (or anything above zero) and EK2_ALT_SOURCE the EKF should be using the range finder as it’s altitude source only when the vehicle is close to the ground. I agree this could be broken but it’s hard for me to promise when we will fix it.
For @Stefan_Menne and anyone else interested in terrain following. In ArduPilot terrain following is implemented outside of the EKF. It’s implemented in the control part of the system and I think this is actually the correct place to implement it. I don’t think we should look to enhance the EKF to improve terrain following.
The fix is pretty simple imho:
Should be “frontend->_altSource < 2” instead “frontend->_altSource == 1” in the first if…clause.
Meanwhile i compiled the firmware myself with that change, flew several times and now everything works as expected.
Thanks very much for that. So my big concern is what will happen if a user flies somewhere where the ground is not perfectly level. So so imagine this situation:
set EK2_RNG_USE_HGT = 70
leave RTL_ALT at the default 15m (1500cm)
takeoff fly vehicle up a hill and hover about 5m above the terrain at the top (maybe the hill is 10m high so the vehicle is flying 15m above home)
switch to RTL mode
When the vehicle is at the top of the hill, I think the relative alt (aka altitude above home) will change (maybe quickly, maybe slowly) and become 5m. When the vehicle enters RTL, it will then climb an additional 10m to the RTL_ALT value of 15m and then return at that altitude. This will be 25m above home which is more than the user specified.
This is just one situation where the behaviour would be unexpected. There are likely other stranger and more dangerous things that we haven’t noticed. Ah, imagine that it wasn’t a hill but rather a valley. I think the vehicle would impact the terrain on it’s way home.
Really it’s just not a good idea to be mixing up altitudes above home and altitudes above terrain. In most outdoor situations they are quite different. Only in indoor environments are they the same thing.
If that is true (and it is probably) i would consider this as a major bug in that firmware:
Alt above home should never rely on a range finder (maybe except indoors; flat ground).
So: seems that some work is required to get this correct, which means imho:
Alt above Home should be derived from baro or gps
Alt above terrain should be read from the rangefinder as the primary alt source, if close enough.
Imho it will climb to 15m at first, but as soon as the region of the hill is left, the baro will become the primary alt source again and it will descend to the user specified 15m
Yes, I generally agree with what you’ve said. Which is why I have added warnings in multiple places that the EK2_ALT_SOURCE should be left at 0 (baro) or maybe 2 (GPS) in some cases.
At the risk of being a bit picky, I wouldn’t go so far as to say it’s a “bug” though. I think it’s a “limitation”. The limitation being that the range finder should not be used in cases where the ground is not completely flat. If this limitation is respected then the rangefinder does give an accurate alt-above-home.
ok, agree, let’s call it limitation. But that limitation isn’t neccessary at all. Which behavior do we exspect if throttle stick = middle ?
I would like this:
If the copter is close to ground, it should follow terrain immediately by rangefinder readings. The reasons:
At low altitudes, the baro is very noisy, exspec. at windy days. Todays rangefinders are very accurate.
Baros tend to drift
So using the rangefinder gives some safety regarding crashes
In higher altitude the copter should not follow the ground level, but maintain it’s altitude above home, or sealevel …
In 100m height, it doesn’t matter,if a baro drifts 1 or 2m…
