134,12 → 134,10 |
* 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
*/ |
|
//void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {} |
|
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
// Pitch to Pitch part |
int8_t xx = (reverse & 1) ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant]; |
int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant]; |
// Roll to Pitch part |
int8_t xy = rotationTab[(quadrant+2)%8]; |
// Pitch to Roll part |
364,7 → 362,7 |
} |
|
// 2.1: Transform axes. |
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1); |
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
|
for (uint8_t axis=0; axis<2; axis++) { |
// 3) Filter. |
386,7 → 384,7 |
/* |
* Now process the data for attitude angles. |
*/ |
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1); |
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
|
gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
402,7 → 400,7 |
// gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter; |
|
// Yaw gyro. |
if (staticParams.imuReversedFlags & 2) |
if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
else |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
414,7 → 412,7 |
acc[axis] = rawAccValue(axis) - accOffset.offsets[axis]; |
} |
|
rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & 4); |
rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY); |
|
for(uint8_t axis=0; axis<3; axis++) { |
filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
599,7 → 597,7 |
accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
int16_t min,max; |
if (axis==Z) { |
if (staticParams.imuReversedFlags & 8) { |
if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) { |
// TODO: This assumes a sensitivity of +/- 2g. |
min = (256-200) * ACC_OVERSAMPLING_Z; |
max = (256+200) * ACC_OVERSAMPLING_Z; |