Moving Average on axis implemented...

[Magnetron]

Hi.
I am implemented a Moving Average algorithm on gyro axis, acc axis and on barometer.
In the attach you can found the v1.9 with my implementation.
I and some other users had tested it with result that the multirotor gain more stability in windy conditions and in vertical down fly...
Would you integrate in in official repository?

[Magnetron]

Had someone tryed it?

[copterrichie]

I would like to try it, can you post the code segment of the changes made please? Reason is, I will have to merge your code in with my modified code.

Thank you.

[Magnetron]

Yes, of corse.
The file are this 2 main:
CONFIG.H
at the end past this code:

Code: Select all

//#######################################################
// FUNCTIONS BY MAGNETRON1 (Michele Ardito)
// Moving Average Gyros by Magnetron1
#define MMGYRO                         // Active Moving Average Function for Gyros
#define MMGYROVECTORLENGHT 10          // Lenght of Moving Average Vector
// Moving Average Accelerometers by Magnetron1
#define MMACC                          // Active Moving Average Function for Accelerometers
#define MMACCVECTORLENGHT 10           // Lenght of Moving Average Vector
// Moving Average Barometer by Magnetron1
//#define MMBARO                         // Active Moving Average Function for Barometer
#define MMBAROVECTORLENGHT 8           // Lenght of Moving Average Vector
// Moving Average ServoGimbal Signal Output by Magnetron1
#define MMSERVOGIMBAL                  // Active Output Moving Average Function for Servos Gimbal
#define MMSERVOGIMBALVECTORLENGHT 32   // Lenght of Moving Average Vector

in file SENSORS.PDE
find and replace the Gyro_Common routine with this:

Code: Select all

// ****************
// GYRO common part
// ****************
void GYRO_Common() {
  static int16_t previousGyroADC[3] = {0,0,0};
  static int32_t g[3];
  uint8_t axis;
#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;
      // 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+3*nunchuk);
      }
    }
    calibratingG--;
  }
#ifdef MMGYRO       
  //Moving Average Gyros by Magnetron1
  mediaMobileGyroIDX = ++mediaMobileGyroIDX % MMGYROVECTORLENGHT;
  for (axis = 0; axis < 3; axis++) {
    gyroADC[axis]  -= gyroZero[axis];
   //new implementation of Moving Average for fast response
   mediaMobileGyroADCSum[axis] -= mediaMobileGyroADC[axis][mediaMobileGyroIDX];
    //anti gyro glitch, limit the variation between two consecutive readings
    mediaMobileGyroADC[axis][mediaMobileGyroIDX] = constrain(gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
   mediaMobileGyroADCSum[axis] += mediaMobileGyroADC[axis][mediaMobileGyroIDX];
   gyroADC[axis] = mediaMobileGyroADCSum[axis] / MMGYROVECTORLENGHT;
#else 
  for (axis = 0; axis < 3; axis++) {
    gyroADC[axis]  -= gyroZero[axis];
    //anti gyro glitch, limit the variation between two consecutive readings
    gyroADC[axis] = constrain(gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
#endif    
    previousGyroADC[axis] = gyroADC[axis];
  }
}

find and replace the ACC_Common routine with this:

Code: Select all

// ****************
// ACC common part
// ****************
void ACC_Common() {
  static int32_t a[3];
  uint8_t axis;
#if defined MMACC       
  // Moving Average Accelerometers by Magnetron1
  //---------------------------------------------------
  static int16_t mediaMobileAccADC[3][MMACCVECTORLENGHT];
  static int32_t mediaMobileAccADCSum[3];
  static uint8_t mediaMobileAccIDX;
  //---------------------------------------------------
#endif
  
  if (calibratingA>0) {
    for (axis = 0; axis < 3; axis++) {
      // Reset a[axis] at start of calibration
      if (calibratingA == 400) a[axis]=0;
      // Sum up 400 readings
      a[axis] +=accADC[axis];
      // Clear global variables for next reading
      accADC[axis]=0;
      accZero[axis]=0;
    }
    // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
    if (calibratingA == 1) {
      accZero[ROLL]  = a[ROLL]/400;
      accZero[PITCH] = a[PITCH]/400;
      accZero[YAW]   = a[YAW]/400-acc_1G; // for nunchuk 200=1G
      accTrim[ROLL]   = 0;
      accTrim[PITCH]  = 0;
      writeParams(); // write accZero in EEPROM
    }
    calibratingA--;
  }
  #if defined(InflightAccCalibration)
      static int32_t b[3];
      static int16_t accZero_saved[3]  = {0,0,0};
      static int16_t  accTrim_saved[2] = {0, 0};
      //Saving old zeropoints before measurement
      if (InflightcalibratingA==50) {
         accZero_saved[ROLL]  = accZero[ROLL] ;
         accZero_saved[PITCH] = accZero[PITCH];
         accZero_saved[YAW]   = accZero[YAW] ;
         accTrim_saved[ROLL] = accTrim[ROLL] ;
         accTrim_saved[PITCH] = accTrim[PITCH] ;
      }
      if (InflightcalibratingA>0) {
        for (axis = 0; axis < 3; axis++) {
          // Reset a[axis] at start of calibration
          if (InflightcalibratingA == 50) b[axis]=0;
          // Sum up 50 readings
          b[axis] +=accADC[axis];
          // Clear global variables for next reading
          accADC[axis]=0;
          accZero[axis]=0;
        }
        //all values are measured
        if (InflightcalibratingA == 1) {
          AccInflightCalibrationMeasurementDone = 1;
          blinkLED(10,10,2);      //buzzer for indicatiing the start inflight
        // recover saved values to maintain current flight behavior until new values are transferred
         accZero[ROLL]  = accZero_saved[ROLL] ;
         accZero[PITCH] = accZero_saved[PITCH];
         accZero[YAW]   = accZero_saved[YAW] ;
         accTrim[ROLL] = accTrim_saved[ROLL] ;
         accTrim[PITCH] = accTrim_saved[PITCH] ;
        }
        InflightcalibratingA--;
      }
      // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
      if (AccInflightCalibrationSavetoEEProm == 1){  //the copter is landed, disarmed and the combo has been done again
        AccInflightCalibrationSavetoEEProm = 0;
        accZero[ROLL]  = b[ROLL]/50;
        accZero[PITCH] = b[PITCH]/50;
        accZero[YAW]   = b[YAW]/50-acc_1G; // for nunchuk 200=1G
        accTrim[ROLL]   = 0;
        accTrim[PITCH]  = 0;
        writeParams(); // write accZero in EEPROM
      }
  #endif
#ifdef MMACC
  //Moving Average Accelerometers by Magnetron1
  mediaMobileAccIDX = ++mediaMobileAccIDX % MMACCVECTORLENGHT;
  for (axis = 0; axis < 3; axis++) {
    accADC[axis]  -= accZero[axis];
   //new implementation of Moving Average for fast response
   mediaMobileAccADCSum[axis] -= mediaMobileAccADC[axis][mediaMobileAccIDX];
    mediaMobileAccADC[axis][mediaMobileAccIDX] = accADC[axis];
   mediaMobileAccADCSum[axis] += mediaMobileAccADC[axis][mediaMobileAccIDX];
   accADC[axis] = mediaMobileAccADCSum[axis] / MMACCVECTORLENGHT;
  }
#else 
  accADC[ROLL]  -=  accZero[ROLL] ;
  accADC[PITCH] -=  accZero[PITCH];
  accADC[YAW]   -=  accZero[YAW] ;
#endif    
}

You are done.

If you want try moving average for baro also you have to change IMU.PDE.
Find GetEstimateAltitude routine, in the variables declaretion on top of routine add this code:

Code: Select all

#if defined MMBARO
  // Moving Average Barometer by Magnetron1
  //---------------------------------------------------
  static int16_t mediaMobileBaroADC[MMBAROVECTORLENGHT];
  static int32_t mediaMobileBaroADCSum;
  static uint8_t mediaMobileBaroIDX;
  //---------------------------------------------------
#endif

just aftes soft start condition add this:

Code: Select all

#ifdef MMBARO
  //Moving Average Barometer by Magnetron1
  //new implementation of Moving Average for fast response
  mediaMobileBaroIDX = ++mediaMobileBaroIDX % MMBAROVECTORLENGHT;
  mediaMobileBaroADCSum -= mediaMobileBaroADC[mediaMobileBaroIDX];
  mediaMobileBaroADC[mediaMobileBaroIDX] = BaroAlt;
  mediaMobileBaroADCSum += mediaMobileBaroADC[mediaMobileBaroIDX];
  BaroAltMM = mediaMobileBaroADCSum / MMBAROVECTORLENGHT;
#endif

In the INITIATE condition fullfil something like this code:

Code: Select all

  if (!inited) {
    inited = 1;
#ifdef MMBARO
  //Moving Average Barometer by Magnetron1
    tmpAlt = BaroAltMM*10;
#else
    tmpAlt = BaroAlt*10;
#endif
    AccScale = 100 * 9.80665f / acc_1G;
  }

and then on the estimation error segment type this:

Code: Select all

  // Estimation Error
#ifdef MMBARO
  //Moving Average Barometer by Magnetron1
  AltError = BaroAltMM - EstAlt; 
#else
  AltError = BaroAlt - EstAlt; 
#endif

If you want moving average also on servo gimbal do this variation in file OUTPUT.PDE:
find writeservos routine and put this in:

Code: Select all

void writeServos() {
  #if defined(SERVO)
    // write primary servos
    #if defined(PRI_SERVO_FROM)
      for(uint8_t i = (PRI_SERVO_FROM-1); i < PRI_SERVO_TO; i++){
        atomicServo[i] = (servo[i]-1000)>>2;
      }
    #endif
    // write secundary servos
    #if defined(SEC_SERVO_FROM)
       #if defined(SERVO_TILT) && defined(MMSERVOGIMBAL)
           // Moving Average Servo Gimbal by Magnetron1
           //---------------------------------------------------
           static int16_t mediaMobileServoGimbalADC[2][MMSERVOGIMBALVECTORLENGHT];
           static int32_t mediaMobileServoGimbalADCSum[2];
           static uint8_t mediaMobileServoGimbalIDX;
         uint8_t axis;
           //---------------------------------------------------
           mediaMobileServoGimbalIDX = ++mediaMobileServoGimbalIDX % MMSERVOGIMBALVECTORLENGHT;
         for (axis=1; axis < 2; axis++) {
              mediaMobileServoGimbalADCSum[axis] -= mediaMobileServoGimbalADC[axis][mediaMobileServoGimbalIDX];
               mediaMobileServoGimbalADC[axis][mediaMobileServoGimbalIDX] = servo[axis];
              mediaMobileServoGimbalADCSum[axis] += mediaMobileServoGimbalADC[axis][mediaMobileServoGimbalIDX];
               atomicServo[axis] = (mediaMobileServoGimbalADCSum[axis] / MMSERVOGIMBALVECTORLENGHT - 1000)>>2;
         }
        #else
           for(uint8_t i = (SEC_SERVO_FROM-1); i < SEC_SERVO_TO; i++){
               atomicServo[i] = (servo[i]-1000)>>2;
            }
       #endif
    #endif
  #endif
}

In config you can enable or disable moving average for single sensor or for gimbal and you can decide the vector lenght of the average.
Please give me some feedback on it.
Some people tryed it and they love my moving average due to increased fly stability.

[copterrichie]

Ok, I will add the above and give it a try on my mini-bicopter tomorrow providing weather is good. Will report back. Thank you

[copterrichie]

I just did an indoors preliminary test, looks very good, will be performing an outdoor test later today. Just wanted to give this small update.

[Magnetron]

I just did an indoors preliminary test, looks very good, will be performing an outdoor test later today. Just wanted to give this small update.

I think that this upgrade must be included in next release, Alex could you test and include it?

[copterrichie]

Ok, I just did a test in the local park with my mini-bicopter. I experienced some oscillation on the Pitch Axis that seemed to progressively increase over the time of the flight where I had to land. I think this maybe due to the way a bicopter controls the pitch of the aircraft. I did not try readjusting the PID but what seems to be needed in my situation is, a means to defend which axis to the moving averaging is applied.

[copterrichie]

As I look over the code, this parameter may have some effect because the Pitch control on a bicopter is with servos.

Code: Select all

// Moving Average ServoGimbal Signal Output by Magnetron1
#define MMSERVOGIMBAL                  // Active Output Moving Average Function for Servos Gimbal
#define MMSERVOGIMBALVECTORLENGHT 32   // Lenght of Moving Average Vector

Can you explain more in detail what this does?

Thank you.

[Luke]

I implemented this on my tri, which uses the Crius SE FC. I had originally needed to set the LPF to 20Hz to get anything remotely stable, and even then after adding a bit more weight (small fpv cam and transmitter) it was spastic and bouncy no matter what PID settings.

It now flies like glass both in and out of attitude control mode, a totally different and amazing experience! This really needs to get tested and fully integrated into the source, both gyro and acc. I'm going to test barometer out tomorrow when there's more light.

Thanks Magnetron!