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
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?

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

Post Reply