Copter: handle rangefinder glitches in alt tracking #5241
Conversation
|
ping @lthall |
|
I've test flown this on an Iris with a LidarLite and it works well |
| if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) { | ||
| rangefinder_state.glitch_count++; | ||
| } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) { | ||
| rangefinder_state.glitch_count--; |
There was a problem hiding this comment.
This will reject a first high glitch but then accept a low glitch (or vice-versa). It may have a low probability of happening but it seems strange that a code handling glitches would accept such a weird one.
There was a problem hiding this comment.
If it has glitched positively then glitches negatively we probably should set the glitch count to -1 rather than just -=1.
There was a problem hiding this comment.
That would still leave the up/down endless loop situation. If we don't want to care about that situation, then I would still suggest two counters and when one is increased the other is set to zero. That means you need 3 consecutive glitches in the same direction for this to assume that's the correct altitude. It does the same as @lthall's suggestion but without extra ifs.
| } | ||
| if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) { | ||
| // shift to the new rangefinder reading | ||
| target_rangefinder_alt = rangefinder_state.alt_cm; |
There was a problem hiding this comment.
Shouldn't this be constrained like before? I understand that it is accepting the new report altitude is the correct one even if a large change happened, but I think the previous constrain was in place to not upset the controllers.
There was a problem hiding this comment.
The constrain that was removed only serves to limit how far the copter can move the set point. By removing the constrain the copter will stay at the same altitude when flying off a balcony and start following the new ground level rather than dropping down to the constraint from the ground level.
This results on nicer inputs to the alt hold controller.
It will also mean the aircraft will not drop after flying over a ledge or climb when you fly over a house.
There was a problem hiding this comment.
That's good, thanks for the detailed explanation Leonard.
|
As a side note, another possible solution is to bring back the mode-filter which was built precisely for the purpose of removing sonar glitches but we apparently dropped it as part of "the onion" which was done between AC3.1.5 and AC3.2.1. Anyway, I don't mind if we handle this with a glitch-detector as I think this makes things better than what we have in master. We should also consider moving to use the EKF's height-above-terrain estimate before the next release. |
|
a mode filter won't produce the same result, and will add lag. I think the glitch removal is the best approach. |
|
Thinking about it more, I agree the glitch detection is the best way. Maybe we just need to answer the last couple of questions from @OXINARF and then we can merge. |
|
Regarding the issue with glitch above and below: I think that what we need to do is define if it is important or not that a glitch happens above or below. Even more unlikely (I would say statistically impossible unless a hardware bug exists) but with the current code it could end up in a endless loop of increasing/decreasing the glitch count. My thinking is that we should have two counters, one for above and another for below. If any of them goes equal or above 3 then we reset. This may take up to 5 readings to reset, but that only happens if two glitches happened in the opposite of the "winning" glitch direction. |
|
I agree with Oxinarf. I can appreciate that this new glitch filter can perform better than the Mode filter. But I think we should make it operate symmetricly like the Mode filter does. Essentially, we just need to track high and low glitches separately. BTW, on Gitter, Tridge stated that this was designed with input from Leonard according to maximum possible climb or descent rates. What rate is this? This glitch checker is run periodically, but has a fixed check distance. So the maximum rate will be influenced by the rate the code is run. Without driving too much complexity, could we just add some comments about the designed maximum rate and intended operating frequency of the code? Just so that we have visibility in the future. |
|
If we find a situation where the glitch is going positive and negative then the only sensible thing is to stop doing terrain following and declare the sensor unhealthy or something like that. I think this is a much more complex problem. I think doing what I suggested above would be a good compromise. On the maximum rate discussion. We did talk about this but the glitch size turned out to not be dependent on rate of change or slope. On the frequency it is run. It looks like it is run on each loop of the altitude hold loop so it should be pretty repeatable. |
| if (rangefinder_state.glitch_count != 0) { | ||
| // we are currently glitching, just use the target rate | ||
| return target_rate; | ||
| } | ||
|
|
||
| // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations) | ||
| distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt); |
There was a problem hiding this comment.
Two lines below this point (I didn't know how to comment there) we have the line:
velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
I think we should constrain this to half the maximum up velocity and half the maximum down velocity. This will ensure the pilot can always ask for a faster rate than the terrain following can request. If you think this is too slow we can make it equal and that way the pilot can always stop a decent or ascent but can't go the other way.
I agree, but then this would have to be done inside the AP_Rangefinder library.
Unless we do the healthy part and stop using terrain following, I still prefer my suggestion of two separate counters and when one is equal to (or greater than) 3, then let it go. That removes the possibility of an endless up/down loop. |
|
Well, it doesn't remove the possibility of an endless up/down loop because for this to happen the sonar/lidar has to be doing almost consecutive up/down glitches. All the approach you suggest will do is make the range reset to one of them and periodically use it to change the altitude. The method I suggested would mean the copter would use pilot input and exclude the sonar input until a firm return was received. I think this is closer to what we want. |
this handles glitches from the rangefinder when tracking the ground. It requires 3 readings in a row to settle on a new target altitude thanks to discussions with Leonard and Randy
see discussion on the PR
tridge
force-pushed
the
pr-copter-rangefinder-glitch
branch
from
ddd1ead
to
9e482a7
Compare
|
I have pushed a simple change to handle alternating glitches. |
|
LGTM. I would squash the two commits, we are starting to have a less clean history (which makes it harder to keep track, at least for me). |
|
squashed and pushed. |
this handles glitches from the rangefinder when tracking the
ground. It requires 3 readings in a row to settle on a new target
altitude
thanks to discussions with Leonard and Randy