PID regulators: Improve precision

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download

PID regulators: Improve precision

Postby ziss_dm » Sun Jun 05, 2011 5:44 am

Hi,

I was investigating the reasons why WMC heading hold mode (without MAG) not as good as commercial heli gyros, and looks like I found the reason. In the computeIMU() procedure we are dividing the actual Gyro output by 8. This is equivalent of dropping 3 LSB. After that the resolution of the ITG3200, for example, would be around 2 deg/sec, everything below that we just cannot "see". So my idea is really simple:
1) In the computeIMU() divide values by 2 (we still need some space in int16_t)
Code: Select all
void computeIMU () {
  uint8_t axis;
  static int16_t gyroADCprevious[3] = {0,0,0};
  static int16_t gyroADCp[3] = {0,0,0};
  int16_t gyroADCinter[3];
  static int16_t lastAccADC[3] = {0,0,0};
  static int16_t similarNumberAccData[3];
  static int16_t gyroDeviation[3];
  static uint32_t timeInterleave;
  static int16_t gyroYawSmooth = 0;

  //we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
  //gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
  //gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
  if (nunchukPresent) {
    annexCode();
    while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    updateIMU(0);
    getEstimatedAttitude(); // computation time must last less than one interleaving delay
    while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    while(updateIMU(0) != 1) ; // For this interleaving reading, we must have a gyro update at this point (less delay)

    for (axis = 0; axis < 3; axis++) {
      // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis]+4)/4/2; // /4 is to average 4 values ; /2 is to reserve 1 bit
      gyroADCprevious[axis] = gyroADC[axis];
    }
  } else {
    #if defined(I2C_ACC) || defined(ADCACC)
      updateIMU(1); //with I2C or ADC ACC
      getEstimatedAttitude();
    #else
      updateIMU(0); //without ACC
    #endif
    for (axis = 0; axis < 3; axis++)
      gyroADCp[axis] =  gyroADC[axis];
    timeInterleave=micros();
    annexCode();
    while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads
    updateIMU(0); //without ACC
    for (axis = 0; axis < 3; axis++) {
      gyroADCinter[axis] =  gyroADC[axis]+gyroADCp[axis];
      // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis]+3)/3/2; // /3 is to average 3 values ; /2 is to reserve 1 bit
      gyroADCprevious[axis] = gyroADCinter[axis]/2;
      #if not defined (I2C_ACC) && not defined (ADCACC)
        accADC[axis]=0;
      #endif
    }
  }
  #if defined(TRI)
    gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW]+1)/3;
    gyroYawSmooth = gyroData[YAW];
  #endif
}


2) Adjust PID controllers to divide gyro readings by 4 AFTER multiplications. This way we would still have gyro values properly scaled, but we preserve 2 bits of resolution:
Code: Select all
  //**** PITCH & ROLL & YAW PID ****   
  for(axis=0;axis<3;axis++) {
    if (accMode == 1 && axis<2 ) { //LEVEL MODE
      errorAngle = rcCommand[axis]/2 - angle[axis]/2;
      PTerm      = (errorAngle)*PLEVEL8/50 - gyroData[axis]*dynP8[axis]/10/4;

      errorAngleI[axis] +=  errorAngle;
      errorAngleI[axis]  = constrain(errorAngleI[axis],-5000,+5000); //WindUp
      ITerm              = errorAngleI[axis] *ILEVEL8/2000;
    } else { //ACRO MODE or YAW axis
      error = rcCommand[axis]*10*4/P8[axis] - gyroData[axis];
      PTerm = rcCommand[axis]-gyroData[axis]*dynP8[axis]/10/4;
     
      errorGyroI[axis] += error;
      errorGyroI[axis]  = constrain(errorGyroI[axis],-8000,+8000); //WindUp
      if (abs(gyroData[axis])>320) errorGyroI[axis] = 0;
      ITerm = errorGyroI[axis]*I8[axis]/1000/4;
    }
    delta          = gyroData[axis] - lastGyro[axis];
    DTerm          = (delta1[axis]+delta2[axis]+delta+1)*dynD8[axis]/3/4;
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;
    lastGyro[axis] = gyroData[axis];

    axisPID[axis] =  PTerm + ITerm - DTerm;
}


The flight tests was quite impressive. In the room, I was able to hover couple of minutes without rudder corrections. Additionally, I have impression, that everything is much more smooth and precise.

Notes:
1) The Gyro values in the GUI would be over the range and limited by 500.
2) Most probably Gyro readings would not be 0 any more
3) It is not necessary to re-adjust PIDs


regards,
ziss_dm
Last edited by ziss_dm on Mon Jun 06, 2011 1:03 pm, edited 1 time in total.
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby gompf-2 » Sun Jun 05, 2011 11:50 am

Hi,

I tested it as mod to 1.7 on a "warthox mini y6 config" with ITG3200/ADXL345. Works fine.

Thanks and regards,
gompf
gompf-2
 
Posts: 136
Joined: Sun Jun 05, 2011 11:46 am

Re: PID regulators: Improve precision

Postby Centurian » Sun Jun 05, 2011 6:01 pm

I basicly made the same change a few weeks ago and see good results as well.
Centurian
 
Posts: 44
Joined: Sat Jan 22, 2011 10:55 am

Re: PID regulators: Improve precision

Postby Alexinparis » Mon Jun 06, 2011 12:14 am

Thank you again for this contribution. I'm sure it will improve the overall multiwii stability.


just some warning about int16_t overflow.

example in the expression, but there are others.
rcCommand[axis]*10*4/P8[axis]

rcCommand[axis]*10*4 = 20000 if rcCommand = 500, and will then produce an overflow.

maybe it's time to use some int32_t variables to secure the potential overflows, with a little more loop time.
Alexinparis
 
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: PID regulators: Improve precision

Postby ziss_dm » Mon Jun 06, 2011 1:01 am

Hi Alex,
You are right, it is better to change type of "error" variable to int32_t, to be on the safe side. But currently, due optimization, it does not make any difference.

But the rest, looks the safe:
1) rcCommand[axis]-gyroData[axis]*dynP8[axis]/10/4 - internally calculated as int32
2) errorGyroI[] already int32
3) DTerm cannot change so fast.

Can you carefully look at this, maybe I'm missing something.

regards,
ziss_dm
Last edited by ziss_dm on Mon Jun 06, 2011 7:50 am, edited 1 time in total.
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby Centurian » Mon Jun 06, 2011 7:45 am

Oops, sorry
Last edited by Centurian on Mon Jun 06, 2011 10:14 am, edited 2 times in total.
Centurian
 
Posts: 44
Joined: Sat Jan 22, 2011 10:55 am

Re: PID regulators: Improve precision

Postby ziss_dm » Mon Jun 06, 2011 10:09 am

Hi,

Just the same idea to preserve 1 LSB of the angle[] in level mode:
Code: Select all
  for(axis=0;axis<3;axis++) {
    if (accMode == 1 && axis<2 ) { //LEVEL MODE
      errorAngle = rcCommand[axis] - angle[axis];
      PTerm      = (errorAngle)*PLEVEL8/50/2 - gyroData[axis]*dynP8[axis]/10/4;

      errorAngleI[axis] +=  errorAngle;
      errorAngleI[axis]  = constrain(errorAngleI[axis],-10000,+10000); //WindUp
      ITerm              = errorAngleI[axis] *ILEVEL8/2000/2;
    } else { //ACRO MODE or YAW axis
      error = rcCommand[axis]*10*4/P8[axis] - gyroData[axis];
      PTerm = rcCommand[axis]-gyroData[axis]*dynP8[axis]/10/4;
     
      errorGyroI[axis] += error;
      errorGyroI[axis]  = constrain(errorGyroI[axis],-8000,+8000); //WindUp
      if (abs(gyroData[axis])>320) errorGyroI[axis] = 0;
      ITerm = errorGyroI[axis]*I8[axis]/1000/4;
    }
    delta          = gyroData[axis] - lastGyro[axis];
    DTerm          = (delta1[axis]+delta2[axis]+delta+1)*dynD8[axis]/3/4;
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;
    lastGyro[axis] = gyroData[axis];

    axisPID[axis] =  PTerm + ITerm - DTerm;
  }


Again, during the room test, feels really good.

@Centurian: It would be nice, if you could create separate thread in "Ideas" section to discuss ideas about Alt. hold. And you probably should start from beginning, as currently I have really vague idea what problems we have and what we trying to archive. Also looks like multiple people trying to improve this, it would also be nice to combine knowledge.

regards,
ziss_dm
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby ziss_dm » Mon Jun 06, 2011 11:41 am

Hi,
I have performed simulation, and looks like I was wrong about: PTerm = rcCommand[axis] - gyroData[axis]*dynP8[axis]/10/4. It was not calculated in int32. With P=4, the overflow could happened with angular rate more than 400deg/sec. I think, it is not acceptable, so we need to forcible use int32 here. This is modified code:
Code: Select all
  //**** PITCH & ROLL & YAW PID ****   
  for(axis=0;axis<3;axis++) {
    if (accMode == 1 && axis<2 ) { //LEVEL MODE
      errorAngle = rcCommand[axis] - angle[axis];
      PTerm      = (errorAngle)*PLEVEL8/50/2 - int32_t(gyroData[axis])*dynP8[axis]/10/4;

      errorAngleI[axis] +=  errorAngle;
      errorAngleI[axis]  = constrain(errorAngleI[axis],-10000,+10000); //WindUp
      ITerm              = errorAngleI[axis] *ILEVEL8/2000/2;
    } else { //ACRO MODE or YAW axis
      error = rcCommand[axis]*10*4/P8[axis] - gyroData[axis];
      PTerm = rcCommand[axis] - int32_t(gyroData[axis])*dynP8[axis]/10/4;
     
      errorGyroI[axis] += error;
      errorGyroI[axis]  = constrain(errorGyroI[axis],-8000,+8000); //WindUp
      if (abs(gyroData[axis])>320) errorGyroI[axis] = 0;
      ITerm = errorGyroI[axis]*I8[axis]/1000/4;
    }
    delta          = gyroData[axis] - lastGyro[axis];
    DTerm          = (delta1[axis]+delta2[axis]+delta+1)*dynD8[axis]/3/4;
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;
    lastGyro[axis] = gyroData[axis];

    axisPID[axis] =  PTerm + ITerm - DTerm;
  }


regards,
ziss_dm
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby ziss_dm » Mon Jun 06, 2011 12:42 pm

Hi,

And after that, it is possible to use full 14bit resolution of the gyro:
Code: Select all
void computeIMU () {
  uint8_t axis;
  static int16_t gyroADCprevious[3] = {0,0,0};
  static int16_t gyroADCp[3] = {0,0,0};
  int16_t gyroADCinter[3];
  static int16_t lastAccADC[3] = {0,0,0};
  static int16_t similarNumberAccData[3];
  static int16_t gyroDeviation[3];
  static uint32_t timeInterleave;
  static int16_t gyroYawSmooth = 0;

  //we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
  //gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
  //gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
  if (nunchukPresent) {
    annexCode();
    while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    updateIMU(0);
    getEstimatedAttitude(); // computation time must last less than one interleaving delay
    while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    while(updateIMU(0) != 1) ; // For this interleaving reading, we must have a gyro update at this point (less delay)

    for (axis = 0; axis < 3; axis++) {
      // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis])/4; // /4 is to average 4 values
      gyroADCprevious[axis] = gyroADC[axis];
    }
  } else {
    #if defined(I2C_ACC) || defined(ADCACC)
      updateIMU(1); //with I2C or ADC ACC
      getEstimatedAttitude();
    #else
      updateIMU(0); //without ACC
    #endif
    for (axis = 0; axis < 3; axis++)
      gyroADCp[axis] =  gyroADC[axis];
    timeInterleave=micros();
    annexCode();
    while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads
    updateIMU(0); //without ACC
    for (axis = 0; axis < 3; axis++) {
      gyroADCinter[axis] =  gyroADC[axis]+gyroADCp[axis];
      // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3; // /3 is to average 3 values 
      gyroADCprevious[axis] = gyroADCinter[axis]/2;
      #if not defined (I2C_ACC) && not defined (ADCACC)
        accADC[axis]=0;
      #endif
    }
  }
  #if defined(TRI)
    gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW]+1)/3;
    gyroYawSmooth = gyroData[YAW];
  #endif
}


Code: Select all
  //**** PITCH & ROLL & YAW PID ****   
  for(axis=0;axis<3;axis++) {
    if (accMode == 1 && axis<2 ) { //LEVEL MODE
      errorAngle = rcCommand[axis] - angle[axis];
      PTerm      = (errorAngle)*PLEVEL8/50/2 - int32_t(gyroData[axis])*dynP8[axis]/10/8;

      errorAngleI[axis] +=  errorAngle;
      errorAngleI[axis]  = constrain(errorAngleI[axis],-10000,+10000); //WindUp
      ITerm              = errorAngleI[axis] *ILEVEL8/2000/2;
    } else { //ACRO MODE or YAW axis
      error = rcCommand[axis]*10*8/P8[axis] - gyroData[axis];
      PTerm = rcCommand[axis] - int32_t(gyroData[axis])*dynP8[axis]/10/8;
     
      errorGyroI[axis] += error;
      errorGyroI[axis]  = constrain(errorGyroI[axis],-16000,+16000); //WindUp
      if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
      ITerm = errorGyroI[axis]*I8[axis]/1000/8;
    }
    delta          = gyroData[axis] - lastGyro[axis];
    DTerm          = (delta1[axis]+delta2[axis]+delta+1)*dynD8[axis]/3/8;
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;
    lastGyro[axis] = gyroData[axis];

    axisPID[axis] =  PTerm + ITerm - DTerm;
  }


regards,
ziss_dm
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby ziss_dm » Mon Jun 06, 2011 2:54 pm

Hi,

And for people, who's transmitter sticks not centering properly, like mine DX7, it is good idea to add "Stick deadband". Easiest way is to add to the end of annexCode() procedure, something like this:

Code: Select all
  // Deadband
  if (abs(rcCommand[PITCH])< 5) rcCommand[PITCH] = 0;
  if (abs(rcCommand[ROLL]) < 5) rcCommand[ROLL] = 0;
  if (abs(rcCommand[YAW])  < 10) rcCommand[YAW] = 0;


Note:
For Yaw it is bigger as I was always touching it when was adjusting Throttle (I'm on Mode 2).

regards,
ziss_dm
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby arnaldo » Tue Jun 07, 2011 3:13 pm

I hope that Alex will add all these mod in his last dev ;)!
arnaldo
 
Posts: 49
Joined: Sun Mar 06, 2011 4:53 pm

Re: PID regulators: Improve precision

Postby Alexinparis » Tue Jun 07, 2011 11:22 pm

arnaldo wrote:I hope that Alex will add all these mod in his last dev ;)!

here you are :D
http://code.google.com/p/multiwii/sourc ... 110607.zip
Alexinparis
 
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: PID regulators: Improve precision

Postby Alexinparis » Tue Jun 07, 2011 11:24 pm

ziss_dm wrote:Hi,

And for people, who's transmitter sticks not centering properly, like mine DX7, it is good idea to add "Stick deadband". Easiest way is to add to the end of annexCode() procedure, something like this:

Code: Select all
  // Deadband
  if (abs(rcCommand[PITCH])< 5) rcCommand[PITCH] = 0;
  if (abs(rcCommand[ROLL]) < 5) rcCommand[ROLL] = 0;
  if (abs(rcCommand[YAW])  < 10) rcCommand[YAW] = 0;


Note:
For Yaw it is bigger as I was always touching it when was adjusting Throttle (I'm on Mode 2).

regards,
ziss_dm


Hi,

I'm not sure about this mod:
There are situations where you must use some trim (tricopter ROLL axis, or unbalanced config).
In this case, this mod won't be so useful if the trim exceed 5 or 10 units.
Alexinparis
 
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: PID regulators: Improve precision

Postby Alexinparis » Tue Jun 07, 2011 11:33 pm

ziss_dm wrote:Hi,

And after that, it is possible to use full 14bit resolution of the gyro:
Code: Select all
void computeIMU () {
  uint8_t axis;
  static int16_t gyroADCprevious[3] = {0,0,0};
  static int16_t gyroADCp[3] = {0,0,0};
  int16_t gyroADCinter[3];
  static int16_t lastAccADC[3] = {0,0,0};
  static int16_t similarNumberAccData[3];
  static int16_t gyroDeviation[3];
  static uint32_t timeInterleave;
  static int16_t gyroYawSmooth = 0;

  //we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
  //gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
  //gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
  if (nunchukPresent) {
    annexCode();
    while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    updateIMU(0);
    getEstimatedAttitude(); // computation time must last less than one interleaving delay
    while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    while(updateIMU(0) != 1) ; // For this interleaving reading, we must have a gyro update at this point (less delay)

    for (axis = 0; axis < 3; axis++) {
      // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis])/4; // /4 is to average 4 values
      gyroADCprevious[axis] = gyroADC[axis];
    }
  } else {
    #if defined(I2C_ACC) || defined(ADCACC)
      updateIMU(1); //with I2C or ADC ACC
      getEstimatedAttitude();
    #else
      updateIMU(0); //without ACC
    #endif
    for (axis = 0; axis < 3; axis++)
      gyroADCp[axis] =  gyroADC[axis];
    timeInterleave=micros();
    annexCode();
    while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads
    updateIMU(0); //without ACC
    for (axis = 0; axis < 3; axis++) {
      gyroADCinter[axis] =  gyroADC[axis]+gyroADCp[axis];
      // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3; // /3 is to average 3 values 
      gyroADCprevious[axis] = gyroADCinter[axis]/2;
      #if not defined (I2C_ACC) && not defined (ADCACC)
        accADC[axis]=0;
      #endif
    }
  }
  #if defined(TRI)
    gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW]+1)/3;
    gyroYawSmooth = gyroData[YAW];
  #endif
}


Code: Select all
  //**** PITCH & ROLL & YAW PID ****   
  for(axis=0;axis<3;axis++) {
    if (accMode == 1 && axis<2 ) { //LEVEL MODE
      errorAngle = rcCommand[axis] - angle[axis];
      PTerm      = (errorAngle)*PLEVEL8/50/2 - int32_t(gyroData[axis])*dynP8[axis]/10/8;

      errorAngleI[axis] +=  errorAngle;
      errorAngleI[axis]  = constrain(errorAngleI[axis],-10000,+10000); //WindUp
      ITerm              = errorAngleI[axis] *ILEVEL8/2000/2;
    } else { //ACRO MODE or YAW axis
      error = rcCommand[axis]*10*8/P8[axis] - gyroData[axis];
      PTerm = rcCommand[axis] - int32_t(gyroData[axis])*dynP8[axis]/10/8;
     
      errorGyroI[axis] += error;
      errorGyroI[axis]  = constrain(errorGyroI[axis],-16000,+16000); //WindUp
      if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
      ITerm = errorGyroI[axis]*I8[axis]/1000/8;
    }
    delta          = gyroData[axis] - lastGyro[axis];
    DTerm          = (delta1[axis]+delta2[axis]+delta+1)*dynD8[axis]/3/8;
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;
    lastGyro[axis] = gyroData[axis];

    axisPID[axis] =  PTerm + ITerm - DTerm;
  }


regards,
ziss_dm


I've followed this principle in the last today dev.
There is no more uint32_t variable in the PID, except for transitional calculation.
Overflow situations should not happen. I've tested it briefly without any issues.

This improve clearly the attitude on a multi where I use an ITG3200.
There is no more "mini spike" and the copter is perfectly smoothed. I can even increase P.

However, with a very noisy WMP clone, the situation seems to be worse: more oscillation and I need to decrease P.

I will try to implement a small hysteresis after the PID calculation and just before the motor order.
Alexinparis
 
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: PID regulators: Improve precision

Postby ziss_dm » Wed Jun 08, 2011 12:30 am

Hi Alex,
I have noticed, that in the ComputeIMU() we have filters for Gyro data and somehow they are different for WMP and others:

WMP:
Code: Select all
    // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis])/4; // /4 is to average 4 values
      gyroADCprevious[axis] = gyroADC[axis];


ITG3200:
Code: Select all
      gyroADCinter[axis] =  gyroADC[axis]+gyroADCp[axis];
      // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3; // /3 is to average 3 values 
      gyroADCprevious[axis] = gyroADCinter[axis]/2;


ITG one looks like 2nd order filter with lower cut-off freq (correct me if I'm wrong). Could you try to use ITG filter for WMP first?

regards,
ziss_dm
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby ziss_dm » Wed Jun 08, 2011 12:59 am

Hi,

The problem here is that sometimes sticks just not centering mechanically. So I can trim it to 1500, but when I move stick right and release it returns to 1502, move left and release it returns to 1498. Many commercial gyros has setting "Stick deadband" to deal with this problem (like this one: http://www.assan.cn/download/GA510_Manual_EN.pdf). Additinally it could be used to prevent unintentional commands.
Just would be nice to have it as an option, much easier than replacing pots in transmitter.. :)

Regards,
ziss_dm
Last edited by ziss_dm on Wed Jun 08, 2011 8:38 am, edited 1 time in total.
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby Centurian » Wed Jun 08, 2011 7:33 am

Alexinparis wrote:
ziss_dm wrote:Hi,

And for people, who's transmitter sticks not centering properly, like mine DX7, it is good idea to add "Stick deadband". Easiest way is to add to the end of annexCode() procedure, something like this:

Code: Select all
  // Deadband
  if (abs(rcCommand[PITCH])< 5) rcCommand[PITCH] = 0;
  if (abs(rcCommand[ROLL]) < 5) rcCommand[ROLL] = 0;
  if (abs(rcCommand[YAW])  < 10) rcCommand[YAW] = 0;


Note:
For Yaw it is bigger as I was always touching it when was adjusting Throttle (I'm on Mode 2).

regards,
ziss_dm


Hi,

I'm not sure about this mod:
There are situations where you must use some trim (tricopter ROLL axis, or unbalanced config).
In this case, this mod won't be so useful if the trim exceed 5 or 10 units.


Agreed, this eliminates the ability to trim and creates a jump at the edge of the deadband. a couple of elses could eliminate the jump, somthing like:
else if rcCommand[PITCH])< 5 rcCommand[PITCH] += 5;
else if rcCommand[PITCH])> 5 rcCommand[PITCH] -= 5;
Centurian
 
Posts: 44
Joined: Sat Jan 22, 2011 10:55 am

Re: PID regulators: Improve precision

Postby ziss_dm » Wed Jun 08, 2011 9:20 am

Hi Alex,
I have just looked in dev version, this PID procedure now 86 bytes shoter even than original one!! (from 1.7) :shock:
Nice work!!

regards,
ziss_dm
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby Alexinparis » Wed Jun 08, 2011 10:34 pm

ziss_dm wrote:Hi Alex,
I have noticed, that in the ComputeIMU() we have filters for Gyro data and somehow they are different for WMP and others:

WMP:
Code: Select all
    // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis])/4; // /4 is to average 4 values
      gyroADCprevious[axis] = gyroADC[axis];


ITG3200:
Code: Select all
      gyroADCinter[axis] =  gyroADC[axis]+gyroADCp[axis];
      // empirical, we take a weighted value of the current and the previous values
      gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3; // /3 is to average 3 values 
      gyroADCprevious[axis] = gyroADCinter[axis]/2;


ITG one looks like 2nd order filter with lower cut-off freq (correct me if I'm wrong). Could you try to use ITG filter for WMP first?

regards,
ziss_dm


Hi,
The first part of the code is only used if you have a nunchuk connected.
So, for a WMP only conf or ITG3200+other sensors, the second part is used: this is the only I tested with the mod.
Alexinparis
 
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: PID regulators: Improve precision

Postby ziss_dm » Mon Jul 04, 2011 3:19 am

Hi Alex,
In the latest dev, we have the following calculation for PTerm in level mode:
Code: Select all
      PTerm      = errorAngle*(P8[PIDLEVEL]/10)/10 ;                              //680*20 = 13600: 16 bits is ok here


I think, it is better to use int32 here:
Code: Select all
      PTerm      = (int32_t)errorAngle*P8[PIDLEVEL]/100;                          //32 bits is needed for calculation 1800*200=360000, 360000/100 = 3600 16 bits is ok for result.


For the following reasons:
1) It is more efficient than 2 divisions (code size reduced by 12 bytes)
2) It gives better precision for the level P (currently everything after decimal point is ignored)

The full PID code:
Code: Select all
  //**** PITCH & ROLL & YAW PID ****   
  for(axis=0;axis<3;axis++) {
    if (accMode == 1 && axis<2 ) { //LEVEL MODE
      errorAngle = rcCommand[axis] - angle[axis];                                 //500+1800 = 2300: 16 bits is ok here
      PTerm      = (int32_t)errorAngle*P8[PIDLEVEL]/100;                          //32 bits is needed for calculation 1800*200=360000, 360000/100 = 3600 16 bits is ok for result.

      errorAngleI[axis] += errorAngle;                                            //16 bits is ok here
      errorAngleI[axis]  = constrain(errorAngleI[axis],-10000,+10000); //WindUp   //16 bits is ok here
      ITerm              = (int32_t)errorAngleI[axis]*I8[PIDLEVEL]/4000;          //32 bits is needed for calculation:10000*I8 could exceed 32768   16 bits is ok for result
    } else { //ACRO MODE or YAW axis
      error = (int32_t)rcCommand[axis]*10*8/P8[axis] - gyroData[axis];            //32 bits is needed for calculation: 500*10*8 = 40000   16 bits is ok for result if P>2
      PTerm = rcCommand[axis];

      errorGyroI[axis] += error;                                                  //16 bits is ok here
      errorGyroI[axis]  = constrain(errorGyroI[axis],-16000,+16000); //WindUp     //16 bits is ok here
      if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
      ITerm = (int32_t)errorGyroI[axis]*I8[axis]/1000/8;                          //32 bits is needed for calculation: 16000*I8  16 bits is ok for result
    }
    PTerm         -= (int32_t)gyroData[axis]*dynP8[axis]/10/8;                    //32 bits is needed for calculation            16 bits is ok for result

    delta          = gyroData[axis] - lastGyro[axis];                             //16 bits is ok here, because the dif between 2 consecutive gyro reads is limited
    DTerm          = (delta1[axis]+delta2[axis]+delta+1)*dynD8[axis]/3/8;         //16 bits is ok here
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;
    lastGyro[axis] = gyroData[axis];

    axisPID[axis] =  PTerm + ITerm - DTerm;
  }



regards,
ziss_dm
ziss_dm
 
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: PID regulators: Improve precision

Postby Alexinparis » Mon Jul 04, 2011 9:54 am

Hi,

You're right,
I did this small hack because there was a clear overflow near 20 or 30deg, and because it's not too important to have a decimal accuracy for P LEVEL.
Your suggestion is better.
Alexinparis
 
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm


Return to Software development

Who is online

Users browsing this forum: No registered users and 6 guests