Rev 1955 | Rev 1961 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1955 | Rev 1960 | ||
---|---|---|---|
Line 98... | Line 98... | ||
98 | /* |
98 | /* |
99 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
99 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
100 | * standing still. They are used for adjusting the gyro and acc. meter values |
100 | * standing still. They are used for adjusting the gyro and acc. meter values |
101 | * to be centered on zero. |
101 | * to be centered on zero. |
102 | */ |
102 | */ |
- | 103 | /* |
|
103 | volatile int16_t gyroOffset[3] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 |
104 | volatile int16_t gyroOffset[3] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 |
104 | * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW }; |
105 | * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW }; |
Line 105... | Line 106... | ||
105 | 106 | ||
106 | volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512 |
107 | volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512 |
- | 108 | * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z }; |
|
- | 109 | */ |
|
- | 110 | ||
- | 111 | sensorOffset_t gyroOffset; |
|
Line 107... | Line 112... | ||
107 | * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z }; |
112 | sensorOffset_t accOffset; |
108 | 113 | ||
109 | /* |
114 | /* |
110 | * This allows some experimentation with the gyro filters. |
115 | * This allows some experimentation with the gyro filters. |
111 | * Should be replaced by #define's later on... |
- | |
112 | */ |
- | |
113 | volatile uint8_t GYROS_PID_FILTER; |
- | |
114 | volatile uint8_t GYROS_ATT_FILTER; |
- | |
Line 115... | Line 116... | ||
115 | volatile uint8_t GYROS_D_FILTER; |
116 | * Should be replaced by #define's later on... |
116 | volatile uint8_t ACC_FILTER; |
117 | */ |
117 | 118 | ||
118 | /* |
119 | /* |
Line 243... | Line 244... | ||
243 | uint16_t getSimplePressure(int advalue) { |
244 | uint16_t getSimplePressure(int advalue) { |
244 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
245 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
245 | } |
246 | } |
Line 246... | Line 247... | ||
246 | 247 | ||
- | 248 | void startAnalogConversionCycle(void) { |
|
247 | void startAnalogConversionCycle(void) { |
249 | analogDataReady = 0; |
248 | // Stop the sampling. Cycle is over. |
250 | // Stop the sampling. Cycle is over. |
249 | for (uint8_t i = 0; i < 8; i++) { |
251 | for (uint8_t i = 0; i < 8; i++) { |
250 | sensorInputs[i] = 0; |
252 | sensorInputs[i] = 0; |
251 | } |
253 | } |
Line 287... | Line 289... | ||
287 | * Process the gyro data for the PID controller. |
289 | * Process the gyro data for the PID controller. |
288 | */ |
290 | */ |
289 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
291 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
290 | // gyro with a wider range, and helps counter saturation at full control. |
292 | // gyro with a wider range, and helps counter saturation at full control. |
Line 291... | Line 293... | ||
291 | 293 | ||
292 | if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
294 | if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) { |
293 | if (tempGyro < SENSOR_MIN_PITCHROLL) { |
295 | if (tempGyro < SENSOR_MIN_PITCHROLL) { |
294 | debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
296 | debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
295 | tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
297 | tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
296 | } else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
298 | } else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
Line 302... | Line 304... | ||
302 | } |
304 | } |
303 | } |
305 | } |
Line 304... | Line 306... | ||
304 | 306 | ||
305 | // 2) Apply sign and offset, scale before filtering. |
307 | // 2) Apply sign and offset, scale before filtering. |
306 | if (GYRO_REVERSED[axis]) { |
308 | if (GYRO_REVERSED[axis]) { |
307 | tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
309 | tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
308 | } else { |
310 | } else { |
309 | tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
311 | tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
Line 310... | Line 312... | ||
310 | } |
312 | } |
311 | 313 | ||
Line 312... | Line 314... | ||
312 | // 3) Scale and filter. |
314 | // 3) Scale and filter. |
313 | tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro) / GYROS_PID_FILTER; |
315 | tempOffsetGyro = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroPIDFilterConstant; |
Line 314... | Line 316... | ||
314 | 316 | ||
315 | // 4) Measure noise. |
317 | // 4) Measure noise. |
Line 316... | Line 318... | ||
316 | measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
318 | measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
317 | 319 | ||
Line 318... | Line 320... | ||
318 | // 5) Differential measurement. |
320 | // 5) Differential measurement. |
Line 326... | Line 328... | ||
326 | */ |
328 | */ |
327 | tempGyro = rawGyroSum[axis]; |
329 | tempGyro = rawGyroSum[axis]; |
Line 328... | Line 330... | ||
328 | 330 | ||
329 | // 1) Apply sign and offset, scale before filtering. |
331 | // 1) Apply sign and offset, scale before filtering. |
330 | if (GYRO_REVERSED[axis]) { |
332 | if (GYRO_REVERSED[axis]) { |
331 | tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
333 | tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
332 | } else { |
334 | } else { |
333 | tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
335 | tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
Line 334... | Line 336... | ||
334 | } |
336 | } |
335 | 337 | ||
336 | // 2) Filter. |
338 | // 2) Filter. |
Line 337... | Line 339... | ||
337 | gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro) / GYROS_ATT_FILTER; |
339 | gyro_ATT[axis] = (gyro_ATT[axis] * (staticParams.gyroATTFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroATTFilterConstant; |
338 | } |
340 | } |
339 | 341 | ||
340 | // Yaw gyro. |
342 | // Yaw gyro. |
341 | rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
343 | rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
342 | if (GYRO_REVERSED[YAW]) |
344 | if (GYRO_REVERSED[YAW]) |
Line 343... | Line 345... | ||
343 | yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
345 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
344 | else |
346 | else |
345 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
347 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
346 | 348 | ||
Line 347... | Line 349... | ||
347 | debugOut.analog[3] = gyro_ATT[PITCH]; |
349 | debugOut.analog[3] = gyro_ATT[PITCH]; |
348 | debugOut.analog[4] = gyro_ATT[ROLL]; |
350 | debugOut.analog[4] = gyro_ATT[ROLL]; |
349 | debugOut.analog[5] = yawGyro; |
351 | debugOut.analog[5] = yawGyro; |
350 | } |
352 | } |
351 | 353 | ||
352 | void analog_updateAccelerometers(void) { |
354 | void analog_updateAccelerometers(void) { |
353 | // Pitch and roll axis accelerations. |
355 | // Pitch and roll axis accelerations. |
Line 354... | Line 356... | ||
354 | for (uint8_t axis=0; axis<2; axis++) { |
356 | for (uint8_t axis=0; axis<2; axis++) { |
Line 355... | Line 357... | ||
355 | if (ACC_REVERSED[axis]) |
357 | if (ACC_REVERSED[axis]) |
356 | acc[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
358 | acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
357 | else |
359 | else |
358 | acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis]; |
360 | acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis]; |
Line 367... | Line 369... | ||
367 | measureNoise(acc[axis], &accNoisePeak[axis], 1); |
369 | measureNoise(acc[axis], &accNoisePeak[axis], 1); |
368 | } |
370 | } |
Line 369... | Line 371... | ||
369 | 371 | ||
370 | // Z acc. |
372 | // Z acc. |
371 | if (ACC_REVERSED[Z]) |
373 | if (ACC_REVERSED[Z]) |
372 | acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z]; |
374 | acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
373 | else |
375 | else |
Line 374... | Line 376... | ||
374 | acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z]; |
376 | acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
375 | 377 | ||
376 | /* |
378 | /* |
377 | stronglyFilteredAcc[Z] = |
379 | stronglyFilteredAcc[Z] = |
378 | (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
- | |
379 | */ |
- | |
380 | - | ||
381 | debugOut.analog[6] = acc[PITCH]; |
380 | (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
Line 382... | Line 381... | ||
382 | debugOut.analog[7] = acc[ROLL]; |
381 | */ |
383 | } |
382 | } |
384 | 383 | ||
Line 477... | Line 476... | ||
477 | 476 | ||
478 | void analog_calibrate(void) { |
477 | void analog_calibrate(void) { |
479 | #define GYRO_OFFSET_CYCLES 32 |
478 | #define GYRO_OFFSET_CYCLES 32 |
480 | uint8_t i, axis; |
479 | uint8_t i, axis; |
481 | int32_t deltaOffsets[3] = { 0, 0, 0 }; |
- | |
482 | - | ||
483 | // Set the filters... to be removed again, once some good settings are found. |
- | |
484 | GYROS_PID_FILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1; |
- | |
485 | GYROS_ATT_FILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1; |
- | |
486 | GYROS_D_FILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1; |
- | |
487 | ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1; |
- | |
488 | 480 | int32_t deltaOffsets[3] = { 0, 0, 0 }; |
|
Line 489... | Line 481... | ||
489 | gyro_calibrate(); |
481 | gyro_calibrate(); |
490 | 482 | ||
491 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
483 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
Line 495... | Line 487... | ||
495 | deltaOffsets[axis] += rawGyroSum[axis]; |
487 | deltaOffsets[axis] += rawGyroSum[axis]; |
496 | } |
488 | } |
497 | } |
489 | } |
Line 498... | Line 490... | ||
498 | 490 | ||
499 | for (axis = PITCH; axis <= YAW; axis++) { |
491 | for (axis = PITCH; axis <= YAW; axis++) { |
500 | gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
492 | gyroOffset.offsets[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
501 | // DebugOut.Analog[20 + axis] = gyroOffset[axis]; |
493 | debugOut.analog[6+axis] = gyroOffset.offsets[axis]; |
Line 502... | Line 494... | ||
502 | } |
494 | } |
503 | 495 | ||
Line -... | Line 496... | ||
- | 496 | // Noise is relativ to offset. So, reset noise measurements when changing offsets. |
|
504 | // Noise is relativ to offset. So, reset noise measurements when changing offsets. |
497 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
505 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
498 | |
506 | 499 | accOffset_readFromEEProm(); |
|
Line 507... | Line 500... | ||
507 | accOffset[PITCH] = GetParamWord(PID_ACC_PITCH); |
500 | // accOffset[PITCH] = getParamWord(PID_ACC_PITCH); |
508 | accOffset[ROLL] = GetParamWord(PID_ACC_ROLL); |
501 | // accOffset[ROLL] = getParamWord(PID_ACC_ROLL); |
509 | accOffset[Z] = GetParamWord(PID_ACC_Z); |
502 | // accOffset[Z] = getParamWord(PID_ACC_Z); |
Line 537... | Line 530... | ||
537 | } |
530 | } |
Line 538... | Line 531... | ||
538 | 531 | ||
539 | for (axis = PITCH; axis <= YAW; axis++) { |
532 | for (axis = PITCH; axis <= YAW; axis++) { |
540 | filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) |
533 | filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) |
541 | / ACC_OFFSET_CYCLES; |
534 | / ACC_OFFSET_CYCLES; |
542 | accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
535 | accOffset.offsets[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
Line 543... | Line 536... | ||
543 | } |
536 | } |
544 | 537 | ||
545 | // Save ACC neutral settings to eeprom |
538 | // Save ACC neutral settings to eeprom |
546 | SetParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
539 | // setParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
- | 540 | // setParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
|
Line 547... | Line 541... | ||
547 | SetParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
541 | // setParamWord(PID_ACC_Z, accOffset[Z]); |
548 | SetParamWord(PID_ACC_Z, accOffset[Z]); |
542 | accOffset_writeToEEProm(); |
549 | 543 | ||
Line 550... | Line 544... | ||
550 | // Noise is relative to offset. So, reset noise measurements when |
544 | // Noise is relative to offset. So, reset noise measurements when |
551 | // changing offsets. |
545 | // changing offsets. |
552 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
546 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
553 | - | ||
554 | // Setting offset values has an influence in the analog.c ISR |
- | |
555 | // Therefore run measurement for 100ms to achive stable readings |
- | |
556 | delay_ms_Mess(100); |
- | |
557 | - | ||
558 | // Set the feedback so that air pressure ends up in the middle of the range. |
- | |
559 | // (raw pressure high --> OCR0A also high...) |
- | |
560 | /* |
- | |
561 | OCR0A += ((rawAirPressure - 1024) / rangewidth) - 1; |
- | |
562 | delay_ms_Mess(1000); |
- | |
563 | - | ||
564 | pressureDiff = 0; |
- | |
565 | // DebugOut.Analog[16] = rawAirPressure; |
- | |
566 | - | ||
567 | #define PRESSURE_CAL_CYCLE_COUNT 5 |
- | |
568 | for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) { |
- | |
569 | savedRawAirPressure = rawAirPressure; |
- | |
570 | OCR0A+=2; |
- | |
571 | delay_ms_Mess(500); |
- | |
572 | // raw pressure will decrease. |
- | |
573 | pressureDiff += (savedRawAirPressure - rawAirPressure); |
- | |
574 | savedRawAirPressure = rawAirPressure; |
- | |
575 | OCR0A-=2; |
- | |
576 | delay_ms_Mess(500); |
- | |
577 | // raw pressure will increase. |
- | |
578 | pressureDiff += (rawAirPressure - savedRawAirPressure); |
- | |
579 | } |
- | |
580 | 547 |