baro.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 // Barometers Freescale TI ADC and Bosch BMP085 3.8MHz, Bosch SMD500 400KHz
00024 
00025 #define BARO_MIN_CLIMB          150.0    // M minimum available barometer climb from origin
00026 #define BARO_MIN_DESCENT        -50.0    //M minimum available barometer descent from origin
00027 
00028 void GetBaroAltitude(void);
00029 void InitBarometer(void);
00030 
00031 void ShowBaroType(void);
00032 void BaroTest(void);
00033 
00034 #define BaroROCFilter HardFilter
00035 
00036 uint16   BaroPressure, BaroTemperature;
00037 boolean  AcquiringPressure;
00038 int16    BaroOffsetDAC;
00039 
00040 #define BARO_BUFF_SIZE              4
00041 
00042 struct {
00043     uint8 Head, Tail;
00044     int24 B[BARO_BUFF_SIZE];
00045 } BaroQ;
00046 
00047 int32    OriginBaroPressure, CompBaroPressure;
00048 real32   BaroRelAltitude, BaroRelAltitudeP;
00049 i16u     BaroVal;
00050 int8     BaroType;
00051 int16    BaroClimbAvailable, BaroDescentAvailable;
00052 int16    AltitudeUpdateRate;
00053 int8     BaroRetries;
00054 
00055 real32   FakeBaroRelAltitude;
00056 int8     SimulateCycles = 0;
00057 
00058 void ShowBaroType(void) {
00059     switch ( BaroType ) {
00060         case BaroMPX4115:
00061             TxString("MPX4115\r\n");
00062             break;
00063         case BaroSMD500:
00064             TxString("SMD500\r\n");
00065             break;
00066         case BaroBMP085:
00067             TxString("BMP085\r\n");
00068             break;
00069         case BaroUnknown:
00070             TxString("None\r\n");
00071             break;
00072         default:
00073             break;
00074     }
00075 } // ShowBaro
00076 
00077 void BaroTest(void) {
00078 
00079     TxString("\r\nAltitude test\r\n");
00080     TxString("Initialising\r\n");
00081 
00082     InitBarometer();
00083 
00084     while ( F.BaroAltitudeValid && ! F.NewBaroValue )
00085         GetBaroAltitude();
00086 
00087     TxString("\r\nType:\t");
00088     ShowBaroType();
00089 
00090     TxString("BaroScale:\t");
00091     TxVal32(P[BaroScale],0,0);
00092     TxString("\r\nInit Retries:\t");
00093     TxVal32((int32)BaroRetries - 2, 0, ' '); // always minimum of 2
00094     if ( BaroRetries >= BARO_INIT_RETRIES )
00095         TxString(" FAILED Init.\r\n");
00096     else
00097         TxNextLine();
00098 
00099     if ( BaroType == BaroMPX4115 ) {
00100         TxString("\r\nAddress :\t0x");
00101         TxValH(MCP4725_ID_Actual);
00102         TxString("\r\nRange   :\t");
00103         TxVal32((int32) BaroDescentAvailable * 10.0, 1, ' ');
00104         TxString("-> ");
00105         TxVal32((int32) BaroClimbAvailable * 10.0, 1, 'M');
00106         TxString(" {Offset ");
00107         TxVal32((int32)BaroOffsetDAC, 0,'}');
00108         if (( BaroClimbAvailable < BARO_MIN_CLIMB ) || (BaroDescentAvailable > BARO_MIN_DESCENT))
00109             TxString(" Bad climb or descent range - offset adjustment?");
00110         TxNextLine();
00111     }
00112 
00113     if ( F.BaroAltitudeValid ) {
00114 
00115         while ( !F.NewBaroValue )
00116             GetBaroAltitude();
00117 
00118         F.NewBaroValue = false;
00119 
00120         TxString("Alt.:     \t");
00121         TxVal32(BaroRelAltitude * 10.0, 1, ' ');
00122         TxString("M\r\n");
00123 
00124     } else
00125         TxString("Barometer FAILED\r\n");
00126 
00127     TxString("\r\nR.Finder: \t");
00128     if ( F.RangefinderAltitudeValid ) {
00129         GetRangefinderAltitude();
00130         TxVal32(RangefinderAltitude * 100.0, 2, ' ');
00131         TxString("M\r\n");
00132     } else
00133         TxString("no rangefinder\r\n");
00134 
00135     TxString("\r\nUAVXArm Shield:\t");
00136     TxVal32((int32)AmbientTemperature.i16, 1, ' ');
00137     TxString("C\r\n");
00138 
00139 } // BaroTest
00140 
00141 void GetBaroAltitude(void) {
00142     static real32 Temp, AltChange;
00143 
00144     if ( BaroType == BaroMPX4115 )
00145         GetFreescaleBaroAltitude();
00146     else
00147         GetBoschBaroAltitude();
00148 
00149     if ( F.NewBaroValue ) {
00150 #ifdef SIMULATE
00151         if ( State == InFlight ) {
00152             if ( ++SimulateCycles >= AltitudeUpdateRate ) {
00153                 FakeBaroRelAltitude += ( DesiredThrottle - CruiseThrottle ) + Comp[Alt];
00154                 if ( FakeBaroRelAltitude < -5.0 )
00155                     FakeBaroRelAltitude = 0.0;
00156 
00157                 SimulateCycles = 0;
00158 
00159                 ROC = FakeBaroRelAltitude - BaroRelAltitudeP;
00160                 BaroRelAltitudeP = FakeBaroRelAltitude;
00161             }
00162             BaroRelAltitude = FakeBaroRelAltitude;
00163         }
00164 #else
00165 
00166         AltChange = BaroRelAltitude - BaroRelAltitudeP;
00167         Temp = AltChange * AltitudeUpdateRate;
00168 
00169         if ( fabs( Temp ) > BARO_SANITY_CHECK_MPS ) {
00170             BaroRelAltitude = BaroRelAltitudeP;    // use previous value
00171             Temp = 0;
00172             Stats[BaroFailS]++;
00173         }
00174 
00175         Temp = Limit1(Temp, BARO_SANITY_CHECK_MPS);
00176         ROC = ROC * 0.9 + Temp * 0.1;
00177         BaroRelAltitudeP = BaroRelAltitude;
00178 
00179 #endif // SIMULATE
00180 
00181         if ( State == InFlight ) {
00182             if ( ROC > Stats[MaxROCS] )
00183                 Stats[MaxROCS] = ROC;
00184             else
00185                 if ( ROC < Stats[MinROCS] )
00186                     Stats[MinROCS] = ROC;
00187 
00188             if ( BaroRelAltitude > Stats[BaroRelAltitudeS] )
00189                 Stats[BaroRelAltitudeS] = BaroRelAltitude;
00190         }
00191     }
00192 
00193 } // GetBaroAltitude
00194 
00195 void InitBarometer(void) {
00196     BaroRelAltitude = BaroRelAltitudeP = CompBaroPressure = OriginBaroPressure = 0;
00197     BaroType = BaroUnknown;
00198 
00199     AltComp = AltDiffSum = AltDSum = 0;
00200     F.BaroAltitudeValid= true; // optimistic
00201 
00202     if ( IsFreescaleBaroActive() )
00203         InitFreescaleBarometer();
00204     else
00205         if ( IsBoschBaroActive() )
00206             InitBoschBarometer();
00207         else
00208             F.BaroAltitudeValid = F.HoldingAlt = false;
00209 
00210 } // InitBarometer
00211 
00212 // -----------------------------------------------------------
00213 
00214 // Freescale ex Motorola MPX4115 Barometer with ADS7823 12bit ADC
00215 
00216 void SetFreescaleMCP4725(int16);
00217 boolean IdentifyMCP4725(void);
00218 void SetFreescaleOffset(void);
00219 void ReadFreescaleBaro(void);
00220 real32 FreescaleToDM(int24);
00221 void GetFreescaleBaroAltitude(void);
00222 boolean IsFreescaleBaroActive(void);
00223 void InitFreescaleBarometer(void);
00224 
00225 uint8 MCP4725_ID_Actual;
00226 
00227 void SetFreescaleMCP4725(int16 d) {
00228     static i16u dd;
00229 
00230     dd.u16 = d << 4;                            // left align
00231 
00232     I2CBARO.start();
00233     if ( I2CBARO.write(MCP4725_ID_Actual) != I2C_ACK ) goto MCP4725Error;
00234     if ( I2CBARO.write(MCP4725_CMD) != I2C_ACK ) goto MCP4725Error;
00235     if ( I2CBARO.write(dd.b1) != I2C_ACK ) goto MCP4725Error;
00236     if ( I2CBARO.write(dd.b0) != I2C_ACK ) goto MCP4725Error;
00237     I2CBARO.stop();
00238 
00239     return;
00240 
00241 MCP4725Error:
00242     I2CBARO.stop();
00243     I2CError[MCP4725_ID_Actual]++;
00244 
00245 } // SetFreescaleMCP4725
00246 
00247 boolean IdentifyMCP4725(void) {
00248 
00249     static boolean r;
00250 
00251     r = true;
00252     MCP4725_ID_Actual = MCP4725_ID_0xCC;
00253     if ( I2CBAROAddressResponds( MCP4725_ID_0xCC ) )
00254         MCP4725_ID_Actual = MCP4725_ID_0xCC;
00255     else
00256         if ( I2CBAROAddressResponds( MCP4725_ID_0xC8 ) )
00257             MCP4725_ID_Actual = MCP4725_ID_0xC8;
00258         else
00259             r = false;
00260 
00261     return(r);
00262 
00263 //   MCP4725_ID_Actual = FORCE_BARO_ID;
00264 //   return(true);
00265 
00266 } // IdentifyMCP4725
00267 
00268 void SetFreescaleOffset(void) {
00269     // Steve Westerfeld
00270     // 470 Ohm, 1uF RC 0.47mS use 2mS for settling?
00271 
00272     TxString("\r\nOffset \tPressure\r\n");
00273 
00274     BaroOffsetDAC = MCP4725_MAX;
00275 
00276     SetFreescaleMCP4725(BaroOffsetDAC);
00277 
00278     Delay1mS(20);                               // initial settling
00279     ReadFreescaleBaro();
00280 
00281     while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*7L)/10L) )
00282             && (BaroOffsetDAC > 20) ) {         // first loop gets close
00283         BaroOffsetDAC -= 20;                    // approach at 20 steps out of 4095
00284         SetFreescaleMCP4725(BaroOffsetDAC);
00285         Delay1mS(20);
00286         ReadFreescaleBaro();
00287         TxVal32(BaroOffsetDAC,0,HT);
00288         TxVal32(BaroVal.u16,0,' ');
00289         TxNextLine();
00290         LEDYellow_TOG;
00291     }
00292 
00293     BaroOffsetDAC += 20;                        // move back up to come at it a little slower
00294     SetFreescaleMCP4725(BaroOffsetDAC);
00295     Delay1mS(100);
00296     ReadFreescaleBaro();
00297 
00298     while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 1) ) {
00299         BaroOffsetDAC -= 1;
00300         SetFreescaleMCP4725(BaroOffsetDAC);
00301         Delay1mS(10); // 10
00302         ReadFreescaleBaro();
00303         TxVal32(BaroOffsetDAC,0,HT);
00304         TxVal32(BaroVal.u16,0,HT);
00305         TxVal32((int32)FreescaleToDM(BaroVal.u16), 1, 0);
00306         TxNextLine();
00307         LEDYellow_TOG;
00308     }
00309 
00310     Delay1mS(200); // wait for caps to settle
00311     F.BaroAltitudeValid = BaroOffsetDAC > 0;
00312 
00313 } // SetFreescaleOffset
00314 
00315 void ReadFreescaleBaro(void) {
00316     static char B[8];
00317     static i16u B0, B1, B2, B3;
00318 
00319     mS[BaroUpdate] = mSClock() + ADS7823_TIME_MS;
00320 
00321     I2CBARO.start();  // start conversion
00322     if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto ADS7823Error;
00323     if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto ADS7823Error;
00324     I2CBARO.stop();
00325 
00326     if ( I2CBARO.blockread(ADS7823_RD, B, 8) ) goto ADS7823Error;
00327 
00328     // read block of 4 baro samples
00329 
00330     B0.b0 = B[1];
00331     B0.b1 = B[0];
00332     B1.b0 = B[3];
00333     B1.b1 = B[2];
00334     B2.b0 = B[5];
00335     B2.b1 = B[4];
00336     B3.b0 = B[7];
00337     B3.b1 = B[6];
00338 
00339     BaroVal.u16 = (uint16)16380 - ( B0.u16 + B1.u16 + B2.u16 + B3.u16 );
00340 
00341     F.BaroAltitudeValid = true;
00342 
00343     return;
00344 
00345 ADS7823Error:
00346     I2CBARO.stop();
00347 
00348     I2CError[ADS7823_ID]++;
00349 
00350     // F.BaroAltitudeValid = F.HoldingAlt = false;
00351     if ( State == InFlight ) {
00352         Stats[BaroFailS]++;
00353         F.BaroFailure = true;
00354     }
00355 
00356 } // ReadFreescaleBaro
00357 
00358 real32 FreescaleToDM(int24 p) { // decreasing pressure is increase in altitude negate and rescale to metre altitude
00359     return( -( (real32)p * 0.8 ) / (real32)P[BaroScale] );
00360 }  // FreescaleToDM
00361 
00362 void GetFreescaleBaroAltitude(void) {
00363     static int24 BaroPressure;
00364 
00365     if ( mSClock() >= mS[BaroUpdate] ) {
00366         ReadFreescaleBaro();
00367         if ( F.BaroAltitudeValid ) {
00368             BaroPressure = (int24)BaroVal.u16; // sum of 4 samples
00369             BaroRelAltitude = FreescaleToDM(BaroPressure - OriginBaroPressure);
00370             F.NewBaroValue = F.BaroAltitudeValid;
00371         }
00372     }
00373 
00374 } // GetFreescaleBaroAltitude
00375 
00376 boolean IsFreescaleBaroActive(void) { // check for Freescale Barometer
00377 
00378     static boolean r;
00379 
00380     r = I2CBAROAddressResponds( ADS7823_ID );
00381     if ( r ) {
00382         BaroType = BaroMPX4115;
00383         r = IdentifyMCP4725();
00384         TrackMinI2CRate(400000);
00385     }
00386     return (r);
00387 
00388 } // IsFreescaleBaroActive
00389 
00390 void InitFreescaleBarometer(void) {
00391     static int16 BaroOriginAltitude, MinAltitude;
00392     static int24 BaroPressureP;
00393 
00394     AltitudeUpdateRate = 1000L/ADS7823_TIME_MS;
00395 
00396     BaroTemperature = 0;
00397     if ( P[BaroScale] <= 0 )
00398         P[BaroScale] = 56; // failsafe setting
00399 
00400     BaroPressure =  0;
00401     BaroRetries = 0;
00402     do {
00403         BaroPressureP = BaroPressure;
00404 
00405         SetFreescaleOffset();
00406 
00407         while ( mSClock() < mS[BaroUpdate] ) {};
00408         ReadFreescaleBaro();
00409         BaroPressure = (int24)BaroVal.u16;
00410     } while ( ( ++BaroRetries < BARO_INIT_RETRIES )
00411               && ( fabs( FreescaleToDM(BaroPressure - BaroPressureP) ) > 5 ) );
00412 
00413     F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;
00414 
00415     OriginBaroPressure = BaroPressure;
00416 
00417     BaroRelAltitudeP = BaroRelAltitude = 0.0;
00418 
00419     MinAltitude = FreescaleToDM((int24)ADS7823_MAX*4);
00420     BaroOriginAltitude = FreescaleToDM(OriginBaroPressure);
00421     BaroDescentAvailable = MinAltitude - BaroOriginAltitude;
00422     BaroClimbAvailable = -BaroOriginAltitude;
00423 
00424     //F.BaroAltitudeValid &= (( BaroClimbAvailable >= BARO_MIN_CLIMB )
00425     // && (BaroDescentAvailable <= BARO_MIN_DESCENT));
00426 
00427 #ifdef SIMULATE
00428     FakeBaroRelAltitude = 0;
00429 #endif // SIMULATE
00430 
00431 } // InitFreescaleBarometer
00432 
00433 // -----------------------------------------------------------
00434 
00435 // Bosch SMD500 and BMP085 Barometers
00436 
00437 void StartBoschBaroADC(boolean);
00438 int24 CompensatedBoschPressure(uint16, uint16);
00439 
00440 void GetBoschBaroAltitude(void);
00441 boolean IsBoschBaroActive(void);
00442 void InitBoschBarometer(void);
00443 
00444 // SMD500 9.5mS (T) 34mS (P)
00445 // BMP085 4.5mS (T) 25.5mS (P) OSRS=3
00446 #define BOSCH_TEMP_TIME_MS            11    // 10 increase to make P+T acq time ~50mS
00447 //#define BMP085_PRESS_TIME_MS         26
00448 //#define SMD500_PRESS_TIME_MS         34
00449 #define BOSCH_PRESS_TIME_MS            38
00450 #define BOSCH_PRESS_TEMP_TIME_MS    50    // pressure and temp time + overheads     
00451 
00452 void StartBoschBaroADC(boolean ReadPressure) {
00453     static uint8 TempOrPress;
00454 
00455     if ( ReadPressure ) {
00456         TempOrPress = BOSCH_PRESS;
00457         mS[BaroUpdate] = mSClock() + BOSCH_PRESS_TIME_MS;
00458     } else {
00459         mS[BaroUpdate] = mSClock() + BOSCH_TEMP_TIME_MS;
00460         if ( BaroType == BaroBMP085 )
00461             TempOrPress = BOSCH_TEMP_BMP085;
00462         else
00463             TempOrPress = BOSCH_TEMP_SMD500;
00464     }
00465 
00466     I2CBARO.start();
00467     if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto BoschError;
00468     // access control register, start measurement
00469     if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto BoschError;
00470     // select 32kHz input, measure temperature
00471     if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto BoschError;
00472     I2CBARO.stop();
00473 
00474     F.BaroAltitudeValid = true;
00475     return;
00476 
00477 BoschError:
00478     I2CBARO.stop();
00479 
00480     I2CError[BOSCH_ID]++;
00481     F.BaroAltitudeValid = false;
00482 
00483 } // StartBoschBaroADC
00484 
00485 void ReadBoschBaro(void) {
00486 
00487     // Possible I2C protocol error - split read of ADC
00488     I2CBARO.start();
00489     if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
00490     if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto BoschError;
00491     I2CBARO.start();    // restart
00492     if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
00493     BaroVal.b1 = I2CBARO.read(I2C_NACK);
00494     I2CBARO.stop();
00495 
00496     I2CBARO.start();
00497     if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
00498     if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto BoschError;
00499     I2CBARO.start();    // restart
00500     if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
00501     BaroVal.b0 = I2CBARO.read(I2C_NACK);
00502     I2CBARO.stop();
00503 
00504     F.BaroAltitudeValid = true;
00505     return;
00506 
00507 BoschError:
00508     I2CBARO.stop();
00509 
00510     I2CError[BOSCH_ID]++;
00511 
00512     F.BaroAltitudeValid = F.HoldingAlt = false;
00513     if ( State == InFlight ) {
00514         Stats[BaroFailS]++;
00515         F.BaroFailure = true;
00516     }
00517 
00518 } // ReadBoschBaro
00519 
00520 #define BOSCH_BMP085_TEMP_COEFF        62L
00521 #define BOSCH_SMD500_TEMP_COEFF        50L
00522 
00523 int24 CompensatedBoschPressure(uint16 BaroPress, uint16 BaroTemp) {
00524     static int24 BaroTempComp;
00525 
00526     if ( BaroType == BaroBMP085 )
00527         BaroTempComp = (BaroTemp * BOSCH_BMP085_TEMP_COEFF + 64L) >> 7;
00528     else
00529         BaroTempComp = (BaroTemp * BOSCH_SMD500_TEMP_COEFF + 8L) >> 4;
00530 
00531     return ((int24)BaroPress + BaroTempComp - OriginBaroPressure);
00532 
00533 } // CompensatedBoschPressure
00534 
00535 void GetBoschBaroAltitude(void) {
00536     static int24 Temp;
00537 
00538     if ( mSClock() >= mS[BaroUpdate] ) {
00539         ReadBoschBaro();
00540         if ( F.BaroAltitudeValid )
00541             if ( AcquiringPressure ) {
00542                 BaroPressure = (int24)BaroVal.u16;
00543                 AcquiringPressure = false;
00544             } else {
00545                 BaroTemperature = (int24)BaroVal.u16;
00546                 AcquiringPressure = true;
00547 
00548                 Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature);
00549                 CompBaroPressure -= BaroQ.B[BaroQ.Head];
00550                 BaroQ.B[BaroQ.Head] = Temp;
00551                 CompBaroPressure += Temp;
00552                 BaroQ.Head = (BaroQ.Head + 1) & (BARO_BUFF_SIZE -1);
00553 
00554                 // Pressure queue has 4 entries corresponding to an average delay at 20Hz of 0.1Sec
00555                 // decreasing pressure is increase in altitude negate and rescale to decimetre altitude
00556 
00557                 BaroRelAltitude = - ( (real32)CompBaroPressure * (real32)P[BaroScale] ) / 1280.0;
00558 
00559                 F.NewBaroValue = F.BaroAltitudeValid;
00560             }
00561         else
00562             AcquiringPressure = true;
00563 
00564         StartBoschBaroADC(AcquiringPressure);
00565     }
00566 } // GetBoschBaroAltitude
00567 
00568 boolean IsBoschBaroActive(void) { // check for Bosch Barometers
00569     static uint8 r;
00570 
00571     I2CBARO.start();
00572     if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschInactive;
00573     if ( I2CBARO.write(BOSCH_TYPE) != I2C_ACK ) goto BoschInactive;
00574     I2CBARO.start();    // restart
00575     if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschInactive;
00576     r = I2CBARO.read(I2C_NACK);
00577     I2CBARO.stop();
00578 
00579     if (r == BOSCH_ID_BMP085 )
00580         BaroType = BaroBMP085;
00581     else
00582         BaroType = BaroSMD500;
00583 
00584     TrackMinI2CRate(400000);
00585 
00586     return(true);
00587 
00588 BoschInactive:
00589 
00590     return(false);
00591 
00592 } // IsBoschBaroActive
00593 
00594 void InitBoschBarometer(void) {
00595     int8 s;
00596     int24 Temp, CompBaroPressureP;
00597 
00598     AltitudeUpdateRate = 1000L / BOSCH_PRESS_TEMP_TIME_MS;
00599 
00600     F.NewBaroValue = false;
00601     CompBaroPressure = 0;
00602 
00603     TxString("Temp. \tPressure\r\n");
00604 
00605     BaroRetries = 0;
00606     do { // occasional I2C misread of Temperature so keep doing it until the Origin is stable!!
00607         CompBaroPressureP = CompBaroPressure;
00608         CompBaroPressure = BaroQ.Head = 0;
00609 
00610         AcquiringPressure = true;
00611         StartBoschBaroADC(AcquiringPressure); // Pressure
00612 
00613         for ( s = 0; s < 4; s++ ) {
00614             while ( mSClock() < mS[BaroUpdate] );
00615             ReadBoschBaro(); // Pressure
00616             BaroPressure = BaroVal.u16;
00617 
00618             AcquiringPressure = !AcquiringPressure;
00619             StartBoschBaroADC(AcquiringPressure); // Temperature
00620             while ( mSClock() < mS[BaroUpdate] );
00621             ReadBoschBaro();
00622             BaroTemperature = BaroVal.u16;
00623 
00624             TxVal32(BaroTemperature,0,HT);
00625             TxVal32(BaroPressure,0,0);
00626             TxNextLine();
00627 
00628             Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature);
00629             BaroQ.B[s] = Temp;
00630             CompBaroPressure += Temp;
00631 
00632             AcquiringPressure = !AcquiringPressure;
00633             StartBoschBaroADC(AcquiringPressure);
00634         }
00635 
00636     } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) && ( abs(CompBaroPressure - CompBaroPressureP) > 12 ) ); // stable within ~0.5M
00637 
00638     OriginBaroPressure = SRS32(CompBaroPressure, 2);
00639 
00640     F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;
00641     BaroRelAltitudeP = BaroRelAltitude = 0.0;
00642 
00643 #ifdef SIMULATE
00644     FakeBaroRelAltitude = 0.0;
00645 #endif // SIMULATE
00646 
00647 } // InitBoschBarometer