Rev 1952 | Rev 1960 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1952 | Rev 1955 | ||
---|---|---|---|
Line 57... | Line 57... | ||
57 | #include "sensors.h" |
57 | #include "sensors.h" |
Line 58... | Line 58... | ||
58 | 58 | ||
59 | // for Delay functions |
59 | // for Delay functions |
Line 60... | Line 60... | ||
60 | #include "timer0.h" |
60 | #include "timer0.h" |
61 | 61 | ||
Line 62... | Line 62... | ||
62 | // For DebugOut |
62 | // For debugOut |
63 | #include "uart0.h" |
63 | #include "uart0.h" |
Line 64... | Line 64... | ||
64 | 64 | ||
65 | // For reading and writing acc. meter offsets. |
65 | // For reading and writing acc. meter offsets. |
Line 66... | Line 66... | ||
66 | #include "eeprom.h" |
66 | #include "eeprom.h" |
67 | 67 | ||
Line 281... | Line 281... | ||
281 | // for various filters... |
281 | // for various filters... |
282 | int16_t tempOffsetGyro, tempGyro; |
282 | int16_t tempOffsetGyro, tempGyro; |
Line 283... | Line 283... | ||
283 | 283 | ||
284 | for (uint8_t axis=0; axis<2; axis++) { |
284 | for (uint8_t axis=0; axis<2; axis++) { |
285 | tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis]; |
- | |
286 | // DebugOut.Analog[6 + 3 * axis ] = tempGyro; |
285 | tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis]; |
287 | /* |
286 | /* |
288 | * Process the gyro data for the PID controller. |
287 | * Process the gyro data for the PID controller. |
289 | */ |
288 | */ |
290 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
289 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
Line 291... | Line 290... | ||
291 | // gyro with a wider range, and helps counter saturation at full control. |
290 | // gyro with a wider range, and helps counter saturation at full control. |
292 | 291 | ||
293 | if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
292 | if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
294 | if (tempGyro < SENSOR_MIN_PITCHROLL) { |
293 | if (tempGyro < SENSOR_MIN_PITCHROLL) { |
295 | DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
294 | debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
296 | tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
295 | tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
297 | } else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
296 | } else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
298 | DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
297 | debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
299 | tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE |
298 | tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE |
300 | + SENSOR_MAX_PITCHROLL; |
299 | + SENSOR_MAX_PITCHROLL; |
301 | } else { |
300 | } else { |
302 | DebugOut.Digital[0] &= ~DEBUG_SENSORLIMIT; |
301 | debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
Line 303... | Line 302... | ||
303 | } |
302 | } |
304 | } |
303 | } |
Line 342... | Line 341... | ||
342 | rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
341 | rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
343 | if (GYRO_REVERSED[YAW]) |
342 | if (GYRO_REVERSED[YAW]) |
344 | yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
343 | yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
345 | else |
344 | else |
346 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
345 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
- | 346 | ||
- | 347 | debugOut.analog[3] = gyro_ATT[PITCH]; |
|
- | 348 | debugOut.analog[4] = gyro_ATT[ROLL]; |
|
- | 349 | debugOut.analog[5] = yawGyro; |
|
347 | } |
350 | } |
Line 348... | Line 351... | ||
348 | 351 | ||
349 | void analog_updateAccelerometers(void) { |
352 | void analog_updateAccelerometers(void) { |
350 | // Pitch and roll axis accelerations. |
353 | // Pitch and roll axis accelerations. |
Line 372... | Line 375... | ||
372 | 375 | ||
373 | /* |
376 | /* |
374 | stronglyFilteredAcc[Z] = |
377 | stronglyFilteredAcc[Z] = |
375 | (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
378 | (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
- | 379 | */ |
|
- | 380 | ||
- | 381 | debugOut.analog[6] = acc[PITCH]; |
|
376 | */ |
382 | debugOut.analog[7] = acc[ROLL]; |
Line 377... | Line 383... | ||
377 | } |
383 | } |
378 | 384 | ||
379 | void analog_updateAirPressure(void) { |
385 | void analog_updateAirPressure(void) { |
380 | static uint16_t pressureAutorangingWait = 25; |
386 | static uint16_t pressureAutorangingWait = 25; |
381 | uint16_t rawAirPressure; |
387 | uint16_t rawAirPressure; |
382 | int16_t newrange; |
388 | int16_t newrange; |
383 | // air pressure |
389 | // air pressure |
384 | if (pressureAutorangingWait) { |
390 | if (pressureAutorangingWait) { |
385 | //A range switch was done recently. Wait for steadying. |
391 | //A range switch was done recently. Wait for steadying. |
386 | pressureAutorangingWait--; |
392 | pressureAutorangingWait--; |
387 | DebugOut.Analog[27] = (uint16_t) OCR0A; |
393 | debugOut.analog[27] = (uint16_t) OCR0A; |
388 | DebugOut.Analog[31] = simpleAirPressure; |
394 | debugOut.analog[31] = simpleAirPressure; |
389 | } else { |
395 | } else { |
390 | rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
396 | rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
391 | if (rawAirPressure < MIN_RAWPRESSURE) { |
397 | if (rawAirPressure < MIN_RAWPRESSURE) { |
Line 415... | Line 421... | ||
415 | } |
421 | } |
416 | } |
422 | } |
Line 417... | Line 423... | ||
417 | 423 | ||
418 | // Even if the sample is off-range, use it. |
424 | // Even if the sample is off-range, use it. |
419 | simpleAirPressure = getSimplePressure(rawAirPressure); |
425 | simpleAirPressure = getSimplePressure(rawAirPressure); |
420 | DebugOut.Analog[27] = (uint16_t) OCR0A; |
426 | debugOut.analog[27] = (uint16_t) OCR0A; |
Line 421... | Line 427... | ||
421 | DebugOut.Analog[31] = simpleAirPressure; |
427 | debugOut.analog[31] = simpleAirPressure; |
422 | 428 | ||
423 | if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
429 | if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
424 | // Danger: pressure near lower end of range. If the measurement saturates, the |
430 | // Danger: pressure near lower end of range. If the measurement saturates, the |
425 | // copter may climb uncontrolledly... Simulate a drastic reduction in pressure. |
431 | // copter may climb uncontrolledly... Simulate a drastic reduction in pressure. |
426 | DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
432 | debugOut.digital[1] |= DEBUG_SENSORLIMIT; |
427 | airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth |
433 | airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth |
428 | + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION |
434 | + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION |
429 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
435 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
430 | } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
436 | } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
431 | // Danger: pressure near upper end of range. If the measurement saturates, the |
437 | // Danger: pressure near upper end of range. If the measurement saturates, the |
432 | // copter may descend uncontrolledly... Simulate a drastic increase in pressure. |
438 | // copter may descend uncontrolledly... Simulate a drastic increase in pressure. |
433 | DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
439 | debugOut.digital[1] |= DEBUG_SENSORLIMIT; |
434 | airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth |
440 | airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth |
435 | + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION |
441 | + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION |
436 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
442 | * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
437 | } else { |
443 | } else { |
438 | // normal case. |
444 | // normal case. |
439 | // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
445 | // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
440 | // The 2 cases above (end of range) are ignored for this. |
446 | // The 2 cases above (end of range) are ignored for this. |
441 | DebugOut.Digital[1] &= ~DEBUG_SENSORLIMIT; |
447 | debugOut.digital[1] &= ~DEBUG_SENSORLIMIT; |
442 | if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1) |
448 | if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1) |
443 | airPressureSum += simpleAirPressure / 2; |
449 | airPressureSum += simpleAirPressure / 2; |
444 | else |
450 | else |
Line 457... | Line 463... | ||
457 | 463 | ||
458 | void analog_updateBatteryVoltage(void) { |
464 | void analog_updateBatteryVoltage(void) { |
459 | // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
465 | // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
460 | // This is divided by 3 --> 10.34 counts per volt. |
466 | // This is divided by 3 --> 10.34 counts per volt. |
461 | UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
467 | UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
462 | DebugOut.Analog[11] = UBat; |
468 | debugOut.analog[11] = UBat; |
Line 463... | Line 469... | ||
463 | } |
469 | } |
464 | 470 | ||
465 | void analog_update(void) { |
471 | void analog_update(void) { |