Rev 1868 | Rev 1870 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1868 | Rev 1869 | ||
---|---|---|---|
Line 77... | Line 77... | ||
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); |