Code: Alles auswählen

void computeIMU () {
  uint8_t axis;
  static int16_t gyroADCprevious[3] = {0,0,0};
  int16_t gyroADCp[3];
  int16_t gyroADCinter[3];
  static uint32_t timeInterleave = 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 (!ACC && nunchuk) {
    annexCode();
    while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    WMP_getRawADC();
    getEstimatedAttitude(); // computation time must last less than one interleaving delay
    while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
    timeInterleave=micros();
    while(WMP_getRawADC() != 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
      // /4 is to average 4 values, note: overflow is not possible for WMP gyro here
      gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis]+2)/4;
      gyroADCprevious[axis] = gyroADC[axis];
    }
  } else {
    #if ACC
      ACC_getADC();
      getEstimatedAttitude();
    #endif
    #if GYRO
      Gyro_getADC();
    #else
      WMP_getRawADC();
      static uint16_t WMP_tmpData[3];
      for (axis = 0; axis < 3; axis++)
        WMP_tmpData[axis] = gyroADC[axis];
      /////////////////  
      gyroADC[ROLL]  = WMP_tmpData[ROLL]; 
      gyroADC[PITCH] = WMP_tmpData[PITCH];
      gyroADC[YAW]   = WMP_tmpData[YAW];
      /////////////////
    #endif
    for (axis = 0; axis < 3; axis++)
      gyroADCp[axis] =  gyroADC[axis];
    timeInterleave=micros();
    annexCode();
    if ((micros()-timeInterleave)>650) {
       annex650_overrun_count++;
    } else {
       while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads
    }
    #if GYRO
      Gyro_getADC();
    #else
      WMP_getRawADC();
    #endif
    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]+1)/3;
      gyroADCprevious[axis] = gyroADCinter[axis]/2;
      if (!ACC) accADC[axis]=0;
    }
  }
  #if defined(GYRO_SMOOTHING)
    static uint8_t Smoothing[3]  = GYRO_SMOOTHING; // How much to smoothen with per axis
    static int16_t gyroSmooth[3] = {0,0,0};
    for (axis = 0; axis < 3; axis++) {
      gyroData[axis] = (gyroSmooth[axis]*(Smoothing[axis]-1)+gyroData[axis]+1)/Smoothing[axis];
      gyroSmooth[axis] = gyroData[axis];
    }
  #elif defined(TRI)
    static int16_t gyroYawSmooth = 0;
    gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW]+1)/3;
    gyroYawSmooth = gyroData[YAW];
  #endif
}

Code: Alles auswählen

      /////////////////  
      gyroADC[ROLL]  = WMP_tmpData[ROLL]; 
      gyroADC[PITCH] = WMP_tmpData[PITCH];
      gyroADC[YAW]   = WMP_tmpData[YAW];
      /////////////////

Code: Alles auswählen

      /////////////////  
      gyroADC[ROLL]  = WMP_tmpData[PITCH]; 
      gyroADC[PITCH] = WMP_tmpData[ROLL];
      gyroADC[YAW]   = WMP_tmpData[YAW];
      /////////////////

Code: Alles auswählen

      gyroADC[ROLL]  = WMP_tmpData[ROLL]; 
      gyroADC[PITCH] = WMP_tmpData[PITCH]*-1;
      gyroADC[YAW]   = WMP_tmpData[YAW];