WebSVN - FlightCtrl - Diff - Rev 2020 and 2026 - /branches/dongfang_FC

 uint16_t simpleAirPressure;
 // Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.
+int32_t filteredAirPressure;
+#define MAX_AIRPRESSURE_WINDOW_LENGTH 64
+int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
+int32_t windowedAirPressure;
-int32_t filteredAirPressure;
+uint8_t windowPtr;
 // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
 int32_t airPressureSum;
         ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
         //Set ADC Control and Status Register B
         //Trigger Source to Free Running Mode
         ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
+        for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
+          airPressureWindow[i] = 0;
+        }
+        windowedAirPressure = 0;
         startAnalogConversionCycle();
         // restore global interrupt flags
 /*
  * Min.: 0
  * Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type.
  */
 uint16_t getSimplePressure(int advalue) {
-        return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
+        uint16_t result = (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
+        result += (acc[Z] * (staticParams.airpressureAccZCorrection-128)) >> 10;
+        return result;
+}
 void startAnalogConversionCycle(void) {
   int16_t newrange;
   // air pressure
   if (pressureAutorangingWait) {
     //A range switch was done recently. Wait for steadying.
     pressureAutorangingWait--;
-    debugOut.analog[27] = (uint16_t) OCR0A;
-    debugOut.analog[31] = simpleAirPressure;
   } else {
     rawAirPressure = sensorInputs[AD_AIRPRESSURE];
     if (rawAirPressure < MIN_RAWPRESSURE) {
       // value is too low, so decrease voltage on the op amp minus input, making the value higher.
       newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (MAX_RAWPRESSURE - rawAirPressure) / (rangewidth * 2) + 1;
+      }
+    }
     // Even if the sample is off-range, use it.
-    simpleAirPressure = getSimplePressure(rawAirPressure);
-    debugOut.analog[27] = (uint16_t) OCR0A;
-    debugOut.analog[31] = simpleAirPressure;
+    simpleAirPressure = getSimplePressure(rawAirPressure);
     if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
       // Danger: pressure near lower end of range. If the measurement saturates, the
       airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
         + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
            * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
     } else {
       // normal case.
-      // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample.
+      // If AIRPRESSURE_OVERSAMPLING 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;
       if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING - 1)
         airPressureSum += simpleAirPressure / 2;
       else
     if (pressureMeasurementCount >= AIRPRESSURE_OVERSAMPLING) {
       filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1)
                              + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER;
       pressureMeasurementCount = airPressureSum = 0;
+    }
+    //int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
+    //int32_t windowedAirPressure = 0;
+    //uint8_t windowPtr;
+    windowedAirPressure += simpleAirPressure;
+    windowedAirPressure -= airPressureWindow[windowPtr];
+    airPressureWindow[windowPtr] = simpleAirPressure;
+    windowPtr = (windowPtr+1) % MAX_AIRPRESSURE_WINDOW_LENGTH;
+  }
+}
 void analog_updateBatteryVoltage(void) {

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.c - Rev 2020 → 2026