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
flyrobot
Posts: 73
Joined: Thu Apr 05, 2012 3:59 pm

Re: Altitude Hold improvement solution

Post by flyrobot »

mahowik wrote:Hi guys,

Here is MultiWii_2_1_b2 ;)

1) controlled ascend/descend mode based on the new Alt Hold routine. It's slowly increase/decrease AltHold altitude proportional to stick movement from point where AltHold activated ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms): solution from alexmos

2) /* Predefined initial throttle for AltHold will be calculated from MID (middle/hover) point of expo throttle from GUI (BUT not taken from current throttle value on AltHold activation).
I.e. pls. use GUI to set MID point of throttle expo as initial throttle.
Note: Value specified by define it's additional compensation for battery consumption */
//#define INITIAL_THROTTLE_HOLD_FROM_MID_EXPO_POINT 40

3) /* Correct throttle according to Z-axis inclination
Default is 100. Don't set above 200 */
//#define THROTTLE_ANGLE_CORRECTION 200

4) // set altitude to RTH. If Alt-hold activated during the RTH (or RTH during Alt-hold) it will keep specified altitude.
#define ALT_TO_RTH 1000 // in cm... = 10m

5) // set altitude after RTH, i.e. when copter reached to home position.
// DON'T USE VALUE LESS THEN 200 (2m) BECAUSE BARO ON ALTITUDES <2m WORKS UNSTABLE!!!
// Correct auto landing will be possible with sonar... soon ;)
#define ALT_TO_RTH_FINISH 350 // in cm... = 3.5m

6) // when roll or pitch more than specified value, PH (position hold) will be OFF and when roll/pitch stick released (i.e. in center) PH will be activated with new coordinates
// it will give possibility to fly in PH mode
#define GPSHOLD_DEADBAND 30

7) other changes from 2.1_b1 http://forum.rcdesign.ru/blogs/83206/blog15204.html
http://forum.rcdesign.ru/blogs/83206/blog15180.html

If you like this don't hesitate to buy me a beer by pressing donate button there ;)

thx-
Alex


Hi Alex,

Today i tried this software version 2.1_b2 only point 6. when i moved the roll and nick in PH mode and AH to other place and i release the stick (back to center) the quad move back to the position coordinate when its start PH on. I tried with limited battery power (i haven't charged the battery full) so i haven't tried tuning PID for AH yet. it still have a good AH (move vertical approx 1 meter) if the quad is flying on altitude more than 2 meter. Next i will try to tuning the PID as you mentioned above.

John

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

Re: Altitude Hold improvement solution

Post by flyrobot »

I just flied with 2.1 b2 with PID tuned I just made the video : http://m.youtube.com/watch?v=cOdZ3TDWQ60
You can see the quad flying back to coordinate when the PH activated after i move the roll to get the quad moving to other place then i put the stick on centre position. Any step i missed ?
The Alt hold is much much better during rth or pos hold, but i see the quad always start going down about 1 meter when the alt hold start and going up again than stable alt hold.

John

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

Re: Altitude Hold improvement solution

Post by mahowik »

flyrobot wrote:I just flied with 2.1 b2 with PID tuned I just made the video : http://m.youtube.com/watch?v=cOdZ3TDWQ60
You can see the quad flying back to coordinate when the PH activated after i move the roll to get the quad moving to other place then i put the stick on centre position. Any step i missed ?

thanks for test!

Strange, probably I missed something... I have tested this in GUI and also one time on the fly... in GUI if you have PH activated (with 3D fix), try to tilt roll/pitch and you should see that PH is OFF when roll/pitch tilted...
flyrobot wrote:The Alt hold is much much better during rth or pos hold, but i see the quad always start going down about 1 meter when the alt hold start and going up again than stable alt hold.

I suppose it depends on the PIDs... i.e. it's oscillation before stabilization in PID controller...

p.s. and for statistics: what acc and baro you are using?

thx-
Alex

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

Re: Altitude Hold improvement solution

Post by mahowik »

special release for Crius SE (atmega328) because it has only 30720 byte maximum, BUT GPS_FILTERING disabled there... but some guys mentioned that it works better w/o filter ;)
http://forum.rcdesign.ru/blogs/83206/blog15302.html

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

Re: Altitude Hold improvement solution

Post by flyrobot »

mahowik wrote:
flyrobot wrote:I just flied with 2.1 b2 with PID tuned I just made the video : http://m.youtube.com/watch?v=cOdZ3TDWQ60
You can see the quad flying back to coordinate when the PH activated after i move the roll to get the quad moving to other place then i put the stick on centre position. Any step i missed ?

thanks for test!

Strange, probably I missed something... I have tested this in GUI and also one time on the fly... in GUI if you have PH activated (with 3D fix), try to tilt roll/pitch and you should see that PH is OFF when roll/pitch tilted...
flyrobot wrote:The Alt hold is much much better during rth or pos hold, but i see the quad always start going down about 1 meter when the alt hold start and going up again than stable alt hold.

I suppose it depends on the PIDs... i.e. it's oscillation before stabilization in PID controller...

p.s. and for statistics: what acc and baro you are using?

thx-
Alex


Hi Alex,

Im using Flyduino Mega (mega board) and 0.3.5 MS (ITG3200, BMP180 and MS5611).

I think, I need more times to tune the PID. Its really takes quite alot of time and battery.

I will check on my tx telemetry for pos hold on and off if i move the roll and nick.

Many thanks,

John

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

Re: Altitude Hold improvement solution

Post by mahowik »

Also for bma180 make sense to set 10hz internal LPF as described here viewtopic.php?f=8&t=2371&start=190#p23819

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

Re: Altitude Hold improvement solution

Post by Termic1 »

mahowik wrote:Hi guys,

Here is MultiWii_2_1_b2 ;)

.....


Alex, I loaded your MultiWii_2_1_b2 but I'm not able to test it as it is based on 2.1 and not on the new DEV. The problem is that I'm using RCTimer UBLOX GPS and EOSBandi has made some mod to GPS.ino in order to configure this popular GPS at startup. These mods hase been integrated in the new dev. Loading your version I've lost all EOSBandi modifications and RCTimer GPS is not working anymore. I can't insert the GPS mod in your code as EOSBandi modified GPS.ino that contains your modification as well....too bad... :cry:

I was really happy to test new ALT to RTH functions but I have to wait a code that integrate your and EOS mods.

Thanks anyway for your effort...

Luciano

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 tested B2 version of Alex.

1- Can't use GPS based function as this dev version has no support for Ublox GPS
2- Altitude control (up and down) is to "soft" , i crashed into ground two times and it tends to lose altitude after a controled climb (verry slow). 0.5 m/s vertical speed is too low.
Also i noticed that in altitude control mode, if you put the stick to minimum, the control is lost and immediat crash happens as motors enters in IDLE mode.


To further test gps function i need to mount an Nmea Medietek 3329 gps or Alex add a define fro ublox :)

my two cents ideea:

Alex needs to work on current shared trunk to have more testers.

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: Altitude Hold improvement solution

Post by jevermeister »

Dragu, if you put throttle to minimum the control algorithm stops completely.If trhottle<mincheck there is no governor active.

Nils
Ps.: nice footage

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

Re: Altitude Hold improvement solution

Post by mahowik »

Sorry guys for issues/bugs! :(
I hope your crashes are light...

I forgot to mention that features from b2 was initially tested only with one flight and in GUI...

dramida wrote:1- Can't use GPS based function as this dev version has no support for Ublox GPS

sorry, but I haven't ublox to check in case of add support for this...

dramida wrote:2- Altitude control (up and down) is to "soft" , i crashed into ground two times and it tends to lose altitude after a controled climb (verry slow). 0.5 m/s vertical speed is too low.

but there +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms... I checked with full throttle in GUI (where alt hold activated in middle point) it gives ~2m/s... probably for fast flights it's not enough though...
Also with current solution possible additional delay because it changes only alt-hold point (i.e. altitude to hold) and PID controller tries to keep that new altitude point according to current PID values. So maybe it's not acceptable solution for the fast flights... i.e. only for correcting/changing altitude in hover...

dramida wrote:my two cents ideea:
Alex needs to work on current shared trunk to have more testers.

make sense :)

wilco1967
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Post by wilco1967 »

Hi,

In a clean-up action, I updated my code to a 'fresh' download rev 1143, and then made the remaining SVN updates again upto 1152.... I was using a patched up base, from numerous SVN updates, which I was not sure were all correct, but flew perfectly.

However, with the code cleaned up, the alt hold does not seem to work anywhere near as good as it used to do after Mahowik's modifications from a few weeks ago....

I compared the current code with the earlier version, which was working perfectly.I noticed a number of relocations, and slighty different syntax, which seems to do the same.

However: One line seems to be 'reversed'
static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
used to be
static float accVelScale = 9.80665f / acc_1G / 10000.0f;

Was this deliberate ?

I haven't had a change to test fly it yet to see if it makes any difference..... I'll report back once I get a chance....

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

Re: Altitude Hold improvement solution

Post by Magnetron »

correct is:
static float accVelScale = 9.80665f / acc_1G / 10000.0f;

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

Re: Altitude Hold improvement solution

Post by Mis »

x/y/z = x/z/y, but arguments order make some differencies in code size.

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

Re: Altitude Hold improvement solution

Post by mahowik »

wilco1967 wrote:However: One line seems to be 'reversed'
static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
used to be
static float accVelScale = 9.80665f / acc_1G / 10000.0f;


Checked in GUI for these two cases. Don't worry. Velocity is the same ;)

Some details: 1st case more optimized and arduino compiler (cheked with 1.0.1 version) replace (9.80665f / 10000.0f) to one value. As result you have size of the compiled sketch less than 2nd variant...

BUT for some other compilers it can be an issue, if compiler start to parse from end to begin. E.g. for 2nd case (temp = acc_1G / 10000.0f) will be calculated at first and then 9.80665f / temp. As you see result will be fully incorrect...
((9.80665f / acc_1G) / 10000.0f) != (9.80665f / (acc_1G / 10000.0f))

@timecop: probably it's an issue with your port on stm32...

thx-
Alex

wilco1967
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Post by wilco1967 »

just tried it with
accVelScale = 9.80665f / acc_1G / 10000.0f;

still the same...... hardly able to hold altitude..... as bad as the old code (before your modifications)....
It DOES hold, just very bad..... PID tuning does not seem to make it any better.... had to reduce Alt_P to below 1 to stop it from yoyo-ing.... it will drop like 10 meters or more... Alt D tried everyting from 10 to 150...

This is the code I'm running (in the latest dev from SVN (1152), and which is not performing well. (it seems to do everything else just fine)

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)

#define applyDeadband(value, deadband)  \
  if(abs(value) < deadband) {           \
    value = 0;                          \
  } else if(value > 0){                 \
    value -= deadband;                  \
  } else if(value < 0){                 \
    value += deadband;                  \
  }

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
 
  #ifndef SUPPRESS_BARO_ALTHOLD

  //P
  int16_t error = constrain(AltHold - EstAlt, -300, 300);
  applyDeadband(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;
  applyDeadband(accZ, ACC_Z_DEADBAND);
  debug[0] = accZ;
 
  static float vel = 0.0f;
  //static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
  static float accVelScale = 9.80665f / acc_1G / 10000.0f; //wilco
 
  // Integrator - velocity, cm/sec
  vel+= accZ * accVelScale * dTime;
 
  static int32_t lastBaroAlt;
  float baroVel = (EstAlt - lastBaroAlt) * 1000000.0f / dTime;
  lastBaroAlt = EstAlt;

  baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
  applyDeadband(baroVel, 10); // to reduce noise near zero 
  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
  applyDeadband(vel, 5);
  BaroPID -= constrain(conf.D8[PIDALT] * vel / 20, -150, 150);
  debug[3] = BaroPID;
 
  #endif
}


And this code works perfect..... (with almost all possible PID parameters.... currently 5, 0.020, 30 (default).

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

Any idea where it really does something different (there is a lot of changes where the code was re-written, but to my untrained eye, seems to do the same).

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

Re: Altitude Hold improvement solution

Post by mahowik »

Also could not find the issue here :)
probably here, when function for deadband was replaced to define

Code: Select all

#define applyDeadband(value, deadband)  \
      if(abs(value) < deadband) {           \
        value = 0;                          \
      } else if(value > 0){                 \
        value -= deadband;                  \
      } else if(value < 0){                 \
        value += deadband;                  \
      }


I will check at home...

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

Re: Altitude Hold improvement solution

Post by mahowik »

It seems I found it! :)

Try to comment applyDeadband(vel, 5)

Code: Select all

      //D
      // applyDeadband(vel, 5);
      BaroPID -= constrain(conf.D8[PIDALT] * vel / 20, -150, 150);
      debug[3] = BaroPID;


In original version it was a function for deadband and "vel" variable in not overridden with deadband, but with define it's overridden! As result velocity integrator will re-zero in each iteration...

wilco1967
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Post by wilco1967 »

mahowik wrote:Also could not find the issue here :)
probably here, when function for deadband was replaced to define

Code: Select all

#define applyDeadband(value, deadband)  \
      if(abs(value) < deadband) {           \
        value = 0;                          \
      } else if(value > 0){                 \
        value -= deadband;                  \
      } else if(value < 0){                 \
        value += deadband;                  \
      }


I will check at home...


Thanks a lot for you help....

Small update:
I replaced the entire IMU.ino with the old one (rest of the code is latest dev), and it seems to work fine again....
(Only tried holding it in my hand, as it is raining outside, but it feels OK).

wilco1967
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Post by wilco1967 »

mahowik wrote:It seems I found it! :)

Try to comment applyDeadband(vel, 5)

Code: Select all

      //D
      // applyDeadband(vel, 5);
      BaroPID -= constrain(conf.D8[PIDALT] * vel / 20, -150, 150);
      debug[3] = BaroPID;


In original version it was a function for deadband and "vel" variable in not overridden with deadband, but with define it's overridden! As result velocity integrator will re-zero in each iteration...


Succes!!!!!

that seems to solve it......
Still raining, so only tested inhouse, holding it.

Thanks a lot again !

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

Re: Altitude Hold improvement solution

Post by Alexinparis »

mahowik wrote:It seems I found it! :)

Try to comment applyDeadband(vel, 5)

Code: Select all

      //D
      // applyDeadband(vel, 5);
      BaroPID -= constrain(conf.D8[PIDALT] * vel / 20, -150, 150);
      debug[3] = BaroPID;


In original version it was a function for deadband and "vel" variable in not overridden with deadband, but with define it's overridden! As result velocity integrator will re-zero in each iteration...


Hi,

Sorry for the bug.
I've just fixed it this way:

Code: Select all

  float vel_tmp = vel;
  applyDeadband(vel_tmp, 5);
  BaroPID -= constrain(conf.D8[PIDALT] * vel_tmp / 20, -150, 150);

and the code is a little bit shorter, mainly because calc on a static variable is a little bit longer :)

Willian_II
Posts: 6
Joined: Tue Sep 18, 2012 3:45 am

Re: Altitude Hold improvement solution

Post by Willian_II »

Hey guys,

I had a problem that whenever i activated the alt hold, the copter would drop.
so, debugging, i found my accelerometer was measuring 1g as 180, and the sensor's acc_1g was defined as 165.
so whenever the copter was standing still, the algorithm thought it was rising!
i changed the sensor acc_1g and it works, now! just need some PID tuning.

It's not actually a bug, since the code assumes acc_1g is correct, i'm just posting to let people with the same problem know what is wrong.

Thank you guys for the hard work =D

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

Re: Altitude Hold improvement solution

Post by dramida »

We need a startup calibration subroutine for 1g value.

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

Re: Altitude Hold improvement solution

Post by capt »

Is 165 the default in code 2.1?
Also what kind of accel do you have and was the 180 what showed in the GUI?
Just looking for more info so others can check theirs.

Willian_II
Posts: 6
Joined: Tue Sep 18, 2012 3:45 am

Re: Altitude Hold improvement solution

Post by Willian_II »

I have a gy-80 chinese IMU, with an ADXL345.
Oops. the default value is 265, changed to 278(was in doubt between 278 and 280)

So, i have now checked out the r1155, had to recalibrate, and now the sensor actually reads 265.

I re-uploaded the version where i had this problem, and after calibration it also shows 265. I guess it was just a calibration problem after all.
Anyway, what happened is that when standing still, the debug[2](vel), instead of remaining constant around zero, remained constant at ~10(if i remember correctly). So whenever i activated the alt-hold, the motors would immediately slow down, trying to stop the copter ascension.

I wonder what caused the sensor to read 278....

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

Re: Altitude Hold improvement solution

Post by dramida »

Can we just make an average of the acc values read at startup, when also giro is calibrated. And then define that average as 1g. G force differs from spot to spot, not much, but maybe enough to have signifficant drift in algorythm.

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

Re: Altitude Hold improvement solution

Post by timecop »

No, because then you'd have to have the copter precisely level at power up.

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

Re: Altitude Hold improvement solution

Post by pm1 »

ACC calibration -> The readout of my acc (ITG3200) is absolutely stable and reproducible over weeks, when the motors are not running.

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

Re: Altitude Hold improvement solution

Post by timecop »

ITG3200 is a gyro.

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

Re: Altitude Hold improvement solution

Post by pm1 »

timecop wrote:ITG3200 is a gyro.

Sry, picked the wrong one... I meant BMA180

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

Re: Altitude Hold improvement solution

Post by Sebbi »

timecop wrote:No, because then you'd have to have the copter precisely level at power up.


Use sqrt(accx*accx + accy*accy + accz*accz) and you wont have to have it level.

A better solution would be to have a similar ACC (and gyro) calibration as with mag calibration where you turn around the copter to find each axis scale factor an bias and optionally any cross axis sensitivity. The MPU6050 is supposed to be factory calibrated in this regard (scale and sensitivity, not bias), but for other sensors this would likely improve things a lot ;-)

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

Re: Altitude Hold improvement solution

Post by timecop »

Use sqrt(accx*accx + accy*accy + accz*accz) and you wont have to have it level.


I dunno about that, some accels have different 1g values on XY and Z axes. Wouldn't that screw it up?

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

Re: Altitude Hold improvement solution

Post by dramida »

timecop wrote:No, because then you'd have to have the copter precisely level at power up.


Good find about startup calibration on a non-level surface.
Solution: We always can apply the per axis angle G corrections as we suppose that level is calibrated and copters stays still on ground. I am no more a software coder, i had my time 10 years ago but some principles are still in my brain.

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

Re: Altitude Hold improvement solution

Post by Sebbi »

timecop wrote:
Use sqrt(accx*accx + accy*accy + accz*accz) and you wont have to have it level.


I dunno about that, some accels have different 1g values on XY and Z axes. Wouldn't that screw it up?


If that's the case, then all other algorithms used in MultiWii are not going to work correct, because they assume 1G is 256 (or 512 for MPU6050). One should not use accelerometers like that anymore ;-)

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

Re: Altitude Hold improvement solution

Post by mahowik »

Sebbi wrote:
timecop wrote:No, because then you'd have to have the copter precisely level at power up.


Use sqrt(accx*accx + accy*accy + accz*accz) and you wont have to have it level.

A better solution would be to have a similar ACC (and gyro) calibration as with mag calibration where you turn around the copter to find each axis scale factor an bias and optionally any cross axis sensitivity. The MPU6050 is supposed to be factory calibrated in this regard (scale and sensitivity, not bias), but for other sensors this would likely improve things a lot ;-)


Alexmos alredy done 3 axis ACC calibration to calculate each axis scale, but as for me it's very boring to do it each time... e.g. in autumn or winter when you need recalibrate ACC outside because of the temperature change inside and outside...

Also you can see in new alt hold solution I have already tried to use "dynamic" acc_1G=1/invG, but as I remember it has an big phase delay during the inclinations...

Code: Select all

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;


so we can use acc_1G calculated, just get it before motors armed... and acc 1G will be correct at least when copter in horizon...

Code: Select all

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;
  }
 
  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G_calculated;


UPDATE: Tested this in GUI. It takes into account ACC 1G inaccuracy... so make sense to include to current alt-hold implementation.
I.e. if ACC not calibrated enough or has inaccuracy because of the temperature drift or not scaled on Z axis (e.g. range from -496 to +520), it will be taken into account at the time when motors arms and will keep accZ near zero on start...

thx-
Alex

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

Re: Altitude Hold improvement solution

Post by Mac9 »

Hi,
I was sure I posted this question but it seems that I didn't. If it ends up on the board twice I apologise.
I have a problem I am running MultiWii_2_1_b2 on a CRIUS AIO Pro with Serial GPS on S2 at 52600 bds. The whole lot guiding a 2.7 Kg Hex built from Frame Wheel arms and plate. It flies very well indeed when not in Level or Althold. In fact its a little on the docile side. It is a different beast when switched to Level of Althold. It climbs very fast and can only be slowed down by almost closing the throttle, this gives a sever YOYO motion as the Hex Climbs and dives, absolutely uncontrollable. Further to this problem if I disarm, on the ground obviously, then rearm with either or Level and Althold selected the motors will not arm, seems like the fc goes to a high throttle setting when ever Leval or Althold is selected regardless of the actual throttle setting.

Any suggestions other than RTFM welcome. The directive RTFM will be followed If you point me to the correct bit to read :)

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

Re: Altitude Hold improvement solution

Post by mahowik »


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

Re: Altitude Hold improvement solution

Post by Termic1 »

Loaded old dev r1129 (alt hold bug free) and ALT HOLD now works great again!
Multiwii on ATMEGA328 and BMP085 with PID 3 - 0,020 - 60
Today I've done a couple of flights using throttle only for takeoff and landing!
ALT Hold is great!

Next tests on ATMega 2560 and MS5611 using r1143 modified to remove the bug

Luciano

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 you have a flight with more than 17 min of alt hold and pos hold flying with pushes up and down and sideways. I used Multiwii R1143 on Crius AIOP.

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

Thank you community for this achievement.
I am looking forward to have an controlled ascend/descent with failsafe landing on baro and use of X-Y acceleration to dampen the pos hold moves.

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

Re: Altitude Hold improvement solution

Post by Termic1 »

dramida wrote:Here you have a flight with more than 17 min of alt hold and pos hold flying with pushes up and down and sideways. I used Multiwii R1143 on Crius AIOP.

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

Thank you community for this achievement.
I am looking forward to have an controlled ascend/descent with failsafe landing on baro and use of X-Y acceleration to dampen the pos hold moves.


Dragu, are you using r1443 as is or have you modified imu.ino? With r1443, even with the alexinparis mod, I'm not able to have a good Alt Hold-Pos Hold (CRIUS AIO Pro).

Loaded "old" r1129 on Crius AIO Pro and now I can see an Alt Hold-Pos Hold pretty like your video with pid ALT 2.5-0.010-60

Luciano

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

Re: Altitude Hold improvement solution

Post by mahowik »

yes, for r1143 you should apply this fix viewtopic.php?f=8&t=2371&start=210#p24276

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

Re: Altitude Hold improvement solution

Post by vpb »

Hi mahowik, in r1143 version, there's no additional config options you added in 2.1b2, isn't it? if it's in the different branch, will it be merged to the main trunk?

jaames74
Posts: 22
Joined: Tue Jun 12, 2012 5:56 pm

Re: Altitude Hold improvement solution

Post by jaames74 »

guys,
which is a bug free version for mahowik's alt hold for crius se (bmp085)??

MultiWii_2_1_b1?
MultiWii_2_1_b2??
MultiWii_2_1_b2_mega328???
r1129????
r1143 version with Alex fix (viewtopic.php?f=8&t=2371&start=210#p24276)????

:?

can someone make this more clear?? Also i have noticed 2 things:

1) Why IMU_BMP085.ino does not exist after MultiWii_2_1_b2?
2) mahowik uses LPF_42Hz in all MultiWii_2_1_b(x) config.h. Is this optional or a must???

Thanks in advance :)

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

Re: Altitude Hold improvement solution

Post by mahowik »

if you want to try alt hold you can use any version specified above...
If you can't to get it working, see this viewtopic.php?f=8&t=2371&p=23923#p23819

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

Re: Altitude Hold improvement solution

Post by Termic1 »

New alt hold routines work great...but you have to be careful not to activate in config.h the new AP_MODE as it deactivates BARO when you move the sticks...and that has driven me mad!!!

If you want to use AP_MODE see viewtopic.php?f=16&t=1944&start=40#p24596

Luciano

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

Re: Altitude Hold improvement solution

Post by mahowik »

Hi guys,

On these weekend I have fixed some bugs in b2 and made RTH video with ALT_TO_RTH (If Alt-hold activated during the RTH or RTH during Alt-hold, it will keep specified altitude) and ALT_TO_RTH_FINISH (altitude after RTH, i.e. when copter reached to home position).

dramida wrote:2- Altitude control (up and down) is to "soft" , i crashed into ground two times and it tends to lose altitude after a controled climb (verry slow). 0.5 m/s vertical speed is too low.

as I wrote before: but there +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms... I checked with full throttle in GUI (where alt hold activated in middle point) it gives ~2m/s... probably for fast flights it's not enough though...
after set of packs flying I can say that is acceptable solution but nice to have beeper (I haven't) on the board and during the ascend/descend altitude you should hear this ;)
...where with default ascend/descend solution (from 2.1) now it's not possible to control altitude at all, because velocity from new alt hold dampens any change of initial throttle...

dramida wrote: Also i noticed that in altitude control mode, if you put the stick to minimum, the control is lost and immediat crash happens as motors enters in IDLE mode.

fixed this issue...

I'm going to post it with b3 soon...

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 think that calibrating at startup 1G acceleration is a must for successful repeatable behavior of all functions using accelerometer.(temperature variations, fabrications marginal errors, even altitude and latitude does count) Please consider a non leveled surface at startup.

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

Re: Altitude Hold improvement solution

Post by Sebbi »

mahowik, I tested your baro code (the part that is in dev _shared) today and made some observations (using default alt PIDs of 5.0 0.020 30):
1) as soon as I activate alt hold the copter drops 1-2 meters
2) it did hold the altitude somewhat by not going below that initial drop, but gained altitude quiet often and than almost killed the throttle to get back to the target altitude (a sign of wrong PIDs I guess)
3) even though you use the time since the last time the code ran it is highly timing sensitive. I changed the frequency of the function call from 40 Hz to 10 Hz to test this and the copter dropped from the sky ... i didn't test by how much it dropped (came down from about 4 meters high), but acc_z certainly didn't help here ...

Am I doing something wrong?

I checked the baro readings from my smartphone and it looked like the copter "knew" it was climbing/descending (change of altitude in GUI). I will redo the test and log the data to confirm.

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

Re: Altitude Hold improvement solution

Post by mahowik »

Sebbi wrote:mahowik, I tested your baro code (the part that is in dev _shared) today and made some observations (using default alt PIDs of 5.0 0.020 30):
1) as soon as I activate alt hold the copter drops 1-2 meters
2) it did hold the altitude somewhat by not going below that initial drop, but gained altitude quiet often and than almost killed the throttle to get back to the target altitude (a sign of wrong PIDs I guess)
3) even though you use the time since the last time the code ran it is highly timing sensitive. I changed the frequency of the function call from 40 Hz to 10 Hz to test this and the copter dropped from the sky ... i didn't test by how much it dropped (came down from about 4 meters high), but acc_z certainly didn't help here ...

Am I doing something wrong?

I checked the baro readings from my smartphone and it looked like the copter "knew" it was climbing/descending (change of altitude in GUI). I will redo the test and log the data to confirm.


Hi Sebbi,

1) yes, in case you activate althold during up/down movement it can change the alt hold point slightly, because EstAlt variable has some inertion because of LPFs there...
2) not sure that I understood you correctly (I'm far from native speaker :)), but if you mean that is not possible to change altitude by using throttle stick, yes as I specified here "where with default ascend/descend solution (from 2.1) now it's not possible to control altitude at all, because velocity from new alt hold dampens any change of initial throttle..."
In b2 I'm using other solution to change altitude on althold:

Code: Select all

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;
         }
        #if defined(BUZZER)
          buzzerFreq = 1; // beep buzzer 1Hz
        #endif
       
        errorAltitudeI = 0;
      }

3) yes. it's because mwii uses dependent on cycle time LPFs (for best performance). If you want time independent code just use LPF with specified cut frequency, e.g. from GPS.ino

Code: Select all

/// Low pass filter cut frequency for derivative calculation.
    ///
    static const float _filter = 7.9577e-3; // Set to  "1 / ( 2 * PI * f_cut )";
    // Examples for _filter:
    // f_cut = 10 Hz -> _filter = 15.9155e-3
    // f_cut = 15 Hz -> _filter = 10.6103e-3
    // f_cut = 20 Hz -> _filter =  7.9577e-3
    // f_cut = 25 Hz -> _filter =  6.3662e-3
    // f_cut = 30 Hz -> _filter =  5.3052e-3
.......
_derivative = _last_derivative + (*dt / ( _filter + *dt)) * (_derivative - _last_derivative);


thx-
Alex

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

Re: Altitude Hold improvement solution

Post by mahowik »

mahowik wrote:I'm going to post it with b3 soon...


Hi guys,

Here is b3... ;)

Also RTH video with ALT_TO_RTH (If Alt-hold activated during the RTH or RTH during Alt-hold, it will keep specified altitude... =10m in config) and ALT_TO_RTH_FINISH (altitude after RTH, i.e. when copter reached to home position... =3m in config).

https://www.youtube.com/watch?v=3gDocCKRQCU

thx-
Alex
Last edited by mahowik on Wed Dec 12, 2012 9:33 pm, edited 1 time in total.

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

Re: Altitude Hold improvement solution

Post by Sebbi »

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?

Post Reply