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++;
}
