59,13 → 59,13 |
// for Delay functions |
#include "timer0.h" |
|
// For DebugOut |
// For debugOut |
#include "uart0.h" |
|
// For reading and writing acc. meter offsets. |
#include "eeprom.h" |
|
// For DebugOut.Digital |
// For debugOut.digital |
#include "output.h" |
|
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
283,7 → 283,6 |
|
for (uint8_t axis=0; axis<2; axis++) { |
tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis]; |
// DebugOut.Analog[6 + 3 * axis ] = tempGyro; |
/* |
* Process the gyro data for the PID controller. |
*/ |
292,14 → 291,14 |
|
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
if (tempGyro < SENSOR_MIN_PITCHROLL) { |
DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
} else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
DebugOut.Digital[0] |= DEBUG_SENSORLIMIT; |
debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE |
+ SENSOR_MAX_PITCHROLL; |
} else { |
DebugOut.Digital[0] &= ~DEBUG_SENSORLIMIT; |
debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
} |
} |
|
344,6 → 343,10 |
yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
else |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
|
debugOut.analog[3] = gyro_ATT[PITCH]; |
debugOut.analog[4] = gyro_ATT[ROLL]; |
debugOut.analog[5] = yawGyro; |
} |
|
void analog_updateAccelerometers(void) { |
374,6 → 377,9 |
stronglyFilteredAcc[Z] = |
(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
*/ |
|
debugOut.analog[6] = acc[PITCH]; |
debugOut.analog[7] = acc[ROLL]; |
} |
|
void analog_updateAirPressure(void) { |
384,8 → 390,8 |
if (pressureAutorangingWait) { |
//A range switch was done recently. Wait for steadying. |
pressureAutorangingWait--; |
DebugOut.Analog[27] = (uint16_t) OCR0A; |
DebugOut.Analog[31] = simpleAirPressure; |
debugOut.analog[27] = (uint16_t) OCR0A; |
debugOut.analog[31] = simpleAirPressure; |
} else { |
rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
if (rawAirPressure < MIN_RAWPRESSURE) { |
417,13 → 423,13 |
|
// Even if the sample is off-range, use it. |
simpleAirPressure = getSimplePressure(rawAirPressure); |
DebugOut.Analog[27] = (uint16_t) OCR0A; |
DebugOut.Analog[31] = simpleAirPressure; |
debugOut.analog[27] = (uint16_t) OCR0A; |
debugOut.analog[31] = simpleAirPressure; |
|
if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
// Danger: pressure near lower end of range. If the measurement saturates, the |
// copter may climb uncontrolledly... Simulate a drastic reduction in pressure. |
DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
debugOut.digital[1] |= DEBUG_SENSORLIMIT; |
airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth |
+ (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION |
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
430,7 → 436,7 |
} else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
// Danger: pressure near upper end of range. If the measurement saturates, the |
// copter may descend uncontrolledly... Simulate a drastic increase in pressure. |
DebugOut.Digital[1] |= DEBUG_SENSORLIMIT; |
debugOut.digital[1] |= DEBUG_SENSORLIMIT; |
airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth |
+ (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION |
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
438,7 → 444,7 |
// normal case. |
// If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
// The 2 cases above (end of range) are ignored for this. |
DebugOut.Digital[1] &= ~DEBUG_SENSORLIMIT; |
debugOut.digital[1] &= ~DEBUG_SENSORLIMIT; |
if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1) |
airPressureSum += simpleAirPressure / 2; |
else |
459,7 → 465,7 |
// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
// This is divided by 3 --> 10.34 counts per volt. |
UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
DebugOut.Analog[11] = UBat; |
debugOut.analog[11] = UBat; |
} |
|
void analog_update(void) { |