MultiWii

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

Maybe another set of eyes can spot something I missed.

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

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

copterrichie wrote:Define long term?

mahowik wrote:

copterrichie wrote:Define long term?

not sure that i understood the question...

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

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

fiendie wrote:

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

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

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

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

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

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

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

Thanks to everyone and the Throttle Expo does work!

fiendie wrote:

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

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

Thanks to everyone and the Throttle Expo does work!

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

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

copterrichie wrote:Semantics.

vpb wrote:

copterrichie wrote:Semantics.

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

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

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

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

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

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

Maybe another set of eyes can spot something I missed.

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

nhadrian wrote:Update:

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

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

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

#define ACC_Z_DEADBAND (acc_1G/40) 

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

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

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


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

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

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

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

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

mahowik wrote:

nhadrian wrote:Update:

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

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

my current PIDs 5.2 - 0.020 - 30

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

Code: Select all

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

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

#define ACC_Z_DEADBAND (acc_1G/40) 

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

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

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


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

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

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

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

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

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

Code: Select all

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

#define ACC_Z_DEADBAND (acc_1G/50) 

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

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

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


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

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

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

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

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

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

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

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

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

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