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
Post Reply
Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Altitude Hold improvement solution

Post by Sebbi »

Here is a video from the drop and a few minutes of altitude hold with only pitch/roll correction from my side: http://www.youtube.com/watch?v=i0dX4NYVpk4

Termic1
Posts: 40
Joined: Tue Aug 21, 2012 11:14 am

Re: Altitude Hold improvement solution

Post by Termic1 »

Tested today GPS HOLD & ALT HOLD using r1129 with AP_MODE BARO bug corrected and I shoud say that now the altitude and position control of the multicopter is really good. I'd like to thank again all the developers of these functions. Can't wait now to test new ALT TO RTH routines ...but I have to wait for a version integrated in the new dev as I can't use 2.1 (2.1 does not support GPS auto config)

Here a short video of my last happy flight ...hands off from the RC transmitter! http://www.youtube.com/watch?v=T3htaJ53Z7E

Luciano

alexia
Posts: 85
Joined: Sun Jun 17, 2012 10:23 pm
Contact:

Re: Altitude Hold improvement solution

Post by alexia »

does it hold well with more winters?

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

Re: Altitude Hold improvement solution

Post by mahowik »

Sebbi wrote:Hi Alex,

1) could very well be, have to confirm is EstAlt was lagging behind ... but the copter was hovering for a few seconds before I activated alt hold. Will try to log that behaviour tomorrow to confirm.
2) and yeah, i noticed in another flight that the code counteracts whatever I do with the trottle, but that's not what i meant (not a native speaker either). I didn't touch the throttle stick, it was rising/cutting throttle on its own. Maybe a result of my changed baro sensor code and certain lag in EstAlt update rate (there is additional LPF there)
3) I understand. That should probably be mentioned somewhere and maybe it would be good to split getEstimatedAltitude into 2 functions, one for actual altitude estimation and one that calculates the PIDs at a much faster rate (because it now relies on the acc values). What's your take on that?


1) probably it's related to your changes... make sense to debug/profile from IN to OUT...
also have an idea to integrate the calculated velocity (and keep it by applying BaroAlt via CF) to get EstAlt w/o (or less) delay... BUT more than one integrator from noisy input data it's double chance to lost control of stable routine :)
2) yes, if EstAlt has another lag/delay, baro velocity (baroVel) is also "other"and it means that you need at least to re-tune vel/baroVel CF... also you can check vel/baroVel vars in GUI and baroVel should approximately repeat vel, during no so fast vertical movements BUT of course with delay...
3) agree, it make sense to split alt estimator and pid controller but I was lazy with this and just applied to 2.1 code... Also you know some kind of Old school IT practice: if it works good just don't touch it! :)

thx-
Alex

Imen
Posts: 20
Joined: Thu Sep 27, 2012 5:11 pm

Re: Altitude Hold improvement solution

Post by Imen »

Hi ......
Is there any issue about copter going to up, when navigating like RTH ?, like speed nav to fast or any effect from ALT_HOLD ?

Mis
Posts: 203
Joined: Fri Apr 01, 2011 12:23 am

Re: Altitude Hold improvement solution

Post by Mis »

Sebbi.
I think, but not sure that altitude drop after switch althold on can be caused by 1G constants difference from real 1G reading.
I make some code changes for runtime 1G measuring. Measuring is done within 1 second after gyro calibration ends.
Additionally i do one more modification for gyro calibration. If the copter is moving during gyro calibration, the procedure is stopped until gyro readings is stable. From now no false gyro calibrations if we move the copter during power-up.

Here is the changes.
Sensors.ino:

Code: Select all

void GYRO_Common() {
  static int16_t previousGyroADC[3] = {0,0,0};
  static int32_t g[3];
  uint8_t axis, tilt=0;

#if defined MMGYRO       
  // Moving Average Gyros by Magnetron1
  //---------------------------------------------------
  static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGHT];
  static int32_t mediaMobileGyroADCSum[3];
  static uint8_t mediaMobileGyroIDX;
  //---------------------------------------------------
#endif

  if (calibratingG>0) {
    for (axis = 0; axis < 3; axis++) {
      // Reset g[axis] at start of calibration
      if (calibratingG == 400) {
        g[axis]=0;
        previousGyroADC[axis] = gyroADC[axis];
      }
      if (calibratingG % 10 == 0) {
        if(abs(gyroADC[axis] - previousGyroADC[axis]) > 8) tilt=1;
        previousGyroADC[axis] = gyroADC[axis];
      }
      // Sum up 400 readings
      g[axis] +=gyroADC[axis];
      // Clear global variables for next reading
      gyroADC[axis]=0;
      gyroZero[axis]=0;
      if (calibratingG == 1) {
        gyroZero[axis]=g[axis]/400;
        blinkLED(10,15,1);
      }
    }
    if(tilt) {
      calibratingG=1000;
      LEDPIN_ON;
    } else {
      calibratingG--;
      LEDPIN_OFF;
    }
    return;
  }
...


IMU.ino:

Code: Select all

...
  // projection of ACC vector to global Z
  // Math: accZ = A * G / |G|
  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;
//  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;

  // runtime measuring 1G after any gyro calibration
  static int16_t measured1G = 8*acc_1G;    // filter init
  static uint8_t count;
  if(calibratingG) count=40;               // restart measuring after gyro calibration
  if(f.ARMED) count=0;                     // stop measuring if armed
  if(!calibratingG && count) {             // within 1 sec after end of gyro calibration (sure that is stable)
    measured1G -= measured1G/8;
    measured1G += accZ;                    // filter ACC readings
    accZ = 0;
    count--;
  } else {
    accZ -= measured1G/8;                  // substraction of measured 1G from calculated accZ
  }
  applyDeadband(accZ, ACC_Z_DEADBAND);
...


This is only tested on the desk, not during flight.

flyrobot
Posts: 73
Joined: Thu Apr 05, 2012 3:59 pm

Re: Altitude Hold improvement solution

Post by flyrobot »

Hi Mahowik,

I just try alt hold on B3 version. i have problem when the alt hold is on and i tried to move the nick and roll stick and the quad become very hard to control and my quad crash, broke one of prop.
I tried B2 version exactly same behavior as Sebbi's video.

Tomorrow i am going to try again the B3 with different PID setting.

Thanks,

John

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

Re: Altitude Hold improvement solution

Post by dramida »

Good move Mis. I'll test your new calibration routine. On what MWC release should i make changes? B3 from Mahowik or some main trunk dev?
/Mihai

Mis wrote:Sebbi.
I think, but not sure that altitude drop after switch althold on can be caused by 1G constants difference from real 1G reading.
I make some code changes for runtime 1G measuring. Measuring is done within 1 second after gyro calibration ends.
Additionally i do one more modification for gyro calibration. If the copter is moving during gyro calibration, the procedure is stopped until gyro readings is stable. From now no false gyro calibrations if we move the copter during power-up.

Here is the changes.
Sensors.ino:

Code: Select all

void GYRO_Common() {
  static int16_t previousGyroADC[3] = {0,0,0};
  static int32_t g[3];
  uint8_t axis, tilt=0;

#if defined MMGYRO       
  // Moving Average Gyros by Magnetron1
  //---------------------------------------------------
  static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGHT];
  static int32_t mediaMobileGyroADCSum[3];
  static uint8_t mediaMobileGyroIDX;
  //---------------------------------------------------
#endif

  if (calibratingG>0) {
    for (axis = 0; axis < 3; axis++) {
      // Reset g[axis] at start of calibration
      if (calibratingG == 400) {
        g[axis]=0;
        previousGyroADC[axis] = gyroADC[axis];
      }
      if (calibratingG % 10 == 0) {
        if(abs(gyroADC[axis] - previousGyroADC[axis]) > 8) tilt=1;
        previousGyroADC[axis] = gyroADC[axis];
      }
      // Sum up 400 readings
      g[axis] +=gyroADC[axis];
      // Clear global variables for next reading
      gyroADC[axis]=0;
      gyroZero[axis]=0;
      if (calibratingG == 1) {
        gyroZero[axis]=g[axis]/400;
        blinkLED(10,15,1);
      }
    }
    if(tilt) {
      calibratingG=1000;
      LEDPIN_ON;
    } else {
      calibratingG--;
      LEDPIN_OFF;
    }
    return;
  }
...


IMU.ino:

Code: Select all

...
  // projection of ACC vector to global Z
  // Math: accZ = A * G / |G|
  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;
//  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;

  // runtime measuring 1G after any gyro calibration
  static int16_t measured1G = 8*acc_1G;    // filter init
  static uint8_t count;
  if(calibratingG) count=40;               // restart measuring after gyro calibration
  if(f.ARMED) count=0;                     // stop measuring if armed
  if(!calibratingG && count) {             // within 1 sec after end of gyro calibration (sure that is stable)
    measured1G -= measured1G/8;
    measured1G += accZ;                    // filter ACC readings
    accZ = 0;
    count--;
  } else {
    accZ -= measured1G/8;                  // substraction of measured 1G from calculated accZ
  }
  applyDeadband(accZ, ACC_Z_DEADBAND);
...


This is only tested on the desk, not during flight.

Mis
Posts: 203
Joined: Fri Apr 01, 2011 12:23 am

Re: Altitude Hold improvement solution

Post by Mis »

I have acces to repository, but before I commit this idea, i need some positive feedback.
Too many my ideas have been nuked by other programmers.

flyrobot
Posts: 73
Joined: Thu Apr 05, 2012 3:59 pm

Re: Altitude Hold improvement solution

Post by flyrobot »

flyrobot wrote:Hi Mahowik,

I just try alt hold on B3 version. i have problem when the alt hold is on and i tried to move the nick and roll stick and the quad become very hard to control and my quad crash, broke one of prop.
I tried B2 version exactly same behavior as Sebbi's video.

Tomorrow i am going to try again the B3 with different PID setting.

Thanks,

John

It seems the problem above is just the stock PID setting need tobe changed. This morning i tried again, have one problem.
When the alt hold switch active the pos hold is not active. Alt hold is better but still the drop 50-1 meter after the alt hold activated.

John

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

Re: Altitude Hold improvement solution

Post by mahowik »

Sebbi wrote:1) could very well be, have to confirm is EstAlt was lagging behind ... but the copter was hovering for a few seconds before I activated alt hold. Will try to log that behaviour tomorrow to confirm.

Mis wrote:I think, but not sure that altitude drop after switch althold on can be caused by 1G constants difference from real 1G reading.

flyrobot wrote:Alt hold is better but still the drop 50-1 meter after the alt hold activated.


Hi guys,

Yesterday I reproduced this issue (drop altitude after switch althold)...
ACC was calibrated at home, but temperature outside was less for 10-15C. So as result accZ was not zero and calculated velocity far from zero accordingly... This because I used static acc_1G offset... After adaptation to outside temperature and ACC recalibration the issue is gone... Also this issue can be resolved with acc_1G_calculated, but you should not move copter copter some seconds before the start:

Code: Select all

// 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));
 
  static int16_t acc_1G_calculated;
  if (!f.ARMED) {
    acc_1G_calculated = 1/invG;
  }


@Mis: I like your solution too, but I suppose measured1G will be incorrect because when power ON, accZ will be stabilized only after 5-10sec, so to use your solution you need to run gyro calibration from TX (transmitter) or just add 5-10sec delay after start... in any case I will try it and let you know... also some optimizations to your solution ;)

Code: Select all

    ...
      // projection of ACC vector to global Z
      // Math: accZ = A * G / |G|
      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;
    //  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;

      // runtime measuring 1G after any gyro calibration
      static int16_t measured1G = acc_1G << 3;    // filter init
      static uint8_t count;
      if(calibratingG) count = 40;               // restart measuring after gyro calibration
      if(f.ARMED) count = 0;                     // stop measuring if armed
      if(!calibratingG && count) {             // within 1 sec after end of gyro calibration (sure that is stable)
        measured1G -= measured1G >> 3;
        measured1G += accZ;                    // filter ACC readings
        //accZ = 0;
        count--;
        if(!count) {
          measured1G = measured1G >> 3;
        }
      } else {
        accZ -= measured1G;                  // substraction of measured 1G from calculated accZ
      }
      applyDeadband(accZ, ACC_Z_DEADBAND);
    ...



thx-
Alex

JFSebastian
Posts: 7
Joined: Sat Mar 10, 2012 7:26 pm

Re: Altitude Hold improvement solution

Post by JFSebastian »

Guys, this thread is getting very confusing.

What version do I use if I want alt hold that works?


Sebastian

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

Re: Altitude Hold improvement solution

Post by mahowik »

JFSebastian wrote:Guys, this thread is getting very confusing.

What version do I use if I want alt hold that works?


Sebastian


if you want just try pls. use last dev http://code.google.com/p/multiwii/downl ... akechanges

JFSebastian
Posts: 7
Joined: Sat Mar 10, 2012 7:26 pm

Re: Altitude Hold improvement solution

Post by JFSebastian »

Thanks ;)

Mis
Posts: 203
Joined: Fri Apr 01, 2011 12:23 am

Re: Altitude Hold improvement solution

Post by Mis »

mahovik:
Your change to my code have one dangerous situation. If you arm the copter before "count" reached zero, the count variable is zeroed, and "measured1G" variable is not divided by 8. Then you substract crazy value from accZ.
I think, about good solution, and maybe fusion of your method and my method should be best choice. Many stronger LPF filter on "measured1G" variable, measure and filter it from startup (afrer startup delay defined in "#define INIT_DELAY" - now done) until to arming moment (until f.ARMED). The filter value is now 8, but we can change it to even 64 (64*512 = 32767) and change "measured1G" type to uint16_t because this value should never gone bellow 0.

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

Re: Altitude Hold improvement solution

Post by dramida »

Here is how altitude holds in wind with MultiWii-R1177 http://www.youtube.com/watch?v=SB_O4swILJU

Thanks alot boys. Any news about baro- AccZ controlled climb/descent?

scrat
Posts: 925
Joined: Mon Oct 15, 2012 9:47 am
Location: Slovenia

Re: Altitude Hold improvement solution

Post by scrat »

dramida wrote:Here is how altitude holds in wind with MultiWii-R1177 http://www.youtube.com/watch?v=SB_O4swILJU

Thanks alot boys. Any news about baro- AccZ controlled climb/descent?



Looking very good.

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

Re: Altitude Hold improvement solution

Post by vpb »

will try r1177 on my Y6 tomorrow with Alt hold, hope it will help the sink/jump yaw :-|

scrat
Posts: 925
Joined: Mon Oct 15, 2012 9:47 am
Location: Slovenia

Re: Altitude Hold improvement solution

Post by scrat »

vpb wrote:will try r1177 on my Y6 tomorrow with Alt hold, hope it will help the sink/jump yaw :-|


Video please ;)

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

Re: Altitude Hold improvement solution

Post by mahowik »

Mis wrote:Your change to my code have one dangerous situation. If you arm the copter before "count" reached zero, the count variable is zeroed, and "measured1G" variable is not divided by 8. Then you substract crazy value from accZ.

It's very toughly...
Also one more issue if do this only after gyro calibration: after ACC calibration (e.g. from TX) it will be not correct...

Mis wrote:I think, about good solution, and maybe fusion of your method and my method should be best choice. Many stronger LPF filter on "measured1G" variable, measure and filter it from startup (afrer startup delay defined in "#define INIT_DELAY" - now done) until to arming moment (until f.ARMED). The filter value is now 8, but we can change it to even 64 (64*512 = 32767) and change "measured1G" type to uint16_t because this value should never gone bellow 0.


My current implementation is:

Code: Select all

  static int16_t acc_1G_calculated = acc_1G*10;
  if (!f.ARMED) {
    acc_1G_calculated -= acc_1G_calculated/10;
    acc_1G_calculated += accZ;
  } 
  accZ -= acc_1G_calculated/10;

flyrobot
Posts: 73
Joined: Thu Apr 05, 2012 3:59 pm

Re: Altitude Hold improvement solution

Post by flyrobot »

I just try R1177. The quad fly is perfect stable and pos hold better, but i found 2 problem :
1. Old problem when alt hold start the quad drop approx 1 meter.
2. When its on alt hold mode, and i try to increase the throttle stick up the quad is going down.

John

Mis
Posts: 203
Joined: Fri Apr 01, 2011 12:23 am

Re: Altitude Hold improvement solution

Post by Mis »

Mahowik:
Can you look at my last method used in this software version: http://romek_by.republika.pl/MK/MultiWii_2_16_MIS.zip ?
In my tests this get good results.

mulder
Posts: 4
Joined: Thu Apr 26, 2012 4:33 pm

Re: Altitude Hold improvement solution

Post by mulder »

hi guys, quite new to MW but not to multirotors. I setting up alt hold on MWC quad yesterday and found it worked terrible compared to my old miktokopter and I thought I read somewhere that MWC is not taking ACC-Z into consideration for alt-hold? is this true or has this been improved in MW 2.1 or are is this what you guys are working on in the latest dev version?

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

Re: Altitude Hold improvement solution

Post by vpb »

I've tried new alt-hold code in r1177 version. With P,I = 0, D = 30, my Y6 drop and I cannot raise the height even with maximum throttle. I kept decreasing D downto 8, it didn't drop too much, about 50-100cm. But is that low-D ok? I feel harder to control my machine with ALT-HOLD activated.

Mac9
Posts: 45
Joined: Thu Oct 20, 2011 2:47 pm
Contact:

Re: Altitude Hold improvement solution

Post by Mac9 »

Hi All,
I keep reviewing what has "gone on " on this thread and to be honest I am having a little bit of a problem. Yes development means changing things however the changes seem to be al most hourly. Have we got a "solution" to the problem, that has been solved by NAZA or are we still trying to change the existing parameters to make a compromise that gets close.
Its no shame for a group of non professional to admit that they can not reproduces what the professionals can and perhaps the variation of "platform" contribute to the confusion. However as a non programmer I must conclude that you are all trying hard but as yet "no Cigar"

Regards

John

Termic1
Posts: 40
Joined: Tue Aug 21, 2012 11:14 am

Re: Altitude Hold improvement solution

Post by Termic1 »

I think that new Mahowik baro routines have made a great improvement in Alt Hold function. IMHO Alt-Hold + GPS Hold offer today a good stable mode that can guarantee an autonomous hovering for many many minutes.

Yes...multiwii is not yet at NAZA level....but it's getting better and better...and it's impressive how it is improving...and it's so interesting to follow its development...what you can see in the video was impossible some month ago...

..in my opinion I've got a lot of cigars.. :)

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

Luciano

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

Re: Altitude Hold improvement solution

Post by shikra »

I'm happy too!

Mac9 - its pretty good and there now if you like playing, but its not mainstream release yet.

If you want naza performance then best to buy one! Its been ahead for years with Acrro. Gap is closing on GPS / atti, but I think its still some way off.

mulder
Posts: 4
Joined: Thu Apr 26, 2012 4:33 pm

Re: Altitude Hold improvement solution

Post by mulder »

naza is not the only project that uses ACC-Z to assist alt hold; Mikrokopter also makes use of the ACC and it worked very well. The source was published but I think its closed these days...

Mac9
Posts: 45
Joined: Thu Oct 20, 2011 2:47 pm
Contact:

Re: Altitude Hold improvement solution

Post by Mac9 »

Hi All,
You are missing the point Shirka in an almost insulting manner. Firstly you assume I do not have a Nasa controller WRONG. I have had one since about MultiWii 1.8, when ever that was. That's why I am able to comment on its performance, not like some who make their comments based on what others have written on various Forums.
Secondly I am not knocking the code writers, when you see the progress that has been made since I first played its almost unbelievable, I am and we all should be great full to them ! As I have failed to see your Tag on any code "snipit" in _shared or anywhere else for that matter I am assuming that you like me are a Tweaker of parameters and uncomenter of #defines. If so, you as I suffer from a total reliance on the code, not knowing what goes on "under the Hood" at the level required to master the beast. I am sure that with a lot more time given to basic tweaking we will both get there however my original comments still stand I AM NOT THERE YET.
Don't smoke all the Cigars you coders have as they are bad for you ;) and you are certainly needed.

Regards

John

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

Re: Altitude Hold improvement solution

Post by shikra »

Why insulting? Not being rude at all.. Actually I thought you were a little... LOL

Don't have to be a coder to provide assistance . That said I posted an update only this morning....

Hey ho - cheer up. Enjoy your Naza and MultiWii. Both awesome

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

Re: Altitude Hold improvement solution

Post by mahowik »

Mac9, please stop your fairytales... Not all people here have so good english... As for me I even couldn't translate your posts... It's much better to spent time and read the thread (at least last 10 pages) to resolve your issues related to new althold, than do SPAM here...

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

Re: Altitude Hold improvement solution

Post by mahowik »

Hi guys,

Just to summarize couple of mandatory things which should go with new althold:

1) I made set of tests and can confirm that even mpu6050 requires correct acc_1g offset before motors ON. It helps to keep accZ (and calculated velocity) near zero even with big temperature drift... Tested with diff 15-20C inside/outside without ACC re-calibration... Otherwise non zero accZ will produce big velocity and incorrect D compensation (visible as jumps or downfall) when actually velocity_Z is zero... althold in this case unusable...
So my current solution is:

Code: Select all

  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;
 
  static int16_t acc_1G_calculated = acc_1G*8;
  if (!f.ARMED) {
    acc_1G_calculated -= acc_1G_calculated/8;
    acc_1G_calculated += accZ;
  } 
  accZ -= acc_1G_calculated/8;
  accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);


2) To control altitude old 2.1 solution can not be used because any change of the throttle just quickly disable and enable (in next cycle iteration) the althold. But D part (of the new althold) dampens immediately throttle changes. As result it's not controllable:

Code: Select all

      if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
        f.BARO_MODE = 0; // so that a new althold reference is defined
      }
      rcCommand[THROTTLE] = initialThrottleHold + BaroPID;

So I propose two solutions:
a) Natural alt change for rapid pilots. It's temporary switch OFF the althold when throttle stick is out of deadband:

Code: Select all

    #if defined(ALTHOLD_FAST_THROTTLE_CHANGE)
      static uint8_t isAltHoldChanged;
      if (abs(rcCommand[THROTTLE]-initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
        errorAltitudeI = 0;
        isAltHoldChanged = 1;
       
        rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -ALT_HOLD_THROTTLE_NEUTRAL_ZONE : ALT_HOLD_THROTTLE_NEUTRAL_ZONE;
     
      } else {
        if (isAltHoldChanged) {
          // much usefull to use BaroAlt instead of EstAlt because it has less delay value when alt hold activated during the vertical movement
          AltHold = BaroAlt;
          isAltHoldChanged = 0;
        }
        rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
      }
    #endif

b) Smooth alt change for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms):

Code: Select all

      static uint8_t isAltHoldChanged;
      if (abs(rcCommand[THROTTLE]-initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
        // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
        AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold;
        if(abs(AltHoldCorr) > 500) {
          AltHold += AltHoldCorr/500;
          AltHoldCorr %= 500;
        }
        errorAltitudeI = 0;
        isAltHoldChanged = 1;
     
      } else if (isAltHoldChanged) {
        AltHold = BaroAlt;
        isAltHoldChanged = 0;
      }

      rcCommand[THROTTLE] = initialThrottleHold + BaroPID;


thx-
Alex

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

Re: Altitude Hold improvement solution

Post by dramida »

I would very like to test the new routines, Alex please tell us where should we copy-paste and what to delete :)

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

Re: Altitude Hold improvement solution

Post by mahowik »

dramida wrote:I would very like to test the new routines, Alex please tell us where should we copy-paste and what to delete :)

What version you would like to test? Dev or 2.1?
I can prepare ZIP file ;)

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Altitude Hold improvement solution

Post by Sebbi »

dramida wrote:I would very like to test the new routines, Alex please tell us where should we copy-paste and what to delete :)


Or give us a link to your repository/branch instead ... copy & paste is error prone ;-)

@mahowik:
Would it be possible to calibrate acc_1g with the barometer during normal flight? And could it be better to use sqrt(acc_x*accx + acc_y*acc_y + acc_z*acc_z) to calculate acc_1g in case the copter is not level when being armed?

The MPU6050 datasheet states that sensitiviy changes by +-0.02% per °C ... that's the scalefactor and why you see different acc_1g at diffent temperature. But the bias is also changing with temperature by +-60 milli-g from 0°C to 70°C or +-0.086% per °C for the z-axis. That should have an effect on level mode calibration ... and maybe also on althold with acc.

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

Re: Altitude Hold improvement solution

Post by vpb »

Hi mahowik,
Yesterday after re-calibrate ACC, my Y6 didnot fall anymore when I activated alt-hold. And I'm on r1177 dev and wanna test that routine too.
Thanks for the good job!

scrat
Posts: 925
Joined: Mon Oct 15, 2012 9:47 am
Location: Slovenia

Re: Altitude Hold improvement solution

Post by scrat »

Hi all,

how much did you set your dedaband for althold in config.h file?

#define ALT_HOLD_THROTTLE_NEUTRAL_ZONE 20 - default value

Thanks for answer.

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

Re: Altitude Hold improvement solution

Post by mahowik »

Sebbi wrote:Or give us a link to your repository/branch instead ... copy & paste is error prone ;-)

yep :) I'm still going to create repo for my own (already 4-6 month) :)

Sebbi wrote:Would it be possible to calibrate acc_1g with the barometer during normal flight?

Alexmos implemented this way to find bias (during normal flight) for each ACC axis, but it was necessary to make each ACC axis calibration to find axis scales. Probable with mpu6050 it has more sense...

Sebbi wrote:And could it be better to use sqrt(acc_x*accx + acc_y*acc_y + acc_z*acc_z) to calculate acc_1g in case the copter is not level when being armed?

Solution above do the same, actually it's calculated offset based on averaged ACC projection to G vector, i.e. it takes into account inclination of the copter before the start too...

Sebbi wrote:The MPU6050 datasheet states that sensitiviy changes by +-0.02% per °C ... that's the scalefactor and why you see different acc_1g at diffent temperature. But the bias is also changing with temperature by +-60 milli-g from 0°C to 70°C or +-0.086% per °C for the z-axis. That should have an effect on level mode calibration ... and maybe also on althold with acc.

Yes, it can be investigated why but it's fact (at least with my board) that acc Z values was changed with temperature (checked in GUI) and produce althold issues (jumps or downfall)...

thx-
Alex

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

Re: Altitude Hold improvement solution

Post by mahowik »

scrat wrote:how much did you set your dedaband for althold in config.h file?
#define ALT_HOLD_THROTTLE_NEUTRAL_ZONE 20 - default value

It depends on expo throttle which is set in GUI... As for me comfortable expo throttle = 35 and ALT_HOLD_THROTTLE_NEUTRAL_ZONE = 40... It helps to find hover point after altitude/throttle change...

thx-
Alex

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

Re: Altitude Hold improvement solution

Post by dramida »

mahowik wrote:
dramida wrote:I would very like to test the new routines, Alex please tell us where should we copy-paste and what to delete :)

What version you would like to test? Dev or 2.1?
I can prepare ZIP file ;)

I prefer to work with dev files R1177 would be fine, i like to test :)

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

Re: Altitude Hold improvement solution

Post by mahowik »

dramida wrote:I prefer to work with dev files R1177 would be fine, i like to test :)


pls. use the following define in config.h to switch between "alt change" solutions:

Code: Select all

    // Natural alt change for rapid pilots. It's temporary switch OFF the althold when throttle stick is out of deadband defined with ALT_HOLD_THROTTLE_NEUTRAL_ZONE
    // but if it's commented: Smooth alt change routine is activated, for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease
    // altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms)
    #define ALTHOLD_FAST_THROTTLE_CHANGE
Last edited by mahowik on Sun Dec 16, 2012 8:22 pm, edited 1 time in total.

User avatar
diyboy
Posts: 28
Joined: Sun Sep 30, 2012 7:12 pm

Re: Altitude Hold improvement solution

Post by diyboy »

mahowik wrote:
dramida wrote:I prefer to work with dev files R1177 would be fine, i like to test :)


pls. use the following define in config.h to switch between "alt change" solutions:

Code: Select all

    // Natural alt change for rapid pilots. It's temporary switch OFF the althold when throttle stick is out of deadband defined with ALT_HOLD_THROTTLE_NEUTRAL_ZONE
    // but if it's commented: Smooth alt change routine is activated, for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease
    // altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms)
    #define ALTHOLD_FAST_THROTTLE_CHANGE


Hi Alex,

Is this new routines already implement to MultiWii_dev_r1232?
Can i test it with MultiWii_dev_r1232?

Thx
Diyboy

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

Re: Altitude Hold improvement solution

Post by mahowik »

diyboy wrote:Is this new routines already implement to MultiWii_dev_r1232?
Can i test it with MultiWii_dev_r1232?


I have merged these solutions with last MultiWii_shared sources... you can take it here http://code.google.com/p/multiwii/sourc ... %2FMahowik

thx-
Alex

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

Re: Altitude Hold improvement solution

Post by dramida »

Are you aware about ALT bug from R1188 afterwards? viewtopic.php?f=8&t=2678

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

Re: Altitude Hold improvement solution

Post by Crashpilot1000 »

Don't download or even look at my version http://fpv-community.de/showthread.php? ... post224135.
I tried the current Dev and i see it's severe limitations in windy and forward flight conditions. I will just look here and take the best parts for my needs. All i can tell you is, that changing the throttle relative to the throttlestick middle will not do a good job. I did it and already switched to changing the target hight (like naza) but that will require fast recognition of the current hight - and the current DEV (even with working baro readout) will never do this because everybody is afraid to increase the cycletime - ROFL, at least Timecop will understand me. The day when the baro is polled at 1HZ i will have a big laugh.

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Altitude Hold improvement solution

Post by Sebbi »

Two things here:
1) without GPS activated baro is read out almost as fast as possible for some time now ... true, altitude gets only calculated at 40 Hz, that's something that can be improved
2) Is your code free to use AND this is exactly what some of use talk about, having patches or zip files with versions floating around all over the internet. Use source control (git prefered) damnit ;-)

P.S.: I looked at your code. It will probably suffer the same problem as the code posted here: temperature change will cause acc_1G to be a different value and accZ might be off a little bit. You do the baro stuff after computeIMU(), is there a reason for this change? Your code is very similar to mahowiks code except for some averaging and an interesting new PID controller. But flips when touching down - however briefly - would be a no go for me. Do you have a video of this in action?

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

Re: Altitude Hold improvement solution

Post by nhadrian »

Dear mahowik!

I like your idea of having throttle-stick based altitude change during alt. hold! BUT!
I'm not a programmer at all but I learnt about regulations during my university studies.
My oppinion is that once we would like to control the altitude too, there should be two different altitude hold modes (just like in mikrokopter, I used it for a while)

1, The first mode should work like throttle stick is showing a conctere altitude. So when throttle stick is moved up or down, multiwii changes the altitude with constans vario to that desired point. So a relation between throttle stick range and altitude range should defined (ie. full throttle stick range is 20m in altitude...). Also that is important that once function is disabled, it gives back the throttle control continously within 3-5 seconds (to avoid fast movements, let pilot reduce or incease throttle for manual piloting.)
What is the hard job in this? For proper working, not altitude but VARIO should be regulated via PID controls!!! Then, it would be easy to ascend or descend with any vario continously.
This function also could be the basic of the later Waypoint navigation!

2, The second mode is similar like yours. But, in this mode, throttle stick position gives a desired vario. So, I think the regulation should be divided into two parts, when throttle stick is inside ALT_HOLD_DEADBAND, thne current altitude regulation should be used (but, angle correction is neccessary for proper forward flight or wind resistance!!! It is only some lines in code but gives much more better solution, I use it too). When throttle stick is out of deadband, it means we need to ascend or descend with a desired vario. So from now on, altitude regulation should use the vario ar segulated parameter, for smooth moving, instead of altitude PID!!!

I'm unfortunatelly not good enough to program these and test myself, but perhaps there is any ideas which could be used...

Thanks for reading.

BR
Adrian

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

Re: Altitude Hold improvement solution

Post by nhadrian »

My simple (but working) altitude correction is (origin from alexmos):

1, define this in config.h :

Code: Select all

#define THROTTLE_ANGLE_CORRECTION 400


2, add this into imu.ino , at the end of getEstimatedAttitude:

Code: Select all

cosZ =  EstG.V.Z / acc_1G * 100.0f; // cos(angleZ) * 100   
  throttleAngleCorrection = THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ, -150, 150) / 100;   


3, add this to multiwii.ino, around the beginning:

Code: Select all

static int16_t throttleAngleCorrection = 0;    
static int8_t cosZ = 100;


4, in multiwii.ino, after this part:

Code: Select all

  #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
    if (f.BARO_MODE) {
      if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
        f.BARO_MODE = 0; // so that a new althold reference is defined
      }
      rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
    }
  #endif


add this:

Code: Select all

  #if THROTTLE_ANGLE_CORRECTION > 0   
      if(f.ANGLE_MODE && cosZ > 0) {
      rcCommand[THROTTLE]+= throttleAngleCorrection;
      }
  #endif


The value of THROTTLE_ANGLE_CORRECTION should be figured out on every multirotor, I don't have any pre-defined values for bigger copters (bigger than 35cm... :D), start from value 200 and raise until you get good behaviour.

This correction works not only when BARO mode is active but every time.
Since I have a small Y6 copter which has to compensate wind with high angles, it is really useful in manual piloting too!!!
This correction helps for the BARO PID regulation, since this correction is a control, not a regulation, there is not any delay in compensation... (I hope you understand what I would say).

BR
Adrian

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

Re: Altitude Hold improvement solution

Post by shikra »

Not tried the latest, but the r1177 still working well for me....

I couldn't resist the challenge set ...

http://www.youtube.com/watch?v=6gRhEFCi8A8

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

Re: Altitude Hold improvement solution

Post by mahowik »

Crashpilot1000 wrote:Don't download or even look at my version http://fpv-community.de/showthread.php? ... post224135.

Could you upload your ZIPs here, because I had some trouble to register on German resource :)

Crashpilot1000 wrote:changing the throttle relative to the throttlestick middle will not do a good job

If you are talking about first solution (2.a) from link below I found it useful for fast flying... i.e. when you need to have possibility to change throttle immediately...
viewtopic.php?f=8&t=2371&start=280#p25699

Crashpilot1000 wrote:I did it and already switched to changing the target hight (like naza) but that will require fast recognition of the current hight

As for me more than less it's implemented in second solution (2.b) and yes it's required up-to-date current altitude... this is why I used almost raw altitude (BaroAlt) when returning to center/deadband zone...
viewtopic.php?f=8&t=2371&start=280#p25699

Code: Select all

  // much usefull to use BaroAlt instead of EstAlt because it has less delay value when alt hold activated during the vertical movement
  AltHold = BaroAlt;


dramida wrote:Are you aware about ALT bug from R1188 afterwards? viewtopic.php?f=8&t=2678

already yes :)
but in any case it will be useful in case of BOSSes will decide to include this to release :)

thx-
Alex

Post Reply