Compensate long-term baro drift #3749
Conversation
hsteinhaus
force-pushed
the
gps_qnh4
branch
2 times, most recently
from
fe38284
to
8deacbb
Compare
ArduCopter/ArduCopter.cpp
Outdated
| @@ -111,6 +111,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { | |||
| SCHED_TASK(three_hz_loop, 3, 75), | |||
| SCHED_TASK(compass_accumulate, 100, 100), | |||
| SCHED_TASK(barometer_accumulate, 50, 90), | |||
| SCHED_TASK(barometer_adjust, 400, 50), | |||
There was a problem hiding this comment.
400Hz update loop rate is massive overkill.
There was a problem hiding this comment.
The comment in L80-L93 says "400 = 1Hz". But the macro definition says rate_hz.
So what is correct? May we should fix either the comment or the macro?
There was a problem hiding this comment.
You can add it to one_hz_loop()
|
Thanks for this patch. It is one of many solutions to this problem so having this "in our bag" as an option will be a plus. There are a few changes I posted which I'd like to see before something like this goes in since it will be a critical change. |
|
@magicrub Thanks for the review, very much appreciated! Obviously some nasty "things" happened when porting forward the patch from AC 3.2.1 to master in a hurry. Fixes will be pushed after a test flight. |
|
while you're at it, can you remove all those .set() and .get()'s from _alt_offset. Just makes it harder to read. |
|
Will do a test flight tomorrow. If everything goes well, I will rebase the reworks into the original commits. I did not touch the comment and the get/set problem, will better create a separate branch for that kind of work. |
|
I also see that all your commits start with 'AP_Baro'. Per our guidleines (http://dev.ardupilot.com/wiki/submitting-patches-back-to-master/) we like to have the directory name as the first thing in the comment. Next time! |
libraries/AP_Baro/AP_Baro.cpp
Outdated
| @@ -367,7 +402,8 @@ void AP_Baro::update(void) | |||
| if (is_zero(ground_pressure) || isnan(ground_pressure) || isinf(ground_pressure)) { | |||
| sensors[i].ground_pressure = sensors[i].pressure; | |||
| } | |||
| float altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure); | |||
| float altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure) + | |||
There was a problem hiding this comment.
Up at line 380 I added a LPF to _alt_offset a few months ago for when step-inputs are done by companion computer or GCS. The result of that is _alt_offset_active. Then at line 405 you add your "raw" offset then at 410 the existing code adds my LPF'd offset again. The offset is applied twice. The offset at line 405 should be removed.
There was a problem hiding this comment.
Very interesting information. Today, I tried a flight, but something was strange. The correction seems to oscillate, what I have never seen with my branch of AC before. I think, this could be a very good explanation for the strange behavior.
Unfortunately it's too late now for another flight, getting dark already here...
|
Just did a successful test flight: The takeoff, all touch-and-gos and the final landing were targeted to exactly the same place on the ground. The reported altitude in was within +-30cm of true value before entering ground effect/pressure bubble.
The periodic signal in the graph is probably from my copter flying without canopy in strong winds. My AUTO pattern was a rectangle of about 2 minutes per round, flying the upwind leg nearly at maximum possible airspeed. The correction signal has been scaled by a factor of 10 for better visualization. |
|
Very interesting work Holger. How do you measure ground-truth when you claim +/-30cm? Would be interesting to get a longer-duration copter and a Lidar. |
|
By loitering in front of me, exactly in the height of my eyes, then reading the altitude shown on the display of my transmitter (which is equivalent to BARO.Alt) ;-) |
| else { | ||
| _gps_alt_over_home.apply(gps_alt - _home_alt, GPS_DT); | ||
| } | ||
|
|
There was a problem hiding this comment.
@magicrub During one test flight I had a home point, that was not on the ground (discussed here: https://groups.google.com/forum/#!topic/drones-discuss/FzWhvnpW0cI ). In that case, (gps_alt-home_alt) is obviously not the altitude over takeoff. Is there any need to handle that case here?
Or better fix the problem that HOME is not on the ground elsewhere?
|
@rmackay9 @hsteinhaus |
There was a problem hiding this comment.
This came up on the dev call today. I think I missed reviewing the changes before now (sorry about that).
Given that Paul's happy with a LPF outside of the EKF for this slowly slewing the baro altitude to match the GPS altitude I think we can rework this to do that. However what is presented here is not the correct approach.
It's updating the offset based on the copter's altitude target, rather then it's current GPS altitude, which means that it's always slowly slewing towards the demand altitude, which would show up during an extended climb for example where the desired altitude was always 7 meters above the current position.
This needs to be reworked to only be filtering the baro altitude against the reported GPS altitude (and only when the GPS unit has a vertical accuracy and it is below a threshold value) should the filter be updated.
This would need to be based around the altitude relative to the EKF origin on copter, and relative to the to home on plane.
A couple of other collected comments: I'm not sure alt_offset is the right place to be accumulating the result, it has a couple of problems: The user can modify it, the modified values aren't being reported to the user, which means that if the user wants to adjust this inflight they messed up the baro compensation.
If you want to rework this to instead be based around just the GPS altitude that would be great, otherwise this is on @rmackay9 and my schedules to try and take a look at getting reworked and in in the near future.
I am not sure if I understand this statement correctly - the altitude already slews towards gps_location().alt .The alt target is only used to ignore segments of the flight where the alt target is not constant, i.e. after the pilot or the mission has requested alt changes (by resetting the LPF on every alt target change). This restriction could probably be relaxed, but I do not see any benefit for the given purpose of the PR. |
| */ | ||
| void Copter::barometer_adjust(void) | ||
| { | ||
| barometer.update_alt_target(pos_control->get_alt_target()*0.01f); |
There was a problem hiding this comment.
watch the alt target to prevent spoiling the LPF with a moving alt target
| @@ -557,6 +568,7 @@ void Copter::update_GPS(void) | |||
| #endif | |||
| } | |||
| } | |||
| barometer.update_gps_alt(float(gps.location().alt)*0.01f); | |||
There was a problem hiding this comment.
update the LPF with GPS altitude (not alt target!)
|
Why do you need to take into account the altitude target changing? You are trying to slew the baro to the GPS which will always be changing? The need for the altitude target to be ingested at all feels very incorrect. |
|
The (infinite response) LPF is lagging behind for a minute or more, so I guess it is the best choice to do a clean restart if the target altitude has changed. May be there are other possible impementations, e.g. based on filtering altitude deltas instead of absolute altitudes, but the one submitted in this PR was the most obvious one that fulfills my requirements: to cancel out the baro drift for long (1 hour) constant-alt auto missions or long static loiters. But even when looking on deltas, I guess the (huge!) phase offset caused by the LPF needs extra care in case of alt changes. Maybe @priseborough can give advice here on the theoretical aspects? |
…mputed from lp-filtered GPS altitude
|
fixed whitespace |
|
I don't understand why the barometer sensor library ever should need to know or care about the altitude target. It's mixing up inputs and outputs. We can't have loops in the sensors -> control path in the code |
|
I have a few comments:
|
|
@priseborough that sounds like a good plan, thanks Paul! |
|
@hsteinhaus Holger, please advice if you can make requested changes. I was planning to implement the same algorithm, so if by any chance you are not willing to finish this, I can take over this PR if you do not mind. |
|
I had forgotten there was exisiting functionality in the EKF's which adjusted origin height using a Bayes filter, however it effectively stopped updating after about a minute due to 1cm precision in the origin height. I am working on a branch that improves the calculation and provides parameter control. https://github.com/priseborough/ardupilot/tree/pr-ekfOriginHeightEst |
|
I think that @priseborough's work is complementary to what @EShamaev is offering to work on. So Paul will allow the current compensation in the EKF (which adjusts the EKF-origin down so that the Baro and GPS altitudes match) to be turned off and then we can add a Baro-compensation feature outside the EKF to instead adjust the baro towards the GPS. That's my guess on how we can proceed but we could create a small chat to clear that up if necessary. |
|
Hi guys, I am perfectly fine with anybody stepping in here, as I am rather busy with other things currently. The approach in this PR was to share back some kind of least effort solution keeping the vehicle roughly on track, as the stock ArduCopter firmware would probably crash into ground due to the baro drift when doing longer low altitude surveys (1 hour @ 30m). @priseborough If I understand your changes correctly, I only have to set OGN_HGT_MASK to 5 and there should be no more long time drift after applying these two changes (04716d2 and 56d31f9) ? |
|
Version 2 is here: #6228 |
|
If there's a new PR, should we close this one? |
|
Maybe it should be open for some time for reference? |
|
closing it doesn't make it disappear. It's referenced in your new PR (thank you!) so it sounds like this one should not be considered "open" |
|
@hsteinhaus The local position reported by the EKF is not corrected, regardless of the parameter setting, however if you set the parameter to 7 (the default) then the origin height reported by the EKF will be adjusted. However unless the copter height controller is using the origin height in it's calculations, that will not help. |
|
My PR based on this seems to be working. More tests are needed, but I agree that this one can be closed. |
This PR slowly corrects the barometric base altitude (ALT_OFFSET parameter) during flight using a low pass filtered signal derived from GPS altitude.
Correcting the base altitude is important during long AUTO missings, otherwise the actual flight altitude above home slowly drifts away from the altitude target. I have regularly seen drifts of more then 20m per hour of flight. This could easily lead to a violation of protected air spaces or to controlled flight into terrain.
The compensation starts after a certain period of flight with constant alt target (configurable by parameter ADJ_DELAY). The rate of compensation (paramter ADJ_RATE) and the time constant for filtering the GPS altitude (parameter ADJ_TC) are configurable as well. The compensation is disabled by default and has to be enabled by the user by setting a non-zero ADJ_RATE.
Fixes #2022 .