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

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