Alt. Hold Ideas and discussion

ziss_dm · Post by **ziss_dm** » Tue Dec 20, 2011 4:36 am

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 · Post by **mahowik** » Tue Dec 20, 2011 4:52 am

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 · Post by **mahowik** » Wed Dec 21, 2011 5:34 pm

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 · Post by **mahowik** » Tue Jan 17, 2012 11:45 pm

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 · Post by **Alexinparis** » Wed Jan 18, 2012 10:44 pm

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 · Post by **Scotth72** » Sat Jan 21, 2012 4:09 pm

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 · Post by **mahowik** » Mon Jan 23, 2012 4:40 am

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 · Post by **ziss_dm** » Mon Jan 23, 2012 6:10 am

Hi Scotth72,

Can you also share your tuning methods?

regards,
ziss_dm

shikra · Post by **shikra** » Mon Jan 23, 2012 11:19 am

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 · Post by **Scotth72** » Mon Jan 23, 2012 4:04 pm

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.

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 · Post by **mahowik** » Tue Jan 24, 2012 5:13 am

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/

guru_florida · Post by **guru_florida** » Fri Feb 03, 2012 4:22 pm

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

guru_florida · Post by **guru_florida** » Fri Feb 03, 2012 4:31 pm

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...

guru_florida · Post by **guru_florida** » Fri Feb 03, 2012 4:39 pm

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

kuki83 · Post by **kuki83** » Fri Feb 10, 2012 10:51 pm

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 · Post by **LuFa** » Fri Feb 10, 2012 11:03 pm

WOW !!!!!

did you like to share your code with us ?

kuki83 · Post by **kuki83** » Fri Feb 10, 2012 11:14 pm

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

LuFa · Post by **LuFa** » Fri Feb 10, 2012 11:48 pm

sounds great ! thanks

cant wait to test your code...
is it with VEL in jusing ? or without ?

kuki83 · Post by **kuki83** » Sat Feb 11, 2012 9:07 pm

next test

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

mahowik · Post by **mahowik** » Sun Feb 12, 2012 4:44 am

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 · Post by **Magnetron** » Sun Feb 12, 2012 10:51 am

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 · Post by **marbalon** » Sun Feb 12, 2012 3:21 pm

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 · Post by **Alexinparis** » Mon Feb 13, 2012 3:10 pm

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 · Post by **marbalon** » Mon Feb 13, 2012 3:26 pm

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 · Post by **bob4432** » Mon Feb 13, 2012 10:01 pm

very nice work

alexmos · Post by **alexmos** » Mon Feb 13, 2012 11:10 pm

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 · Post by **marbalon** » Mon Feb 13, 2012 11:35 pm

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);
}

marbalon · Post by **marbalon** » Tue Feb 14, 2012 7:29 am

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 · Post by **ziss_dm** » Tue Feb 14, 2012 8:07 am

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 · Post by **marbalon** » Tue Feb 14, 2012 10:06 am

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.

Magnetron · Post by **Magnetron** » Wed Feb 15, 2012 12:49 pm

marbalon wrote:
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.

Hi,
there are some news about Kuki83 routine to optimize baro handling?

Magnetron · Post by **Magnetron** » Wed Feb 15, 2012 12:51 pm

marbalon wrote: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;
      }
  }

..

I am using 20120302 version and I would to try your routine on it but I assume that you are using 20111220.
What kind of modification must I put in your routine to run it on my 20120302 version? So I can submit you my feedback.
Thanks.

Magnetron · Post by **Magnetron** » Wed Feb 15, 2012 3:44 pm

Hi,
I have integrated marbalon barometric routine in the dev20120302 activable by define on the bottom of config.h file.
In this dev there is also my moving average for gyros and accelerometers to give more stability in fly, also activable by defines in that file.
I do not understant so good the question between centimeter (in previous version like 20111220) and decimeter (in this version) so I have copied and pasted the code in getEstimatedAltitude in file IMU.PDE. So I must multiply or divide for 10 the limit reference.

Original marbalon code written for dev_20111220 (also inserted in this my dev)

Code: Select all

  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);
  debug4 = BaroPID

So what modification to adjust cm to dm?

Noctaro · Post by **Noctaro** » Wed Feb 15, 2012 4:44 pm

hey,
as i did try to compile your release, i got this:

Code: Select all

MultiWii_dev_20120203.cpp: In function 'void getEstimatedAltitude()':
MultiWii_dev_20120203.pde:-1: error: redeclaration of 'uint8_t inited'
MultiWii_dev_20120203.pde:-1: error: 'uint8_t inited' previously declared here
MultiWii_dev_20120203.pde:-1: error: redeclaration of 'uint32_t deadLine'
MultiWii_dev_20120203.pde:-1: error: 'uint32_t deadLine' previously declared here
MultiWii_dev_20120203.pde:-1: error: expected `;' before '}' token

i did try with an edited config.h and the original you provided.

Is there anything i have forgotten? (Using Arduino V1.0)

greetz Noc

Magnetron · Post by **Magnetron** » Wed Feb 15, 2012 5:00 pm

Noctaro wrote:hey,
as i did try to compile your release, i got this:
Code: Select all
MultiWii_dev_20120203.cpp: In function 'void getEstimatedAltitude()':
MultiWii_dev_20120203.pde:-1: error: redeclaration of 'uint8_t inited'
MultiWii_dev_20120203.pde:-1: error: 'uint8_t inited' previously declared here
MultiWii_dev_20120203.pde:-1: error: redeclaration of 'uint32_t deadLine'
MultiWii_dev_20120203.pde:-1: error: 'uint32_t deadLine' previously declared here
MultiWii_dev_20120203.pde:-1: error: expected `;' before '}' token
i did try with an edited config.h and the original you provided.

Is there anything i have forgotten? (Using Arduino V1.0)

greetz Noc

I check... now...

Magnetron · Post by **Magnetron** » Wed Feb 15, 2012 5:04 pm

I have corrected the problem...
Download this new one.

howardhb · Post by **howardhb** » Wed Feb 15, 2012 7:24 pm

Can you please advise starting P I D to test your code?
Going to test it now.
Thanks

Howard

marbalon · Post by **marbalon** » Wed Feb 15, 2012 7:27 pm

Please start at 3 /0.015/ 12

howardhb · Post by **howardhb** » Wed Feb 15, 2012 7:29 pm

Thanks....

Magnetron · Post by **Magnetron** » Wed Feb 15, 2012 7:33 pm

good!!!
Could you give us feedback? Some videos?

howardhb · Post by **howardhb** » Wed Feb 15, 2012 8:29 pm

Not sure why yet, but I get no output debug for AltPID.
I see you are using FREEIMUv035MS using MS561101BA
I'm using individual sensors:
ITG3205
BMA180
BMP085
HMC5883
Also, PPM sum with Spektrum 6ch Orange clone Rx

Magnetron · Post by **Magnetron** » Wed Feb 15, 2012 8:39 pm

Yes.
Have you tested?

howardhb · Post by **howardhb** » Wed Feb 15, 2012 9:54 pm

Copter falls immediately on Baro activation.
BaroPID is always negative. ( -80) so RcCommand[Throttle] drops by 80 on Baro activation

ACCZ is 255 with copter stationary on the desk (with motors running)

Magnetron · Post by **Magnetron** » Wed Feb 15, 2012 10:16 pm

howardhb wrote:Copter falls immediately on Baro activation.
BaroPID is always negative. ( -80) so RcCommand[Throttle] drops by 80 on Baro activation

ACCZ is 255 with copter stationary on the desk (with motors running)

Ok I will modificate the main baro code again when marbalon will try it. Standby some days... Thanks

marbalon · Post by **marbalon** » Wed Feb 15, 2012 10:48 pm

Hi,

Today I finished my version I think it is the best I can do using baro only. This is version for 20111220. Now I will try to switch to latest dev and merge it.

Suggested PID:
MS5611 - 3.0/15/12
BMP085 - 1.6/15/7

Changes until last version:
- temperature compensation for MS5611 form Kuki83
- replace LFP filter for EstAlt with average from last 20 samples
- for BMP I changed precision from 0.25 to 0.3 but thanks to this we can get faster readings (40 unique samples per second) so average should be more smooth.

Main file:

Code: Select all

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

...
    if (accMode == 1) STABLEPIN_ON else STABLEPIN_OFF;

    if(BARO) {
      if ((rcOptions1 & activate1[BOXBARO]) || (rcOptions2 & activate2[BOXBARO])) {
        if (baroMode == 0) {
          baroMode = 1;
          AltHold = EstAlt;
          initialThrottleHold = rcCommand[THROTTLE];
          errorAltitudeI = 0;      
          BaroPID=0;
        }
      } else baroMode = 0;
    }

...

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

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.8 + BaroAlt*0.2;
  EstAlt = BaroHigh/(BARO_TAB_SIZE/2);
  
  temp32 = constrain( AltHold - EstAlt, -1000, 1000);
  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;   
  BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150
  
  //I
  errorAltitudeI += temp32*I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  temp32 = errorAltitudeI / 500; //I in range +/-60
  BaroPID+=temp32;
}

Sensors.pde

for Baro BMP085

Code: Select all

...
#define OSS 2 //we can get more uique samples and get better precision using average
...

void Baro_update() {
  if (currentTime < bmp085_ctx.deadline) return; 
  bmp085_ctx.deadline = currentTime;
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, BMP085 is ok with this speed
  switch (bmp085_ctx.state) {
    case 0: 
      i2c_BMP085_UT_Start(); 
      bmp085_ctx.state++; bmp085_ctx.deadline += 4600; 
      break;
    case 1: 
      i2c_BMP085_UT_Read(); 
      bmp085_ctx.state++; 
      break;
    case 2: 
      i2c_BMP085_UP_Start(); 
      bmp085_ctx.state++; bmp085_ctx.deadline += 14000; 
      break;
    case 3: 
      i2c_BMP085_UP_Read(); 
      i2c_BMP085_Calculate(); 
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f;
      bmp085_ctx.state = 0; bmp085_ctx.deadline += 5000; //little delay to get about 40 samples per second
      return;
  }
 return; 
}
...

For MS5611:

Code: Select all

void i2c_MS561101BA_Calculate() {
  int64_t off2=0,sens2=0,temperature=0,t2=0;
  int64_t dT   = ms561101ba_ctx.ut.val - ((uint32_t)ms561101ba_ctx.c[5] << 8);  //int32_t according to the spec, but int64_t here to avoid cast after
  int64_t off  = ((uint32_t)ms561101ba_ctx.c[2] <<16) + ((dT * ms561101ba_ctx.c[4]) >> 7);
  int64_t sens = ((uint32_t)ms561101ba_ctx.c[1] <<15) + ((dT * ms561101ba_ctx.c[3]) >> 8);
  pressure     = (( (ms561101ba_ctx.up.val * sens ) >> 21) - off) >> 15;
  temperature = (2000 + dT * (uint32_t)ms561101ba_ctx.c[5] / pow(2, 23)); 
  if (temperature < 2000) 
  { 
    // temperature lower than 20st.C 
    t2 = (dT*dT)/pow(2,31); 
    off2 = 5 * (((int32_t)temperature-2000)*((int32_t)temperature-2000))/2; 
    sens2 = 5 * (((int32_t)temperature-2000)*((int32_t)temperature-2000))/4; 
    if (temperature < -1500) 
    { 
      // temperature lower than -15st.C 
      off2 = off2 + 7 * (((int32_t)temperature+1500)*((int32_t)temperature+1500)); 
      sens2 = sens2 + 11 * (((int32_t)temperature+1500)*((int32_t)temperature+1500))/2; 
    } 
  } 
  temperature = temperature - t2; 
  off = off - off2; 
  sens = sens - sens2; 
  pressure     = (( (ms561101ba_ctx.up.val * sens ) / pow(2,21)) - off) / pow(2,15);  
}

void 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;
}

Noctaro · Post by **Noctaro** » Thu Feb 16, 2012 12:53 am

Hey,
thank you for reuploading the fixed release. Compiles fine. I will give feedback when some testing is done.

greetz noc

nhadrian · Post by **nhadrian** » Thu Feb 16, 2012 6:41 am

Hi,

I have both BMP085 and MS5611 installed on my copter.
When I select BMP085, with your code it drops down after ALT is enabled.
When I select MS5611, if starts climbing as soon as I enable ALT. I used your suggested PIDs for both.
So I coulnd't test the behaviour.

BR
Adrian

marbalon · Post by **marbalon** » Thu Feb 16, 2012 7:25 am

nhadrian wrote:Hi,

I have both BMP085 and MS5611 installed on my copter.
When I select BMP085, with your code it drops down after ALT is enabled.
When I select MS5611, if starts climbing as soon as I enable ALT. I used your suggested PIDs for both.
So I coulnd't test the behaviour.

BR
Adrian

- Did you test with my patches or with Magneton version?
- Remember if you want to test it outside pleaseleft quad for about 15min, this sensors don't like different temperatures.

Magnetron · Post by **Magnetron** » Thu Feb 16, 2012 11:40 am

Hi,
in the attach there is dev 20111220 with Marbalon baro implementation for BMP085 and MS5611 and my moving average for gyros and accelerometers.
Please download and try it.
Give us yours feedbacks.

mlebret · Post by **mlebret** » Thu Feb 16, 2012 2:43 pm

Hello,

I had a try with my 20120203 Magetron latest Marbalon dev. Baro is MS5611 and I cannot obtain a good Altitude Hold. I try with the suggested settings with no luck (slow down or slow up), I increased I to 6 then 9 and 12 but no good improvement. What is the most significant parameter (I, D or P)?

Waiting for tip,

Thanks,

MLB