Altitude Hold improvement solution

shikra · Post by **shikra** » Mon Sep 10, 2012 10:19 am

Alex - awesome effort. It looks pretty good on the vid. Look forward to testing this soon.

vpb · Post by **vpb** » Mon Sep 10, 2012 1:15 pm

@dramida: I've done the ALT tuning, and decided to stop at 3.0 15 10. It's similar to your PID, my tri now hold atitude very good, low 50cm, 3m or 20m. For the I term, it seems that at zero it's good, higher value like 20 30 I saw no noticeable difference except sometimes it loosed height slowly, I went out of all my battery packs so can not do further test on I term, maybe I'll do it again tomorrow.

One small problem, when it held alt, it started to dift (back & right)... so I'll do ACC tuning again.

mahowik · Post by **mahowik** » Mon Sep 10, 2012 3:20 pm

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

Maybe another set of eyes can spot something I missed.

Hi timecop,

Actually I can give some guaranties in case of using ms5611 and mpu6050 (also bma180 probably as it has good precision and right internal tunable LPF).

What acc and baro you are using? In case of bmp085 it has precision at least 5 time less

(

I have bma020+bmp085 and I will try to tune algorithm for that sensors and let you know soon...

thx-
Alex

timecop · Post by **timecop** » Mon Sep 10, 2012 3:24 pm

Thanks for reply.

I forget what was in my test quad. I think rev3 naze32 so MMA8452 acc + MPU3050 + BMP085.
Buut, with default alt pids and BMP085 things more or less worked (yes it bounced a bit but nothing like after these changes).
I believe nicog is going to be testing on hardware with mma8452+mpu6050+MS5611 soon.

copterrichie · Post by **copterrichie** » Mon Sep 10, 2012 3:33 pm

Just curious here being what controls Altitude on a Rotor copter is the throttle, have anyone made any adjustments to the MWC's Throttle Curve? Personally, I get good altitude holding without the usage of a baro.

Crashpilot1000 · Post by **Crashpilot1000** » Mon Sep 10, 2012 3:43 pm

@mahowik:
Thank you very much for making your code public. I will try to integrate it as well.

Thanks!

Rob

mahowik · Post by **mahowik** » Mon Sep 10, 2012 3:44 pm

copterrichie wrote:Just curious here being what controls Altitude on a Rotor copter is the throttle, have anyone made any adjustments to the MWC's Throttle Curve? Personally, I get good altitude holding without the usage of a baro.

Yes, expo throttle can help to keep altitude BUT not for long term and in calm...

copterrichie · Post by **copterrichie** » Mon Sep 10, 2012 3:47 pm

mahowik wrote:Yes, expo throttle can help to keep altitude BUT not for long term and in calm...

Define long term?

mahowik · Post by **mahowik** » Mon Sep 10, 2012 4:19 pm

copterrichie wrote:Define long term?

not sure that i understood the question...

copterrichie · Post by **copterrichie** » Mon Sep 10, 2012 4:22 pm

mahowik wrote:
copterrichie wrote:Define long term?

not sure that i understood the question...

Here is your statement:"Yes, expo throttle can help to keep altitude BUT not for long term and in calm..."

How do you defineLONG TERM? In other words, what does Long Term means to you?

vpb · Post by **vpb** » Mon Sep 10, 2012 4:22 pm

@copterrichie: do you mean RC expo? I always fly with 40-45% expo, with BARO always ON, I feel smooth throttle, just that, no idea about ALT Hold

. I think mahowik meant the ALT Hold for long time.

fiendie · Post by **fiendie** » Mon Sep 10, 2012 4:27 pm

copterrichie wrote:Here is your statement:"Yes, expo throttle can help to keep altitude BUT not for long term and in calm..."
How do you define LONG TERM? In other words, what does Long Term means to you?

You can't be serious...
Are you really insinuating that you can hold your copter at a certain altitude indefinitely with just Throttle Expo?
And please, try yawing around with your method and tell me what happens.

If you don't need baro-based altitude hold, that's fine.
But please don't derail the conversation.

copterrichie · Post by **copterrichie** » Mon Sep 10, 2012 4:28 pm

vpb wrote:@copterrichie: do you mean RC expo? I always fly with 40-45% expo, with BARO always ON, I feel smooth throttle, just that, no idea about ALT Hold . I think mahowik meant the ALT Hold for long time.

Here is the deal in MY OPINION, because each copter build and AUW are unique, the point where they will hover on throttle will vary. Ideally, we want that hover zone at about 50-60% throttle (for me anyways), so we want to flatten the curve at that point. So I do not get the up and down motion as much, yes in high winds, there is added lift as the wind crosses the props but nothing major.

copterrichie · Post by **copterrichie** » Mon Sep 10, 2012 4:29 pm

fiendie wrote:
copterrichie wrote:Here is your statement:"Yes, expo throttle can help to keep altitude BUT not for long term and in calm..."
How do you define LONG TERM? In other words, what does Long Term means to you?

You can't be serious...
Are you really insinuating that you can hold your copter at a certain altitude indefinitely with just Throttle Expo?
And please, try yawing around with your method and tell me what happens.

If you don't need baro-based altitude hold, that's fine.
But please don't derail the conversation.

Believe what you wish, I am very happy with the performance of my copters and the Work that has gone into the MWC.

Thanks to everyone and the Throttle Expo does work!

mahowik · Post by **mahowik** » Mon Sep 10, 2012 4:36 pm

copterrichie wrote:Here is your statement:"Yes, expo throttle can help to keep altitude BUT not for long term and in calm..."

How do you defineLONG TERM? In other words, what does Long Term means to you?

I mean when you tune expo throttle in mwii from GUI (or on your TX) it's very simple to find the throttle stick position manually when copter starts to hover. But after some time (e.g. in calm weather) about 10-20sec on my current config it's start to move up or down... So i mean that even you have correct setup of expo throttle it will keep altitude only for some time (10..20sec), i.e. not in long term and we need alt hold function in any case...

vpb · Post by **vpb** » Mon Sep 10, 2012 4:37 pm

copterrichie wrote:Here is the deal in MY OPINION, because each copter build and AUW are unique, the point where they will hover on throttle will vary. Ideally, we want that hover zone want that hover zone at about 50-60% throttle (for me anyways), so we want to flatten the curve at that point. So I do not that that up and down motion, yes in high winds, there is added lift as the wind crosses the props but nothing major.

I think different, at hover zone, (as you say it's 50-60% throttle), you dont want to go up or down much, you want to stay, so the curve should step in for the in-sensitive throttle signal. It should be curve, not flat

. But, it depends on kit and pilot's behavior.

fiendie · Post by **fiendie** » Mon Sep 10, 2012 4:40 pm

copterrichie wrote:Believe what you wish, I am very happy with the performance of my copters and the Work that has gone into the MWC.

I totally believe you. But remind me: What's that got to do with the improvements to baro-based alt hold which mahowik propesd?

Thanks to everyone and the Throttle Expo does work!

Seriously, how can you use that as an opportunity for brownnosing?

Either you are trolling or you're completely missing the point.

copterrichie · Post by **copterrichie** » Mon Sep 10, 2012 4:40 pm

"I think different, at hover zone, (as you say it's 50-60% throttle), you dont want to go up or down much, you want to stay, so the curve should step in for the in-sensitive throttle signal. It should be curve, not flat

. But, it depends on kit and pilot's behavior."

Semantics.

copterrichie · Post by **copterrichie** » Mon Sep 10, 2012 4:43 pm

fiendie wrote:
copterrichie wrote:Believe what you wish, I am very happy with the performance of my copters and the Work that has gone into the MWC.

I totally believe you. But remind me: What's that got to do with the improvements to baro-based alt hold which mahowik propesd?

Thanks to everyone and the Throttle Expo does work!

Seriously, how can you use that as an opportunity for brownnosing?

Either you are trolling or you're completely missing the point.

IMO and (I don't use a Baro), if you start with a good throttle curve/control, it should make the Baro routine and tuning much easier.

vpb · Post by **vpb** » Mon Sep 10, 2012 4:52 pm

copterrichie wrote:Semantics.

Sorry I dont know what you mean (English is not my primary language).

Btw, I dont think many people fly multicopter to just hold at a level for a long time, just stand a couple of seconds then fly away, a stable copter w/ or w/o baro can help. With my tricopter, I see the big different in throttle smoothness with expo.

copterrichie · Post by **copterrichie** » Mon Sep 10, 2012 4:55 pm

vpb wrote:
copterrichie wrote:Semantics.

Sorry I dont know what you mean (English is not my primary language).

Btw, I dont think many people fly multicopter to just hold at a level for a long time, just stand a couple of seconds then fly away, a stable copter w/ or w/o baro can help. With my tricopter, I see the big different in throttle smoothness with expo.

My copters and flying skills are tailored around aerial video, so I have trained myself and built my copters around that theme. Yes, I do remain stationary in one altitude and locations for minutes at a time.

mahowik · Post by **mahowik** » Mon Sep 10, 2012 5:01 pm

Guys! Pls. stop this SPAM!!! This topic about alt-hold improvement solutions!!!

copterrichie · Post by **copterrichie** » Mon Sep 10, 2012 5:05 pm

mahowik wrote:Guys! Pls. stop this SPAM!!! This topic about alt-hold improvement solutions!!!

LOL, I just give you the most important factor to improve Alt-holding. LOL LOL LOL

nicog · Post by **nicog** » Mon Sep 10, 2012 5:07 pm

mahowik I will try to do a test of your code tomorrow.

With old algo I can get a very good althold while hover. But It would be a plus if I can do dynamic movements at same altitude.

vpb · Post by **vpb** » Mon Sep 10, 2012 5:08 pm

Glad to hear that, I'll search some of your videos. I'm new to multiwii & multicopter, I want to fly FPV slow, can stop to watch, that's why I build multicopter... but I'm far from the topic's point

.

mahowik · Post by **mahowik** » Mon Sep 10, 2012 5:30 pm

nicog wrote:mahowik I will try to do a test of your code tomorrow.

With old algo I can get a very good althold while hover. But It would be a plus if I can do dynamic movements at same altitude.

one more advantage with using acc+baro algorithm: when you go up/down with velocity of 1-2m/s and activate alt hold, it must stop immediately and start to hover

p.s. I have already got one feedback from russian forum. It keeps +/-30cm outside during the wind

http://forum.rcdesign.ru/f123/thread283 ... ost3613804

nhadrian · Post by **nhadrian** » Mon Sep 10, 2012 6:25 pm

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

Maybe another set of eyes can spot something I missed.

Hi,

unfortunatelly the same here. Doesn't matter what PID I set up...

BR
Adrian

mahowik · Post by **mahowik** » Mon Sep 10, 2012 6:29 pm

probably bmp085 no so precise and has big noise for this solution/approach... i will check in a few day...

timecop · Post by **timecop** » Mon Sep 10, 2012 6:33 pm

bmp isn't that bad. I'm talking this thing going up 20+ meters.
I have reasonable althold on it at default settings.
might try with other hw tomorrow.

dramida · Post by **dramida** » Mon Sep 10, 2012 8:59 pm

Today I flew with alt hold and I can say it is satisfying stable. It could be better but right now, I got the sweet spot. 20 cm of error. Video soon.

mahowik · Post by **mahowik** » Mon Sep 10, 2012 9:10 pm

dramida wrote:Today I flew with alt hold and I can say it is satisfying stable. It could be better but right now, I got the sweet spot. 20 cm of error. Video soon.

Do you want to test baro+acc solution?

viewtopic.php?f=8&t=2371&p=22485#p22485

As I remember default 2.1 alt hold could not keep altitude in wind... only calm and only static because during the flights baro hasn't necessary reaction to produce compensations...

nhadrian · Post by **nhadrian** » Tue Sep 11, 2012 11:36 am

Update:

I tried again with Ales's code, now I succeed, with the 5;0,030;30 PIDs it works great, even in windy conditions with GPS hold!!!

mahowik · Post by **mahowik** » Tue Sep 11, 2012 1:33 pm

nhadrian wrote:Update:

I tried again with Ales's code, now I succeed, with the 5;0,030;30 PIDs it works great, even in windy conditions with GPS hold!!!

Many thanks for the tests! Which acc and baro you are using?

my current PIDs 5.2 - 0.020 - 30

@timecop: I also tried to test bma020+bmp085 yesterday (in GUI only). Seems it works but required some tuning of coefficients/factors accordingly... pls try this one

Code: Select all

..................
#define ACC_LPF_FOR_VELOCITY 15
..................

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

#define ACC_Z_DEADBAND (acc_1G/40) 

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

  static int16_t baroHistTab[BARO_TAB_SIZE];
  static int8_t baroHistIdx;
  static int32_t baroHigh;
  
 
  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  uint16_t dTime = currentTime - deadLine;
  deadLine = currentTime;
  

  //**** Alt. Set Point stabilization PID ****
  baroHistTab[baroHistIdx] = BaroAlt/10;
  baroHigh += baroHistTab[baroHistIdx];
  baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
  
  baroHistIdx++;
  if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;


  //EstAlt = baroHigh*10/(BARO_TAB_SIZE-1);
  EstAlt = EstAlt*0.65f + (baroHigh*10.0f/(BARO_TAB_SIZE-1))*0.35f; // additional LPF to reduce baro noise
  
  //P
  int16_t error = constrain(AltHold - EstAlt, -300, 300);
  error = applyDeadband16(error, 10); //remove small P parametr to reduce noise near zero position
  BaroPID = constrain((conf.P8[PIDALT] * error / 100), -150, +150);
  
  //I
  errorAltitudeI += error * conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  BaroPID += (errorAltitudeI / 500); //I in range +/-60
  
  
  // projection of ACC vector to global Z, with 1G subtructed
  // Math: accZ = A * G / |G| - 1G
  float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G; 
  //int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - 1/invG; 
  accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);
  debug[0] = accZ; 
  
  static float vel = 0.0f;
  static float accVelScale = 9.80665f / acc_1G / 10000.0f;
  
  // Integrator - velocity, cm/sec
  vel+= accZ * accVelScale * dTime;
  
  static int32_t lastBaroAlt = EstAlt;
  float baroVel = (EstAlt - lastBaroAlt) / (dTime/1000000.0f);
  baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
  baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero  
  lastBaroAlt = EstAlt;
  debug[1] = baroVel;
  
  // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). 
  // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
  vel = vel * 0.987f + baroVel * 0.013f;
  //vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s 
  debug[2] = vel;
  
  //D
  BaroPID -= constrain(conf.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
  debug[3] = BaroPID; 
}

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

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

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

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

mahowik · Post by **mahowik** » Tue Sep 11, 2012 1:38 pm

Also here is little bit refactored version for ms5611

Code: Select all

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

#define ACC_Z_DEADBAND (acc_1G/50) 

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

  static int16_t baroHistTab[BARO_TAB_SIZE];
  static int8_t baroHistIdx;
  static int32_t baroHigh;
  
 
  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  uint16_t dTime = currentTime - deadLine;
  deadLine = currentTime;
  

  //**** Alt. Set Point stabilization PID ****
  baroHistTab[baroHistIdx] = BaroAlt/10;
  baroHigh += baroHistTab[baroHistIdx];
  baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
  
  baroHistIdx++;
  if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;


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

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

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

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

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

wilco1967 · Post by **wilco1967** » Tue Sep 11, 2012 6:34 pm

Hi Mahowik,

I tried your version 2.1 code (download) and it worked pretty good (Tricopter with Flyduino mega, MPU6050 and BMP-085).

Then I tried to incorporatie the modified code into the current shared version, and there I must have missed something....
(There are so many other improvements in the shared since the official 2.1, that I do not want to go back)

As Timecop also mentioned a few pages back after he implemented your code, when activating alt-hold, mine also shoots into the air.... In the downloaded version, it did not do this..... It does seem to respond though after climbing a couple of meters.
It seems to me like an initalisation step or so is missing upon activation of alt-holt.... could be something completely different though :roll:

.

Is there somewhere in the code some other modification, that you did not show in your posts ? (but was obviously included in your 2.1 downloadable file).

There are soo many differences between the original 2.1 code, and the current shared, that I loose track.

Thanks a lot....
I really think you're on the right path with alt-hold

nhadrian · Post by **nhadrian** » Tue Sep 11, 2012 7:03 pm

mahowik wrote:

nhadrian wrote:Update:

I tried again with Ales's code, now I succeed, with the 5;0,030;30 PIDs it works great, even in windy conditions with GPS hold!!!

Many thanks for the tests! Which acc and baro you are using?

my current PIDs 5.2 - 0.020 - 30

@timecop: I also tried to test bma020+bmp085 yesterday (in GUI only). Seems it works but required some tuning of coefficients/factors accordingly... pls try this one

Code: Select all

..................
#define ACC_LPF_FOR_VELOCITY 15
..................

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

#define ACC_Z_DEADBAND (acc_1G/40) 

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

  static int16_t baroHistTab[BARO_TAB_SIZE];
  static int8_t baroHistIdx;
  static int32_t baroHigh;
  
 
  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  uint16_t dTime = currentTime - deadLine;
  deadLine = currentTime;
  

  //**** Alt. Set Point stabilization PID ****
  baroHistTab[baroHistIdx] = BaroAlt/10;
  baroHigh += baroHistTab[baroHistIdx];
  baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
  
  baroHistIdx++;
  if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;


  //EstAlt = baroHigh*10/(BARO_TAB_SIZE-1);
  EstAlt = EstAlt*0.65f + (baroHigh*10.0f/(BARO_TAB_SIZE-1))*0.35f; // additional LPF to reduce baro noise
  
  //P
  int16_t error = constrain(AltHold - EstAlt, -300, 300);
  error = applyDeadband16(error, 10); //remove small P parametr to reduce noise near zero position
  BaroPID = constrain((conf.P8[PIDALT] * error / 100), -150, +150);
  
  //I
  errorAltitudeI += error * conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  BaroPID += (errorAltitudeI / 500); //I in range +/-60
  
  
  // projection of ACC vector to global Z, with 1G subtructed
  // Math: accZ = A * G / |G| - 1G
  float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G; 
  //int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - 1/invG; 
  accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);
  debug[0] = accZ; 
  
  static float vel = 0.0f;
  static float accVelScale = 9.80665f / acc_1G / 10000.0f;
  
  // Integrator - velocity, cm/sec
  vel+= accZ * accVelScale * dTime;
  
  static int32_t lastBaroAlt = EstAlt;
  float baroVel = (EstAlt - lastBaroAlt) / (dTime/1000000.0f);
  baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
  baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero  
  lastBaroAlt = EstAlt;
  debug[1] = baroVel;
  
  // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). 
  // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
  vel = vel * 0.987f + baroVel * 0.013f;
  //vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s 
  debug[2] = vel;
  
  //D
  BaroPID -= constrain(conf.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
  debug[3] = BaroPID; 
}

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

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

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

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

Hi, I'm running on Crius AIO PRO board which means MS5611 and MPU6050 acc.

Here is a test video of my mini Y-6 copter in quite windy conditions:
http://youtu.be/POSwO33wFUU

PIDs are: P5.0 I0.030 D30

Works quite well, as you can see in the video!!! (please note that the copter is really small, 500g AUW so the movements in the video is not as much as it seems...)

BR Adrian

fiendie · Post by **fiendie** » Tue Sep 11, 2012 11:19 pm

mahowik wrote:Also here is little bit refactored version for ms5611

Code: Select all

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

#define ACC_Z_DEADBAND (acc_1G/50) 

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

  static int16_t baroHistTab[BARO_TAB_SIZE];
  static int8_t baroHistIdx;
  static int32_t baroHigh;
  
 
  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  uint16_t dTime = currentTime - deadLine;
  deadLine = currentTime;
  

  //**** Alt. Set Point stabilization PID ****
  baroHistTab[baroHistIdx] = BaroAlt/10;
  baroHigh += baroHistTab[baroHistIdx];
  baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
  
  baroHistIdx++;
  if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;


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

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

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

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

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

@mahowik: Would it be possible to provide the code as a patch against the current _shared repo? I would love to put a feature branch up on my GitHub and test it.
Thanks for the effort you put in so far!

Cheers
Andy

timecop · Post by **timecop** » Wed Sep 12, 2012 1:22 am

What would be really nice is all the constants broken out with explanations of what they do.
The last 2-3 code copies you pasted all look identical - probably just changing some fixed value somewhere, but its really hard to read them as they're not patches,, so, which numbers are you changing?

mahowik · Post by **mahowik** » Wed Sep 12, 2012 5:22 am

Hi guys!

All necessary changes for new baro+acc alt hold only in one file IMU.ino

You can just compare this with official IMU.ino from 2.1 version with any merge tool and apply necessary diff where you want: dev or your own...

1) for ms5611 - pls. use IMU_ms5611.ino
2) for bmp085 - at first also try to use IMU_ms5611.ino... and if it's not stable try IMU_bmp085.ino

Some notes:
- for mpu6050 pls turn on 42hz LPF (#define MPU6050_LPF_42HZ)... for other ACCs it's already done in Sensors.ino
- wait 10-15 sec after power on OR after ACC calibration... all LPFs, velocity integrator and CF need to be syncronized (i'm going to reduce init time... i.e. it's in my todo list

)
- after init you can check debug0 (z axis acceleration) and debug2 (calculated velocity) in GUI (with max zoom = 10)... it should swim near zero point: +/-2..5 for acceleration (debug0) and +/-5..15 for velocity (debug2)
- then turn on motors (with 40..50% power about throttle hover), keep it in hands, don't switch to alt hold and check the same (debug0, debug2) in GUI... without vertical movements z axis acceleration and calculated velocity should be in calm...
- then try to move up/down... you should see acceleration and velocity on each debug graphics
- try to turn on alt hold and move up/down... you should get immediate resistance when trying to push down and visible throttle reduce when going up...

Guys from our forum prepared the video

http://www.youtube.com/watch?v=qUlphS7D ... r_embedded

thx-
Alex

timecop · Post by **timecop** » Wed Sep 12, 2012 8:12 am

Hmm. More changes. I diff'd the posted with original 2.1 and made same changes to baseflight code.
it doesn't shoot up in the air anymore, but it also doesn't really hold, either. I've had better althold w/bmp085 and regular non-accZ code w/default PIDs. Used 5.0/0.03/30 for the test video:
http://www.youtube.com/watch?v=rQuw0RCMhbo
The video was done with acc_hardware=1 (MMA845x), but I then flew again with acc_hardware=2 (MPU6050 accel) and it was pretty much same.
Changes here:
http://code.google.com/p/afrodevices/so ... tail?r=217

mahowik · Post by **mahowik** » Wed Sep 12, 2012 2:06 pm

if you are talking about sketch from this post viewtopic.php?f=8&t=2371&start=40#p22485, yes there are more changes, but related to the new alt hold - all in IMU.ino...
I have a look to your code changes, seems it's ok.
So pls. check the following:
- cycle time should be about 3-4ms... otherwise you should re-tune LPFs and CF
- pls. make all test on altitude >=2m to avoid ground effect (on less altitudes measurement baro can give -1..2m and become very unstable)
- you dont' like this by actually bmp085 has less precision (+/-1m as I remember) BUT it make sense to tune PIDs

PIDs tuning:
1) make shure that velocity calculation works... set P and I to zero (only D=30) and try to activate alt hold when copter goes up (with velocity 1..2m/s). It's should try to stop copter immediatelly (but possible to drift because P and I are zero)! If no effect, something wrong at all and next steps doesn't make sense...
If ok, play with D to find "best stop effect"... probably D=20 it's enough...
2) keep "I" about 0.01... It's should be enough according to baro precision to avoid slow oscillations produced by "I" part
3) set P to 1.0 and try to increase before it start oscillate, then reduce for 20-30%

I suppose for bmp085 it should be about 2.0-0.01-20 or 3.0-0.01-25

If you get +/-50cm for bmp085 in calm weather I suppose it's quite ok BUT it will keep altitude in wind and during the flights... Default alt hold from 2.1 could not do that because it hasn't reaction with slow sensor like baro...

Good luck!

copterrichie · Post by **copterrichie** » Wed Sep 12, 2012 2:11 pm

I get better than +/- 50cm with nothing but Throttle control.

timecop · Post by **timecop** » Wed Sep 12, 2012 2:19 pm

copterrichie wrote:I get better than +/- 50cm with nothing but Throttle control.

Yes, we get it.
The point is having similar ( or better ) WITHOUT throttle control. Thanks!

mahowik · Post by **mahowik** » Wed Sep 12, 2012 2:25 pm

copterrichie wrote:I get better than +/- 50cm with nothing but Throttle control.

E.g. you can keep with more precise on altitude >30m during FPV in wind?
@copterrichie: again especially for you! stop SPAM!

copterrichie · Post by **copterrichie** » Wed Sep 12, 2012 2:33 pm

I have not looked for it but Alex made a statement back when the Alt-hold was introduced into the MWC. I'm paraphrasing but his statement was, in order for the Alt-hold to work, the copter first had to be pretty stable and held altitude without enabling the Alt-hold. Makes perfect sense to me, Garbage in, surely garbage out.

nicog · Post by **nicog** » Wed Sep 12, 2012 2:34 pm

I'm going to do a test when rain stops, if it stops one day here.

By the way I can have even better precision with my tongue also. I get around 1.2, 1.5 mm. Good enough copteriche? :-)

timecop · Post by **timecop** » Wed Sep 12, 2012 2:54 pm

mahowik wrote:So pls. check the following:
- cycle time should be about 3-4ms... otherwise you should re-tune LPFs and CF
- pls. make all test on altitude >=2m to avoid ground effect (on less altitudes measurement baro can give -1..2m and become very unstable)
- you dont' like this by actually bmp085 has less precision (+/-1m as I remember) BUT it make sense to tune PIDs

That test was with MS5611 and cycletime of 3000. I didn't cover it at the time, but later on (off-camera) I taped some foam over it, and there wasn't much difference. There's a plastic cover on velcro over the FC, so while there's some wind that might get into it I don't think its enough to majorly affect it. I'll try playing around with PID I suppose. Also I'm not having very clear accZ effect - its there, but not as much as in demo video. Same result with both MMA845x or MPU6050 accel.

fiendie · Post by **fiendie** » Wed Sep 12, 2012 3:18 pm

copterrichie wrote:I have not looked for it but Alex made a statement back when the Alt-hold was introduced into the MWC. I'm paraphrasing but his statement was, in order for the Alt-hold to work, the copter first had to be pretty stable and held altitude without enabling the Alt-hold. Makes perfect sense to me, Garbage in, surely garbage out.

Still trolling are we?

Let me put it this way: The more accurate it works without your uber-awesome "I-can-hold-my-altitude-with-sheer-willpower"-optimizations the better, don't you think?

copterrichie · Post by **copterrichie** » Wed Sep 12, 2012 3:34 pm

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

This Thread started out BASHING the Fine work and Accomplishments of this outstanding Open source project. Just because people don't like open OPINIONS, that is no concern of mines. :lol:

mahowik · Post by **mahowik** » Wed Sep 12, 2012 5:10 pm

timecop wrote:Also I'm not having very clear accZ effect - its there, but not as much as in demo video.

have you tried to check in GUI as I described above?