Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
baro.c@0:62a1c91a859a, 2011-02-18 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 18 22:28:05 2011 +0000
- Revision:
- 0:62a1c91a859a
- Child:
- 1:1e3318a30ddd
First release
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gke | 0:62a1c91a859a | 1 | // =============================================================================================== |
gke | 0:62a1c91a859a | 2 | // = UAVXArm Quadrocopter Controller = |
gke | 0:62a1c91a859a | 3 | // = Copyright (c) 2008 by Prof. Greg Egan = |
gke | 0:62a1c91a859a | 4 | // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = |
gke | 0:62a1c91a859a | 5 | // = http://code.google.com/p/uavp-mods/ http://uavp.ch = |
gke | 0:62a1c91a859a | 6 | // =============================================================================================== |
gke | 0:62a1c91a859a | 7 | |
gke | 0:62a1c91a859a | 8 | // This is part of UAVXArm. |
gke | 0:62a1c91a859a | 9 | |
gke | 0:62a1c91a859a | 10 | // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU |
gke | 0:62a1c91a859a | 11 | // General Public License as published by the Free Software Foundation, either version 3 of the |
gke | 0:62a1c91a859a | 12 | // License, or (at your option) any later version. |
gke | 0:62a1c91a859a | 13 | |
gke | 0:62a1c91a859a | 14 | // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without |
gke | 0:62a1c91a859a | 15 | // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
gke | 0:62a1c91a859a | 16 | // See the GNU General Public License for more details. |
gke | 0:62a1c91a859a | 17 | |
gke | 0:62a1c91a859a | 18 | // You should have received a copy of the GNU General Public License along with this program. |
gke | 0:62a1c91a859a | 19 | // If not, see http://www.gnu.org/licenses/ |
gke | 0:62a1c91a859a | 20 | |
gke | 0:62a1c91a859a | 21 | #include "UAVXArm.h" |
gke | 0:62a1c91a859a | 22 | |
gke | 0:62a1c91a859a | 23 | // Barometers Freescale TI ADC and Bosch BMP085 3.8MHz, Bosch SMD500 400KHz |
gke | 0:62a1c91a859a | 24 | |
gke | 0:62a1c91a859a | 25 | #define BARO_MIN_CLIMB 150.0 // M minimum available barometer climb from origin |
gke | 0:62a1c91a859a | 26 | #define BARO_MIN_DESCENT -50.0 //M minimum available barometer descent from origin |
gke | 0:62a1c91a859a | 27 | |
gke | 0:62a1c91a859a | 28 | void GetBaroAltitude(void); |
gke | 0:62a1c91a859a | 29 | void InitBarometer(void); |
gke | 0:62a1c91a859a | 30 | |
gke | 0:62a1c91a859a | 31 | void ShowBaroType(void); |
gke | 0:62a1c91a859a | 32 | void BaroTest(void); |
gke | 0:62a1c91a859a | 33 | |
gke | 0:62a1c91a859a | 34 | #define BaroROCFilter HardFilter |
gke | 0:62a1c91a859a | 35 | |
gke | 0:62a1c91a859a | 36 | uint16 BaroPressure, BaroTemperature; |
gke | 0:62a1c91a859a | 37 | boolean AcquiringPressure; |
gke | 0:62a1c91a859a | 38 | int16 BaroOffsetDAC; |
gke | 0:62a1c91a859a | 39 | |
gke | 0:62a1c91a859a | 40 | #define BARO_BUFF_SIZE 4 |
gke | 0:62a1c91a859a | 41 | |
gke | 0:62a1c91a859a | 42 | struct { |
gke | 0:62a1c91a859a | 43 | uint8 Head, Tail; |
gke | 0:62a1c91a859a | 44 | int24 B[BARO_BUFF_SIZE]; |
gke | 0:62a1c91a859a | 45 | } BaroQ; |
gke | 0:62a1c91a859a | 46 | |
gke | 0:62a1c91a859a | 47 | int32 OriginBaroPressure, CompBaroPressure; |
gke | 0:62a1c91a859a | 48 | real32 BaroRelAltitude, BaroRelAltitudeP; |
gke | 0:62a1c91a859a | 49 | i16u BaroVal; |
gke | 0:62a1c91a859a | 50 | int8 BaroType; |
gke | 0:62a1c91a859a | 51 | int16 BaroClimbAvailable, BaroDescentAvailable; |
gke | 0:62a1c91a859a | 52 | int16 AltitudeUpdateRate; |
gke | 0:62a1c91a859a | 53 | int8 BaroRetries; |
gke | 0:62a1c91a859a | 54 | |
gke | 0:62a1c91a859a | 55 | real32 FakeBaroRelAltitude; |
gke | 0:62a1c91a859a | 56 | int8 SimulateCycles = 0; |
gke | 0:62a1c91a859a | 57 | |
gke | 0:62a1c91a859a | 58 | void ShowBaroType(void) { |
gke | 0:62a1c91a859a | 59 | switch ( BaroType ) { |
gke | 0:62a1c91a859a | 60 | case BaroMPX4115: |
gke | 0:62a1c91a859a | 61 | TxString("MPX4115\r\n"); |
gke | 0:62a1c91a859a | 62 | break; |
gke | 0:62a1c91a859a | 63 | case BaroSMD500: |
gke | 0:62a1c91a859a | 64 | TxString("SMD500\r\n"); |
gke | 0:62a1c91a859a | 65 | break; |
gke | 0:62a1c91a859a | 66 | case BaroBMP085: |
gke | 0:62a1c91a859a | 67 | TxString("BMP085\r\n"); |
gke | 0:62a1c91a859a | 68 | break; |
gke | 0:62a1c91a859a | 69 | case BaroUnknown: |
gke | 0:62a1c91a859a | 70 | TxString("None\r\n"); |
gke | 0:62a1c91a859a | 71 | break; |
gke | 0:62a1c91a859a | 72 | default: |
gke | 0:62a1c91a859a | 73 | break; |
gke | 0:62a1c91a859a | 74 | } |
gke | 0:62a1c91a859a | 75 | } // ShowBaro |
gke | 0:62a1c91a859a | 76 | |
gke | 0:62a1c91a859a | 77 | void BaroTest(void) { |
gke | 0:62a1c91a859a | 78 | TxString("\r\nAltitude test\r\n"); |
gke | 0:62a1c91a859a | 79 | |
gke | 0:62a1c91a859a | 80 | TxString("Initialising\r\n"); |
gke | 0:62a1c91a859a | 81 | |
gke | 0:62a1c91a859a | 82 | InitBarometer(); |
gke | 0:62a1c91a859a | 83 | |
gke | 0:62a1c91a859a | 84 | while ( F.BaroAltitudeValid && ! F.NewBaroValue ) |
gke | 0:62a1c91a859a | 85 | GetBaroAltitude(); |
gke | 0:62a1c91a859a | 86 | |
gke | 0:62a1c91a859a | 87 | TxString("\r\nType:\t"); |
gke | 0:62a1c91a859a | 88 | ShowBaroType(); |
gke | 0:62a1c91a859a | 89 | |
gke | 0:62a1c91a859a | 90 | TxString("Init Retries:\t"); |
gke | 0:62a1c91a859a | 91 | TxVal32((int32)BaroRetries - 2, 0, ' '); // always minimum of 2 |
gke | 0:62a1c91a859a | 92 | if ( BaroRetries >= BARO_INIT_RETRIES ) |
gke | 0:62a1c91a859a | 93 | TxString(" FAILED Init.\r\n"); |
gke | 0:62a1c91a859a | 94 | else |
gke | 0:62a1c91a859a | 95 | TxNextLine(); |
gke | 0:62a1c91a859a | 96 | |
gke | 0:62a1c91a859a | 97 | if ( BaroType == BaroMPX4115 ) { |
gke | 0:62a1c91a859a | 98 | TxString("Range :\t"); |
gke | 0:62a1c91a859a | 99 | TxVal32((int32) BaroDescentAvailable * 10.0, 1, ' '); |
gke | 0:62a1c91a859a | 100 | TxString("-> "); |
gke | 0:62a1c91a859a | 101 | TxVal32((int32) BaroClimbAvailable * 10.0, 1, 'M'); |
gke | 0:62a1c91a859a | 102 | TxString(" {Offset "); |
gke | 0:62a1c91a859a | 103 | TxVal32((int32)BaroOffsetDAC, 0,'}'); |
gke | 0:62a1c91a859a | 104 | if (( BaroClimbAvailable < BARO_MIN_CLIMB ) || (BaroDescentAvailable > BARO_MIN_DESCENT)) |
gke | 0:62a1c91a859a | 105 | TxString(" Bad climb or descent range - offset adjustment?"); |
gke | 0:62a1c91a859a | 106 | TxNextLine(); |
gke | 0:62a1c91a859a | 107 | } |
gke | 0:62a1c91a859a | 108 | |
gke | 0:62a1c91a859a | 109 | if ( !F.BaroAltitudeValid ) goto BAerror; |
gke | 0:62a1c91a859a | 110 | |
gke | 0:62a1c91a859a | 111 | while ( !F.NewBaroValue ) |
gke | 0:62a1c91a859a | 112 | GetBaroAltitude(); |
gke | 0:62a1c91a859a | 113 | F.NewBaroValue = false; |
gke | 0:62a1c91a859a | 114 | |
gke | 0:62a1c91a859a | 115 | TxString("Alt.: \t"); |
gke | 0:62a1c91a859a | 116 | TxVal32(BaroRelAltitude * 10.0, 1, ' '); |
gke | 0:62a1c91a859a | 117 | TxString("M\r\n"); |
gke | 0:62a1c91a859a | 118 | |
gke | 0:62a1c91a859a | 119 | TxString("\r\nR.Finder: \t"); |
gke | 0:62a1c91a859a | 120 | if ( F.RangefinderAltitudeValid ) { |
gke | 0:62a1c91a859a | 121 | GetRangefinderAltitude(); |
gke | 0:62a1c91a859a | 122 | TxVal32(RangefinderAltitude * 100.0, 2, ' '); |
gke | 0:62a1c91a859a | 123 | TxString("M\r\n"); |
gke | 0:62a1c91a859a | 124 | } else |
gke | 0:62a1c91a859a | 125 | TxString("no rangefinder\r\n"); |
gke | 0:62a1c91a859a | 126 | |
gke | 0:62a1c91a859a | 127 | TxString("\r\nAmbient :\t"); |
gke | 0:62a1c91a859a | 128 | TxVal32((int32)AmbientTemperature.i16, 1, ' '); |
gke | 0:62a1c91a859a | 129 | TxString("C\r\n"); |
gke | 0:62a1c91a859a | 130 | |
gke | 0:62a1c91a859a | 131 | return; |
gke | 0:62a1c91a859a | 132 | BAerror: |
gke | 0:62a1c91a859a | 133 | TxString("FAIL\r\n"); |
gke | 0:62a1c91a859a | 134 | } // BaroTest |
gke | 0:62a1c91a859a | 135 | |
gke | 0:62a1c91a859a | 136 | void GetBaroAltitude(void) { |
gke | 0:62a1c91a859a | 137 | static real32 Temp, AltChange; |
gke | 0:62a1c91a859a | 138 | |
gke | 0:62a1c91a859a | 139 | if ( BaroType == BaroMPX4115 ) |
gke | 0:62a1c91a859a | 140 | GetFreescaleBaroAltitude(); |
gke | 0:62a1c91a859a | 141 | else |
gke | 0:62a1c91a859a | 142 | GetBoschBaroAltitude(); |
gke | 0:62a1c91a859a | 143 | |
gke | 0:62a1c91a859a | 144 | if ( F.NewBaroValue ) { |
gke | 0:62a1c91a859a | 145 | #ifdef SIMULATE |
gke | 0:62a1c91a859a | 146 | if ( State == InFlight ) { |
gke | 0:62a1c91a859a | 147 | if ( ++SimulateCycles >= AltitudeUpdateRate ) { |
gke | 0:62a1c91a859a | 148 | FakeBaroRelAltitude += ( DesiredThrottle - CruiseThrottle ) + Comp[Alt]; |
gke | 0:62a1c91a859a | 149 | if ( FakeBaroRelAltitude < -5.0 ) |
gke | 0:62a1c91a859a | 150 | FakeBaroRelAltitude = 0.0; |
gke | 0:62a1c91a859a | 151 | |
gke | 0:62a1c91a859a | 152 | SimulateCycles = 0; |
gke | 0:62a1c91a859a | 153 | |
gke | 0:62a1c91a859a | 154 | ROC = FakeBaroRelAltitude - BaroRelAltitudeP; |
gke | 0:62a1c91a859a | 155 | BaroRelAltitudeP = FakeBaroRelAltitude; |
gke | 0:62a1c91a859a | 156 | } |
gke | 0:62a1c91a859a | 157 | BaroRelAltitude = FakeBaroRelAltitude; |
gke | 0:62a1c91a859a | 158 | } |
gke | 0:62a1c91a859a | 159 | #else |
gke | 0:62a1c91a859a | 160 | |
gke | 0:62a1c91a859a | 161 | AltChange = BaroRelAltitude - BaroRelAltitudeP; |
gke | 0:62a1c91a859a | 162 | Temp = AltChange * AltitudeUpdateRate; |
gke | 0:62a1c91a859a | 163 | |
gke | 0:62a1c91a859a | 164 | if ( fabs( Temp ) > BARO_SANITY_CHECK_MPS ) { |
gke | 0:62a1c91a859a | 165 | BaroRelAltitude = BaroRelAltitudeP; // use previous value |
gke | 0:62a1c91a859a | 166 | Temp = 0; |
gke | 0:62a1c91a859a | 167 | Stats[BaroFailS]++; |
gke | 0:62a1c91a859a | 168 | } |
gke | 0:62a1c91a859a | 169 | |
gke | 0:62a1c91a859a | 170 | Temp = Limit( Temp , -BARO_SANITY_CHECK_MPS, BARO_SANITY_CHECK_MPS ); |
gke | 0:62a1c91a859a | 171 | ROC = ROC * 0.9 + Temp * 0.1; |
gke | 0:62a1c91a859a | 172 | BaroRelAltitudeP = BaroRelAltitude; |
gke | 0:62a1c91a859a | 173 | |
gke | 0:62a1c91a859a | 174 | #endif // SIMULATE |
gke | 0:62a1c91a859a | 175 | |
gke | 0:62a1c91a859a | 176 | if ( State == InFlight ) { |
gke | 0:62a1c91a859a | 177 | if ( ROC > Stats[MaxROCS] ) |
gke | 0:62a1c91a859a | 178 | Stats[MaxROCS] = ROC; |
gke | 0:62a1c91a859a | 179 | else |
gke | 0:62a1c91a859a | 180 | if ( ROC < Stats[MinROCS] ) |
gke | 0:62a1c91a859a | 181 | Stats[MinROCS] = ROC; |
gke | 0:62a1c91a859a | 182 | |
gke | 0:62a1c91a859a | 183 | if ( BaroRelAltitude > Stats[BaroRelAltitudeS] ) |
gke | 0:62a1c91a859a | 184 | Stats[BaroRelAltitudeS] = BaroRelAltitude; |
gke | 0:62a1c91a859a | 185 | } |
gke | 0:62a1c91a859a | 186 | } |
gke | 0:62a1c91a859a | 187 | |
gke | 0:62a1c91a859a | 188 | } // GetBaroAltitude |
gke | 0:62a1c91a859a | 189 | |
gke | 0:62a1c91a859a | 190 | void InitBarometer(void) { |
gke | 0:62a1c91a859a | 191 | BaroRelAltitude = BaroRelAltitudeP = CompBaroPressure = OriginBaroPressure = 0; |
gke | 0:62a1c91a859a | 192 | BaroType = BaroUnknown; |
gke | 0:62a1c91a859a | 193 | |
gke | 0:62a1c91a859a | 194 | Comp[Alt] = AltDiffSum = AltDSum = 0; |
gke | 0:62a1c91a859a | 195 | F.BaroAltitudeValid= true; // optimistic |
gke | 0:62a1c91a859a | 196 | |
gke | 0:62a1c91a859a | 197 | if ( IsFreescaleBaroActive() ) |
gke | 0:62a1c91a859a | 198 | InitFreescaleBarometer(); |
gke | 0:62a1c91a859a | 199 | else |
gke | 0:62a1c91a859a | 200 | if ( IsBoschBaroActive() ) |
gke | 0:62a1c91a859a | 201 | InitBoschBarometer(); |
gke | 0:62a1c91a859a | 202 | else { |
gke | 0:62a1c91a859a | 203 | F.BaroAltitudeValid = F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 204 | Stats[BaroFailS]++; |
gke | 0:62a1c91a859a | 205 | } |
gke | 0:62a1c91a859a | 206 | } // InitBarometer |
gke | 0:62a1c91a859a | 207 | |
gke | 0:62a1c91a859a | 208 | // ----------------------------------------------------------- |
gke | 0:62a1c91a859a | 209 | |
gke | 0:62a1c91a859a | 210 | // Freescale ex Motorola MPX4115 Barometer with ADS7823 12bit ADC |
gke | 0:62a1c91a859a | 211 | |
gke | 0:62a1c91a859a | 212 | void SetFreescaleMCP4725(int16); |
gke | 0:62a1c91a859a | 213 | void SetFreescaleOffset(void); |
gke | 0:62a1c91a859a | 214 | void ReadFreescaleBaro(void); |
gke | 0:62a1c91a859a | 215 | real32 FreescaleToDM(int24); |
gke | 0:62a1c91a859a | 216 | void GetFreescaleBaroAltitude(void); |
gke | 0:62a1c91a859a | 217 | boolean IsFreescaleBaroActive(void); |
gke | 0:62a1c91a859a | 218 | void InitFreescaleBarometer(void); |
gke | 0:62a1c91a859a | 219 | |
gke | 0:62a1c91a859a | 220 | void SetFreescaleMCP4725(int16 d) { |
gke | 0:62a1c91a859a | 221 | static i16u dd; |
gke | 0:62a1c91a859a | 222 | static uint8 r; |
gke | 0:62a1c91a859a | 223 | |
gke | 0:62a1c91a859a | 224 | dd.u16 = d << 4; // left align |
gke | 0:62a1c91a859a | 225 | |
gke | 0:62a1c91a859a | 226 | I2CBARO.start(); |
gke | 0:62a1c91a859a | 227 | r = I2CBARO.write(MCP4725_WR) != I2C_ACK; |
gke | 0:62a1c91a859a | 228 | r = I2CBARO.write(MCP4725_CMD) != I2C_ACK; |
gke | 0:62a1c91a859a | 229 | r = I2CBARO.write(dd.b1) != I2C_ACK; |
gke | 0:62a1c91a859a | 230 | r = I2CBARO.write(dd.b0) != I2C_ACK; |
gke | 0:62a1c91a859a | 231 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 232 | |
gke | 0:62a1c91a859a | 233 | } // SetFreescaleMCP4725 |
gke | 0:62a1c91a859a | 234 | |
gke | 0:62a1c91a859a | 235 | void SetFreescaleOffset(void) { |
gke | 0:62a1c91a859a | 236 | // Steve Westerfeld |
gke | 0:62a1c91a859a | 237 | // 470 Ohm, 1uF RC 0.47mS use 2mS for settling? |
gke | 0:62a1c91a859a | 238 | |
gke | 0:62a1c91a859a | 239 | TxString("\r\nOffset \tPressure\r\n"); |
gke | 0:62a1c91a859a | 240 | |
gke | 0:62a1c91a859a | 241 | BaroOffsetDAC = MCP4725_MAX; |
gke | 0:62a1c91a859a | 242 | |
gke | 0:62a1c91a859a | 243 | SetFreescaleMCP4725(BaroOffsetDAC); |
gke | 0:62a1c91a859a | 244 | |
gke | 0:62a1c91a859a | 245 | Delay1mS(20); // initial settling |
gke | 0:62a1c91a859a | 246 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 247 | |
gke | 0:62a1c91a859a | 248 | while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*7L)/10L) ) |
gke | 0:62a1c91a859a | 249 | && (BaroOffsetDAC > 20) ) { // first loop gets close |
gke | 0:62a1c91a859a | 250 | BaroOffsetDAC -= 20; // approach at 20 steps out of 4095 |
gke | 0:62a1c91a859a | 251 | SetFreescaleMCP4725(BaroOffsetDAC); |
gke | 0:62a1c91a859a | 252 | Delay1mS(20); |
gke | 0:62a1c91a859a | 253 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 254 | TxVal32(BaroOffsetDAC,0,HT); |
gke | 0:62a1c91a859a | 255 | TxVal32(BaroVal.u16,0,' '); |
gke | 0:62a1c91a859a | 256 | TxNextLine(); |
gke | 0:62a1c91a859a | 257 | LEDYellow_TOG; |
gke | 0:62a1c91a859a | 258 | } |
gke | 0:62a1c91a859a | 259 | |
gke | 0:62a1c91a859a | 260 | BaroOffsetDAC += 20; // move back up to come at it a little slower |
gke | 0:62a1c91a859a | 261 | SetFreescaleMCP4725(BaroOffsetDAC); |
gke | 0:62a1c91a859a | 262 | Delay1mS(100); |
gke | 0:62a1c91a859a | 263 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 264 | |
gke | 0:62a1c91a859a | 265 | while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 2) ) { |
gke | 0:62a1c91a859a | 266 | BaroOffsetDAC -= 2; |
gke | 0:62a1c91a859a | 267 | SetFreescaleMCP4725(BaroOffsetDAC); |
gke | 0:62a1c91a859a | 268 | Delay1mS(10); |
gke | 0:62a1c91a859a | 269 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 270 | TxVal32(BaroOffsetDAC,0,HT); |
gke | 0:62a1c91a859a | 271 | TxVal32(BaroVal.u16,0,' '); |
gke | 0:62a1c91a859a | 272 | TxNextLine(); |
gke | 0:62a1c91a859a | 273 | LEDYellow_TOG; |
gke | 0:62a1c91a859a | 274 | } |
gke | 0:62a1c91a859a | 275 | |
gke | 0:62a1c91a859a | 276 | Delay1mS(200); // wait for caps to settle |
gke | 0:62a1c91a859a | 277 | F.BaroAltitudeValid = BaroOffsetDAC > 0; |
gke | 0:62a1c91a859a | 278 | |
gke | 0:62a1c91a859a | 279 | } // SetFreescaleOffset |
gke | 0:62a1c91a859a | 280 | |
gke | 0:62a1c91a859a | 281 | void ReadFreescaleBaro(void) { |
gke | 0:62a1c91a859a | 282 | static char B[8]; |
gke | 0:62a1c91a859a | 283 | static i16u B0, B1, B2, B3; |
gke | 0:62a1c91a859a | 284 | |
gke | 0:62a1c91a859a | 285 | mS[BaroUpdate] = mSClock() + ADS7823_TIME_MS; |
gke | 0:62a1c91a859a | 286 | |
gke | 0:62a1c91a859a | 287 | I2CBARO.start(); // start conversion |
gke | 0:62a1c91a859a | 288 | |
gke | 0:62a1c91a859a | 289 | if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto FSError; |
gke | 0:62a1c91a859a | 290 | if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto FSError; |
gke | 0:62a1c91a859a | 291 | |
gke | 0:62a1c91a859a | 292 | I2CBARO.read(ADS7823_RD, B, 8); // read block of 4 baro samples |
gke | 0:62a1c91a859a | 293 | |
gke | 0:62a1c91a859a | 294 | B0.b0 = B[1]; |
gke | 0:62a1c91a859a | 295 | B0.b1 = B[0]; |
gke | 0:62a1c91a859a | 296 | B1.b0 = B[3]; |
gke | 0:62a1c91a859a | 297 | B1.b1 = B[2]; |
gke | 0:62a1c91a859a | 298 | B2.b0 = B[5]; |
gke | 0:62a1c91a859a | 299 | B2.b1 = B[4]; |
gke | 0:62a1c91a859a | 300 | B3.b0 = B[7]; |
gke | 0:62a1c91a859a | 301 | B3.b1 = B[6]; |
gke | 0:62a1c91a859a | 302 | |
gke | 0:62a1c91a859a | 303 | BaroVal.u16 = (uint16)16380 - ( B0.u16 + B1.u16 + B2.u16 + B3.u16 ); |
gke | 0:62a1c91a859a | 304 | |
gke | 0:62a1c91a859a | 305 | F.BaroAltitudeValid = true; |
gke | 0:62a1c91a859a | 306 | |
gke | 0:62a1c91a859a | 307 | return; |
gke | 0:62a1c91a859a | 308 | |
gke | 0:62a1c91a859a | 309 | FSError: |
gke | 0:62a1c91a859a | 310 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 311 | |
gke | 0:62a1c91a859a | 312 | F.BaroAltitudeValid = F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 313 | if ( State == InFlight ) { |
gke | 0:62a1c91a859a | 314 | Stats[BaroFailS]++; |
gke | 0:62a1c91a859a | 315 | F.BaroFailure = true; |
gke | 0:62a1c91a859a | 316 | } |
gke | 0:62a1c91a859a | 317 | return; |
gke | 0:62a1c91a859a | 318 | } // ReadFreescaleBaro |
gke | 0:62a1c91a859a | 319 | |
gke | 0:62a1c91a859a | 320 | real32 FreescaleToDM(int24 p) { // decreasing pressure is increase in altitude negate and rescale to metre altitude |
gke | 0:62a1c91a859a | 321 | return( -( (real32)p * 0.8 ) / (real32)P[BaroScale] ); |
gke | 0:62a1c91a859a | 322 | } // FreescaleToDM |
gke | 0:62a1c91a859a | 323 | |
gke | 0:62a1c91a859a | 324 | void GetFreescaleBaroAltitude(void) { |
gke | 0:62a1c91a859a | 325 | static int24 BaroPressure; |
gke | 0:62a1c91a859a | 326 | |
gke | 0:62a1c91a859a | 327 | if ( mSClock() >= mS[BaroUpdate] ) { |
gke | 0:62a1c91a859a | 328 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 329 | if ( F.BaroAltitudeValid ) { |
gke | 0:62a1c91a859a | 330 | BaroPressure = (int24)BaroVal.u16; // sum of 4 samples |
gke | 0:62a1c91a859a | 331 | |
gke | 0:62a1c91a859a | 332 | BaroRelAltitude = FreescaleToDM(BaroPressure - OriginBaroPressure); |
gke | 0:62a1c91a859a | 333 | |
gke | 0:62a1c91a859a | 334 | F.NewBaroValue = F.BaroAltitudeValid; |
gke | 0:62a1c91a859a | 335 | } |
gke | 0:62a1c91a859a | 336 | } |
gke | 0:62a1c91a859a | 337 | |
gke | 0:62a1c91a859a | 338 | } // GetFreescaleBaroAltitude |
gke | 0:62a1c91a859a | 339 | |
gke | 0:62a1c91a859a | 340 | boolean IsFreescaleBaroActive(void) { // check for Freescale Barometer |
gke | 0:62a1c91a859a | 341 | |
gke | 0:62a1c91a859a | 342 | I2CBARO.start(); |
gke | 0:62a1c91a859a | 343 | if ( I2CBARO.write(ADS7823_ID) != I2C_ACK ) goto FreescaleInactive; |
gke | 0:62a1c91a859a | 344 | |
gke | 0:62a1c91a859a | 345 | BaroType = BaroMPX4115; |
gke | 0:62a1c91a859a | 346 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 347 | |
gke | 0:62a1c91a859a | 348 | TrackMinI2CRate(400000); |
gke | 0:62a1c91a859a | 349 | |
gke | 0:62a1c91a859a | 350 | return(true); |
gke | 0:62a1c91a859a | 351 | |
gke | 0:62a1c91a859a | 352 | FreescaleInactive: |
gke | 0:62a1c91a859a | 353 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 354 | return(false); |
gke | 0:62a1c91a859a | 355 | |
gke | 0:62a1c91a859a | 356 | } // IsFreescaleBaroActive |
gke | 0:62a1c91a859a | 357 | |
gke | 0:62a1c91a859a | 358 | void InitFreescaleBarometer(void) { |
gke | 0:62a1c91a859a | 359 | static int16 BaroOriginAltitude, MinAltitude; |
gke | 0:62a1c91a859a | 360 | real32 Error; |
gke | 0:62a1c91a859a | 361 | static int24 BaroPressureP; |
gke | 0:62a1c91a859a | 362 | |
gke | 0:62a1c91a859a | 363 | AltitudeUpdateRate = 1000L/ADS7823_TIME_MS; |
gke | 0:62a1c91a859a | 364 | |
gke | 0:62a1c91a859a | 365 | BaroTemperature = 0; |
gke | 0:62a1c91a859a | 366 | Error = ( (int16)P[BaroScale] * 20 ) / 16; // 0.2M |
gke | 0:62a1c91a859a | 367 | BaroPressure = 0; |
gke | 0:62a1c91a859a | 368 | |
gke | 0:62a1c91a859a | 369 | BaroRetries = 0; |
gke | 0:62a1c91a859a | 370 | do { |
gke | 0:62a1c91a859a | 371 | BaroPressureP = BaroPressure; |
gke | 0:62a1c91a859a | 372 | |
gke | 0:62a1c91a859a | 373 | SetFreescaleOffset(); |
gke | 0:62a1c91a859a | 374 | |
gke | 0:62a1c91a859a | 375 | while ( mSClock() < mS[BaroUpdate] ) {}; |
gke | 0:62a1c91a859a | 376 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 377 | BaroPressure = (int24)BaroVal.u16; |
gke | 0:62a1c91a859a | 378 | } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) |
gke | 0:62a1c91a859a | 379 | && ( abs((int16)(BaroPressure - BaroPressureP)) > Error ) ); |
gke | 0:62a1c91a859a | 380 | |
gke | 0:62a1c91a859a | 381 | F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES; |
gke | 0:62a1c91a859a | 382 | |
gke | 0:62a1c91a859a | 383 | OriginBaroPressure = BaroPressure; |
gke | 0:62a1c91a859a | 384 | |
gke | 0:62a1c91a859a | 385 | BaroRelAltitudeP = BaroRelAltitude = 0.0; |
gke | 0:62a1c91a859a | 386 | |
gke | 0:62a1c91a859a | 387 | MinAltitude = FreescaleToDM((int24)ADS7823_MAX*4); |
gke | 0:62a1c91a859a | 388 | BaroOriginAltitude = FreescaleToDM(OriginBaroPressure); |
gke | 0:62a1c91a859a | 389 | BaroDescentAvailable = MinAltitude - BaroOriginAltitude; |
gke | 0:62a1c91a859a | 390 | BaroClimbAvailable = -BaroOriginAltitude; |
gke | 0:62a1c91a859a | 391 | |
gke | 0:62a1c91a859a | 392 | //F.BaroAltitudeValid &= (( BaroClimbAvailable >= BARO_MIN_CLIMB ) |
gke | 0:62a1c91a859a | 393 | // && (BaroDescentAvailable <= BARO_MIN_DESCENT)); |
gke | 0:62a1c91a859a | 394 | |
gke | 0:62a1c91a859a | 395 | #ifdef SIMULATE |
gke | 0:62a1c91a859a | 396 | FakeBaroRelAltitude = 0; |
gke | 0:62a1c91a859a | 397 | #endif // SIMULATE |
gke | 0:62a1c91a859a | 398 | |
gke | 0:62a1c91a859a | 399 | } // InitFreescaleBarometer |
gke | 0:62a1c91a859a | 400 | |
gke | 0:62a1c91a859a | 401 | // ----------------------------------------------------------- |
gke | 0:62a1c91a859a | 402 | |
gke | 0:62a1c91a859a | 403 | // Bosch SMD500 and BMP085 Barometers |
gke | 0:62a1c91a859a | 404 | |
gke | 0:62a1c91a859a | 405 | void StartBoschBaroADC(boolean); |
gke | 0:62a1c91a859a | 406 | int24 CompensatedBoschPressure(uint16, uint16); |
gke | 0:62a1c91a859a | 407 | |
gke | 0:62a1c91a859a | 408 | void GetBoschBaroAltitude(void); |
gke | 0:62a1c91a859a | 409 | boolean IsBoschBaroActive(void); |
gke | 0:62a1c91a859a | 410 | void InitBoschBarometer(void); |
gke | 0:62a1c91a859a | 411 | |
gke | 0:62a1c91a859a | 412 | // SMD500 9.5mS (T) 34mS (P) |
gke | 0:62a1c91a859a | 413 | // BMP085 4.5mS (T) 25.5mS (P) OSRS=3 |
gke | 0:62a1c91a859a | 414 | #define BOSCH_TEMP_TIME_MS 11 // 10 increase to make P+T acq time ~50mS |
gke | 0:62a1c91a859a | 415 | //#define BMP085_PRESS_TIME_MS 26 |
gke | 0:62a1c91a859a | 416 | //#define SMD500_PRESS_TIME_MS 34 |
gke | 0:62a1c91a859a | 417 | #define BOSCH_PRESS_TIME_MS 38 |
gke | 0:62a1c91a859a | 418 | #define BOSCH_PRESS_TEMP_TIME_MS 50 // pressure and temp time + overheads |
gke | 0:62a1c91a859a | 419 | |
gke | 0:62a1c91a859a | 420 | void StartBoschBaroADC(boolean ReadPressure) { |
gke | 0:62a1c91a859a | 421 | static uint8 TempOrPress; |
gke | 0:62a1c91a859a | 422 | |
gke | 0:62a1c91a859a | 423 | if ( ReadPressure ) { |
gke | 0:62a1c91a859a | 424 | TempOrPress = BOSCH_PRESS; |
gke | 0:62a1c91a859a | 425 | mS[BaroUpdate] = mSClock() + BOSCH_PRESS_TIME_MS; |
gke | 0:62a1c91a859a | 426 | } else { |
gke | 0:62a1c91a859a | 427 | mS[BaroUpdate] = mSClock() + BOSCH_TEMP_TIME_MS; |
gke | 0:62a1c91a859a | 428 | if ( BaroType == BaroBMP085 ) |
gke | 0:62a1c91a859a | 429 | TempOrPress = BOSCH_TEMP_BMP085; |
gke | 0:62a1c91a859a | 430 | else |
gke | 0:62a1c91a859a | 431 | TempOrPress = BOSCH_TEMP_SMD500; |
gke | 0:62a1c91a859a | 432 | } |
gke | 0:62a1c91a859a | 433 | |
gke | 0:62a1c91a859a | 434 | I2CBARO.start(); |
gke | 0:62a1c91a859a | 435 | if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto SBerror; |
gke | 0:62a1c91a859a | 436 | |
gke | 0:62a1c91a859a | 437 | // access control register, start measurement |
gke | 0:62a1c91a859a | 438 | if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto SBerror; |
gke | 0:62a1c91a859a | 439 | |
gke | 0:62a1c91a859a | 440 | // select 32kHz input, measure temperature |
gke | 0:62a1c91a859a | 441 | if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto SBerror; |
gke | 0:62a1c91a859a | 442 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 443 | |
gke | 0:62a1c91a859a | 444 | F.BaroAltitudeValid = true; |
gke | 0:62a1c91a859a | 445 | return; |
gke | 0:62a1c91a859a | 446 | |
gke | 0:62a1c91a859a | 447 | SBerror: |
gke | 0:62a1c91a859a | 448 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 449 | F.BaroAltitudeValid = F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 450 | return; |
gke | 0:62a1c91a859a | 451 | } // StartBoschBaroADC |
gke | 0:62a1c91a859a | 452 | |
gke | 0:62a1c91a859a | 453 | void ReadBoschBaro(void) { |
gke | 0:62a1c91a859a | 454 | // Possible I2C protocol error - split read of ADC |
gke | 0:62a1c91a859a | 455 | I2CBARO.start(); |
gke | 0:62a1c91a859a | 456 | if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 457 | if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 458 | I2CBARO.start(); // restart |
gke | 0:62a1c91a859a | 459 | if ( I2CBARO.write(BOSCH_ID+1) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 460 | BaroVal.b1 = I2CBARO.read(I2C_NACK); |
gke | 0:62a1c91a859a | 461 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 462 | |
gke | 0:62a1c91a859a | 463 | I2CBARO.start(); |
gke | 0:62a1c91a859a | 464 | if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 465 | if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 466 | I2CBARO.start(); // restart |
gke | 0:62a1c91a859a | 467 | if ( I2CBARO.write(BOSCH_ID+1) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 468 | BaroVal.b0 = I2CBARO.read(I2C_NACK); |
gke | 0:62a1c91a859a | 469 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 470 | |
gke | 0:62a1c91a859a | 471 | F.BaroAltitudeValid = true; |
gke | 0:62a1c91a859a | 472 | return; |
gke | 0:62a1c91a859a | 473 | |
gke | 0:62a1c91a859a | 474 | RVerror: |
gke | 0:62a1c91a859a | 475 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 476 | |
gke | 0:62a1c91a859a | 477 | F.BaroAltitudeValid = F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 478 | if ( State == InFlight ) { |
gke | 0:62a1c91a859a | 479 | Stats[BaroFailS]++; |
gke | 0:62a1c91a859a | 480 | F.BaroFailure = true; |
gke | 0:62a1c91a859a | 481 | } |
gke | 0:62a1c91a859a | 482 | return; |
gke | 0:62a1c91a859a | 483 | } // ReadBoschBaro |
gke | 0:62a1c91a859a | 484 | |
gke | 0:62a1c91a859a | 485 | #define BOSCH_BMP085_TEMP_COEFF 62L |
gke | 0:62a1c91a859a | 486 | #define BOSCH_SMD500_TEMP_COEFF 50L |
gke | 0:62a1c91a859a | 487 | |
gke | 0:62a1c91a859a | 488 | int24 CompensatedBoschPressure(uint16 BaroPress, uint16 BaroTemp) { |
gke | 0:62a1c91a859a | 489 | static int24 BaroTempComp; |
gke | 0:62a1c91a859a | 490 | |
gke | 0:62a1c91a859a | 491 | if ( BaroType == BaroBMP085 ) |
gke | 0:62a1c91a859a | 492 | BaroTempComp = (BaroTemp * BOSCH_BMP085_TEMP_COEFF + 64L) >> 7; |
gke | 0:62a1c91a859a | 493 | else |
gke | 0:62a1c91a859a | 494 | BaroTempComp = (BaroTemp * BOSCH_SMD500_TEMP_COEFF + 8L) >> 4; |
gke | 0:62a1c91a859a | 495 | |
gke | 0:62a1c91a859a | 496 | return ((int24)BaroPress + BaroTempComp - OriginBaroPressure); |
gke | 0:62a1c91a859a | 497 | |
gke | 0:62a1c91a859a | 498 | } // CompensatedBoschPressure |
gke | 0:62a1c91a859a | 499 | |
gke | 0:62a1c91a859a | 500 | void GetBoschBaroAltitude(void) { |
gke | 0:62a1c91a859a | 501 | static int24 Temp; |
gke | 0:62a1c91a859a | 502 | |
gke | 0:62a1c91a859a | 503 | if ( mSClock() >= mS[BaroUpdate] ) { |
gke | 0:62a1c91a859a | 504 | ReadBoschBaro(); |
gke | 0:62a1c91a859a | 505 | if ( F.BaroAltitudeValid ) |
gke | 0:62a1c91a859a | 506 | if ( AcquiringPressure ) { |
gke | 0:62a1c91a859a | 507 | BaroPressure = (int24)BaroVal.u16; |
gke | 0:62a1c91a859a | 508 | AcquiringPressure = false; |
gke | 0:62a1c91a859a | 509 | } else { |
gke | 0:62a1c91a859a | 510 | BaroTemperature = (int24)BaroVal.u16; |
gke | 0:62a1c91a859a | 511 | AcquiringPressure = true; |
gke | 0:62a1c91a859a | 512 | |
gke | 0:62a1c91a859a | 513 | Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature); |
gke | 0:62a1c91a859a | 514 | CompBaroPressure -= BaroQ.B[BaroQ.Head]; |
gke | 0:62a1c91a859a | 515 | BaroQ.B[BaroQ.Head] = Temp; |
gke | 0:62a1c91a859a | 516 | CompBaroPressure += Temp; |
gke | 0:62a1c91a859a | 517 | BaroQ.Head = (BaroQ.Head + 1) & (BARO_BUFF_SIZE -1); |
gke | 0:62a1c91a859a | 518 | |
gke | 0:62a1c91a859a | 519 | // Pressure queue has 4 entries corresponding to an average delay at 20Hz of 0.1Sec |
gke | 0:62a1c91a859a | 520 | // decreasing pressure is increase in altitude negate and rescale to decimetre altitude |
gke | 0:62a1c91a859a | 521 | |
gke | 0:62a1c91a859a | 522 | BaroRelAltitude = - ( (real32)CompBaroPressure * (real32)P[BaroScale] ) / 1280.0; |
gke | 0:62a1c91a859a | 523 | |
gke | 0:62a1c91a859a | 524 | F.NewBaroValue = F.BaroAltitudeValid; |
gke | 0:62a1c91a859a | 525 | } |
gke | 0:62a1c91a859a | 526 | else { |
gke | 0:62a1c91a859a | 527 | AcquiringPressure = true; |
gke | 0:62a1c91a859a | 528 | Stats[BaroFailS]++; |
gke | 0:62a1c91a859a | 529 | } |
gke | 0:62a1c91a859a | 530 | |
gke | 0:62a1c91a859a | 531 | StartBoschBaroADC(AcquiringPressure); |
gke | 0:62a1c91a859a | 532 | } |
gke | 0:62a1c91a859a | 533 | } // GetBoschBaroAltitude |
gke | 0:62a1c91a859a | 534 | |
gke | 0:62a1c91a859a | 535 | boolean IsBoschBaroActive(void) { // check for Bosch Barometers |
gke | 0:62a1c91a859a | 536 | static uint8 r; |
gke | 0:62a1c91a859a | 537 | |
gke | 0:62a1c91a859a | 538 | I2CBARO.start(); |
gke | 0:62a1c91a859a | 539 | if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto BoschInactive; |
gke | 0:62a1c91a859a | 540 | if ( I2CBARO.write(BOSCH_TYPE) != I2C_ACK ) goto BoschInactive; |
gke | 0:62a1c91a859a | 541 | I2CBARO.start(); // restart |
gke | 0:62a1c91a859a | 542 | if ( I2CBARO.write(BOSCH_ID+1) != I2C_ACK ) goto BoschInactive; |
gke | 0:62a1c91a859a | 543 | r = I2CBARO.read(I2C_NACK); |
gke | 0:62a1c91a859a | 544 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 545 | |
gke | 0:62a1c91a859a | 546 | if (r == BOSCH_ID_BMP085 ) |
gke | 0:62a1c91a859a | 547 | BaroType = BaroBMP085; |
gke | 0:62a1c91a859a | 548 | else |
gke | 0:62a1c91a859a | 549 | BaroType = BaroSMD500; |
gke | 0:62a1c91a859a | 550 | |
gke | 0:62a1c91a859a | 551 | TrackMinI2CRate(400000); |
gke | 0:62a1c91a859a | 552 | |
gke | 0:62a1c91a859a | 553 | return(true); |
gke | 0:62a1c91a859a | 554 | |
gke | 0:62a1c91a859a | 555 | BoschInactive: |
gke | 0:62a1c91a859a | 556 | return(false); |
gke | 0:62a1c91a859a | 557 | |
gke | 0:62a1c91a859a | 558 | } // IsBoschBaroActive |
gke | 0:62a1c91a859a | 559 | |
gke | 0:62a1c91a859a | 560 | void InitBoschBarometer(void) { |
gke | 0:62a1c91a859a | 561 | int8 s; |
gke | 0:62a1c91a859a | 562 | int24 Temp, CompBaroPressureP; |
gke | 0:62a1c91a859a | 563 | |
gke | 0:62a1c91a859a | 564 | AltitudeUpdateRate = 1000L / BOSCH_PRESS_TEMP_TIME_MS; |
gke | 0:62a1c91a859a | 565 | |
gke | 0:62a1c91a859a | 566 | F.NewBaroValue = false; |
gke | 0:62a1c91a859a | 567 | CompBaroPressure = 0; |
gke | 0:62a1c91a859a | 568 | |
gke | 0:62a1c91a859a | 569 | TxString("Temp. \tPressure\r\n"); |
gke | 0:62a1c91a859a | 570 | |
gke | 0:62a1c91a859a | 571 | BaroRetries = 0; |
gke | 0:62a1c91a859a | 572 | do { // occasional I2C misread of Temperature so keep doing it until the Origin is stable!! |
gke | 0:62a1c91a859a | 573 | CompBaroPressureP = CompBaroPressure; |
gke | 0:62a1c91a859a | 574 | CompBaroPressure = BaroQ.Head = 0; |
gke | 0:62a1c91a859a | 575 | |
gke | 0:62a1c91a859a | 576 | AcquiringPressure = true; |
gke | 0:62a1c91a859a | 577 | StartBoschBaroADC(AcquiringPressure); // Pressure |
gke | 0:62a1c91a859a | 578 | |
gke | 0:62a1c91a859a | 579 | for ( s = 0; s < 4; s++ ) { |
gke | 0:62a1c91a859a | 580 | while ( mSClock() < mS[BaroUpdate] ); |
gke | 0:62a1c91a859a | 581 | ReadBoschBaro(); // Pressure |
gke | 0:62a1c91a859a | 582 | BaroPressure = BaroVal.u16; |
gke | 0:62a1c91a859a | 583 | |
gke | 0:62a1c91a859a | 584 | AcquiringPressure = !AcquiringPressure; |
gke | 0:62a1c91a859a | 585 | StartBoschBaroADC(AcquiringPressure); // Temperature |
gke | 0:62a1c91a859a | 586 | while ( mSClock() < mS[BaroUpdate] ); |
gke | 0:62a1c91a859a | 587 | ReadBoschBaro(); |
gke | 0:62a1c91a859a | 588 | BaroTemperature = BaroVal.u16; |
gke | 0:62a1c91a859a | 589 | |
gke | 0:62a1c91a859a | 590 | TxVal32(BaroTemperature,0,HT); |
gke | 0:62a1c91a859a | 591 | TxVal32(BaroPressure,0,0); |
gke | 0:62a1c91a859a | 592 | TxNextLine(); |
gke | 0:62a1c91a859a | 593 | |
gke | 0:62a1c91a859a | 594 | Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature); |
gke | 0:62a1c91a859a | 595 | BaroQ.B[s] = Temp; |
gke | 0:62a1c91a859a | 596 | CompBaroPressure += Temp; |
gke | 0:62a1c91a859a | 597 | |
gke | 0:62a1c91a859a | 598 | AcquiringPressure = !AcquiringPressure; |
gke | 0:62a1c91a859a | 599 | StartBoschBaroADC(AcquiringPressure); |
gke | 0:62a1c91a859a | 600 | } |
gke | 0:62a1c91a859a | 601 | |
gke | 0:62a1c91a859a | 602 | } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) && ( abs(CompBaroPressure - CompBaroPressureP) > 12 ) ); // stable within ~0.5M |
gke | 0:62a1c91a859a | 603 | |
gke | 0:62a1c91a859a | 604 | OriginBaroPressure = SRS32(CompBaroPressure, 2); |
gke | 0:62a1c91a859a | 605 | |
gke | 0:62a1c91a859a | 606 | F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES; |
gke | 0:62a1c91a859a | 607 | BaroRelAltitudeP = BaroRelAltitude = 0.0; |
gke | 0:62a1c91a859a | 608 | |
gke | 0:62a1c91a859a | 609 | #ifdef SIMULATE |
gke | 0:62a1c91a859a | 610 | FakeBaroRelAltitude = 0.0; |
gke | 0:62a1c91a859a | 611 | #endif // SIMULATE |
gke | 0:62a1c91a859a | 612 | |
gke | 0:62a1c91a859a | 613 | } // InitBoschBarometer |