Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2091 → Rev 2092

/branches/dongfang_FC_rewrite/analog.c
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;