Alt. Hold Ideas and discussion

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

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

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
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

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
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

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?
Attachments
MultiWii_dev_20120203_Magnetron_Marbalon.rar
(57.77 KiB) Downloaded 327 times

Noctaro
Posts: 280
Joined: Thu Sep 08, 2011 11:15 am
Contact:

Re: Alt. Hold Ideas and discussion

Post by Noctaro »

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
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

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
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

I have corrected the problem...
Download this new one.
Attachments
MultiWii_dev_20120203_Magnetron_Marbalon.rar
(57.77 KiB) Downloaded 373 times

User avatar
howardhb
Posts: 189
Joined: Tue Oct 11, 2011 7:10 pm
Location: Port Elizabeth, South Africa

Re: Alt. Hold Ideas and discussion

Post by howardhb »

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

Howard

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

Re: Alt. Hold Ideas and discussion

Post by marbalon »

Please start at 3 /0.015/ 12

User avatar
howardhb
Posts: 189
Joined: Tue Oct 11, 2011 7:10 pm
Location: Port Elizabeth, South Africa

Re: Alt. Hold Ideas and discussion

Post by howardhb »

Thanks....

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

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

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

User avatar
howardhb
Posts: 189
Joined: Tue Oct 11, 2011 7:10 pm
Location: Port Elizabeth, South Africa

Re: Alt. Hold Ideas and discussion

Post by howardhb »

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
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

Yes.
Have you tested?

User avatar
howardhb
Posts: 189
Joined: Tue Oct 11, 2011 7:10 pm
Location: Port Elizabeth, South Africa

Re: Alt. Hold Ideas and discussion

Post by howardhb »

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
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

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
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Alt. Hold Ideas and discussion

Post by marbalon »

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
Posts: 280
Joined: Thu Sep 08, 2011 11:15 am
Contact:

Re: Alt. Hold Ideas and discussion

Post by Noctaro »

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

greetz noc

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Alt. Hold Ideas and discussion

Post by nhadrian »

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
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Alt. Hold Ideas and discussion

Post by marbalon »

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
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

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.
Attachments
MultiWii_dev_20111220_Magnetron_Marbalon.rar
(49.86 KiB) Downloaded 348 times

mlebret
Posts: 17
Joined: Thu Jan 26, 2012 9:12 am

Re: Alt. Hold Ideas and discussion

Post by mlebret »

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

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

Re: Alt. Hold Ideas and discussion

Post by marbalon »

I have a question to Alex. Today I will try to include may patches to latest dev, and will try to add ACCZ support. But can we switchback to cm instead dm? I know precision for baro is not in cm, but when you get average form last few samples the best if the result is with better precision. And I think we will need Vel pids settings again in GUI, because D is currently used for Alt PID.

What do you think?

And another question for current implementation in last dev. zVelocity is user to remove noise from Baro right ?

Ziss, could you join to discussion ;)

Regards,
Marcin.

noobee
Posts: 66
Joined: Fri Feb 25, 2011 2:57 pm

Re: Alt. Hold Ideas and discussion

Post by noobee »

mlebret wrote: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


perhaps it is better to separate the magetron and marbalon changes and apply them one step at a time; starting from marbalon's changes and test that alone first, and then followed by the combo?

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

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

I have tested 20111220 with Magnetron & Marbalon modification.

Positive note for Magnetron software modifications:
The stability due to moving average give some positive effects like immunity to vibrations and windy conditions, also go down vertical fly is better.

Positive note for Marbalon software modifications:
The baro altitude hold was increased and the baro works very well with this patch with PID suggested.

Now we must go to the next dev to implement this routines and add support to TRUSTED_ACCZ.
Marbalon we can start from v1.9 TRUSTED_ACCZ routine, what do you think?

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Alt. Hold Ideas and discussion

Post by nhadrian »

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

...

...



I tried this modification in my dev_20111220 . I tried it inside at 21 C degrees...

BR
Adrian

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

Re: Alt. Hold Ideas and discussion

Post by Alexinparis »

marbalon wrote:I have a question to Alex. Today I will try to include may patches to latest dev, and will try to add ACCZ support. But can we switchback to cm instead dm? I know precision for baro is not in cm, but when you get average form last few samples the best if the result is with better precision. And I think we will need Vel pids settings again in GUI, because D is currently used for Alt PID.

What do you think?


Hi,
With a resolution of 1dm, we can use a 16bit integer in this range (-1600m -> +1600m), and we avoid a /10 division in Serial com.
But I'm agree, this will anyway overflow after 1600m, and the calculation must be done in 32 bit anyway. ok to switch back to cm.
the VEL PID settings are back in the last dev code

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

Re: Alt. Hold Ideas and discussion

Post by marbalon »

Ok I will switch to cm, but do you mean last dev from svn ? Last dev MultiWii_dev_20120203.zip don't have this boxes.

Regards,
Marcin.

KeesvR
Posts: 194
Joined: Fri May 27, 2011 6:51 pm
Location: The Netherlands

Re: Alt. Hold Ideas and discussion

Post by KeesvR »

Look in Trunk R568.

Noctaro
Posts: 280
Joined: Thu Sep 08, 2011 11:15 am
Contact:

Re: Alt. Hold Ideas and discussion

Post by Noctaro »

hey,
edit: i tested your release MultiWii_dev_20111220_Magnetron_Marbalon.rar, acc/gyro smoothing seems to work fine. Had a nice stable flight. Also seems that the temperature/time drift that occurs on my quad, was a bit more controlable. Baro did not do the trick, but its around 0°C outside. (GUI is slow at showing the level)

greetz noc

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

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

I have tested today 20111220 with new baro routine.
When I activate baro with suggested pid the quad tends to stay in that altitube but after some seconds it goes up and the after few seconds goes down... and after that it became a little stable.

Noctaro wrote:hey,
edit: i tested your release MultiWii_dev_20111220_Magnetron_Marbalon.rar, acc/gyro smoothing seems to work fine. Had a nice stable flight. Also seems that the temperature/time drift that occurs on my quad, was a bit more controlable. Baro did not do the trick, but its around 0°C outside. (GUI is slow at showing the level)

greetz noc

I was sure the overall stability is increased due to moving average on gyros/accs.

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

Re: Alt. Hold Ideas and discussion

Post by marbalon »

Did you test it outside? Maybe this is because temperature drift for baro?

ps. Here is another test from polish forum. The same code as posted.

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

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

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

I have tested it outside more times at about 8°C and before plug battery I waited for about 30 minutes.
I think the baro come to work but some correction must done.

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Alt. Hold Ideas and discussion

Post by nhadrian »

Dear Marbalon,

could you transfer your idea to the r569 version?

BR
Adrian

Noctaro
Posts: 280
Joined: Thu Sep 08, 2011 11:15 am
Contact:

Re: Alt. Hold Ideas and discussion

Post by Noctaro »

Hey,
i have tested also indoors(acclimated) now, but was not able to do a safe flight. Holding my copter in hands, switching on baro leads to a critical power reducing. Even if i alter height about ~1,50m there is no reaction from motors. Btw. i am using the PID you suggested to start. By Temp drift i meant my gyro(idg600 i think as its a orig. wmp+) its constatly drifting off even if temp changes are very limited ~4°C. By now i only was able to reduce this drift in levelmode by adjusting the "Gyr_cmpf rate" (hope i typed it correct). And averaging seems also to counteract this drift a bit in levelmode. (It does not eliminate it.)

greetz noc

pm1
Posts: 136
Joined: Sun Jan 22, 2012 7:26 pm

Re: Alt. Hold Ideas and discussion

Post by pm1 »

I am using the BMP085. I have now observed the output in different conditions for a while.
- The output is quite noisy
- It has reaction times of 0.5 to 2 seconds (I don't know why, but sometimes it is faster, other times slower)
- Sometimes the altitude jumps 2m for a couple of seconds, the jumps back, when the copter is sitting on the ground

I have now done several things:

1. Average the signal. I am only changing the old value, if I see, there are 10 more samples with the difference in one direction than the other:

Code: Select all

void Baro_update() {
...
  switch (bmp085_ctx.state) {
    case 0:
      if ((itemp++ & 0x3) == 0) { // get Temp only every 4th cycle
        i2c_BMP085_UT_Start();
        bmp085_ctx.state++;
        bmp085_ctx.deadline += 5000;
      }
      else {
        bmp085_ctx.state = 2;
      }   
      break;
    case 1:
      i2c_BMP085_UT_Read();
      bmp085_ctx.state++;
      break;
    case 2:
      i2c_BMP085_UP_Start();
      bmp085_ctx.state++;
      bmp085_ctx.deadline += 26000;
      break;
    case 3:
      i2c_BMP085_UP_Read();
      i2c_BMP085_Calculate();
      xalt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f;
     
      a_alt[ialt++] = xalt;
     
      if (ialt == ALTS) {
        ialt = 0;
      }
      if (xalt >= BaroAlt) {
        count++;
      }
      else {
        count--;
      }
      if (abs(count) > 10) {
        BaroAlt = 0;
        for (int i = 0; i < ALTS; i++) {       
          BaroAlt += a_alt[i];
        }
        BaroAlt /= ALTS;
        count = 0;
      } 
      bmp085_ctx.state = 0;
      bmp085_ctx.deadline += 2000;
      break;
  }
}


2. Average the temperature value of the sensor

Code: Select all

void i2c_BMP085_Calculate() {
...
  b5 = x1 + x2;

  if (mb5 == 0) {
    mb5 = b5;
  }
  mb5 = (5 * mb5 + b5) / 6;  // A little averge to remove noise
   
  // Pressure calculations
  b6 = mb5 - 4000;
...


3. The P Term now works as follows:
- Increase/Decrease the throttle by a fixed amount (I use the Alt-D for this)
- The increase is done for a time proportional to the altitude error (Scalable with ALT-P)
- Turn off the throttle chnage and wait for one second to get a good sensor value
4. The I-Term is done normal with 300 ms cycle time

Code: Select all

  if(BARO) {
    if (baroMode) {
      if (abs(rcCommand[THROTTLE]-initialThrottleHold)<20) {
        if (currentTime > next_itime) {
          next_itime = currentTime + 300000;
          error = AltHold - EstAlt;

          EstAltI += constrain(error, -50, 50);
          EstAltI = constrain(EstAltI,-30000,30000);
          AltITerm = constrain((int32_t)I8[PIDALT] * EstAltI /20000, -70, 70);
         
          if (pstate == 0) {
            abserr=abs(error);
           
            if (abserr > 10) {
              pstate = 1;
              AltPTerm = D8[PIDALT] * (error/abserr);  // Increase/decrease the throttle with a fixed amount
              next_ptime = currentTime + 200L * P8[PIDALT] * constrain(abserr,-1000,1000); // Time is proportional to error
            }
          }           
        }
       
        if ((pstate > 0) && (currentTime > next_ptime) ) {
            if (pstate == 1) {
              AltPTerm = 0;
              pstate = 2;
              next_ptime = currentTime + 1000000; // Now a pause to get a useful baro reading
            }
            else {
              pstate = 0;
            }   
          }
          AltPID = AltPTerm + AltITerm;     
          rcCommand[THROTTLE] = initialThrottleHold + AltPID;
        }   
      }
  }


With this, I have a not very elegant, but working altitude hold without increasing oszillation. The advantage for me at the moment is: I have an audible feedback due to the hard throttle changes, where the estimated altitude is.

I have done this on base of 1.9
http://dl.dropbox.com/u/60532252/alt_20120219.diff
http://dl.dropbox.com/u/60532252/MultiW ... 120219.rar -> Use this only for reference there are some other changes in there, which might not ok for you...

Start with ALT P 3.0, I 0.100, D 30

Maybe this works for you too....

Best regards
Peter

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

Re: Alt. Hold Ideas and discussion

Post by Alexinparis »

Hi,

I've released a new dev version which include mods of marbalon.
It seems to work for me (at least inside)

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Alt. Hold Ideas and discussion

Post by nhadrian »

Hi Alex,

I can confirm that dev_20120219 works with MS5611-01, tested inside only today.
It also works with the WinGUI!
Great job!!!

BR
Adrian

capt
Posts: 54
Joined: Wed Jan 19, 2011 10:54 pm

Re: Alt. Hold Ideas and discussion

Post by capt »

I think you guys nailed it!
Steady hold within 1m (maybe.5m!) stock PIDS
Seeduino mega
WM+ BMA020
BMP085
compass
0 deg C no wind
Attachments
altdev.JPG
altdev.JPG

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Alt. Hold Ideas and discussion

Post by nhadrian »

Hi all,

I made several tests today, I can confirm that MS5611-01 BA works great over 20 C, but under.... :(
The code for low temp compensation doesn't work, there should be a serious calculation error since temp jumps from 2000 to around 23000, and alt falls into negative values, when temperature falls under 20C. I tried to figure out what is the problem but couldn't... :(

BR
Adrian

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

Re: Alt. Hold Ideas and discussion

Post by Alexinparis »

nhadrian wrote:Hi all,

I made several tests today, I can confirm that MS5611-01 BA works great over 20 C, but under.... :(
The code for low temp compensation doesn't work, there should be a serious calculation error since temp jumps from 2000 to around 23000, and alt falls into negative values, when temperature falls under 20C. I tried to figure out what is the problem but couldn't... :(

BR
Adrian


Hi,
I think the current pressure calculation with temperature compensation is buggy.
this one should work:

Code: Select all

void i2c_MS561101BA_Calculate() {
  int32_t temperature,off2,sens2,delt;
 
  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);

  temperature  = 2000 + (dT * ms561101ba_ctx.c[6])>>23 ;
  if (temperature < 2000) { // temperature lower than 20st.C
    delt = temperature-2000;
    delt  = delt*delt;
    off2  = (5 * delt)>>1;
    sens2 = (5 * delt)>>2;
    if (temperature < -1500) { // temperature lower than -15st.C
      delt  = temperature+1500;
      delt  = delt*delt;
      off2  += 7 * delt;
      sens2 += (11 * delt)>>1;
    }
  }
  off  -= off2;
  sens -= sens2;
  pressure     = (( (ms561101ba_ctx.up.val * sens ) >> 21) - off) >> 15;
}

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Alt. Hold Ideas and discussion

Post by nhadrian »

Looks better but temperature value is not valid I think.
On my desk it shows around 500 for temperature (printed out to debug2), but should be 2000-2100 because we have 20C inside now.

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

Re: Alt. Hold Ideas and discussion

Post by alexmos »

Hi guys! Here is my alt hold implementation: http://code.google.com/p/multiwii-alexm ... 2FMultiWii
It is very simular to ziss_dm method introduced in 1.9, but it was developed independetly and has some nice features. The main target was to get STRONG AltHold in fast flight. I tested it with baro BMP085, acc BMA180, sonar HC-SR04. You can freely use any ideas (see comments in source how it works).
http://www.youtube.com/watch?v=-0X74oV8 ... r_embedded
http://www.youtube.com/watch?v=nuJkHBdo ... r_embedded

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

Re: Alt. Hold Ideas and discussion

Post by Alexinparis »

nhadrian wrote:Looks better but temperature value is not valid I think.
On my desk it shows around 500 for temperature (printed out to debug2), but should be 2000-2100 because we have 20C inside now.


right,
another bug:

=>

Code: Select all

  temperature  = 2000 + ((dT * ms561101ba_ctx.c[6])>>23) ;

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

Re: Alt. Hold Ideas and discussion

Post by guru_florida »

Great videos! What VEL/ALT PID do you recommend? You are using the VEL pids right because you speak of velocity estimator.

alexmos wrote:Hi guys! Here is my alt hold implementation: http://code.google.com/p/multiwii-alexm ... 2FMultiWii
It is very simular to ziss_dm method introduced in 1.9, but it was developed independetly and has some nice features. The main target was to get STRONG AltHold in fast flight. I tested it with baro BMP085, acc BMA180, sonar HC-SR04. You can freely use any ideas (see comments in source how it works).
http://www.youtube.com/watch?v=-0X74oV8 ... r_embedded
http://www.youtube.com/watch?v=nuJkHBdo ... r_embedded

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

Re: Alt. Hold Ideas and discussion

Post by guru_florida »

Hi alexmos,

No luck here. Copter just flies away (into the sky).

I adjusted the THROTTLE_HOVER/SHIFT to 1275 which was my throttle just before the copter would take off. Maybe it's just my PIDs are way off. I left P as default 4.7. TRUSTED_ACCZ is enabled. I am using the BMP085 like your setup.

Also, as a warning to others who try, It seems the code ignores the AUX modes as setup in the GUI. Looks like AUX1 activates HOLD and is hard coded. Just to be safe, test your setup by holding the copter first - with a good grip. This is not a big deal if the algorithm works, easy to fix afterwards.

I'm really looking forward to seeing Alt hold working!

Colin

alexmos wrote:Hi guys! Here is my alt hold implementation: http://code.google.com/p/multiwii-alexm ... 2FMultiWii
It is very simular to ziss_dm method introduced in 1.9, but it was developed independetly and has some nice features. The main target was to get STRONG AltHold in fast flight. I tested it with baro BMP085, acc BMA180, sonar HC-SR04. You can freely use any ideas (see comments in source how it works).
http://www.youtube.com/watch?v=-0X74oV8 ... r_embedded
http://www.youtube.com/watch?v=nuJkHBdo ... r_embedded

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

Re: Alt. Hold Ideas and discussion

Post by marbalon »

Yesterday I've made another version. It is still using baro only. I will cleanup the code and will share it today.

http://youtu.be/ia5bRUce74A

Marcin.
Last edited by marbalon on Wed Feb 22, 2012 9:18 am, edited 1 time in total.

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

Re: Alt. Hold Ideas and discussion

Post by alexmos »

guru_florida wrote:Hi alexmos,

No luck here. Copter just flies away (into the sky).

I adjusted the THROTTLE_HOVER/SHIFT to 1275 which was my throttle just before the copter would take off. Maybe it's just my PIDs are way off. I left P as default 4.7. TRUSTED_ACCZ is enabled. I am using the BMP085 like your setup.

Also, as a warning to others who try, It seems the code ignores the AUX modes as setup in the GUI. Looks like AUX1 activates HOLD and is hard coded. Just to be safe, test your setup by holding the copter first - with a good grip. This is not a big deal if the algorithm works, easy to fix afterwards.

I'm really looking forward to seeing Alt hold working!


My PIDS for Alt are: P=10, I=0.05, D=10
Vel PIDS are not used.

Alt Hold activated by BARO box, and it start working only if BARO becomes green in GUI. Sonar is ON by default (if you enabled it in config), but may be turned off by activating PASSTHRU on Aux1.

Before flight, strongly recomended to test it in GUI:
- enable ALT_DEBUG in config.h
- in GUI, debug1= sensor alt (sonar or baro or mixed), debug2, debug3= estimation errors, debug4= throttle correction, heading=velocity, alt=estimated alt

Estimated alt should follow sensor alt, and after enabling Alt Hold, throttle correction should be near zero if position doesn't changes, and react to any change in altitude.

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

Re: Alt. Hold Ideas and discussion

Post by alexmos »

And try to disable THROTTLE_EXPO - this mod is not related to alt hold! This is only trottle curve correction.

MikSam
Posts: 10
Joined: Wed Feb 22, 2012 5:27 am

Re: Alt. Hold Ideas and discussion

Post by MikSam »

marbalon wrote:Yesterday I've made another version. It is still using baro only. I will cleanup the code and share it today.

http://youtu.be/ia5bRUce74A

Marcin.

That is pretty impressive!
Can you achieve such accuracy with BMP085-baro too?

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

Re: Alt. Hold Ideas and discussion

Post by Magnetron »

Alexinparis wrote:
nhadrian wrote:Looks better but temperature value is not valid I think.
On my desk it shows around 500 for temperature (printed out to debug2), but should be 2000-2100 because we have 20C inside now.


right,
another bug:

=>

Code: Select all

  temperature  = 2000 + ((dT * ms561101ba_ctx.c[6])>>23) ;


Hi Alex,
I have corrected the bug in the dev 20120219.
I am reading the temperature compensation code that is:

Code: Select all

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

I think that is more efficient to rewrite "pow" functions and divisions in ">>" like this:

Code: Select all

  if (temperature < 2000) 
  {
    // temperature lower than 20st.C
    t2 = (dT*dT)>>31;
    off2 = 5 * (((int32_t)temperature-2000)*((int32_t)temperature-2000))>>1;
    sens2 = 5 * (((int32_t)temperature-2000)*((int32_t)temperature-2000))>>2;
    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))>>1;
    }
  }
  temperature = temperature - t2;
  off = off - off2;
  sens = sens - sens2;
  pressure     = (( (ms561101ba_ctx.up.val * sens ) / pow(2,21)) - off)>>15;

Is it exact?

msev
Posts: 186
Joined: Thu Apr 14, 2011 11:49 am

Re: Alt. Hold Ideas and discussion

Post by msev »

Capt would you be so kind to make an outdoor test video of your setup in altitude hold, since I'm planning to use the same hardware (bma020,bmp085)...
Marbalon you have also one setup with bmp085 right? Would you also be so kind to make an outside test video?
Thank you both guys!

Post Reply