Subversion Repositories FlightCtrl

Rev

Rev 1868 | Rev 1870 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1868 Rev 1869
Line 76... Line 76...
76 
 * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
 76 
 * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
77 
 * the offsets with the DAC.
 77 
 * the offsets with the DAC.
78 
 */
 78 
 */
79 
volatile int16_t rawGyroSum[3];
 79 
volatile int16_t rawGyroSum[3];
80 
volatile int16_t acc[3];
 80 
volatile int16_t acc[3];
81 
volatile int16_t filteredAcc[2] = { 0, 0 };
 81 
volatile int16_t filteredAcc[2] = { 0,0 };
- 
 
 82 
volatile int32_t stronglyFilteredAcc[3] = { 0,0,0 };
Line 82... Line 83...
82 
 
 83 
 
83 
/*
 84 
/*
84 
 * These 4 exported variables are zero-offset. The "PID" ones are used
 85 
 * These 4 exported variables are zero-offset. The "PID" ones are used
85 
 * in the attitude control as rotation rates. The "ATT" ones are for
 86 
 * in the attitude control as rotation rates. The "ATT" ones are for
Line 270... Line 271...
270 
        case 8: // Z acc
 271 
        case 8: // Z acc
271 
                if (ACC_REVERSED[Z])
 272 
                if (ACC_REVERSED[Z])
272 
                        acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
 273 
                        acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
273 
                else
 274 
                else
274 
                        acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
 275 
                        acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
- 
 
 276 
 
- 
 
 277 
        stronglyFilteredAcc[Z] =
- 
 
 278 
            (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
- 
 
 279 
 
275 
                break;
 280 
                break;
Line 276... Line 281...
276 
 
 281 
 
277 
        case 11: // yaw gyro
 282 
        case 11: // yaw gyro
278 
                rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
 283 
                rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
Line 286... Line 291...
286 
                if (ACC_REVERSED[PITCH])
 291 
                if (ACC_REVERSED[PITCH])
287 
                        acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
 292 
                        acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
288 
                else
 293 
                else
289 
                        acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
 294 
                        acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
Line -... Line 295...
- 
 
 295 
 
290 
 
 296 
                filteredAcc[PITCH] =
- 
 
 297 
                    (filteredAcc[PITCH] * (ACC_FILTER - 1) + acc[PITCH]) / ACC_FILTER;
291 
                filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER - 1) + acc[PITCH])
 298 
 
- 
 
 299 
                stronglyFilteredAcc[PITCH] =
- 
 
 300 
                    (stronglyFilteredAcc[PITCH] * 99 + acc[PITCH] * 10) / 100;
- 
 
 301 
 
292 
                                / ACC_FILTER;
 302 
 
293 
                measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);
 303 
                measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);
Line 294... Line 304...
294 
                break;
 304 
                break;
295 
 
 305 
 
296 
        case 13: // roll axis acc.
 306 
        case 13: // roll axis acc.
297 
                if (ACC_REVERSED[ROLL])
 307 
                if (ACC_REVERSED[ROLL])
298 
                        acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
 308 
                        acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
- 
 
 309 
                else
299 
                else
 310 
                        acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
- 
 
 311 
                filteredAcc[ROLL] =
300 
                        acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
 312 
                    (filteredAcc[ROLL] * (ACC_FILTER - 1) + acc[ROLL]) / ACC_FILTER;
- 
 
 313 
 
- 
 
 314 
        stronglyFilteredAcc[ROLL] =
301 
                filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER - 1) + acc[ROLL])
 315 
            (stronglyFilteredAcc[ROLL] * 99 + acc[ROLL] * 10) / 100;
302 
                                / ACC_FILTER;
 316 
 
Line 303... Line 317...
303 
                measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1);
 317 
                measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1);
304 
                break;
 318 
                break;
Line 495... Line 509...
495 
        for (axis = PITCH; axis <= YAW; axis++) {
 509 
        for (axis = PITCH; axis <= YAW; axis++) {
496 
                gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
 510 
                gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
497 
                // DebugOut.Analog[20 + axis] = gyroOffset[axis];
 511 
                // DebugOut.Analog[20 + axis] = gyroOffset[axis];
498 
        }
 512 
        }
Line 499... Line 513...
499 
 
 513 
 
500 
        // Noise is relative to offset. So, reset noise measurements when changing offsets.
 514 
        // Noise is relativ to offset. So, reset noise measurements when changing offsets.
Line 501... Line 515...
501 
        gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
 515 
        gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
502 
 
 516 
 
503 
        accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);
 517 
        accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);