Altitude Hold improvement solution

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
User avatar
shikra
Posts: 783
Joined: Wed Mar 30, 2011 7:58 pm

Re: Altitude Hold improvement solution

Post by shikra »

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

vpb
Posts: 231
Joined: Mon Jul 23, 2012 4:09 pm

Re: Altitude Hold improvement solution

Post by vpb »

@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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Post by timecop »

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
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

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.

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Altitude Hold improvement solution

Post by Crashpilot1000 »

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

Thanks!

Rob

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

Re: Altitude Hold improvement solution

Post by mahowik »

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
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

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


Define long term?

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

Re: Altitude Hold improvement solution

Post by mahowik »

copterrichie wrote:Define long term?

not sure that i understood the question...

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

Re: Altitude Hold improvement solution

Post by copterrichie »

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?
Last edited by copterrichie on Mon Sep 10, 2012 4:23 pm, edited 1 time in total.

vpb
Posts: 231
Joined: Mon Jul 23, 2012 4:09 pm

Re: Altitude Hold improvement solution

Post by vpb »

@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 :D. I think mahowik meant the ALT Hold for long time.

fiendie
Posts: 151
Joined: Fri Apr 20, 2012 4:22 pm

Re: Altitude Hold improvement solution

Post by fiendie »

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
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

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 :D. 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.
Last edited by copterrichie on Mon Sep 10, 2012 4:36 pm, edited 1 time in total.

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

Re: Altitude Hold improvement solution

Post by copterrichie »

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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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
Posts: 231
Joined: Mon Jul 23, 2012 4:09 pm

Re: Altitude Hold improvement solution

Post by vpb »

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 :D. But, it depends on kit and pilot's behavior.

fiendie
Posts: 151
Joined: Fri Apr 20, 2012 4:22 pm

Re: Altitude Hold improvement solution

Post by fiendie »

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
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

"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 :D. But, it depends on kit and pilot's behavior."

Semantics.

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

Re: Altitude Hold improvement solution

Post by copterrichie »

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
Posts: 231
Joined: Mon Jul 23, 2012 4:09 pm

Re: Altitude Hold improvement solution

Post by vpb »

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
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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

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

Re: Altitude Hold improvement solution

Post by copterrichie »

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
Posts: 88
Joined: Tue Aug 21, 2012 2:21 pm

Re: Altitude Hold improvement solution

Post by nicog »

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
Posts: 231
Joined: Mon Jul 23, 2012 4:09 pm

Re: Altitude Hold improvement solution

Post by vpb »

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

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

Re: Altitude Hold improvement solution

Post by mahowik »

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
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Altitude Hold improvement solution

Post by nhadrian »

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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Post by timecop »

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.

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

Re: Altitude Hold improvement solution

Post by dramida »

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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Altitude Hold improvement solution

Post by nhadrian »

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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Post by wilco1967 »

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
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Altitude Hold improvement solution

Post by nhadrian »

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
Posts: 151
Joined: Fri Apr 20, 2012 4:22 pm

Re: Altitude Hold improvement solution

Post by fiendie »

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
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Post by timecop »

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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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
Last edited by mahowik on Sat Dec 15, 2012 11:18 pm, edited 1 time in total.

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Post by timecop »

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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

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

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Post by timecop »

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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

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
Posts: 88
Joined: Tue Aug 21, 2012 2:21 pm

Re: Altitude Hold improvement solution

Post by nicog »

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
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Post by timecop »

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
Posts: 151
Joined: Fri Apr 20, 2012 4:22 pm

Re: Altitude Hold improvement solution

Post by fiendie »

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
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

"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
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

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?

Post Reply