| 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) { |