1,4 → 1,3 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/pgmspace.h> |
|
52,15 → 51,6 |
* ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z }; |
|
/* |
* This allows some experimentation with the gyro filters. |
* Should be replaced by #define's later on... |
*/ |
volatile uint8_t GYROS_PID_FILTER; |
volatile uint8_t GYROS_ATT_FILTER; |
volatile uint8_t GYROS_D_FILTER; |
volatile uint8_t ACC_FILTER; |
|
/* |
* Air pressure |
*/ |
volatile uint8_t rangewidth = 106; |
189,21 → 179,31 |
return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
} |
|
/* |
* In the MK coordinate system, nose-down is positive and left-roll is positive. |
* If a sensor is used in an orientation where one but not both of the axes has |
* an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true). |
*/ |
void transformPRGyro(int16_t* result) { |
static const uint8_t tab[] = {1,1,0,0-1,-1,-1,0,1}; |
int8_t pp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+4)%8] : tab[GYRO_QUADRANT]; |
// Pitch to Pitch part |
int8_t pp = PR_GYROS_ORIENTATION_REVERSED ? tab[(GYRO_QUADRANT+4)%8] : tab[GYRO_QUADRANT]; |
// Pitch to Roll part |
int8_t pr = tab[(GYRO_QUADRANT+2)%8]; |
int8_t rp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+2)%8] : tab[(GYRO_QUADRANT+6)%8]; |
// Roll to Roll part |
int8_t rp = PR_GYROS_ORIENTATION_REVERSED ? tab[(GYRO_QUADRANT+2)%8] : tab[(GYRO_QUADRANT+6)%8]; |
// Roll to Roll part |
int8_t rr = tab[GYRO_QUADRANT]; |
|
int16_t temp = result[0]; |
result[0] = pp*result[0] + pr*result[1]; |
result[1] = rp*temp + rr*result[1]; |
int16_t pitchIn = result[PITCH]; |
|
result[PITCH] = pp*result[PITCH] + pr*result[ROLL]; |
result[ROLL] = rp*pitchIn + rr*result[ROLL]; |
} |
|
/***************************************************** |
* Interrupt Service Routine for ADC |
* Runs at 312.5 kHz or 3.2 µs. When all states are |
* Runs at 312.5 kHz or 3.2 us. When all states are |
* processed the interrupt is disabled and further |
* AD conversions are stopped. |
*****************************************************/ |
241,29 → 241,14 |
|
case 11: // yaw gyro |
rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
if (YAW_REVERSED) |
if (YAW_GYRO_REVERSED) |
tempOffsetGyro[0] = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
else |
tempOffsetGyro[0] = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
gyroD[YAW] = (gyroD[YAW] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[0] - yawGyro)) / GYROS_D_FILTER; |
gyroD[YAW] = (gyroD[YAW] * (staticParams.DGyroFilter - 1) + (tempOffsetGyro[0] - yawGyro)) / staticParams.DGyroFilter; |
yawGyro = tempOffsetGyro[0]; |
break; |
case 13: // roll axis acc. |
/* |
for (axis=0; axis<2; axis++) { |
if (ACC_REVERSED[axis]) |
tempSensor[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
else |
tempSensor[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis]; |
} |
if (AXIS_TRANSFORM) { |
acc[PITCH] = tempSensor[PITCH] + tempSensor[ROLL]; |
acc[ROLL] = tempSensor[ROLL] - tempSensor[PITCH]; |
} else { |
acc[PITCH] = tempSensor[PITCH]; |
acc[ROLL] = tempSensor[ROLL]; |
} |
*/ |
|
// We have no sensor installed... |
acc[PITCH] = acc[ROLL] = 0; |
270,7 → 255,7 |
|
for (axis=0; axis<2; axis++) { |
filteredAcc[axis] = |
(filteredAcc[axis] * (ACC_FILTER - 1) + acc[axis]) / ACC_FILTER; |
(filteredAcc[axis] * (staticParams.accFilter - 1) + acc[axis]) / staticParams.accFilter; |
measureNoise(acc[axis], &accNoisePeak[axis], 1); |
} |
break; |
379,13 → 364,13 |
|
// 3) Scale and filter. |
for (axis=0; axis<2; axis++) { |
tempOffsetGyro[axis] = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro[axis]) / GYROS_PID_FILTER; |
tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.PIDGyroFilter - 1) + tempOffsetGyro[axis]) / staticParams.PIDGyroFilter; |
|
// 4) Measure noise. |
measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
|
// 5) Differential measurement. |
gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / GYROS_D_FILTER; |
gyroD[axis] = (gyroD[axis] * (staticParams.DGyroFilter - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.DGyroFilter; |
|
// 6) Done. |
gyro_PID[axis] = tempOffsetGyro[axis]; |
401,8 → 386,8 |
transformPRGyro(tempOffsetGyro); |
|
// 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. |
gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[PITCH]) / GYROS_ATT_FILTER; |
gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[ROLL]) / GYROS_ATT_FILTER; |
gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter; |
gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter; |
break; |
|
case 17: |
438,12 → 423,6 |
uint8_t i, axis; |
int32_t deltaOffsets[3] = { 0, 0, 0 }; |
|
// Set the filters... to be removed again, once some good settings are found. |
GYROS_PID_FILTER = (staticParams.sensorFilterSettings & (0x7 & (1<<0))) + 1; |
GYROS_ATT_FILTER = 1; |
GYROS_D_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<4))) + 1; |
ACC_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<6))) + 1; |
|
gyro_calibrate(); |
|
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |