Altitude Hold improvement solution

[dramida]

Let's face it: Right now, altitude hold is the weakest function in MWC. Right now ALt Hold it is based on a PI loop using a noisy pressure sensor (BMP085 or MS6511) and the D term in GUI is actually a term witch simply ads a proportional term with calculated Z acceleration to the response.

I've made a test with P=0, I=0, D=16 and the copter was opposing me immediately and keeping its altitude in spite of my up and down pushes, based ONLY on a proportional Acc term. The challenge is to correct the errors in altitude accumulated over time by Accz overshoots/undershoots. Let's add I and D term for Acc and give different time bounded weight to AccZ and Baro ALT.

I propose an improved algoritm witch gives a better response based on both sensors and their specific response over time:
An "fast" PID loop based solely on accelerometer (filtered to about 5 Hz) (the short term response)
An "slow" PI loop witch averages the baro sensor values over 5 to 10 seconds (long term response with little noise)
The responses of the both loops should be added (with configurable weight factors in front of each response)

Example: On short term,let's say less than 10 seconds, the AccZ is very accurate in keeping the altitude (see experiment described above). On the long run, the baro sensor gives an good approximation of altitude based on an average filter over a few seconds period and can correct the AccZ overcorection/undercorection errors.

Bennefits:
1-The newly introduced I and D terms of ACC loop, by itself, will even better compensate the quick altitude changes.
2- Over longer periods, the baro PI loop will accurately set the altitude without oscillations, as long term filtration removes noise from wind.

As i am not a C programmer, i invite a brave coder to implement this algorithm in MWC. Please give the possibilities to tweak the length of averaging vector for baro. and the weight of each sensor in final response to engine

[copterrichie]

I think if you were to try making the P = 1 and D as you have it, it may just work as expected. This is just a guess but well worth a try.

[dramida]

I think if you were to try making the P = 1 and D as you have it, it may just work as expected. This is just a guess but well worth a try.

I did as you told and i have better results with and P=0.7, D=16, I=255 than standard P=1.6, I=15, D=7.

Anyway, in my opinion, the key in Alt hold is TIMING, not only the proportional weight of each factor. As i wrote above, it is needed to filter the baro response longer periods (seconds) and accelerometer should be filtered to 5 to 10 samples / second

[copterrichie]

But the loop time will vary according to each configuration, there is no way in my opinion to regulate that at present. There was a discussion awhile back about making the loop time more consistent but that has not happened yet.

[dramida]

Yes, those two loops are executed slower than main stabilization loop, but this is not a problem. I wrote the immediate and obvious benefits in my first post of the topic . I need a second opinion and one brave coder to do the job.

[copterrichie]

I have done some work with ACC'es, Good Luck.

[mahowik]

Hi,

I have spent tooooo much time with alt hold improvements 6-8 months ago... And can say exactly that main issue is: ACC values should be double intergrated before correction according baro values (e.g. by using Complementary Filter) AND after that integrations it's far from the real distance... To use ACC raw data it should be perfectly calibrated and scaled for 3 axis etc.
I played with ziss_dm algoritm from 1.9 release. It gave best results for me... Also did a lot of my own tests and tried to tune algoritm from alexmos... And everywhere the same issues: vibrations sensitivity, initial perfect calibrations and as i say above - double integrated ACC data and errors accumulations...

viewtopic.php?f=7&t=363
http://forum.rcdesign.ru/f123/thread265409.html - here is our thread and story on this... try to use google translate...
http://diydrones.com/profiles/blogs/imp ... e-estimate

And good luck!
As for me i'm pretty tired from alt holds and so on...

thx-
Alex

[shikra]

Firstly I think we should recognise the contributions of everyone to get us where we are today.
Without doubt, ACRO is simply amazing on Multiwii.

Personally I would love to see improvements to BARO rather than new functionality / bling. But I know its so much more fun / interesting to work on new things.....

Are we out of idea share? Is there anything worth gaining from the other Open Source multi solutions?

[Lapino]

Wouldn't it be simpler to derive the values of the baro, get them a lpf, integrate the values of the acc one time to get the speed and send it through a hpf ? I know deriving online sucks but that way we can set the velocity to zero for alt hold (pilot is commanding velocities)

Integrating acc values two times brings up huge drift errors...

[alexia]

Firstly I think we should recognise the contributions of everyone to get us where we are today.
Without doubt, ACRO is simply amazing on Multiwii.

Personally I would love to see improvements to BARO rather than new functionality / bling. But I know its so much more fun / interesting to work on new things.....

Are we out of idea share? Is there anything worth gaining from the other Open Source multi solutions?

fully agree with that..
we spend so much time to working on other news feature before to improve already fonction like baro

[copterrichie]

In my opinion, a Baro on a Rotor copter is going to have problems because of the pressure differences created by the spinning props. Not to speak of vibration issues. What is needed is a laser range finder pointed at the ground.

[pm1]

Hi,
... - double integrated ACC data and errors accumulations...

exactly my experience too.

I did then concentrate on baro only, I denoise the input in a different way. I now have a D term, which prevents fast vertical velocity. In hover I it is as good as the baro delivers (sometimes I see jumps of 1-2m, which are not noise, but the baro changes mind for 10-20 seconds ). The only problem I still have is when fast corrections are neccessary, like going from hover to fast forward flight. Here the copter drops 1-2m and gains height slowly after 1-2s later

[copterrichie]

The GPS Altitude readings are more accurate for reference then the Baro, in my opinion.

[dramida]

In my opinion, a Baro on a Rotor copter is going to have problems because of the pressure differences created by the spinning props. Not to speak of vibration issues. ......

Guys, let's have a positive attitude about the Alt Hold. If anyone ask themself if it's possible, you should see DJI Naze how amazing holds its altitude based on same sensors as MWC.
Please look at the graph and see the spectrum of the noise for accelerommeter and barometer. Filter out (LPF) those frequencies and you will get a clean signal.

The ideea is simple (as all good things in life): we don't need a fast control loop for altitude as for stabilization. If we have an 5 to 10 HZ filter for Z accel this will filter out ALL vibes. If we have an 5-10 seconds averaging vector for altitude, all the noise is gone. So mixing the resul of those two control loops will do a far better job than actual P controller for Acc.
In support for this theory, i use with good results a very low "P" gain for Baro = 0.7 and big gain for the integral term of the baro I=255. The "I" term has a filtering behavior as it retains the history of the signal.
I have so many directions in this project and learning C programming (again) is not the best investment in time for me.

Let's make an effort and never talk about altitude hold.

Mihai.

[mahowik]

Hi,
... - double integrated ACC data and errors accumulations...

exactly my experience too.

I did then concentrate on baro only, I denoise the input in a different way. I now have a D term, which prevents fast vertical velocity. In hover I it is as good as the baro delivers (sometimes I see jumps of 1-2m, which are not noise, but the baro changes mind for 10-20 seconds ). The only problem I still have is when fast corrections are neccessary, like going from hover to fast forward flight. Here the copter drops 1-2m and gains height slowly after 1-2s later

could you share your code on this?

[copterrichie]

In my opinion, a Baro on a Rotor copter is going to have problems because of the pressure differences created by the spinning props. Not to speak of vibration issues. ......

Guys, let's have a positive attitude about the Alt Hold. If anyone ask themself if it's possible, you should see DJI Naze how amazing holds its altitude based on same sensors as MWC.
Please look at the graph and see the spectrum of the noise for accelerommeter and barometer. Filter out (LPF) those frequencies and you will get a clean signal.


Mihai.

Being the DJI NAZA is a closed system(not GNU), do you have knowledge how they are doing it? Talk about being positive.

P.S. If you are so smart, why not program it yourself and enlighten everyone here.

[pm1]

Filter out (LPF) those frequencies and you will get a clean signal.

The LPF is not the point. On ACC you have zero point *drift* because of vibration. Due to the integration you can run into problems

If we have an 5-10 seconds averaging vector for altitude, all the noise is gone. So mixing the resul of those two control loops will do a far better job than actual P controller for Acc.

I filter to about 300 ms and have a relative clean signal. I cannot do anything, if the BMP085 decides to change the altitude about 2m for 20 seconds.....

[copterrichie]

Here is what warthox has to say about Baro!

viewtopic.php?f=8&t=649&p=15357&hilit=warthox+baro#p15357

[pm1]

could you share your code on this?

Yes, here my latest code.... Download

My alt pids: 1.0 0.010 20

[copterrichie]

My alt pids: 1.0 0.010 20

Bingo.

[Crashpilot1000]

@pm1: Thank you very much for disclosing your source and efforts here. On the first glance i really like your Sensors/BMP approach: if the barodifference from currenthight is for 6 times off 10cm do the next average from list. Have a look at my code here also: viewtopic.php?f=17&t=1564&start=10#p21537, the Accz part is really crude and far from perfect, but the pure Baro part is not doing bad for now. It is a P controller with a variobrake for oscillations. I found out that getting a good groundalt at powerup makes it easier to read out and evaluate the following baroalt values, because you can simply constrain them to a reasonable difference to the last readout. I altered the readout of ms and bmp baro a little, so that they have equal timings now (new value every 30ms). Besides i moved from the bmp to the ms baro. The reactiontime and precision of the ms baro is better but the bmp is not so shabby after all. I found out that too much filtering of the baro makes it unusable because of the lag. It is better to feed, fast, a little crazy values to the motors because they filter with their own reactiontime and momentum than to do too much softwaresmoothing.
So long
Kraut Rob

[pm1]

@pm1: ...the Accz part is really crude and far from perfect...

In my code you see the last step *back* from the full integration and calculations of the distance error in comment. I wanted to implement only a z axis dampener this way. But even that did not work sufficiently. So in fact, I did change 2 things: 1. Changed averaging of BMP085 output. 2. Made an lpf for the D term.


And her a vido showing PH+Alt hold: https://vimeo.com/48646152

[Alexinparis]

The GPS Altitude readings are more accurate for reference then the Baro, in my opinion.

That's not true.
A GPS has a very poor alt resolution (some 10 meters), much more worse than its ground 2D accuracy.
A MS baro have a real 10cm alt resolution on the desk.

[copterrichie]

The GPS Altitude readings are more accurate for reference then the Baro, in my opinion.

That's not true.
A GPS has a very poor alt resolution (some 10 meters), much more worse than its ground 2D accuracy.
A MS baro have a real 10cm alt resolution on the desk.

Correction, In some Incidences and based upon geographical location. Granted, the GPS Altitude readings are unreliable but so are the Barometric readings In my opinion, at least from my studies.

[copterrichie]

Augmentation of a global navigation satellite system (GNSS) is a method of improving the navigation system's attributes, such as accuracy, reliability, and availability, through the integration of external information into the calculation process. There are many such systems in place and they are generally named or described based on how the GNSS sensor receives the external information. Some systems transmit additional information about sources of error (such as clock drift, ephemeris, or ionospheric delay), others provide direct measurements of how much the signal was off in the past, while a third group provide additional vehicle information to be integrated in the calculation process.

http://en.wikipedia.org/wiki/GNSS_Augmentation

[mahowik]

Hi guys,

if someone interesting, here is accZ calculation invarianted from inclination. It's projection of ACC vector to global Z axis (estimated by gyro vector G), with 1G subtructed:
(I got this during a lot of discussions with alexmos on althold...)

Math: accZ = A * G / |G| - 1G

Code: Select all


int16_t invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG  - acc_1G

#define K_ACC_1G 200 // scale for calculated acceleration
int16_t scaledAccZ = accZ  * K_ACC_1G / acc_1G;

float InvSqrt (float x){
  union{ 
    int32_t i; 
    float   f;
  } conv;
  conv.f = x;
  conv.i = 0x5f3759df - (conv.i >> 1);
  return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}

int32_t isq(int32_t x){return x * x;} 


Then it's also possible to take into account Acc drift. Actually the length of the estimated gyro vector G ~= 1G BUT includes the error/drift of the ACC:

Code: Select all

// acc_1G ~=  1/invG
int16_t invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG  - 1/invG;


then to reduce the noise we can use the deadband near zero:

Code: Select all

#define ACC_Z_DEADBAND 10
if(abs(accZ) < ACC_Z_DEADBAND) {
   accZ = 0;
  } else if(accZ > 0){
   accZ -= ACC_Z_DEADBAND;
  } else if(accZ < 0){
   accZ += ACC_Z_DEADBAND;
  }


Still hope that we get useful alt hold!

One day ago i have big crash with my new FPV quadr because of the alt hold fail... ((

thx-
Alex

[mahowik]

And her a vido showing PH+Alt hold: https://vimeo.com/48646152

It's not quite correct to test algoritms based on baro data near the ground. Because if altitude less than 60-100cm you get -1..2m in measuring because of the air cushion effect there...

[pm1]

It's not quite correct to test algoritms based on baro data near the ground. Because if altitude less than 60-100cm you get -1..2m in measuring because of the air cushion effect there...

I know, but otherwise you could not judge from the video, how big the deviation are .

[pm1]

[quote="Alexinparis"
That's not true.
A GPS has a very poor alt resolution (some 10 meters), much more worse than its ground 2D accuracy.[/quote]


Don't mix up precision with resolution. If you expect absolute correct values over long time, you are right. Depending on the count and angles of the satellites, the error might be 20+ m. But consecutive measurements in areas without steep slopes within short time differ not much and are quite usable.

[rbirdie001]

I'm just fiddling with useful height measurement for my OSD and must say that GPS height values are really bad for that. Even with 9-10 satellites in view are results on a multicopter not acurate and some way very slow. I leave my GPS (MT3329) for some minutes on the ground to get 8+ satelites, then save home and climb my tricopter to e.g. 20m. I'm already about 20m high but displayed height value remains e.g. 2m. I keep the same 20m height and value slowly (10+ seconds) goes to e.g. 15m. Then I descent back to ground and sit there for some seconds while OSD keeps display e.g. 12m. After 10 more seconds height value ends at e.g. 6m or another time at -3m and remains there.
GPS probably use some software filtering to make that inacurate height value looking better.
Another observation is that when GPS is moving also in 2D (in a plane), height seems to be changing faster but when XY position remains the same (copter climbing straight up), height value seems to be very lagy.
Just few observations - I'd be glad if someone can confirm or correct it...
Roman

[nhadrian]

Using GPS data for Alt hold is not a solution.

My oppinion is that for good alt hold we need a PID loop on baro data, plus corrected with a P-term of acc-z (for fastinfluences), as I said many times on this forum. I wish somebody could help and write this, but I sware once I'll have time during winter and I'll learn how to do this!!!!!!

BR
Adrian

[shikra]

Agree - GPS vertical altitude is very poor. I see variations +/-2 m in very short term even with 10 sats. Horizontal is much better.
Over time, both are worse.

[carlonb]

+1
I agree, GPS height is not accurate I saw +/- 10 mt on OSD.
With mobidrone OSD and MWii 2.1 code (with baro BMP085) I saw that the altitude is almost accurate +/- 1mt.
I started from ground (0mt), then fly at 15mt...20mt of height (up and down many times) and finally land after 6 minutes. Final heigth at ground indicate 0mt again on OSD.
For me is enough.
Bye

[carlonb]

Hi guys,
Just few minutes ago I saw an exciting phenomena with my hexa connected to WiiGui observing the baro altidtude...
Started the GUI I had about 160mt at ground.
After 10 min about I noted a quick lowering of the altitude...strange... ten minute later the altitude lowered at about 143mt without do nothing.
I realized may be weather changing....after ten minute more i hear a strange noise out of the closed window of my room, I opened the window and saw a strong wind storm...after few minutes more starts an heavy rain.
Ok...we have 25 min weather forecasts...

Bye all, Carlo

[dramida]

Sudden variation of pressure announce weather changes. Funny thing happened:)
I wonder how the inflight noise spectrum of bmp085 and MS6511 look like. Also accelerometer Zaxis would pe interesting. The spectrum gives us an exact LPF where stands the informations and where is noise. I will yry to log the values with winGui and use some old mathlab skills.

[Crashpilot1000]

@nhadrian:
I am very interested implementing your mathematics into a working althold. Please disclose them as a diagram or something. I still have problems interpreting the accZ right, because it is difficult to eleminate the "swing back" values when the movement stops and not to interpret them as new movement. Do you have a solution for this? I fiddled with accz extremes and moving averages over time etc. The accz extremes approach seemed to be more promising but its still a long way for me. Is there a good publication of inertial height navigation regarding the accz integration?

So long
Kraut Rob

[jevermeister]

Hi,
the GPS height data may not be accurate but it is very slow, it is the same trick that is used in stable mode, we use acc and gyro there too.
The gyro is VERY accurate but has ha bad drift, the acc is slow but is not drifting. I think the same principle can be used in an gps aided altitude mode.

Nils

ps.: The loop time play a big role here too, I made the experience, that while using serial GPS the altitude hold mdoe is rather useless because of the spikes in loop time.

[Crashpilot1000]

I sum it up:
2 people (dramida & nhadrian) hold the holy grail of alt hold in their hands but haven't been able to disclose any concise details on the last 4 pages. Just some general ideas, you could get from anyone in the street.
The only constructive comments i can see from: copterrichie, mahowik, pm1, jevermeister they all got their hands dirty and tried something !!
Than we have the important observation that a change in weather can alter the airpressure - i think now it's time to rethink our complete strategy. The opinion of warthox concerning the baro is also very important because he is known to be the driving force behind alt hold improvements and doing highspeed flips makes him an expert for every copter related subject!
And then the rewarming of GPS - terrific. One look at the althold performance of Naza could tell you instantly that GPS is def. not necessary. Even the chief himself commented on this clear subject.
Never forget: Bold characters always underline that you have something really important so say.

That's my summary of the last 4 pages.

So long
Kraut ROB

P.s: Put on your sunglasses and get your popcorn ready now!

[jevermeister]

Hi,
Not shure if I was the target for CAPS

I did not know that the Naza was that accurate.
It is a shame, that Mikrokopter and DJI is a closed source. They benefit from miltiwii open soirce but are not willing to share.
A friend of line is using a MK and the althold was very good. I do not know if MK is using baro in the navcontrol bob or simply gps.

I would prefer altitude hold without gps. as stated before:serial gps even brings the mega 2560 to it's knees.I will build myself a i2c gps in winter.

nils
ps.: I am out of popcorn

[copterrichie]

I would prefer altitude hold without gps. as stated before:serial gps even brings the mega 2560 to it's knees.I will build myself a i2c gps in winter.

nils
ps.: I am out of popcorn

Some claims that the i2c gps is nothing more than a hack but I don't agree. I think the longevity of this project is to offload the GPS, sonar, Telemetry and other non-essential flight functions to a secondary controller. The problem becomes the communication link between the controllers which is limited to one i2c bus.

[dramida]

I sum it up:
2 people (dramida & nhadrian) hold the holy grail of alt hold in their hands but haven't been able to disclose any concise details on the last 4 pages. Just some general ideas, you could get from anyone in the street.
......
So long
Kraut ROB

As i mentioned in first post, i am not a Arduino C programmer, i played with leds, switches, serial com and servos and that's all. The algorithm behind altitude hold that i suggested is constructed around some obvious facts as:

-DJI uses accelerometer for Altitude Hold (try to push it in flying and will react instantaneously to your push);
-Every FC has about same sensor accuracy (even use the same baro, gps and Acc/giro) but has significant better results due to program;
-GPS has an instantaneous 2 m error accuracy in X-Y plane and at least same for altitude Z so is far worst than baro;
- More time we collect data from a sensor, more accurate the reading is;
- Accelerometer offers an instant respunse to movement but over time it can't ceep the altitude fixed because does not sense slow movements below the level of its noise
- Barometer MS6511 has an noisy output but when averaging the output for a few seconds, the altitude is clearly revealed. This could compensate the long term drift of altitude keept by accelerometer.

I hope this remarks could inspire a nice programmer to improve the Alt Hold algorithm.
And BTW, with 3.5, 0.15, 10 i have a quite good alt hold with slow variations around 50 cm.

[vpb]

Hi dramida, about your idea - the ALT hold solution and the ALT PID 3.5 0.15 10, what is your tuning procedure? I want to test your idea. Tks!

[dramida]

Hi dramida, about your idea - the ALT hold solution and the ALT PID 3, 0.15, 7 what is your tuning procedure? I want to test your idea. Tks!

I just checked and the actual values Alt PID are 3, 0.15, 7, i played a lot and i forgot the changes since it works satisfactory even in forward flight. Please see video and hear the engines when alt hold and gps pos hold are activated

http://www.youtube.com/watch?v=B4D2CRxx7zk



My easy Alt Hold setup procedure based on actual 2.1 average algorithm:

1-Put zero on Alt P and Alt I. Then play with D (in reality D term represents the proportional accelerometer influence over thrust) and trying to find a sweet spot where copter maintains altitude on short term. You know you got a good "D" value when pushed down or up, the copter reacts instantly to your change and keeps its altitude. Unfortunately, there is one single proportional term to this loop, so it is less perfect than it could be (PID).

2- After finding the sweet spot accelerometer response D, any other influence on thrust like baro (P, I) would make the copter overshoot. So we need to lower a fraction of D and complete the response with Baro PI loop. My sweet spot of D is 15 and i lowered it to 7. You could try some 50 to 80% from the initial D find, my opinion is that D should be more than half.

3-Now leave alone the shrinked D (I=0 during this time) and start increase P term until you got altitude BIG oscillation with an continous increasing amplitude . When you got altitude self increasing oscillation, half the P. Now the copter should hold its altitude in a range of about 1 meter because the P only term loop can't fix the altitude error exactly.

4- Leave alone P and D found as described above and increase I term until the copter stays in a range of 20 cm up or down. 20 cm is the noise of inflight baro MS6511. That's it.


Note:
The 20cm result is valable using MS6511 baro.
The baro shoud be protected by direct sunlight and direct wind.
All test below must be made at an altitude higher than 1.5 meters to avoid ground effect.
All tests should be made on a calm weather.
Blutooth data connection with GUI is invaluable for graph visualisation of Baro pressure or even inflight parameter settings as i did. I let the copter on GPS Pos Hold howering above a field and i played with the parameters from my laptop.

[vpb]

Hi dramida, thanks for your instructions.

I've just tested tuning ALT recently for the method I read somewhere (maybe yours), yes luckily it's like your way, I put P&I at 0, and play with D. I increase big step to 30, it yoyo-ed , I decrease some steps downto 22, it looks fine. But I couldnot continue due to late evening and the black cloud was going to cover my head. I'll continue to tune tomorrow with your instructions.

As I see, it seems that D play the main part in ALT HOLD. And yes, my board is AIO, so the baro sensor is ms6511.

[copterrichie]

Than the Fixed cycle time that hamburger introduced should be the magic patch.

[rbirdie001]

Hi!
I did comparation inflight test between GPS and BARO altitude measurement and it again confirmed that GPS really can't be used (for anything including altitude hold).
My goal was to see on OSD during FPV how high I'm in reality because I still have problems to guess it from the video and GPS meauserement turned to be unusable for that.
Anyone interested can see test here: http://www.youtube.com/watch?v=Ux266uz0WQ0
@ Dramida: Can you please share, where you found description for what is the D term in 2.1 alt hold really used?
While I was playing with with AH PIDs in 2.1, I got more feeling that lovering D "limits" influence of BARO to throttle.
From my observation high D causes "dramatic" responses on the throttle and low D makes AH more smooth but maybe too weak to compensate when battery is getting low.
I ended up with AH D=3 and my Tri is quite stable.
Just observations to discuss, I can be wrong...
Anyway I'll surely try your method of PID tunning!
Roman

[vpb]

Hi rbirdie! What OSD are you using? Very nice video but why you didn't try flying much higher, about 100-200m, I think the GPS alt is just for reference.

[vpb]

I just checked and the actual values Alt PID are 3, 0.15, 7, i played a lot and i forgot the changes since it works satisfactory even in forward flight. Please see video and hear the engines when alt hold and gps pos hold are activated

http://www.youtube.com/watch?v=B4D2CRxx7zk

Very very "beautiful" test-flight dramida! Were you the pilot? Look like it flied very high, and the pilot controlled visually not FPV, how could the pilot see the kit ?
For the BARO hold, can you explain me some more?
- With Baro on, at which throttle level, the FC will try to maintain the height or the ALT PID will play the role? With higher throttle as we want to reach higher height or lower throttle as we want to landing, will it try to fight this?
- Do you have any experience on ALT_HOLD_THROTTLE_NEUTRAL_ZONE? It reads "neutral zone of throttle stick during altitude hold", the stock setting is +/- 20, and it will be added directly to throttle, like 1500+/- 20? I read some little info that it's the sensitive zone, for exp 1480-1520, what is sensitivity meaning here? the throttle stick will be very sensitive or in-sensitve in the throttle zone?

I always fly with BARO on, increasing height is easy but decreasing is a bit not smooth, maybe the ALT is not tuned, or I land too fast.
Thank you!

[mahowik]

Hi guys!

This weekend I became restless again that we are worse than others!

I have already tried and spent too much time 6-8 month ago without real success, but today I have collected all my knowledge again and made second attempt.
In two words! Now I can say "I got real results with baro+acc Alt Hold!"

http://www.youtube.com/watch?v=nePjkz_leR8

It's:
- take into acount inclinations
- doesn't see horizontal accelerations
- keeps altitude on the wind (tried with 15-20km/h)
- has very good reaction for pushing
- has accuracy +/-20..40cm in calm weather according to baro accuracy
- has accuracy about +/-1m during the flights... flying 4 packs with initial PIDs without any tuning because I was too satisfied with results

Code: Select all

#define ACC_LPF_FOR_VELOCITY 10
static float accLPFVel[3];

static t_fp_vector EstG;

void getEstimatedAttitude(){

.....................................
.....................................

    #if defined(ACC_LPF_FACTOR)
      accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
    
     accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f/ACC_LPF_FOR_VELOCITY)) + accADC[axis] * (1.0f/ACC_LPF_FOR_VELOCITY);


Code: Select all

#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY      4000000  // 4 sec initialization delay
#define BARO_TAB_SIZE   40

#define ACC_Z_DEADBAND (acc_1G/50)

void getEstimatedAltitude(){
  uint8_t index;
  static uint32_t deadLine = INIT_DELAY;

  static int16_t BaroHistTab[BARO_TAB_SIZE];
  static int8_t BaroHistIdx;
  static int32_t BaroHigh;
 
 
  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  uint16_t dTime = currentTime - deadLine;
  deadLine = currentTime;
 

  //**** Alt. Set Point stabilization PID ****
  BaroHistTab[BaroHistIdx] = BaroAlt/10;
  BaroHigh += BaroHistTab[BaroHistIdx];
  index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
  BaroHigh -= BaroHistTab[index];
 
  BaroHistIdx++;
  if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;


  //EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
  EstAlt = EstAlt*0.6f + (BaroHigh*10.0f/(BARO_TAB_SIZE/2))*0.4f; // additional LPF to reduce baro noise
 
  //P
  int16_t error = applyDeadband16(AltHold - EstAlt, 10); //remove small P parametr to reduce noise near zero position
  BaroPID = constrain((conf.P8[PIDALT] * error / 100), -100, +100);
 
  //I
  errorAltitudeI += error * conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  BaroPID += (errorAltitudeI / 500); //I in range +/-60
 
 
  // projection of ACC vector to global Z, with 1G subtructed
  // Math: accZ = A * G / |G| - 1G
  float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
  //int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - acc_1G;
  //int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - 1/invG;
  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
  accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);
  debug[0] = accZ;
 
  static float vel = 0.0f;
  static float accVelScale = 9.80665f / acc_1G / 10000.0f;
 
  // Integrator - velocity, cm/sec
  vel+= accZ * accVelScale * dTime;
 
  static int32_t lastBaroAlt = EstAlt;
  float baroVel = (EstAlt - lastBaroAlt) / (dTime/1000000.0f);
  baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
  baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero 
  lastBaroAlt = EstAlt;
  debug[1] = baroVel;
 
  // apply Complimentary Filter to keep near zero caluculated velocity based on baro velocity
  vel = vel * 0.985f + baroVel * 0.015f;
  //vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
  debug[2] = vel;
  //debug[3] = applyDeadbandFloat(vel, 5);
 
  //D
  BaroPID -= constrain(conf.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
  debug[3] = BaroPID;
}

int16_t applyDeadband16(int16_t value, int16_t deadband) {
  if(abs(value) < deadband) {
    value = 0;
  } else if(value > 0){
    value -= deadband;
  } else if(value < 0){
    value += deadband;
  }
  return value;
}

float applyDeadbandFloat(float value, int16_t deadband) {
  if(abs(value) < deadband) {
    value = 0;
  } else if(value > 0){
    value -= deadband;
  } else if(value < 0){
    value += deadband;
  }
  return value;
}

float InvSqrt (float x){
  union{ 
    int32_t i; 
    float   f;
  } conv;
  conv.f = x;
  conv.i = 0x5f3759df - (conv.i >> 1);
  return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}

int32_t isq(int32_t x){return x * x;}



How it works:
1. new array for acc values (accLPFvel) was created to prepare specialy filtered ACC data for alt hold, where fast acceleration still visible but not noise/vibrations. Because accADC is too noisy raw data even with MPU6050_LPF_42HZ filter, but accLPF has too big delay and shift of phase...
2. was used BaroHistTab and additional LPF to reduce baro noise (LPF too soft to doesn't lost the phase, i.e. to avoid increase of the delay )
3. then "P" calculated based on baro with applied deadband (=10) to reduce noise near zero position
4. then "I" calculated as before... nothing special here
5. accZ calculted as projection of ACC vector to global Z, with 1G subtructed (accZ = A * G / |G| - 1G) where (EstG vector taken from IMU). And again apply deababnd for 2% of acc_1G (acc_1G/50).
6. then vertical velocity calculated from accZ in integrator (cm/sec)
- and here we got MAIN issue: HOW TO KEEP VELOCITY NEAR ZERO BY USING INTEGRATOR?!
...answers in points below
7. baro velocity calculated from filtered baro values, BUT it still very noisy... so applying of deadband (=10) little bit solve the problem
8. !!! then we apply Complimentary Filter (CF) to keep the calculated velocity based on baro velocity (i.e. near real velocity). By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
9. So then calculated velocity from CF (with applyed deadband = 5) used for D

Code is still required some performance optimizations (e.g. replace float calculations to integer where it's possible) but in general it's good I think...
In debug you can see:
- debug[0] - accZ
- debug[1] - baro velocity
- debug[2] - velocity after CF (velocity from acc corrected by baro velocity through the CF)
- debug[3] - sum of the P+I+D

I tried PIDs 5.0-0.030-30 from first attempt and was satisfied with 4 packs of flying... so I hadn't tried to tune that, but it make sense

Note: I'm used mpu6050+ms5611 (AIO from rctimer) with PCB just screwed on the board, BUT i'm not sure about other accelerometers like ADXL345 (too sensitive to the noise) or bma020 with low 1G resolution (64 only) for 8G range... bma180 should be good I suppose.

thx-
Alex

[timecop]

I moved this code to baseflight and tried it out. Turning on baro just makes quad instantly shoot up in the air.
I see some attempts at keeping altitude, but its very erratic and 'strong', I guess due to P=5.0.
Changes are here:
http://code.google.com/p/afrodevices/so ... tail?r=214
I'm pretty sure I carried everything over same way - and I tried this with looptime=3000 which means it should be close to timing of arduino stuff.

Maybe another set of eyes can spot something I missed.