int Baro_update(void)
{
static uint32_t baroDeadline = 0;
static int state = 0;
if ((int32_t)(currentTime - baroDeadline) < 0)
return 0;
baroDeadline = currentTime;
if (state) {
baro.get_up();
baro.start_ut();
baroDeadline += baro.ut_delay;
baro.calculate(&baroPressure, &baroTemperature);
state = 0;
return 2;
} else {
baro.get_ut();
baro.start_up();
Baro_Common();
state = 1;
baroDeadline += baro.up_delay;
return 1;
}
}
void Baro_update(void)
{
static uint32_t baroDeadline = 0;
static uint8_t state = 0;
int32_t pressure;
if ((int32_t)(currentTime - baroDeadline) < 0)
return;
baroDeadline = currentTime;
switch (state) {
case 0:
baro.start_ut();
state++;
baroDeadline += baro.ut_delay;
break;
case 1:
baro.get_ut();
state++;
break;
case 2:
baro.start_up();
state++;
baroDeadline += baro.up_delay;
break;
case 3:
baro.get_up();
pressure = baro.calculate();
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter
state = 0;
baroDeadline += baro.repeat_delay;
break;
}
}
void baroUpdate(uint32_t currentTime)
{
static uint32_t baroDeadline = 0;
static barometerState_e state = BAROMETER_NEEDS_SAMPLES;
if ((int32_t)(currentTime - baroDeadline) < 0)
return;
baroDeadline = currentTime;
switch (state) {
case BAROMETER_NEEDS_SAMPLES:
baro.get_ut();
baro.start_up();
state = BAROMETER_NEEDS_CALCULATION;
baroDeadline += baro.up_delay;
break;
case BAROMETER_NEEDS_CALCULATION:
baro.get_up();
baro.start_ut();
baroDeadline += baro.ut_delay;
baro.calculate(&baroPressure, &baroTemperature);
state = BAROMETER_NEEDS_PROCESSING;
break;
case BAROMETER_NEEDS_PROCESSING:
state = BAROMETER_NEEDS_SAMPLES;
baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
break;
}
}
/*
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++;
}
uint32_t baroUpdate(void)
{
static barometerState_e state = BAROMETER_NEEDS_SAMPLES;
switch (state) {
default:
case BAROMETER_NEEDS_SAMPLES:
baro.get_ut();
baro.start_up();
state = BAROMETER_NEEDS_CALCULATION;
return baro.up_delay;
break;
case BAROMETER_NEEDS_CALCULATION:
baro.get_up();
baro.start_ut();
baro.calculate(&baroPressure, &baroTemperature);
baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure);
state = BAROMETER_NEEDS_SAMPLES;
return baro.ut_delay;
break;
}
}
void Baro_update(void) // Note Pressure is now global for telemetry 1hPa = 1mBar
{
static float BaroTab[5], BaroSpikeTab[5]; // Note: We don't care about runup bufferstate since first 50 runs are discarded anyway
static uint32_t LastGeneraltime;
static uint16_t baroDeadline = 0;
static uint8_t state = 0, Bidx = 0, SkipCnt = 0;
float temp;
bool rdy = false;
uint8_t i, maxsortidx = 4;
newbaroalt = false; // Reset Newbarovalue since it's iterative and not interrupt driven it's OK
if (micros() - LastGeneraltime < baroDeadline) return; // Make it rollover friendly
switch (state)
{
case 0:
baro.start_ut(); // Temperature Conversion start
LastGeneraltime = micros();
baroDeadline = baro.ut_delay - 1;
SkipCnt = 0; // Reset Skipcounter, reduces 27ms delay to average 20ms delay for ms baro (37Hz to 50Hz)
state++;
break;
case 1:
baro.get_ut(); // Readout Temp fall through case
state++;
case 2:
baro.start_up(); // Pressure Conversion start
LastGeneraltime = micros();
baroDeadline = baro.up_delay - 1;
state++;
break;
case 3:
baro.get_up(); // Readout Pressure
baroDeadline = 0; // Don't use delay between read. Cycletime is enough. Before: TimeNowMicros + baro.repeat_delay - 1;
ActualPressure = baro.calculate(); // ActualPressure needed by mavlink
BaroSpikeTab[0] = (1.0f - pow(ActualPressure / 101325.0f, 0.190295f)) * 4433000.0f; // I stick to the "slower", method - gives better results.
BaroSpikeTab[4] = BaroSpikeTab[0];
while(!rdy) // Spikefilter now
{
rdy = true;
for (i = 0; i < maxsortidx; i++)
{
temp = BaroSpikeTab[i];
if (temp > BaroSpikeTab[i + 1])
{
BaroSpikeTab[i] = BaroSpikeTab[i + 1];
BaroSpikeTab[i + 1] = temp;
rdy = false;
}
}
maxsortidx --;
}
BaroTab[Bidx] = BaroSpikeTab[2];
Bidx++;
if (Bidx == 5) Bidx = 0;
BaroAlt = 0;
for (i = 0; i < 5; i++) BaroAlt += BaroTab[i];
BaroAlt *= 0.2f;
SkipCnt++;
if (SkipCnt == 2 || baro.baro_type == 1) state = 0; // Read new Temp every 2nd run gives us little more speed without loosing resolution. However it worsens BMP - so not done there // baro_type: 1 = BMP 2 = MS
else state = 2;
newbaroalt = true;
break;
}
}