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 { |