| 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!) |