1,6 → 1,7 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
#include <stdlib.h> |
|
#include "analog.h" |
#include "attitude.h" |
346,11 → 347,11 |
} |
|
// 2.1: Transform axes. |
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
|
for (uint8_t axis=0; axis<2; axis++) { |
// 3) Filter. |
tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / staticParams.gyroPIDFilterConstant; |
tempOffsetGyro[axis] = (gyro_PID[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant; |
|
// 4) Measure noise. |
measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
376,7 → 377,7 |
/* |
* Now process the data for attitude angles. |
*/ |
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
|
dampenGyroActivity(); |
gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
386,12 → 387,12 |
measureGyroActivity(tempOffsetGyro[ROLL]); |
|
// Yaw gyro. |
if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
else |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
|
measureGyroActivity(yawGyro*staticParams.yawRateFactor); |
measureGyroActivity(yawGyro*IMUConfig.yawRateFactor); |
} |
|
void analog_updateAccelerometers(void) { |
400,14 → 401,14 |
acc[axis] = rawAccValue(axis) - accOffset.offsets[axis]; |
} |
|
rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY); |
rotate(acc, IMUConfig.accQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_XY); |
for(uint8_t axis=0; axis<3; axis++) { |
filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
filteredAcc[axis] = (filteredAcc[axis] * (IMUConfig.accFilterConstant - 1) + acc[axis]) / IMUConfig.accFilterConstant; |
measureNoise(acc[axis], &accNoisePeak[axis], 1); |
} |
|
// Z acc. |
if (staticParams.imuReversedFlags & 8) |
if (IMUConfig.imuReversedFlags & 8) |
acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
else |
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
611,7 → 612,7 |
accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
int16_t min,max; |
if (axis==Z) { |
if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) { |
if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_Z) { |
// TODO: This assumes a sensitivity of +/- 2g. |
min = (256-200) * ACC_OVERSAMPLING_Z; |
max = (256+200) * ACC_OVERSAMPLING_Z; |