Rev 2089 | Rev 2095 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2089 | Rev 2092 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
| 2 | #include <avr/interrupt.h> |
2 | #include <avr/interrupt.h> |
| 3 | #include <avr/pgmspace.h> |
3 | #include <avr/pgmspace.h> |
| - | 4 | #include <stdlib.h> |
|
| Line 4... | Line 5... | ||
| 4 | 5 | ||
| 5 | #include "analog.h" |
6 | #include "analog.h" |
| 6 | #include "attitude.h" |
7 | #include "attitude.h" |
| 7 | #include "sensors.h" |
8 | #include "sensors.h" |
| Line 344... | Line 345... | ||
| 344 | // 2) Apply sign and offset, scale before filtering. |
345 | // 2) Apply sign and offset, scale before filtering. |
| 345 | tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
346 | tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
| 346 | } |
347 | } |
| Line 347... | Line 348... | ||
| 347 | 348 | ||
| 348 | // 2.1: Transform axes. |
349 | // 2.1: Transform axes. |
| Line 349... | Line 350... | ||
| 349 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
350 | rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
| 350 | 351 | ||
| 351 | for (uint8_t axis=0; axis<2; axis++) { |
352 | for (uint8_t axis=0; axis<2; axis++) { |
| Line 352... | Line 353... | ||
| 352 | // 3) Filter. |
353 | // 3) Filter. |
| 353 | tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / staticParams.gyroPIDFilterConstant; |
354 | tempOffsetGyro[axis] = (gyro_PID[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant; |
| Line 354... | Line 355... | ||
| 354 | 355 | ||
| Line 374... | Line 375... | ||
| 374 | } |
375 | } |
| Line 375... | Line 376... | ||
| 375 | 376 | ||
| 376 | /* |
377 | /* |
| 377 | * Now process the data for attitude angles. |
378 | * Now process the data for attitude angles. |
| 378 | */ |
379 | */ |
| Line 379... | Line 380... | ||
| 379 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
380 | rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
| 380 | 381 | ||
| 381 | dampenGyroActivity(); |
382 | dampenGyroActivity(); |
| Line 382... | Line 383... | ||
| 382 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
383 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
| 383 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
384 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
| Line 384... | Line 385... | ||
| 384 | 385 | ||
| 385 | measureGyroActivity(tempOffsetGyro[PITCH]); |
386 | measureGyroActivity(tempOffsetGyro[PITCH]); |
| 386 | measureGyroActivity(tempOffsetGyro[ROLL]); |
387 | measureGyroActivity(tempOffsetGyro[ROLL]); |
| 387 | 388 | ||
| 388 | // Yaw gyro. |
389 | // Yaw gyro. |
| Line 389... | Line 390... | ||
| 389 | if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
390 | if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
| 390 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
391 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
| Line 391... | Line 392... | ||
| 391 | else |
392 | else |
| 392 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
393 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
| 393 | 394 | ||
| 394 | measureGyroActivity(yawGyro*staticParams.yawRateFactor); |
395 | measureGyroActivity(yawGyro*IMUConfig.yawRateFactor); |
| 395 | } |
396 | } |
| Line 396... | Line 397... | ||
| 396 | 397 | ||
| 397 | void analog_updateAccelerometers(void) { |
398 | void analog_updateAccelerometers(void) { |
| 398 | // Pitch and roll axis accelerations. |
399 | // Pitch and roll axis accelerations. |
| 399 | for (uint8_t axis=0; axis<2; axis++) { |
400 | for (uint8_t axis=0; axis<2; axis++) { |
| 400 | acc[axis] = rawAccValue(axis) - accOffset.offsets[axis]; |
401 | acc[axis] = rawAccValue(axis) - accOffset.offsets[axis]; |
| Line 401... | Line 402... | ||
| 401 | } |
402 | } |
| 402 | 403 | ||
| 403 | rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY); |
404 | rotate(acc, IMUConfig.accQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_XY); |
| 404 | for(uint8_t axis=0; axis<3; axis++) { |
405 | for(uint8_t axis=0; axis<3; axis++) { |
| 405 | filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
406 | filteredAcc[axis] = (filteredAcc[axis] * (IMUConfig.accFilterConstant - 1) + acc[axis]) / IMUConfig.accFilterConstant; |
| Line 406... | Line 407... | ||
| 406 | measureNoise(acc[axis], &accNoisePeak[axis], 1); |
407 | measureNoise(acc[axis], &accNoisePeak[axis], 1); |
| Line 609... | Line 610... | ||
| 609 | 610 | ||
| 610 | for (axis = PITCH; axis <= YAW; axis++) { |
611 | for (axis = PITCH; axis <= YAW; axis++) { |
| 611 | accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
612 | accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
| 612 | int16_t min,max; |
613 | int16_t min,max; |
| 613 | if (axis==Z) { |
614 | if (axis==Z) { |
| 614 | if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) { |
615 | if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_Z) { |
| 615 | // TODO: This assumes a sensitivity of +/- 2g. |
616 | // TODO: This assumes a sensitivity of +/- 2g. |
| 616 | min = (256-200) * ACC_OVERSAMPLING_Z; |
617 | min = (256-200) * ACC_OVERSAMPLING_Z; |
| 617 | max = (256+200) * ACC_OVERSAMPLING_Z; |
618 | max = (256+200) * ACC_OVERSAMPLING_Z; |
| 618 | } else { |
619 | } else { |