File content as of revision 2:90292f8bd179:

// ===============================================================================================
// =                              UAVXArm Quadrocopter Controller                                =
// =                           Copyright (c) 2008 by Prof. Greg Egan                             =
// =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
// =                           http://code.google.com/p/uavp-mods/                               =
// ===============================================================================================

//    This is part of UAVXArm.

//    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
//    General Public License as published by the Free Software Foundation, either version 3 of the
//    License, or (at your option) any later version.

//    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
//    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
//    See the GNU General Public License for more details.

//    You should have received a copy of the GNU General Public License along with this program.
//    If not, see http://www.gnu.org/licenses/

#include "UAVXArm.h"

// Barometers Freescale TI ADC and Bosch BMP085 3.8MHz, Bosch SMD500 400KHz

#define BARO_MIN_CLIMB          150.0    // M minimum available barometer climb from origin
#define BARO_MIN_DESCENT        -50.0    //M minimum available barometer descent from origin

void GetBaroAltitude(void);
void InitBarometer(void);

void ShowBaroType(void);
void BaroTest(void);

#define BaroROCFilter HardFilter

uint16   BaroPressure, BaroTemperature;
boolean  AcquiringPressure;
int16    BaroOffsetDAC;

#define BARO_BUFF_SIZE              4

struct {
    uint8 Head, Tail;
    int24 B[BARO_BUFF_SIZE];
} BaroQ;

int32    OriginBaroPressure, CompBaroPressure;
real32   BaroRelAltitude, BaroRelAltitudeP;
i16u     BaroVal;
int8     BaroType;
int16    BaroClimbAvailable, BaroDescentAvailable;
int16    AltitudeUpdateRate;
int8     BaroRetries;

real32   FakeBaroRelAltitude;
int8     SimulateCycles = 0;

void ShowBaroType(void) {
    switch ( BaroType ) {
        case BaroMPX4115:
            TxString("MPX4115\r\n");
            break;
        case BaroSMD500:
            TxString("SMD500\r\n");
            break;
        case BaroBMP085:
            TxString("BMP085\r\n");
            break;
        case BaroUnknown:
            TxString("None\r\n");
            break;
        default:
            break;
    }
} // ShowBaro

void BaroTest(void) {

    TxString("\r\nAltitude test\r\n");
    TxString("Initialising\r\n");

    InitBarometer();

    while ( F.BaroAltitudeValid && ! F.NewBaroValue )
        GetBaroAltitude();

    TxString("\r\nType:\t");
    ShowBaroType();

    TxString("BaroScale:\t");
    TxVal32(P[BaroScale],0,0);
    TxString("\r\nInit Retries:\t");
    TxVal32((int32)BaroRetries - 2, 0, ' '); // always minimum of 2
    if ( BaroRetries >= BARO_INIT_RETRIES )
        TxString(" FAILED Init.\r\n");
    else
        TxNextLine();

    if ( BaroType == BaroMPX4115 ) {
        TxString("\r\nAddress :\t0x");
        TxValH(MCP4725_ID_Actual);
        TxString("\r\nRange   :\t");
        TxVal32((int32) BaroDescentAvailable * 10.0, 1, ' ');
        TxString("-> ");
        TxVal32((int32) BaroClimbAvailable * 10.0, 1, 'M');
        TxString(" {Offset ");
        TxVal32((int32)BaroOffsetDAC, 0,'}');
        if (( BaroClimbAvailable < BARO_MIN_CLIMB ) || (BaroDescentAvailable > BARO_MIN_DESCENT))
            TxString(" Bad climb or descent range - offset adjustment?");
        TxNextLine();
    }

    if ( F.BaroAltitudeValid ) {

        while ( !F.NewBaroValue )
            GetBaroAltitude();

        F.NewBaroValue = false;

        TxString("Alt.:     \t");
        TxVal32(BaroRelAltitude * 10.0, 1, ' ');
        TxString("M\r\n");

    } else
        TxString("Barometer FAILED\r\n");

    TxString("\r\nR.Finder: \t");
    if ( F.RangefinderAltitudeValid ) {
        GetRangefinderAltitude();
        TxVal32(RangefinderAltitude * 100.0, 2, ' ');
        TxString("M\r\n");
    } else
        TxString("no rangefinder\r\n");

    TxString("\r\nUAVXArm Shield:\t");
    TxVal32((int32)AmbientTemperature.i16, 1, ' ');
    TxString("C\r\n");

} // BaroTest

void GetBaroAltitude(void) {
    static real32 Temp, AltChange;

    if ( BaroType == BaroMPX4115 )
        GetFreescaleBaroAltitude();
    else
        GetBoschBaroAltitude();

    if ( F.NewBaroValue ) {
#ifdef SIMULATE
        if ( State == InFlight ) {
            if ( ++SimulateCycles >= AltitudeUpdateRate ) {
                FakeBaroRelAltitude += ( DesiredThrottle - CruiseThrottle ) + Comp[Alt];
                if ( FakeBaroRelAltitude < -5.0 )
                    FakeBaroRelAltitude = 0.0;

                SimulateCycles = 0;

                ROC = FakeBaroRelAltitude - BaroRelAltitudeP;
                BaroRelAltitudeP = FakeBaroRelAltitude;
            }
            BaroRelAltitude = FakeBaroRelAltitude;
        }
#else

        AltChange = BaroRelAltitude - BaroRelAltitudeP;
        Temp = AltChange * AltitudeUpdateRate;

        if ( fabs( Temp ) > BARO_SANITY_CHECK_MPS ) {
            BaroRelAltitude = BaroRelAltitudeP;    // use previous value
            Temp = 0;
            Stats[BaroFailS]++;
        }

        Temp = Limit1(Temp, BARO_SANITY_CHECK_MPS);
        ROC = ROC * 0.9 + Temp * 0.1;
        BaroRelAltitudeP = BaroRelAltitude;

#endif // SIMULATE

        if ( State == InFlight ) {
            if ( ROC > Stats[MaxROCS] )
                Stats[MaxROCS] = ROC;
            else
                if ( ROC < Stats[MinROCS] )
                    Stats[MinROCS] = ROC;

            if ( BaroRelAltitude > Stats[BaroRelAltitudeS] )
                Stats[BaroRelAltitudeS] = BaroRelAltitude;
        }
    }

} // GetBaroAltitude

void InitBarometer(void) {
    BaroRelAltitude = BaroRelAltitudeP = CompBaroPressure = OriginBaroPressure = 0;
    BaroType = BaroUnknown;

    AltComp = AltDiffSum = AltDSum = 0;
    F.BaroAltitudeValid= true; // optimistic

    if ( IsFreescaleBaroActive() )
        InitFreescaleBarometer();
    else
        if ( IsBoschBaroActive() )
            InitBoschBarometer();
        else
            F.BaroAltitudeValid = F.HoldingAlt = false;

} // InitBarometer

// -----------------------------------------------------------

// Freescale ex Motorola MPX4115 Barometer with ADS7823 12bit ADC

void SetFreescaleMCP4725(int16);
boolean IdentifyMCP4725(void);
void SetFreescaleOffset(void);
void ReadFreescaleBaro(void);
real32 FreescaleToDM(int24);
void GetFreescaleBaroAltitude(void);
boolean IsFreescaleBaroActive(void);
void InitFreescaleBarometer(void);

uint8 MCP4725_ID_Actual;

void SetFreescaleMCP4725(int16 d) {
    static i16u dd;

    dd.u16 = d << 4;                            // left align

    I2CBARO.start();
    if ( I2CBARO.write(MCP4725_ID_Actual) != I2C_ACK ) goto MCP4725Error;
    if ( I2CBARO.write(MCP4725_CMD) != I2C_ACK ) goto MCP4725Error;
    if ( I2CBARO.write(dd.b1) != I2C_ACK ) goto MCP4725Error;
    if ( I2CBARO.write(dd.b0) != I2C_ACK ) goto MCP4725Error;
    I2CBARO.stop();

    return;

MCP4725Error:
    I2CBARO.stop();
    I2CError[MCP4725_ID_Actual]++;

} // SetFreescaleMCP4725

boolean IdentifyMCP4725(void) {

    static boolean r;

    r = true;
    MCP4725_ID_Actual = MCP4725_ID_0xCC;
    if ( I2CBAROAddressResponds( MCP4725_ID_0xCC ) )
        MCP4725_ID_Actual = MCP4725_ID_0xCC;
    else
        if ( I2CBAROAddressResponds( MCP4725_ID_0xC8 ) )
            MCP4725_ID_Actual = MCP4725_ID_0xC8;
        else
            r = false;

    return(r);

//   MCP4725_ID_Actual = FORCE_BARO_ID;
//   return(true);

} // IdentifyMCP4725

void SetFreescaleOffset(void) {
    // Steve Westerfeld
    // 470 Ohm, 1uF RC 0.47mS use 2mS for settling?

    TxString("\r\nOffset \tPressure\r\n");

    BaroOffsetDAC = MCP4725_MAX;

    SetFreescaleMCP4725(BaroOffsetDAC);

    Delay1mS(20);                               // initial settling
    ReadFreescaleBaro();

    while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*7L)/10L) )
            && (BaroOffsetDAC > 20) ) {         // first loop gets close
        BaroOffsetDAC -= 20;                    // approach at 20 steps out of 4095
        SetFreescaleMCP4725(BaroOffsetDAC);
        Delay1mS(20);
        ReadFreescaleBaro();
        TxVal32(BaroOffsetDAC,0,HT);
        TxVal32(BaroVal.u16,0,' ');
        TxNextLine();
        LEDYellow_TOG;
    }

    BaroOffsetDAC += 20;                        // move back up to come at it a little slower
    SetFreescaleMCP4725(BaroOffsetDAC);
    Delay1mS(100);
    ReadFreescaleBaro();

    while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 1) ) {
        BaroOffsetDAC -= 1;
        SetFreescaleMCP4725(BaroOffsetDAC);
        Delay1mS(10); // 10
        ReadFreescaleBaro();
        TxVal32(BaroOffsetDAC,0,HT);
        TxVal32(BaroVal.u16,0,HT);
        TxVal32((int32)FreescaleToDM(BaroVal.u16), 1, 0);
        TxNextLine();
        LEDYellow_TOG;
    }

    Delay1mS(200); // wait for caps to settle
    F.BaroAltitudeValid = BaroOffsetDAC > 0;

} // SetFreescaleOffset

void ReadFreescaleBaro(void) {
    static char B[8];
    static i16u B0, B1, B2, B3;

    mS[BaroUpdate] = mSClock() + ADS7823_TIME_MS;

    I2CBARO.start();  // start conversion
    if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto ADS7823Error;
    if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto ADS7823Error;
    I2CBARO.stop();

    if ( I2CBARO.blockread(ADS7823_RD, B, 8) ) goto ADS7823Error;

    // read block of 4 baro samples

    B0.b0 = B[1];
    B0.b1 = B[0];
    B1.b0 = B[3];
    B1.b1 = B[2];
    B2.b0 = B[5];
    B2.b1 = B[4];
    B3.b0 = B[7];
    B3.b1 = B[6];

    BaroVal.u16 = (uint16)16380 - ( B0.u16 + B1.u16 + B2.u16 + B3.u16 );

    F.BaroAltitudeValid = true;

    return;

ADS7823Error:
    I2CBARO.stop();

    I2CError[ADS7823_ID]++;

    // F.BaroAltitudeValid = F.HoldingAlt = false;
    if ( State == InFlight ) {
        Stats[BaroFailS]++;
        F.BaroFailure = true;
    }

} // ReadFreescaleBaro

real32 FreescaleToDM(int24 p) { // decreasing pressure is increase in altitude negate and rescale to metre altitude
    return( -( (real32)p * 0.8 ) / (real32)P[BaroScale] );
}  // FreescaleToDM

void GetFreescaleBaroAltitude(void) {
    static int24 BaroPressure;

    if ( mSClock() >= mS[BaroUpdate] ) {
        ReadFreescaleBaro();
        if ( F.BaroAltitudeValid ) {
            BaroPressure = (int24)BaroVal.u16; // sum of 4 samples
            BaroRelAltitude = FreescaleToDM(BaroPressure - OriginBaroPressure);
            F.NewBaroValue = F.BaroAltitudeValid;
        }
    }

} // GetFreescaleBaroAltitude

boolean IsFreescaleBaroActive(void) { // check for Freescale Barometer

    static boolean r;

    r = I2CBAROAddressResponds( ADS7823_ID );
    if ( r ) {
        BaroType = BaroMPX4115;
        r = IdentifyMCP4725();
        TrackMinI2CRate(400000);
    }
    return (r);

} // IsFreescaleBaroActive

void InitFreescaleBarometer(void) {
    static int16 BaroOriginAltitude, MinAltitude;
    static int24 BaroPressureP;

    AltitudeUpdateRate = 1000L/ADS7823_TIME_MS;

    BaroTemperature = 0;
    if ( P[BaroScale] <= 0 )
        P[BaroScale] = 56; // failsafe setting

    BaroPressure =  0;
    BaroRetries = 0;
    do {
        BaroPressureP = BaroPressure;

        SetFreescaleOffset();

        while ( mSClock() < mS[BaroUpdate] ) {};
        ReadFreescaleBaro();
        BaroPressure = (int24)BaroVal.u16;
    } while ( ( ++BaroRetries < BARO_INIT_RETRIES )
              && ( fabs( FreescaleToDM(BaroPressure - BaroPressureP) ) > 5 ) );

    F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;

    OriginBaroPressure = BaroPressure;

    BaroRelAltitudeP = BaroRelAltitude = 0.0;

    MinAltitude = FreescaleToDM((int24)ADS7823_MAX*4);
    BaroOriginAltitude = FreescaleToDM(OriginBaroPressure);
    BaroDescentAvailable = MinAltitude - BaroOriginAltitude;
    BaroClimbAvailable = -BaroOriginAltitude;

    //F.BaroAltitudeValid &= (( BaroClimbAvailable >= BARO_MIN_CLIMB )
    // && (BaroDescentAvailable <= BARO_MIN_DESCENT));

#ifdef SIMULATE
    FakeBaroRelAltitude = 0;
#endif // SIMULATE

} // InitFreescaleBarometer

// -----------------------------------------------------------

// Bosch SMD500 and BMP085 Barometers

void StartBoschBaroADC(boolean);
int24 CompensatedBoschPressure(uint16, uint16);

void GetBoschBaroAltitude(void);
boolean IsBoschBaroActive(void);
void InitBoschBarometer(void);

// SMD500 9.5mS (T) 34mS (P)
// BMP085 4.5mS (T) 25.5mS (P) OSRS=3
#define BOSCH_TEMP_TIME_MS            11    // 10 increase to make P+T acq time ~50mS
//#define BMP085_PRESS_TIME_MS         26
//#define SMD500_PRESS_TIME_MS         34
#define BOSCH_PRESS_TIME_MS            38
#define BOSCH_PRESS_TEMP_TIME_MS    50    // pressure and temp time + overheads     

void StartBoschBaroADC(boolean ReadPressure) {
    static uint8 TempOrPress;

    if ( ReadPressure ) {
        TempOrPress = BOSCH_PRESS;
        mS[BaroUpdate] = mSClock() + BOSCH_PRESS_TIME_MS;
    } else {
        mS[BaroUpdate] = mSClock() + BOSCH_TEMP_TIME_MS;
        if ( BaroType == BaroBMP085 )
            TempOrPress = BOSCH_TEMP_BMP085;
        else
            TempOrPress = BOSCH_TEMP_SMD500;
    }

    I2CBARO.start();
    if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto BoschError;
    // access control register, start measurement
    if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto BoschError;
    // select 32kHz input, measure temperature
    if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto BoschError;
    I2CBARO.stop();

    F.BaroAltitudeValid = true;
    return;

BoschError:
    I2CBARO.stop();

    I2CError[BOSCH_ID]++;
    F.BaroAltitudeValid = false;

} // StartBoschBaroADC

void ReadBoschBaro(void) {

    // Possible I2C protocol error - split read of ADC
    I2CBARO.start();
    if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
    if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto BoschError;
    I2CBARO.start();    // restart
    if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
    BaroVal.b1 = I2CBARO.read(I2C_NACK);
    I2CBARO.stop();

    I2CBARO.start();
    if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
    if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto BoschError;
    I2CBARO.start();    // restart
    if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
    BaroVal.b0 = I2CBARO.read(I2C_NACK);
    I2CBARO.stop();

    F.BaroAltitudeValid = true;
    return;

BoschError:
    I2CBARO.stop();

    I2CError[BOSCH_ID]++;

    F.BaroAltitudeValid = F.HoldingAlt = false;
    if ( State == InFlight ) {
        Stats[BaroFailS]++;
        F.BaroFailure = true;
    }

} // ReadBoschBaro

#define BOSCH_BMP085_TEMP_COEFF        62L
#define BOSCH_SMD500_TEMP_COEFF        50L

int24 CompensatedBoschPressure(uint16 BaroPress, uint16 BaroTemp) {
    static int24 BaroTempComp;

    if ( BaroType == BaroBMP085 )
        BaroTempComp = (BaroTemp * BOSCH_BMP085_TEMP_COEFF + 64L) >> 7;
    else
        BaroTempComp = (BaroTemp * BOSCH_SMD500_TEMP_COEFF + 8L) >> 4;

    return ((int24)BaroPress + BaroTempComp - OriginBaroPressure);

} // CompensatedBoschPressure

void GetBoschBaroAltitude(void) {
    static int24 Temp;

    if ( mSClock() >= mS[BaroUpdate] ) {
        ReadBoschBaro();
        if ( F.BaroAltitudeValid )
            if ( AcquiringPressure ) {
                BaroPressure = (int24)BaroVal.u16;
                AcquiringPressure = false;
            } else {
                BaroTemperature = (int24)BaroVal.u16;
                AcquiringPressure = true;

                Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature);
                CompBaroPressure -= BaroQ.B[BaroQ.Head];
                BaroQ.B[BaroQ.Head] = Temp;
                CompBaroPressure += Temp;
                BaroQ.Head = (BaroQ.Head + 1) & (BARO_BUFF_SIZE -1);

                // Pressure queue has 4 entries corresponding to an average delay at 20Hz of 0.1Sec
                // decreasing pressure is increase in altitude negate and rescale to decimetre altitude

                BaroRelAltitude = - ( (real32)CompBaroPressure * (real32)P[BaroScale] ) / 1280.0;

                F.NewBaroValue = F.BaroAltitudeValid;
            }
        else
            AcquiringPressure = true;

        StartBoschBaroADC(AcquiringPressure);
    }
} // GetBoschBaroAltitude

boolean IsBoschBaroActive(void) { // check for Bosch Barometers
    static uint8 r;

    I2CBARO.start();
    if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschInactive;
    if ( I2CBARO.write(BOSCH_TYPE) != I2C_ACK ) goto BoschInactive;
    I2CBARO.start();    // restart
    if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschInactive;
    r = I2CBARO.read(I2C_NACK);
    I2CBARO.stop();

    if (r == BOSCH_ID_BMP085 )
        BaroType = BaroBMP085;
    else
        BaroType = BaroSMD500;

    TrackMinI2CRate(400000);

    return(true);

BoschInactive:

    return(false);

} // IsBoschBaroActive

void InitBoschBarometer(void) {
    int8 s;
    int24 Temp, CompBaroPressureP;

    AltitudeUpdateRate = 1000L / BOSCH_PRESS_TEMP_TIME_MS;

    F.NewBaroValue = false;
    CompBaroPressure = 0;

    TxString("Temp. \tPressure\r\n");

    BaroRetries = 0;
    do { // occasional I2C misread of Temperature so keep doing it until the Origin is stable!!
        CompBaroPressureP = CompBaroPressure;
        CompBaroPressure = BaroQ.Head = 0;

        AcquiringPressure = true;
        StartBoschBaroADC(AcquiringPressure); // Pressure

        for ( s = 0; s < 4; s++ ) {
            while ( mSClock() < mS[BaroUpdate] );
            ReadBoschBaro(); // Pressure
            BaroPressure = BaroVal.u16;

            AcquiringPressure = !AcquiringPressure;
            StartBoschBaroADC(AcquiringPressure); // Temperature
            while ( mSClock() < mS[BaroUpdate] );
            ReadBoschBaro();
            BaroTemperature = BaroVal.u16;

            TxVal32(BaroTemperature,0,HT);
            TxVal32(BaroPressure,0,0);
            TxNextLine();

            Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature);
            BaroQ.B[s] = Temp;
            CompBaroPressure += Temp;

            AcquiringPressure = !AcquiringPressure;
            StartBoschBaroADC(AcquiringPressure);
        }

    } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) && ( abs(CompBaroPressure - CompBaroPressureP) > 12 ) ); // stable within ~0.5M

    OriginBaroPressure = SRS32(CompBaroPressure, 2);

    F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;
    BaroRelAltitudeP = BaroRelAltitude = 0.0;

#ifdef SIMULATE
    FakeBaroRelAltitude = 0.0;
#endif // SIMULATE

} // InitBoschBarometer