Rev 2019 | Rev 2026 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2019 | Rev 2020 | ||
---|---|---|---|
Line 132... | Line 132... | ||
132 | * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed |
132 | * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed |
133 | * 6: pp=0, pr=1, rp=1, rr=0 // +270 degrees with pitch reversed |
133 | * 6: pp=0, pr=1, rp=1, rr=0 // +270 degrees with pitch reversed |
134 | * 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
134 | * 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
135 | */ |
135 | */ |
Line 136... | Line -... | ||
136 | - | ||
137 | //void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {} |
- | |
138 | 136 | ||
139 | void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
137 | void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
140 | static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
138 | static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
141 | // Pitch to Pitch part |
139 | // Pitch to Pitch part |
142 | int8_t xx = (reverse & 1) ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant]; |
140 | int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant]; |
143 | // Roll to Pitch part |
141 | // Roll to Pitch part |
144 | int8_t xy = rotationTab[(quadrant+2)%8]; |
142 | int8_t xy = rotationTab[(quadrant+2)%8]; |
145 | // Pitch to Roll part |
143 | // Pitch to Roll part |
146 | int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8]; |
144 | int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8]; |
Line 362... | Line 360... | ||
362 | // 2) Apply sign and offset, scale before filtering. |
360 | // 2) Apply sign and offset, scale before filtering. |
363 | tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
361 | tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
364 | } |
362 | } |
Line 365... | Line 363... | ||
365 | 363 | ||
366 | // 2.1: Transform axes. |
364 | // 2.1: Transform axes. |
Line 367... | Line 365... | ||
367 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1); |
365 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
368 | 366 | ||
369 | for (uint8_t axis=0; axis<2; axis++) { |
367 | for (uint8_t axis=0; axis<2; axis++) { |
Line 384... | Line 382... | ||
384 | } |
382 | } |
Line 385... | Line 383... | ||
385 | 383 | ||
386 | /* |
384 | /* |
387 | * Now process the data for attitude angles. |
385 | * Now process the data for attitude angles. |
388 | */ |
386 | */ |
Line 389... | Line 387... | ||
389 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1); |
387 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
390 | 388 | ||
Line 391... | Line 389... | ||
391 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
389 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
Line 400... | Line 398... | ||
400 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
398 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
401 | // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter; |
399 | // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter; |
402 | // gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter; |
400 | // gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter; |
Line 403... | Line 401... | ||
403 | 401 | ||
404 | // Yaw gyro. |
402 | // Yaw gyro. |
405 | if (staticParams.imuReversedFlags & 2) |
403 | if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
406 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
404 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
407 | else |
405 | else |
408 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
406 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
Line 412... | Line 410... | ||
412 | // Pitch and roll axis accelerations. |
410 | // Pitch and roll axis accelerations. |
413 | for (uint8_t axis=0; axis<2; axis++) { |
411 | for (uint8_t axis=0; axis<2; axis++) { |
414 | acc[axis] = rawAccValue(axis) - accOffset.offsets[axis]; |
412 | acc[axis] = rawAccValue(axis) - accOffset.offsets[axis]; |
415 | } |
413 | } |
Line 416... | Line 414... | ||
416 | 414 | ||
Line 417... | Line 415... | ||
417 | rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & 4); |
415 | rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY); |
418 | 416 | ||
419 | for(uint8_t axis=0; axis<3; axis++) { |
417 | for(uint8_t axis=0; axis<3; axis++) { |
420 | filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
418 | filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
Line 597... | Line 595... | ||
597 | 595 | ||
598 | for (axis = PITCH; axis <= YAW; axis++) { |
596 | for (axis = PITCH; axis <= YAW; axis++) { |
599 | accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
597 | accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
600 | int16_t min,max; |
598 | int16_t min,max; |
601 | if (axis==Z) { |
599 | if (axis==Z) { |
602 | if (staticParams.imuReversedFlags & 8) { |
600 | if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) { |
603 | // TODO: This assumes a sensitivity of +/- 2g. |
601 | // TODO: This assumes a sensitivity of +/- 2g. |
604 | min = (256-200) * ACC_OVERSAMPLING_Z; |
602 | min = (256-200) * ACC_OVERSAMPLING_Z; |
605 | max = (256+200) * ACC_OVERSAMPLING_Z; |
603 | max = (256+200) * ACC_OVERSAMPLING_Z; |
606 | } else { |
604 | } else { |