Alt. Hold Ideas and discussion

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: Alt. Hold Ideas and discussion

Post by jevermeister »

Hi there,
I try to get forward with the baro mode.
I have all PID (VEL/ALT) set to zero and start incrementing ALT P.
I circled into 11 as a good value, everything else is set to 0.

The copter oscilates +/-0,5m in the air, I think this is very good for the beginning but can be optimized.


But now I started thinking: Is this the correct approach? I read that one should start with vel.

Can you tell me what vel does exactly? I understood that it changes the velocity of the copter, but does that even have an effect if baro mode is turned off? I don't want to loose agility when baro is off.

I am still not happy with my shielding, I strapped the sensor to my centerplate and put some foam onto it and shield it with adhesive tape. I read about a min distance to the case, anyone can tell me any details?

nils

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Alt. Hold Ideas and discussion

Post by ziss_dm »

Hi jevermeister,

But now I started thinking: Is this the correct approach? I read that one should start with vel.


I'm usually adjusting PID's in the following order:
1) VEL D
2) VEL P
3) ALT P

More details here: viewtopic.php?f=7&t=363&start=30

How it works:
Complimentary filter: Mixing data from baro and accelerometers, to get estimation of vertical speed and altitude.
Velocity PID: Uses estimated vertical speed and adjusts throttle to hold desired vertical speed.
Altitude PID: Uses estimated altitude and adjusts vertical speed (Outpud is feeded to the Velocity PID)


But in the latest 1.8 has the following changes:
- the output of Altitude PID is feed forwarded. That means that you can use the ALT PID alone.
- The velocity PID is actually PD controller (I term is ignored)
- The altitude PID is actually PI controller (D term is ignored)

regards,
ziss_dm

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: Alt. Hold Ideas and discussion

Post by jevermeister »

We should write stuff like this into a beginners guide,
I had a hard time finding this, and people are asking it over and over again in rcgroups, even in PM.

I would volunteer for that, but I am not an expert in all the funtions, but if you guys would help me...

Nils

crashlander
Posts: 506
Joined: Thu May 05, 2011 8:13 am
Location: Slovenia

Re: Alt. Hold Ideas and discussion

Post by crashlander »

ziss_dm wrote:Hi,
1) Set Vel. and Alt. PID values to the zero

O.K.
ziss_dm wrote:2) Start increasing Vel. D. You should start feeling that quick throttle stick movements are dampened. Increase until really fast osculations starts in response on quick throttle stick movement. Than drop a little bit. My current value: 30

I can't see no oscillations from D=0 to D=50 (max.) so I settled at 37
ziss_dm wrote:3) Start increasing Vel. P. Throttle response should be softer and softer and quad should start holding altitude. The tricki part here, that CF using baro data for vertical speed and it is fluctuating, but you can roughly assest amount from GUI. The good values for Vel. PID should give you the following behavior: After flicking switch the quad should stop and slowly drift.

My current P=0.3 With anything higher etc. 5,6,7,... my Wii starts to gaining hight really fast (or sometimes drops like stone) without stop, direction of altitude goes into opposite direction of slow drift at the time of switch change (alt. hold activation).
ziss_dm wrote:4) Start increasing Alt. P. The quad should stabilize around set point. Increase until it start oscilating, the value could be really high: I have 20
5) Start increasing Alt. I. The stabilization should become more precise.
6) experiment.. :)

It would be nice to collect statistics of working PID values, as we need to decide do we really need full PIDs for both Alt. and Vel. and probably re-adjust PID ranges.

regards,
ziss_dm

After 4 LiPo packs I stopped here and will try to change Alt. P and D in following days.

So my current state is Vel. D=37 and Vel. P=0.3 with that my Wii drifts from ground (where bounces) up to 5m high.

Any comments or suggestions are welcome!

My setup is:
-Atmel AVRSBIN1,
-BMP085
-Ardu. ProMini,
-Turnigy Plush 10A,
-2213N 800Kv motor,
-10x4.7 props.,
-QuadX (simetrical), 60cm motor to motor, 900g flying weight
-ITG3200_LPF_20HZ or ITG3200_LPF_42HZ

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: Alt. Hold Ideas and discussion

Post by mr.rc-cam »

So my current state is Vel. D=37 and Vel. P=0.3 with that my Wii drifts from ground (where bounces) up to 5m high.
Any comments or suggestions are welcome!
My setup is:
-Atmel AVRSBIN1,
-BMP085
-Ardu. ProMini,
-Turnigy Plush 10A,
-2213N 800Kv motor,
-10x4.7 props.,
-QuadX (simetrical), 60cm motor to motor, 900g flying weight

I'm having the same alt-hold problems. My setup is very similar to yours but my Quad is a bit heavier (~1300g). I've noticed that the users that have had alt-hold success have models that are smaller, lighter, and more agile.

I've used the recommended procedure, also tried every combination of PID settings, some witchcraft and chants, and nothing yet has helped me achieve useful alt-hold. So I am hoping you solve your problem and share your success!

copterrichie
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Alt. Hold Ideas and discussion

Post by copterrichie »

MultiX4 ALT Hold no GPS

http://www.youtube.com/watch?v=RA1a-Ya4xag&feature=colike

Altitude hold with BOSH BMP-085

ultrahigh resolution mode 17/25.5 millisecond refresh rate
temperature compensate calculation

pressure = p + (x1 + x2 + 3791)/4
b4 = (b7 / b9) * 2
x1 = (p/8) * (p * 16); ( set to 65535 step )
x1 = (x1 * 2038) *16;
x2 = (-7957 * p) *16; ( res For Thailand )

Katch
Posts: 280
Joined: Thu Aug 04, 2011 1:44 pm

Re: Alt. Hold Ideas and discussion

Post by Katch »

Just starting to try and get my Alt hold dialed in too.

Having similar experiences as mr.rc and crash. Getting a yo-yo-ing quad but no real hold yet. This is on the MS version of Fabio's freeIMU.

Feels like the motor compensation takes too long to respond and then over-corrects.

Not very good at PID tuning yet so it's a bit like throwing dice.

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: Alt. Hold Ideas and discussion

Post by Alexinparis »

Hi,
Before trying to tune the VEL PID, I would suggest to tune only the ALT P parameter and leave everything else to 0 (ATL I, ATL D, VEL P, VEL I, VEL D = 0)

Katch
Posts: 280
Joined: Thu Aug 04, 2011 1:44 pm

Re: Alt. Hold Ideas and discussion

Post by Katch »

Thanks Alex,

I'll try that next time I get out in the field.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Alt. Hold Ideas and discussion

Post by ziss_dm »

Hi mr.rc-cam,

With the BMP085 baro sensor, and the patched (but otherwise stock) code, I could reliably achieve ±5 meter Alt-Hold precision. This is awesome considering the performance I was getting with the original code. And left alone I think it would make most users very happy. But I tried to squeeze out more performance with the BMP085. With some additional code changes I am now experiencing reliable ±2 meter precision, which is not bad at all. I think it could be less than 1 meter with the EagleTree Baro sensor, but I will continue to use the BMP085 for the time being.

Some basic things that helped me out:
Added baro data filter.
Rescaled ALT I term.
Reworked I term integrator.
Changed some constrains.

In case anyone wants to try it, I can add my experimental Alt-Hold code to a clean MultiWii file set. But I want to wait for the next formal release that has all the latest fixes to V1.8p_2. Any idea when that will be?


Can you just post your changes with comments for now? ;)

Added baro data filter.

Is it just LPF? Have you noticed changes in raw baro graph (debug1) in GUI? Have you increased refresh rate accordinly?
When I was experimenting with that, I have found that BMP noise is not really gaussian and all types of "averaging" filters not really working. Better results I was getting just by reducing refresh rate.

Reworked I term integrator.

Do you mean decreased update rate?

regards,
ziss_dm

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

Re: Alt. Hold Ideas and discussion

Post by mr.rc-cam »

Hi mr.rc-cam, Can you just post your changes with comments for now? ;)

Sure. I'm still refining the code so this is a good time to discuss it in case we can find areas to improve. When Alex publishes the next formal release (with the fixes to V1.8p2) I will post a file set with these edits in it so others can try it out.

Here's a summary of some changes I've made so far:

[1] Baro Data Filter:
I'm currently using modified-medium (Olympic scoring) averaging to remove some of the random values from the BMP085 baro sensor. It helps out, but the BMP085 baro data is still nothing to brag about. This is the code:

Code: Select all

#define BUFFSZ 4                              // DO NOT Change this value.
float baro_filter(float fdata)
{
    int8_t i;
    float d_big = 0;
    float d_small = 0;
    static uint8_t offset = 0;
    static float filter[BUFFSZ];

    filter[offset] = fdata;                  // Store filter data in table.
    fdata = 0;
    for(i=0; i<BUFFSZ; i++) {        // Add up all data values.
   fdata += filter[i];
    }

    d_small = filter[0];                        // Prime the sort engine.
    d_big = filter[0];              // Prime the sort engine.
    for(i=1; i<BUFFSZ; i++) {            // Find extreme values.
   if(d_small > filter[i]) {        // Find smallest value.
     d_small = filter[i];
   }
   if(d_big < filter[i]) {           // Find largest value.
     d_big = filter[i];
   }
    }

    fdata = fdata - (d_big + d_small);   // Remove extreme values.
    fdata = fdata / 2.0f;                  // Obtain avg using medium.

    if (++offset >= BUFFSZ) offset = 0;   // Wrap around.
   
    return(fdata);
}


[2] I rescaled the ALT I term to reduce its weighting. From my recent tests I think I needs to be reduce some more, but that can come later. Here are my current values:

Code: Select all

      errorAltitudeI = constrain(errorAltitudeI,-4000,4000); // Was previously ±5000.  
      ITerm = (int32_t)I8[PIDALT]*errorAltitudeI/8000;  // Was previously decimated by 4000.


[3] I found the I term would regularly overshoot the intended correction. So data squelching was added (error accumulation is no longer symmetrical):

Code: Select all

      //**** Alt. Set Point stabilization PID ****
      error = constrain((AltHold - EstAlt)*10, -100, 100); //  +/-10m,  1 decimeter accuracy
      errorAltitudeI += error;                  // Integrate error.
     
      if(error>0 && errorAltitudeI<100) {       // Overshoot protection.
          errorAltitudeI += 10;                 // Quickly Squelch I error.
      }
      else if(error<0 && errorAltitudeI>100) {  // Overshoot protection.
          errorAltitudeI -= 10;                 // Quickly Squelch I error.
      }


[4] The throttle constrain was a bit limiting so I expanded it, as follows:

Code: Select all

rcCommand[THROTTLE] = initialThrottleHold + constrain(AltPID - (PTerm - DTerm),-125,+125)  // Was previously limited to ±100. ±150 may be better choice for some models.


[5] My model is a battery pig. If I hovered in the same spot for more than 45 seconds the Alt-Hold begins to lose altitude. This is because of the PID code constrains don't allow enough headroom in the throttle calculations as the battery voltage decreases. The reasonable solution was to add voltage loss compensation to alt-hold. The GUI's unused ALT D term is used to adjust the weight of the voltage compensation. This works great for my model and if it is not needed the D value can be set to zero. Here is the code:

Code: Select all

      rcCommand[THROTTLE] = initialThrottleHold + constrain(AltPID - (PTerm - DTerm),-125,+125);  // This is the existing throttle PID.

      // **** Battery Voltage Compensation *****
      #ifdef VBAT
       if(initial_vbat > vbat) {        // Battery voltage is decreasing.
        delta_vbat = initial_vbat - vbat;  // initial_vbat value was set when alt-hold was enabled.
       }
       else {
          delta_vbat = 0;              // Battery voltage has not decreased.
       }
       vbatThrCompensation = (D8[PIDALT] * delta_vbat)/4;  // Use ALT-D GUI value for compensation weight.
       vbatThrCompensation = constrain(vbatThrCompensation,0,+150); // Limit the compensation for safety.
       rcCommand[THROTTLE] = rcCommand[THROTTLE] + vbatThrCompensation;  // Add the voltage compensation to the throttle.
      #endif

User avatar
dramida
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest
Contact:

Re: Alt. Hold Ideas and discussion

Post by dramida »

Testing the VEL parameter, i shaked up and down the octo and saw that Z axis effect on engines was not the same. Some times the amplitude of engine speed variation was larger, other times, insignificant.Test conducted using V1.9 MWC with trusted acc, ofcorse, ADXL345, ALT pid 0,0,0, vel PID 20,0,0

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

As for me, I got very good results +/-0.5..1m when it's not windy outside

PID ALT: 3.0 - 0.015 - 0
PID VEL: 5.0 - 0.000 - 45

User avatar
dramida
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest
Contact:

Re: Alt. Hold Ideas and discussion

Post by dramida »

What accelerometer and software version are you using? i'll test your settings tomorrow, thank you.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

dramida wrote:What accelerometer and software version are you using? i'll test your settings tomorrow, thank you.

bma020(set to 8g range: acc_1g=63)
software 1.9

most of all you should keep high VEL D... then play with VEL P and ALT P

thx-
Alex

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

One more note about alt hold stability...

At the beginning I got a good results and stable alt hold with using accSmooth instead of accADC in getEstimatedAltitude().
And today I tried to use original version (i.e. with accADC) and have found out that is not working for me... It became very unstable and unpredictable...

Code: Select all

//InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale +  AltErrorI / 1000;
InstAcc = (accSmooth[YAW] * (1 - acc_1G * InvSqrt(isq(accSmooth[ROLL]) + isq(accSmooth[PITCH]) + isq(accSmooth[YAW])))) * AccScale +  AltErrorI / 1000;   


So in case of you have unstable alt hold you can try to use accSmooth as solution.

thx-
Alex

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Alt. Hold Ideas and discussion

Post by ziss_dm »

Hi Alex,

With normal ACC bandwith (10-25 Hz)?

regards,
ziss_dm

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

ziss_dm wrote:With normal ACC bandwith (10-25 Hz)?


Bandwidth 190 Hz
but ACC LPF ~= 2..3Hz

Code: Select all

static float accTempF[3];
.......
        #define lfpFactor 0.987f
        accTempF[axis] = accTempF[axis]*lfpFactor + accADC[axis]*(1.0f - lfpFactor);
        accSmooth[axis] = accTempF[axis];


Yes! I see what you wanna to say! ;)
It (190hz) was too high for the altitude calculation?
In any case with LPF 2..3Hz it gives very stable ALT hold...

thx-
Alex

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Alt. Hold Ideas and discussion

Post by ziss_dm »

Hi Alex,

Not really that.. ;) I think, increased bw is good for Alt.Hold. (in theory)
But integrator currently skipping cycles, assuming bw: 20Hz. So, for increased BW, you need to adjust that. (busically integrate every cycle, for around 200Hz)

BTW: What a waight of your model? What power system you have? (the increased bw is good for light and powerful models, I think...)

regards
ziss_dm

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

My config quadX:
- turnigy 2209 28turn 1050kv 15A Outrunner
- props HK 9x5
- plush 18a
- mega1280 + itg3205 + bma020(bw 190hz + LPF 2..3Hz) + hmc5883 + bmp085
- accum 3S 20C 2200mah or 2650mah

Total weight ~800..900g

https://picasaweb.google.com/lh/photo/G ... directlink

thx-
Alex

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Alt. Hold Ideas and discussion

Post by ziss_dm »

Hm...

2Hz is enough? How you testing? I'm usually do the following tests, to assest dynamic:
1) Enter to alt. hold with vertical speed (better to do it with baro clamped to constant value)
- go up - flick switch - should stop
- go down - flick switch - should stop
2) Set a set point 2-3 meters higher than current position

regards,
ziss_dm

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

ziss_dm wrote:2Hz is enough? How you testing? I'm usually do the following tests, to assest dynamic:
1) Enter to alt. hold with vertical speed (better to do it with baro clamped to constant value)
- go up - flick switch - should stop
- go down - flick switch - should stop
2) Set a set point 2-3 meters higher than current position


probably I'm mistaken but for the first look intuitively it give 2..3hz with cycle time ~3000
I have found lfpFactor=0.987 empirically when float LPF almost cut the ACC noise but still has enough of speed for IMU...

Code: Select all

        #define lfpFactor 0.987f //(1.0 / ACC_LPF_FACTOR)
        //lfpFactor = 0.9f + constrain(rcData[AUX2]-1000, 0, 990)/10000.0f; // k=0..0.99
        accTempF[axis] = accTempF[axis]*lfpFactor + accADC[axis]*(1.0f - lfpFactor);
        accSmooth[axis] = accTempF[axis];

I'm also using slightly increased GYR_CMPF_FACTOR=340.0f
It reduces the ACC value and increases the system feedback

so the answer: for me it enough :)

p.s. will do the tests as you described and post feedback soon..

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

ziss_dm wrote:2Hz is enough? How you testing? I'm usually do the following tests, to assest dynamic:
1) Enter to alt. hold with vertical speed (better to do it with baro clamped to constant value)
- go up - flick switch - should stop
- go down - flick switch - should stop
2) Set a set point 2-3 meters higher than current position


I have done the 1st test this night. Yes, it looks a little bit crazy! Imagine some guy playing with copter IN THE NIGHT near the supermarket (it has big free area)... :))))
So the results:
- go up with vertical speed ~1m/s => flick switch => it's starting do reduce the vertical speed and after 1..2 meters smoothly goes down to position where the alt hold was activated...
- particularly the same for "go down"

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

Hey Alex!

Why you remove velocity PD controller (i see that in trunk)?!
I'm sure it will not work without PD (particularly without D part)... I'm trying to integrate the sonar now and (this sensor has good precision... ~0.3cm), but i didn't get stability with only PI regulator (played with PID params and diff code variations about 2 weeks) but hasn't good results... it always has overshoot or low power to keep it stable in constant altitude...
So I suppose to compensate the altitude inertion I think we should use PI-PD as was before (i.e. ziss is right here :) ). You exactly know how our PI-PD regulator works for stable mode, where for PD the high frequency gyro data is used. For the ALT hold case it was velocity based on Z acc axis...

thx-
Alex

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: Alt. Hold Ideas and discussion

Post by Alexinparis »

Hi,
I removed it because:
- most of the working alt hold configs don't use the VEL PD
- velocity estimation is not very accurate (doesn't return to 0 when the copter is not moving), even with 8G settings

But I understand the analogy with GYRO/ACC. It's maybe be a temporary code.

Scotth72
Posts: 23
Joined: Sat Jan 21, 2012 4:11 am

Re: Alt. Hold Ideas and discussion

Post by Scotth72 »

I have two quads that hold altitude quite well using the VEL PD. Both are of different weights, and power systems. They just needed some fine tuning to get them dialed. I get up to a meter of fluctuation, in the wind. Most of the time less than a meter. BMA180 for accel.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

Scotth72 wrote:I have two quads that hold altitude quite well using the VEL PD. Both are of different weights, and power systems. They just needed some fine tuning to get them dialed. I get up to a meter of fluctuation, in the wind. Most of the time less than a meter. BMA180 for accel.

just for the statistics... what PIDs do you use for the ALT and VEL?

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Alt. Hold Ideas and discussion

Post by ziss_dm »

Hi Scotth72,

Can you also share your tuning methods? ;)

regards,
ziss_dm

User avatar
shikra
Posts: 783
Joined: Wed Mar 30, 2011 7:58 pm

Re: Alt. Hold Ideas and discussion

Post by shikra »

Actually mine work very well too.
No tuning method for me - pure trial and error. Very close to default settings

Maybe it is luck, but the one thing maybe that is possibly a hint - I use a heli tx and spend time to make sure the throttle curve suits me and most relevant - the centre "notch" point of the throttle is bang on the hover. So the code maybe has an easier time to start from. Just putting tx to throttle midpoint makes it very close to hover. Not sure it would be so accurate without heli tx


Not really looked at Baro code as not of interest to me until GPS hold is there (so now becoming very interested :) :)
(in other words -

I wonder following suggestions...
- Tuning mode. No idea how to implement easily...
- A constrained +- MAXBAROVAR change value around a BAROMID value. Set in sketch following testing.
- Moving average - learn BAROMID
- Using vbat to help determine any bias over time. Horrible, but my bat changes 20% from full-->flat. Most is within 10% variation though
- Reset I to zero when passing midpoint

For last one...
Has come from some different FPV/level mode flying options I have been exploring...
Because of the relatively long time to recover to the desired position I think I is maxing out to its constrained value. Even when it passes the required point, the accumulated I is causing an issue with overshoot.
I wonder what the result would be to reset I to zero each time it passes midpoint. Maybe can run a higher I value without bad effects?

Again - I'm saying this without reading the code... I way be completely wrong with interpreation.....

Scotth72
Posts: 23
Joined: Sat Jan 21, 2012 4:11 am

Re: Alt. Hold Ideas and discussion

Post by Scotth72 »

I took the defaults someone posted on this thread: viewtopic.php?f=7&t=363&start=10#p6619.

I tuned from there. A screenshot from one of my quads.

Image

I had an oscillation, so I turned down the ALT P. I then messed with the other parameters to fine tune.

I had little luck getting it to work until I turned on trusted accel z. Once enabled, the quad seemed to react quite a bit faster to up/down movements. Both baros are covered. One has a ear bud foam over it, the other a cotton ball. The larger quad also has a cd spindle case cover, with 2 3mm holes in in for air passage. The cd case seems to help with wind gusts.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

I have similar PID settings with good alt hold which I have put to my custom sketch

P8[PIDALT] = 30; I8[PIDALT] = 10; D8[PIDALT] = 0;
P8[PIDVEL] = 55; I8[PIDVEL] = 0; D8[PIDVEL] = 45;

http://forum.rcdesign.ru/blogs/83206/

User avatar
guru_florida
Posts: 45
Joined: Sat Mar 26, 2011 4:51 am

Re: Alt. Hold Ideas and discussion

Post by guru_florida »

I was just pointed to this thread after PMing ziss_dm about my initial stab at getting baro hold to work. I've tried to tackle 2 of the hardest things to deal with when working on coding baro-hold:
1. Having to walk to the park everytime for testing
2. Dealing with the nuances of coding big-boy algorithms in small microcontrollers

I haven't read the 9 pages of this thread yet...so I have some reading to do but for some background, here is my message to ziss_dm:

Hi Dimitry,

My name is Colin MacKenzie, I am the designer of the Quadrino. I have been working on the altitude hold code for the last few weeks. I was in touch with Alex and he mentioned you were the one that has been working on this feature for the most part. I'd like to work together on solving the problem. I've been testing some different code I made and I can see some promise. This has been a much tougher problem to solve than I ever thought, all because of the difficulty in velocity estimation.

I've been working in Matlab. I think the biggest problem with coding this feature is (1) the long debug cycle and (2) visibility into the control signals (variables) while in flight. I have to go to the park to test out each time - this slows things down a lot! So I've tried to solve those problems first. I made a matlab plugin that allows me to read the sensors from the MultiWii board in realtime. I can test new velocity estimators this way. I also just finished a test stand so I can fly the copter beside my computer without crashing into everything - and I can also have usb plugged in at the same time. I would go wireless (bluetooth or wifly), but I am concerned the latency would be too great.I am hoping this weekend, I will not only just read sensors values, but be setting throttle/output commands via usb as well. This would allow me to test algorithms through Matlab in realtime - bringing my computer into the control loop. Latency aside, I hope it can weed out what algorithm has a chance. Once I feel an algorithm would work then i would distill the matlab code into the arduino.

I would love to share what I have if any of this would help you out. My control theory comes from servo design. I am still learning a lot of this as I go. I'm not even a matlab expert - but I'm liking it!

The code changes I made so far were to the position control loop in the main MultiWii.pde file. Like your code, I have a position loop that controls a velocity loop - and thus requires the velocity estimator to work right. The difference in my code is that I dont servo the output around a saved throttle value. Regardless of what the throttle was when the user clicked "Alt Hold" I connect the velocity PID output directly to the throttle. (For the moment the throttle input is totally disconnected, I'll deal with it when the alt hold is solved.) The real trick here though is that I feed-forward the gravity value into the velocity loop. So essentially a constant value is fed into the velocity loop input to anticipate the effect of gravity. This seems to work nicely at first, but after a few seconds the velocity estimator sign changes and the copter falls. I graph the velocity estimate value and I see it suddenly change. The latest dev is better but it still seems to happen.

I made a kalman filter that fuses ACC and BARO readings and it seems to estimate velocity nicely, but I havent tested it in flight yet. We'll see this weekend how it works under vibration from the motors. I am new to kalman and complementary filters so I'm not saying mine will work better than yours - I have no idea, it's just somewhere to start. But with a quick debug cycle, I hope I/we can solve this rather quickly once I get going.

I can create a thread, or join an existing one. I just wanted to touch base with you personally first. My email address is nospam2@colinmackenzie.net if you want to email personally, or just let me know and I can start discussion in a forum thread.

Thanks,
Colin

User avatar
guru_florida
Posts: 45
Joined: Sat Mar 26, 2011 4:51 am

Re: Alt. Hold Ideas and discussion

Post by guru_florida »

To follow up. I've gotten the copter to fly with MATLAB in the loop. You can see my setup at:
http://www.youtube.com/watch?v=yAdAK-Sz2Yg

I've also gotten baro hold to work....BUT...I find the tether wire hanging from my test stand provides too much of a dampening effect. The copter can hover pretty well without any algorithm! So I am going to modify the test stand to have the wire go up, over a 2x garden style pully in a "hangman" style boom and away from the copter. This should balance the weight of the wire and take away the dampening effect. The velocity estimator seems to be working well and I can see the signals in the realtime graph.

I have more work to do. I really just got this setup working. Flew for a while on the power supply and then the copter power started dying. not sure why - ESCs were cool, motors were a little hot but not much. One motor's bearing seems shot. I have to shelf the project for a few days while I catch up on some other work.

I cant wait to see this working!!!! One way or another... :)

User avatar
guru_florida
Posts: 45
Joined: Sat Mar 26, 2011 4:51 am

Re: Alt. Hold Ideas and discussion

Post by guru_florida »

Here is a screenshot of the tuning UI I made that plugs into MATLAB (It is a C# dll). The UI allows me to change the control loop parameters in realtime and see the effect. The process I use so far is to (switch to baro mode) then increase the Gravity Prefeed until the copter is just about to lift off the ground. Then I switch baro mode off, manually fly the copter into the air and again activate baro hold. The copter will stay level with the tuned "Gravity Feed Forward" making the PID loop have very little to correct for.

The throttle low pass makes sure the control algorithm doesnt make any sudden throttle changes. It basically tweens the throttle between target values at a nice pace. A little easier on the motors that way too.

C

tuning-ui.png

kuki83
Posts: 4
Joined: Sat Mar 19, 2011 7:40 pm

Re: Alt. Hold Ideas and discussion

Post by kuki83 »

hi guys

what do you think about it.

I made some modifications to the code.
Current version 1.9 + my modifications :-)

Pressure Sensors MS5611

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

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Alt. Hold Ideas and discussion

Post by LuFa »

WOW !!!!! :shock:

did you like to share your code with us ?

kuki83
Posts: 4
Joined: Sat Mar 19, 2011 7:40 pm

Re: Alt. Hold Ideas and discussion

Post by kuki83 »

of course, but tomorrow, because he wants to check something

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Alt. Hold Ideas and discussion

Post by LuFa »

sounds great ! thanks ;)
cant wait to test your code...
is it with VEL in jusing ? or without ?

kuki83
Posts: 4
Joined: Sat Mar 19, 2011 7:40 pm

Re: Alt. Hold Ideas and discussion

Post by kuki83 »


mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Alt. Hold Ideas and discussion

Post by mahowik »

kuki83 wrote:hi guys

what do you think about it.

I made some modifications to the code.
Current version 1.9 + my modifications :-)

Pressure Sensors MS5611

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


It's really cool !!!
Ineteresting to look at the code changes.

thx-
Alex

Magnetron
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

kuki83 wrote:hi guys

what do you think about it.

I made some modifications to the code.
Current version 1.9 + my modifications :-)

Pressure Sensors MS5611

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


Really cool!
Could you post your code modifications?

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Alt. Hold Ideas and discussion

Post by marbalon »

Hi,
After changing some experience with Wiktorx I made new procedures for altitude hold. Idea is simple, just full PID regulator

- P depend on error, bu use filtered data from baro
- I use the same filtered data, thanks to this parameter there is no problem with battery drain
- D is the most important to prevent overshot held altitude. But calculate delta is quite complicated for baro, because it is not to fast sensor. I decide to save 40 last samples in 40Hz idle, then use this samples to calculate delta. But for this I use raw BaroAlt value to eliminate delay.

I've tested my quad on booth BMP085 and MS5611 baro, and works fine for booth, but for BMP you need to use lower pids, so it overshot position sometimes, but generaly works.

My pids for this sensors:

MS5611 - 3.0/10/12
BMP - 1.6/10/7

- BMP video is here:
http://www.youtube.com/watch?v=DXCPc1kh_cA

- MS5611 video:
http://youtu.be/YXC32jrD93c

Be careful when you test your quad in low temperatures. I found that MS5611 diver in sensors.pde is buggy and will try to fix it. If you want to test it outside please left your quad for about 15min outside then connect battery.

Here is all code. Acc part is removed.

IMU.pde file:

Code: Select all

...
#define UPDATE_INTERVAL 25000
#define INIT_DELAY 4000000
#define BARO_TAB_SIZE  40
 
void getEstimatedAltitude(){
  static uint8_t inited = 0;
  static uint32_t deadLine = INIT_DELAY;
  static int16_t BaroHistTab[BARO_TAB_SIZE];
  static int8_t BaroHistIdx=0;
  int32_t BaroHigh,BaroLow;
  int32_t temp32;
   
  Baro_update();
   
  if (currentTime < deadLine)
    return;
  deadLine = currentTime + UPDATE_INTERVAL;
  // Soft start
  if (!inited) {
    inited = 1;
    EstAlt = BaroAlt;
  }
 
  EstAlt = EstAlt*0.95 + BaroAlt*0.05;
 
  //**** Alt. Set Point stabilization PID ****
  temp32 = constrain( AltHold - EstAlt, -1000, 1000); //  +/-10cm,  1 decimeter accuracy

  //P
  BaroPID = P8[PIDALT]*constrain(temp32,-70,70)/100;   
  //I
  errorAltitudeI += temp32*I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  temp32 = errorAltitudeI / 500; //I in range +/-60
  BaroPID+=temp32;
   
  //calculate speed
  BaroHistTab[BaroHistIdx] = BaroAlt; 
  BaroHigh = 0;
  BaroLow = 0;
  for (temp32=0;temp32 < BARO_TAB_SIZE/2; temp32++)
  {
    BaroHigh+=BaroHistTab[(BaroHistIdx - temp32 + BARO_TAB_SIZE)%BARO_TAB_SIZE];  //sum last half samples
    BaroLow+=BaroHistTab[(BaroHistIdx + temp32 + BARO_TAB_SIZE)%BARO_TAB_SIZE];  //sum older samples
  }
  BaroHistIdx++;
  if (BaroHistIdx >= BARO_TAB_SIZE)
    BaroHistIdx = 0;

  temp32 = D8[PIDALT]*(BaroHigh - BaroLow) / 400;
  BaroPID-=temp32;
  BaroPID = constrain(BaroPID,-150,+150);
}



Main file:

Code: Select all

//global variables

static int32_t  BaroAlt;
static int32_t  EstAlt;             // in cm
static int32_t AltHold;
int16_t BaroPID = 0;
int16_t errorAltitudeI = 0;


...
//in main loop
  if(BARO){     
      if (baroMode)
      {
        if (abs(rcCommand[THROTTLE]-initialThrottleHold)>20)
        {
          baroMode = 0;
          errorAltitudeI = 0;
          BaroPID=0;
        }
        rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
      }
  }

..


Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: Alt. Hold Ideas and discussion

Post by Alexinparis »

Hi,

Thank you for sharing your experiment and your code.

1)
I noticed especially this part:

Code: Select all

temp32 = constrain( AltHold - EstAlt, -1000, 1000); //  +/-10cm,  1 decimeter accuracy

  //P
  BaroPID = P8[PIDALT]*constrain(temp32,-70,70)/100;   


Whith your PID value of 3 and 1.6, the result is:
3: 30*70/100 = 21
1.6: 16*70/100 = 11
Quite small in fact in the PID total term.

2)
You don't use anymore the ACCZ.
ACCZ + ziss_dm code clearly helps to match altitude change faster on EstAlt term.
I don't know if it can be usefull or not in your implementation due to the high LPF.

3)
In the last devs, BaroAlt is in decimeter and not in centimeter.

Anyway, I will try it.

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Alt. Hold Ideas and discussion

Post by marbalon »

I still use version 20111220. And you are right I think error used for P calculation can be in bigger range, but like you see on the videos it works fine - I think thanks to good D parameter. Maybe I have smaller PIDs because my quad have too much power it have about 2,8kg thrust and weight 0,7kg. Please try it and give me a feedback.

I think ACCZ can help but it is a little complicated, and results with baro only is satisfied in my opinion.

Regards,
Marcin.

bob4432
Posts: 51
Joined: Sat Jan 29, 2011 2:51 am
Location: USA

Re: Alt. Hold Ideas and discussion

Post by bob4432 »

very nice work :)

alexmos
Posts: 108
Joined: Tue Aug 09, 2011 12:12 pm

Re: Alt. Hold Ideas and discussion

Post by alexmos »

I agreed with Alex - it is not a good idea to get D-part from noisy baro. Accelerometer is the best source for D-term. To get real effect of Alt Hold in real flight, you will need much stronger PID settings.

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Alt. Hold Ideas and discussion

Post by marbalon »

alexmos wrote:I agreed with Alex - it is not a good idea to get D-part from noisy baro. Accelerometer is the best source for D-term. To get real effect of Alt Hold in real flight, you will need much stronger PID settings.


You are right. D is not used to compensate error, it is used to reduce oscillations when quad return to base position. But in fact D also reduce vertical speed when it goes in wrong direction. Small P in my settings is related with too much power in my quad. Please try to test it, like you see on the videos there quad have no problem with return to held altitude. But I agree If we can mix ACCZ with this method we can try it too.

Today I've made some changes in MS5611 to get more samples from Ms5611, delays are to big, now I have about 40 samples per second and getAltitude works with the same speed. Now I have smaller noise.

I also changed my procedures a little:

- now I get EstAlt as average from last 20 samples
- remove D for small errors - this remove noise near held position
- P range depend on P8[PIDALT] parametr to get more power in other quads.

I will load boring video with full battery drain with this code ;)

Here are modifications:

Sensors.pde

Code: Select all

voi Baro_update() {
  if (currentTime < ms561101ba_ctx.deadline) return;
  ms561101ba_ctx.deadline = currentTime;
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, MS5611 is ok with this speed
  switch (ms561101ba_ctx.state) {
    case 0:
      i2c_MS561101BA_UT_Start();
      ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 10000; //according to the specs, the pause should be at least 8.22ms
      break;
    case 1:
      i2c_MS561101BA_UT_Read();
      ms561101ba_ctx.state++;
      break;
    case 2:
      i2c_MS561101BA_UP_Start();
      ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 10000; //according to the specs, the pause should be at least 8.22ms
      break;
    case 3:
      i2c_MS561101BA_UP_Read();
      i2c_MS561101BA_Calculate();
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f;
      ms561101ba_ctx.state = 0; ms561101ba_ctx.deadline += 4000;
      return;
  }
  return;
}



IMU.pde

Code: Select all

#define UPDATE_INTERVAL 25000
#define INIT_DELAY 4000000
#define BARO_TAB_SIZE  40
 
void getEstimatedAltitude(){
  static uint8_t inited = 0;
  static uint32_t deadLine = INIT_DELAY;
  static int16_t BaroHistTab[BARO_TAB_SIZE];
  static int8_t BaroHistIdx=0;
  int32_t BaroHigh,BaroLow;
  int32_t temp32;
   
  Baro_update();
   
  if (currentTime < deadLine)
    return;
  deadLine = currentTime + UPDATE_INTERVAL;
  // Soft start
  if (!inited) {
    inited = 1;
    EstAlt = BaroAlt;
  }

  //**** Alt. Set Point stabilization PID ****
 
  //calculate speed for D calculation
  BaroHistTab[BaroHistIdx] = BaroAlt; 
  BaroHigh = 0;
  BaroLow = 0;
  BaroPID = 0;
  for (temp32=0;temp32 < BARO_TAB_SIZE/2; temp32++)
  {
    BaroHigh+=BaroHistTab[(BaroHistIdx - temp32 + BARO_TAB_SIZE)%BARO_TAB_SIZE];  //sum last half samples
    BaroLow+=BaroHistTab[(BaroHistIdx + temp32 + BARO_TAB_SIZE)%BARO_TAB_SIZE];  //sum older samples
  }
  BaroHistIdx++;
  if (BaroHistIdx >= BARO_TAB_SIZE)
    BaroHistIdx = 0;

  temp32 = D8[PIDALT]*(BaroHigh - BaroLow) / 400;
  BaroPID-=temp32;
 
  //EstAlt = EstAlt*0.95 + BaroAlt*0.05;
  EstAlt = BaroHigh/(BARO_TAB_SIZE/2);
 
  temp32 = constrain( AltHold - EstAlt, -1000, 1000); //  +/-10cm,  1 decimeter accuracy
  if (abs(temp32) < 10 && BaroPID < 10)
    BaroPID = 0;  //remove small D parametr to reduce noise near zoro position
  //P
  BaroPID += P8[PIDALT]*constrain(temp32,(-2)*P8[PIDALT],2*P8[PIDALT])/100;   
  //I
  errorAltitudeI += temp32*I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  temp32 = errorAltitudeI / 500; //I in range +/-60
  BaroPID+=temp32;
 
  BaroPID = constrain(BaroPID,-150,+150);
}

Last edited by marbalon on Wed Feb 15, 2012 8:14 am, edited 1 time in total.

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Alt. Hold Ideas and discussion

Post by marbalon »

Here is boring video 13min hover until battery is empty.

http://youtu.be/0kgnnLgZZJs

PID tuning algorithm is simple.

- Try from suggested PID's eg. 3/15/12.
- if you want to faster reaction increase P
- Ty to push your quad up and down when it overshot position increase D

But if you get to big values quad became "nervous" in hover.

Regards,
Marcin.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Alt. Hold Ideas and discussion

Post by ziss_dm »

Hi marbalon,
Really impressive!!! ;)
But huge D... Can you try to set setpoint couple of meters higher?

And you have used 40 taps FIR filter, have you tried IIR?
And why 40? Just experemental?

regards,
ziss_dm

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Alt. Hold Ideas and discussion

Post by marbalon »

ziss_dm wrote:Hi marbalon,
Really impressive!!! ;)
But huge D... Can you try to set setpoint couple of meters higher?

I need to fix problem with MS5611 in low temperatures. Kuki83 have a solution, after this I can test it outside. But my gyro L3G4200 don't like low temperatures too. But will try.

ziss_dm wrote:And you have used 40 taps FIR filter, have you tried IIR?
And why 40? Just experemental?


Yes is is experimental. I'm not a guru in filters and controllers, but just try to fallow PID idea, and try to give correct parameters to this controller. Like you see it works ;) If you have some time you can try to filter delta in other way, but in my opinion it is good now and works with BMP085. I have also idea to tune BMP085 readings and will try to do it today.

Post Reply