BUG: GYRO_ORIENTATION is not applied to WMP

[ciskje]

You must add this to the end of gyro_common in sensors.pde to apply GYRO_ORIENTATION to WMP:

Code: Select all

void GYRO_Common() {
  static int16_t previousGyroADC[3] = {0,0,0};
  static int32_t g[3];
  uint8_t axis;
  
  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--;
  }
  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);
    previousGyroADC[axis] = gyroADC[axis];
  }
  int roll,pitch,yaw;
  
  // BUG Correction GYRO_ORIENTATION is not applied when you have WMP
  // You must save it to exclude swap between vars
  roll=gyroADC[ROLL];
  pitch=gyroADC[PITCH];
  yaw=gyroADC[YAW];
  GYRO_ORIENTATION( roll, pitch, yaw);
}

[ciskje]

Code: Select all

int roll,pitch,yaw;
  
  // BUG Correction GYRO_ORIENTATION is not applied when you have WMP
  // You must save it to exclude swap between vars
  roll=gyroADC[ROLL];
  pitch=gyroADC[PITCH];
  yaw=gyroADC[YAW];
  GYRO_ORIENTATION( roll, pitch, yaw);

In the last dev version there aren't these lines of code to apply different orientation also to a Wii sensors. Please add it.

[ciskje]

Is this integrated in the source code?

[Alexinparis]

You must add this to the end of gyro_common in sensors.pde to apply GYRO_ORIENTATION to WMP:

Code: Select all

void GYRO_Common() {
  static int16_t previousGyroADC[3] = {0,0,0};
  static int32_t g[3];
  uint8_t axis;
  
  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--;
  }
  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);
    previousGyroADC[axis] = gyroADC[axis];
  }
  int roll,pitch,yaw;
  
  // BUG Correction GYRO_ORIENTATION is not applied when you have WMP
  // You must save it to exclude swap between vars
  roll=gyroADC[ROLL];
  pitch=gyroADC[PITCH];
  yaw=gyroADC[YAW];
  GYRO_ORIENTATION( roll, pitch, yaw);
}

Doing this at this place in the code would create a double switch axis for all non other WMP gyro. (GYRO_ORIENTATION would be applied twice)
This code should work, but in the WMP part code only, not in GYRO_common part.

[ciskje]

Code: Select all

void Gyro_getADC () {
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
  i2c_getSixRawADC(ITG3200_ADDRESS,0X1D);
  GYRO_ORIENTATION(  + ( ((rawADC[2]<<8) | rawADC[3])/4) , // range: +/- 8192; +/- 2000 deg/sec
                     - ( ((rawADC[0]<<8) | rawADC[1])/4 ) ,
                     - ( ((rawADC[4]<<8) | rawADC[5])/4 ) );
  GYRO_Common();
}

Remove GYRO_ORIENTATION from GYRO_getADC.
It is a correct place for GYRO_ORIENTATION macro to be in the "common" part.

[oli]

Would it be possible to fix this bug within the next release?
I would also move the GYRO_ORIENTATION macro into Gyro_common, because it affects all Gyro sensors and you follow the rules of "Don't repeat yourself".

[carlonb]

Hi all,
I added Gyro orientation for WMP only here, and modified the orientation in the last (as default commented) lines of "Config.h", and already done some test flight and seems run OK with quad + configuration.

Code: Select all

uint8_t WMP_getRawADC() {
....
....
// Wii Motion Plus Data
  if ( (rawADC[5]&0x03) == 0x02 ) {
    // Assemble 14bit data 
//    gyroADC[ROLL]  = - ( ((rawADC[5]>>2)<<8) | rawADC[2] ); //range: +/- 8192
//    gyroADC[PITCH] = - ( ((rawADC[4]>>2)<<8) | rawADC[1] );
//    gyroADC[YAW]  =  - ( ((rawADC[3]>>2)<<8) | rawADC[0] );
   
GYRO_ORIENTATION(  + ( ((rawADC[5]>>2)<<8) | rawADC[2] ) , // ADDED  Gyro orientation by Carlonb
                     - ( ((rawADC[4]>>2)<<8) | rawADC[1] ) ,  
                     - ( ((rawADC[3]>>2)<<8) | rawADC[0] ) );
 
GYRO_Common();
....
....

Let me know if it's OK
Ciao, Carlo.