static void GPS_NewData(uint16_t c) // Called by uart2Init interrupt { static int32_t LatSpikeTab[5], LonSpikeTab[5]; static bool FilterCleared = false; int32_t diff; uint8_t i; if (GPS_newFrame(c)) { if(!GPS_FIX) // Don't fill spikefilter with pure shit { if(!FilterCleared) { for (i = 0; i < 5; i++) { LatSpikeTab[i] = 0; LonSpikeTab[i] = 0; } FilterCleared = true; } } else // We have a fix. { FilterCleared = false; FiveElementSpikeFilterINT32(IRQGPS_coord[LAT], LatSpikeTab); // Always feed filter FiveElementSpikeFilterINT32(IRQGPS_coord[LON], LonSpikeTab); if (LatSpikeTab[2] && LonSpikeTab[2]) // Is the Spikefilter valid and not in runup phase? { if(!GPS_Conf600[LON]) // No scaling? { IRQGPS_coord[LAT] = LatSpikeTab[2]; IRQGPS_coord[LON] = LonSpikeTab[2]; } else // We have Scaling. Is new GPS val more than 6m away? { // If so take the spikefiltervalue if(LatSpikeTab[2] > IRQGPS_coord[LAT]) diff = LatSpikeTab[2] - IRQGPS_coord[LAT]; else diff = IRQGPS_coord[LAT] - LatSpikeTab[2]; if(abs(diff) > GPS_Conf600[LAT]) IRQGPS_coord[LAT] = LatSpikeTab[2]; if(LonSpikeTab[2] > IRQGPS_coord[LON]) diff = LonSpikeTab[2] - IRQGPS_coord[LON]; else diff = IRQGPS_coord[LON] - LonSpikeTab[2]; if(abs(diff) > GPS_Conf600[LON]) IRQGPS_coord[LON] = LonSpikeTab[2]; } } } TimestampNewGPSdata = millis(); // Set timestamp of Data arrival in MS } }
/* Current Timing: MS5611: 51 Hz BMP085: 32.9 Hz Some Notes: I read out baro temperature every time because a sustained datarate is better than having jitter data at a little higher rate. Barometic formula taken from "calcSHABarASLAlt(..)" https://gentlenav.googlecode.com/svn-history/r3598/branches/MatrixPilot_DB_Breeze/MatrixPilot/barometerCntrl.c There you can read: "This is using a super high accuracy barometric altitude computation, technicaly, +-1.5 M of the standard atmosphere tables in the troposphere (up to 11,000 m amsl)". The implementation here does not constantly calculate a new temperature correction factor because doing that only once on startup gives me the better results - that maybe because of the pressure / temperature correction already done in the Baro - driver. The calculation of that temperature correction factor is IMHO the key why it works better than using a fixed number ("BaroTempCorMeter"). The calculation of the formula is not split up because the use of logf() and expf() together is 9,8 times FASTER than a single powf() instruction (measured on naze). Benchtests suggest it is working better and 9,8 times faster than this: #define AbsAltExponent 0.1902612003f // More precise number by using less rounding on physical constants #define BaroTempCorMeter 44330.7692307692f // Let compiler wrap this to float. Was 44330 before. FiveElementSpikeFilterINT32(3200.0f * ((1.0f - powf(ActualPressure / GroundPressure, AbsAltExponent)) * BaroTempCorMeter), BaroSpikeTab32); // Result in cm * 32 */ void Baro_update(void) // Note Pressure is now global for telemetry 1hPa = 1mBar { static int32_t BaroSpikeTab32[5], lastlastspike32; // Note: We don't care about runup bufferstate. static uint32_t LastGeneraltime, LastDataOutPut; static uint16_t baroDeadline = 0; static uint8_t state = 0; float PressScale; int32_t lastspike32, BaroSum, i; if (micros() - LastGeneraltime < baroDeadline) return; // Make it rollover friendly switch (state) // Statemachine just to schedule Baro I2C actions { case 0: baro.start_ut(); // Temperature Conversion start baroDeadline = baro.ut_delay; break; case 1: baro.get_ut(); // Readout Temp baro.start_up(); // Start the Pressure Conversion baroDeadline = baro.up_delay; break; case 2: baro.get_up(); // Readout Pressure baroDeadline = baro.rep_delay; break; } LastGeneraltime = micros(); // Timestamp after I2C actions if (state == 2) { state = 0; // Reset statemachine ActualPressure = baro.calculate(); // ActualPressure needed by mavlink. Unit: Pa (Pascal). Note: 100Pa = 1hPa = 1mbar lastspike32 = BaroSpikeTab32[2]; // Save lastval from spiketab PressScale = 0.190259f * logf(ActualPressure / GroundPressure);// Scale will be way off during initialization, thats why we zero out below FiveElementSpikeFilterINT32(3200.0f * (BaroGroundTempScale * (1.0f - expf(PressScale))), BaroSpikeTab32); // Result in cm * 32 BaroSum = BaroSpikeTab32[2] + lastspike32 + lastlastspike32; lastlastspike32 = lastspike32; BaroAlt = (float)BaroSum * ((1.0f / 32.0f) / 3.0f); Newbaroalt = true; if (!GroundAltInitialized) { BaroDtUS = LastGeneraltime - LastDataOutPut; // We just need the time between reads during init. First read will be off, settleloop fixes that LastDataOutPut = LastGeneraltime; for (i = 0; i < 5; i++) BaroSpikeTab32[i] = 0; // Zero out during ini. No need to do it better, just more EEPROM and Stack waste. lastlastspike32 = 0; } } else state++; }