Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
baro.c@1:1e3318a30ddd, 2011-02-25 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 25 01:35:24 2011 +0000
- Revision:
- 1:1e3318a30ddd
- Parent:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.
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 | 1:1e3318a30ddd | 127 | TxString("\r\nUAVXArm Shield:\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 | 1:1e3318a30ddd | 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 | 1:1e3318a30ddd | 213 | boolean IdentifyMCP4725(void); |
gke | 0:62a1c91a859a | 214 | void SetFreescaleOffset(void); |
gke | 0:62a1c91a859a | 215 | void ReadFreescaleBaro(void); |
gke | 0:62a1c91a859a | 216 | real32 FreescaleToDM(int24); |
gke | 0:62a1c91a859a | 217 | void GetFreescaleBaroAltitude(void); |
gke | 0:62a1c91a859a | 218 | boolean IsFreescaleBaroActive(void); |
gke | 0:62a1c91a859a | 219 | void InitFreescaleBarometer(void); |
gke | 0:62a1c91a859a | 220 | |
gke | 1:1e3318a30ddd | 221 | uint8 MCP4725_ID_Actual; |
gke | 1:1e3318a30ddd | 222 | |
gke | 0:62a1c91a859a | 223 | void SetFreescaleMCP4725(int16 d) { |
gke | 0:62a1c91a859a | 224 | static i16u dd; |
gke | 0:62a1c91a859a | 225 | static uint8 r; |
gke | 0:62a1c91a859a | 226 | |
gke | 0:62a1c91a859a | 227 | dd.u16 = d << 4; // left align |
gke | 0:62a1c91a859a | 228 | |
gke | 0:62a1c91a859a | 229 | I2CBARO.start(); |
gke | 1:1e3318a30ddd | 230 | r = I2CBARO.write(MCP4725_ID_Actual) != I2C_ACK; |
gke | 0:62a1c91a859a | 231 | r = I2CBARO.write(MCP4725_CMD) != I2C_ACK; |
gke | 0:62a1c91a859a | 232 | r = I2CBARO.write(dd.b1) != I2C_ACK; |
gke | 0:62a1c91a859a | 233 | r = I2CBARO.write(dd.b0) != I2C_ACK; |
gke | 0:62a1c91a859a | 234 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 235 | |
gke | 0:62a1c91a859a | 236 | } // SetFreescaleMCP4725 |
gke | 0:62a1c91a859a | 237 | |
gke | 1:1e3318a30ddd | 238 | boolean IdentifyMCP4725(void) { |
gke | 1:1e3318a30ddd | 239 | |
gke | 1:1e3318a30ddd | 240 | static boolean r; |
gke | 1:1e3318a30ddd | 241 | |
gke | 1:1e3318a30ddd | 242 | r = true; |
gke | 1:1e3318a30ddd | 243 | if ( I2CBAROAddressResponds( MCP4725_ID_0xC8 ) ) |
gke | 1:1e3318a30ddd | 244 | MCP4725_ID_Actual = MCP4725_ID_0xC8; |
gke | 1:1e3318a30ddd | 245 | else |
gke | 1:1e3318a30ddd | 246 | if ( I2CBAROAddressResponds( MCP4725_ID_0xCC ) ) |
gke | 1:1e3318a30ddd | 247 | MCP4725_ID_Actual = MCP4725_ID_0xCC; |
gke | 1:1e3318a30ddd | 248 | else |
gke | 1:1e3318a30ddd | 249 | r = false; |
gke | 1:1e3318a30ddd | 250 | return(r); |
gke | 1:1e3318a30ddd | 251 | } // IdentifyMCP4725 |
gke | 1:1e3318a30ddd | 252 | |
gke | 0:62a1c91a859a | 253 | void SetFreescaleOffset(void) { |
gke | 0:62a1c91a859a | 254 | // Steve Westerfeld |
gke | 0:62a1c91a859a | 255 | // 470 Ohm, 1uF RC 0.47mS use 2mS for settling? |
gke | 0:62a1c91a859a | 256 | |
gke | 0:62a1c91a859a | 257 | TxString("\r\nOffset \tPressure\r\n"); |
gke | 0:62a1c91a859a | 258 | |
gke | 0:62a1c91a859a | 259 | BaroOffsetDAC = MCP4725_MAX; |
gke | 0:62a1c91a859a | 260 | |
gke | 0:62a1c91a859a | 261 | SetFreescaleMCP4725(BaroOffsetDAC); |
gke | 0:62a1c91a859a | 262 | |
gke | 0:62a1c91a859a | 263 | Delay1mS(20); // initial settling |
gke | 0:62a1c91a859a | 264 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 265 | |
gke | 0:62a1c91a859a | 266 | while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*7L)/10L) ) |
gke | 0:62a1c91a859a | 267 | && (BaroOffsetDAC > 20) ) { // first loop gets close |
gke | 0:62a1c91a859a | 268 | BaroOffsetDAC -= 20; // approach at 20 steps out of 4095 |
gke | 0:62a1c91a859a | 269 | SetFreescaleMCP4725(BaroOffsetDAC); |
gke | 0:62a1c91a859a | 270 | Delay1mS(20); |
gke | 0:62a1c91a859a | 271 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 272 | TxVal32(BaroOffsetDAC,0,HT); |
gke | 0:62a1c91a859a | 273 | TxVal32(BaroVal.u16,0,' '); |
gke | 0:62a1c91a859a | 274 | TxNextLine(); |
gke | 0:62a1c91a859a | 275 | LEDYellow_TOG; |
gke | 0:62a1c91a859a | 276 | } |
gke | 0:62a1c91a859a | 277 | |
gke | 0:62a1c91a859a | 278 | BaroOffsetDAC += 20; // move back up to come at it a little slower |
gke | 0:62a1c91a859a | 279 | SetFreescaleMCP4725(BaroOffsetDAC); |
gke | 0:62a1c91a859a | 280 | Delay1mS(100); |
gke | 0:62a1c91a859a | 281 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 282 | |
gke | 0:62a1c91a859a | 283 | while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 2) ) { |
gke | 0:62a1c91a859a | 284 | BaroOffsetDAC -= 2; |
gke | 0:62a1c91a859a | 285 | SetFreescaleMCP4725(BaroOffsetDAC); |
gke | 0:62a1c91a859a | 286 | Delay1mS(10); |
gke | 0:62a1c91a859a | 287 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 288 | TxVal32(BaroOffsetDAC,0,HT); |
gke | 0:62a1c91a859a | 289 | TxVal32(BaroVal.u16,0,' '); |
gke | 0:62a1c91a859a | 290 | TxNextLine(); |
gke | 0:62a1c91a859a | 291 | LEDYellow_TOG; |
gke | 0:62a1c91a859a | 292 | } |
gke | 0:62a1c91a859a | 293 | |
gke | 0:62a1c91a859a | 294 | Delay1mS(200); // wait for caps to settle |
gke | 0:62a1c91a859a | 295 | F.BaroAltitudeValid = BaroOffsetDAC > 0; |
gke | 0:62a1c91a859a | 296 | |
gke | 0:62a1c91a859a | 297 | } // SetFreescaleOffset |
gke | 0:62a1c91a859a | 298 | |
gke | 0:62a1c91a859a | 299 | void ReadFreescaleBaro(void) { |
gke | 0:62a1c91a859a | 300 | static char B[8]; |
gke | 0:62a1c91a859a | 301 | static i16u B0, B1, B2, B3; |
gke | 0:62a1c91a859a | 302 | |
gke | 0:62a1c91a859a | 303 | mS[BaroUpdate] = mSClock() + ADS7823_TIME_MS; |
gke | 0:62a1c91a859a | 304 | |
gke | 0:62a1c91a859a | 305 | I2CBARO.start(); // start conversion |
gke | 0:62a1c91a859a | 306 | |
gke | 0:62a1c91a859a | 307 | if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto FSError; |
gke | 0:62a1c91a859a | 308 | if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto FSError; |
gke | 0:62a1c91a859a | 309 | |
gke | 1:1e3318a30ddd | 310 | I2CBARO.blockread(ADS7823_RD, B, 8); // read block of 4 baro samples |
gke | 0:62a1c91a859a | 311 | |
gke | 0:62a1c91a859a | 312 | B0.b0 = B[1]; |
gke | 0:62a1c91a859a | 313 | B0.b1 = B[0]; |
gke | 0:62a1c91a859a | 314 | B1.b0 = B[3]; |
gke | 0:62a1c91a859a | 315 | B1.b1 = B[2]; |
gke | 0:62a1c91a859a | 316 | B2.b0 = B[5]; |
gke | 0:62a1c91a859a | 317 | B2.b1 = B[4]; |
gke | 0:62a1c91a859a | 318 | B3.b0 = B[7]; |
gke | 0:62a1c91a859a | 319 | B3.b1 = B[6]; |
gke | 0:62a1c91a859a | 320 | |
gke | 0:62a1c91a859a | 321 | BaroVal.u16 = (uint16)16380 - ( B0.u16 + B1.u16 + B2.u16 + B3.u16 ); |
gke | 0:62a1c91a859a | 322 | |
gke | 0:62a1c91a859a | 323 | F.BaroAltitudeValid = true; |
gke | 0:62a1c91a859a | 324 | |
gke | 0:62a1c91a859a | 325 | return; |
gke | 0:62a1c91a859a | 326 | |
gke | 0:62a1c91a859a | 327 | FSError: |
gke | 0:62a1c91a859a | 328 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 329 | |
gke | 0:62a1c91a859a | 330 | F.BaroAltitudeValid = F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 331 | if ( State == InFlight ) { |
gke | 0:62a1c91a859a | 332 | Stats[BaroFailS]++; |
gke | 0:62a1c91a859a | 333 | F.BaroFailure = true; |
gke | 0:62a1c91a859a | 334 | } |
gke | 0:62a1c91a859a | 335 | return; |
gke | 0:62a1c91a859a | 336 | } // ReadFreescaleBaro |
gke | 0:62a1c91a859a | 337 | |
gke | 0:62a1c91a859a | 338 | real32 FreescaleToDM(int24 p) { // decreasing pressure is increase in altitude negate and rescale to metre altitude |
gke | 0:62a1c91a859a | 339 | return( -( (real32)p * 0.8 ) / (real32)P[BaroScale] ); |
gke | 0:62a1c91a859a | 340 | } // FreescaleToDM |
gke | 0:62a1c91a859a | 341 | |
gke | 0:62a1c91a859a | 342 | void GetFreescaleBaroAltitude(void) { |
gke | 0:62a1c91a859a | 343 | static int24 BaroPressure; |
gke | 0:62a1c91a859a | 344 | |
gke | 0:62a1c91a859a | 345 | if ( mSClock() >= mS[BaroUpdate] ) { |
gke | 0:62a1c91a859a | 346 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 347 | if ( F.BaroAltitudeValid ) { |
gke | 0:62a1c91a859a | 348 | BaroPressure = (int24)BaroVal.u16; // sum of 4 samples |
gke | 0:62a1c91a859a | 349 | BaroRelAltitude = FreescaleToDM(BaroPressure - OriginBaroPressure); |
gke | 0:62a1c91a859a | 350 | F.NewBaroValue = F.BaroAltitudeValid; |
gke | 0:62a1c91a859a | 351 | } |
gke | 0:62a1c91a859a | 352 | } |
gke | 0:62a1c91a859a | 353 | |
gke | 0:62a1c91a859a | 354 | } // GetFreescaleBaroAltitude |
gke | 0:62a1c91a859a | 355 | |
gke | 0:62a1c91a859a | 356 | boolean IsFreescaleBaroActive(void) { // check for Freescale Barometer |
gke | 0:62a1c91a859a | 357 | |
gke | 1:1e3318a30ddd | 358 | static boolean r; |
gke | 0:62a1c91a859a | 359 | |
gke | 1:1e3318a30ddd | 360 | r = I2CBAROAddressResponds( ADS7823_ID ); |
gke | 1:1e3318a30ddd | 361 | if ( r ) { |
gke | 1:1e3318a30ddd | 362 | BaroType = BaroMPX4115; |
gke | 1:1e3318a30ddd | 363 | r = IdentifyMCP4725(); |
gke | 1:1e3318a30ddd | 364 | TrackMinI2CRate(400000); |
gke | 1:1e3318a30ddd | 365 | } |
gke | 1:1e3318a30ddd | 366 | return (r); |
gke | 0:62a1c91a859a | 367 | |
gke | 0:62a1c91a859a | 368 | } // IsFreescaleBaroActive |
gke | 0:62a1c91a859a | 369 | |
gke | 0:62a1c91a859a | 370 | void InitFreescaleBarometer(void) { |
gke | 0:62a1c91a859a | 371 | static int16 BaroOriginAltitude, MinAltitude; |
gke | 0:62a1c91a859a | 372 | real32 Error; |
gke | 0:62a1c91a859a | 373 | static int24 BaroPressureP; |
gke | 0:62a1c91a859a | 374 | |
gke | 0:62a1c91a859a | 375 | AltitudeUpdateRate = 1000L/ADS7823_TIME_MS; |
gke | 0:62a1c91a859a | 376 | |
gke | 0:62a1c91a859a | 377 | BaroTemperature = 0; |
gke | 0:62a1c91a859a | 378 | Error = ( (int16)P[BaroScale] * 20 ) / 16; // 0.2M |
gke | 0:62a1c91a859a | 379 | BaroPressure = 0; |
gke | 0:62a1c91a859a | 380 | |
gke | 0:62a1c91a859a | 381 | BaroRetries = 0; |
gke | 0:62a1c91a859a | 382 | do { |
gke | 0:62a1c91a859a | 383 | BaroPressureP = BaroPressure; |
gke | 0:62a1c91a859a | 384 | |
gke | 0:62a1c91a859a | 385 | SetFreescaleOffset(); |
gke | 0:62a1c91a859a | 386 | |
gke | 0:62a1c91a859a | 387 | while ( mSClock() < mS[BaroUpdate] ) {}; |
gke | 0:62a1c91a859a | 388 | ReadFreescaleBaro(); |
gke | 0:62a1c91a859a | 389 | BaroPressure = (int24)BaroVal.u16; |
gke | 0:62a1c91a859a | 390 | } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) |
gke | 0:62a1c91a859a | 391 | && ( abs((int16)(BaroPressure - BaroPressureP)) > Error ) ); |
gke | 0:62a1c91a859a | 392 | |
gke | 0:62a1c91a859a | 393 | F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES; |
gke | 0:62a1c91a859a | 394 | |
gke | 0:62a1c91a859a | 395 | OriginBaroPressure = BaroPressure; |
gke | 0:62a1c91a859a | 396 | |
gke | 0:62a1c91a859a | 397 | BaroRelAltitudeP = BaroRelAltitude = 0.0; |
gke | 0:62a1c91a859a | 398 | |
gke | 0:62a1c91a859a | 399 | MinAltitude = FreescaleToDM((int24)ADS7823_MAX*4); |
gke | 0:62a1c91a859a | 400 | BaroOriginAltitude = FreescaleToDM(OriginBaroPressure); |
gke | 0:62a1c91a859a | 401 | BaroDescentAvailable = MinAltitude - BaroOriginAltitude; |
gke | 0:62a1c91a859a | 402 | BaroClimbAvailable = -BaroOriginAltitude; |
gke | 0:62a1c91a859a | 403 | |
gke | 0:62a1c91a859a | 404 | //F.BaroAltitudeValid &= (( BaroClimbAvailable >= BARO_MIN_CLIMB ) |
gke | 0:62a1c91a859a | 405 | // && (BaroDescentAvailable <= BARO_MIN_DESCENT)); |
gke | 0:62a1c91a859a | 406 | |
gke | 0:62a1c91a859a | 407 | #ifdef SIMULATE |
gke | 0:62a1c91a859a | 408 | FakeBaroRelAltitude = 0; |
gke | 0:62a1c91a859a | 409 | #endif // SIMULATE |
gke | 0:62a1c91a859a | 410 | |
gke | 0:62a1c91a859a | 411 | } // InitFreescaleBarometer |
gke | 0:62a1c91a859a | 412 | |
gke | 0:62a1c91a859a | 413 | // ----------------------------------------------------------- |
gke | 0:62a1c91a859a | 414 | |
gke | 0:62a1c91a859a | 415 | // Bosch SMD500 and BMP085 Barometers |
gke | 0:62a1c91a859a | 416 | |
gke | 0:62a1c91a859a | 417 | void StartBoschBaroADC(boolean); |
gke | 0:62a1c91a859a | 418 | int24 CompensatedBoschPressure(uint16, uint16); |
gke | 0:62a1c91a859a | 419 | |
gke | 0:62a1c91a859a | 420 | void GetBoschBaroAltitude(void); |
gke | 0:62a1c91a859a | 421 | boolean IsBoschBaroActive(void); |
gke | 0:62a1c91a859a | 422 | void InitBoschBarometer(void); |
gke | 0:62a1c91a859a | 423 | |
gke | 0:62a1c91a859a | 424 | // SMD500 9.5mS (T) 34mS (P) |
gke | 0:62a1c91a859a | 425 | // BMP085 4.5mS (T) 25.5mS (P) OSRS=3 |
gke | 0:62a1c91a859a | 426 | #define BOSCH_TEMP_TIME_MS 11 // 10 increase to make P+T acq time ~50mS |
gke | 0:62a1c91a859a | 427 | //#define BMP085_PRESS_TIME_MS 26 |
gke | 0:62a1c91a859a | 428 | //#define SMD500_PRESS_TIME_MS 34 |
gke | 0:62a1c91a859a | 429 | #define BOSCH_PRESS_TIME_MS 38 |
gke | 0:62a1c91a859a | 430 | #define BOSCH_PRESS_TEMP_TIME_MS 50 // pressure and temp time + overheads |
gke | 0:62a1c91a859a | 431 | |
gke | 0:62a1c91a859a | 432 | void StartBoschBaroADC(boolean ReadPressure) { |
gke | 0:62a1c91a859a | 433 | static uint8 TempOrPress; |
gke | 0:62a1c91a859a | 434 | |
gke | 0:62a1c91a859a | 435 | if ( ReadPressure ) { |
gke | 0:62a1c91a859a | 436 | TempOrPress = BOSCH_PRESS; |
gke | 0:62a1c91a859a | 437 | mS[BaroUpdate] = mSClock() + BOSCH_PRESS_TIME_MS; |
gke | 0:62a1c91a859a | 438 | } else { |
gke | 0:62a1c91a859a | 439 | mS[BaroUpdate] = mSClock() + BOSCH_TEMP_TIME_MS; |
gke | 0:62a1c91a859a | 440 | if ( BaroType == BaroBMP085 ) |
gke | 0:62a1c91a859a | 441 | TempOrPress = BOSCH_TEMP_BMP085; |
gke | 0:62a1c91a859a | 442 | else |
gke | 0:62a1c91a859a | 443 | TempOrPress = BOSCH_TEMP_SMD500; |
gke | 0:62a1c91a859a | 444 | } |
gke | 0:62a1c91a859a | 445 | |
gke | 0:62a1c91a859a | 446 | I2CBARO.start(); |
gke | 0:62a1c91a859a | 447 | if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto SBerror; |
gke | 0:62a1c91a859a | 448 | |
gke | 0:62a1c91a859a | 449 | // access control register, start measurement |
gke | 0:62a1c91a859a | 450 | if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto SBerror; |
gke | 0:62a1c91a859a | 451 | |
gke | 0:62a1c91a859a | 452 | // select 32kHz input, measure temperature |
gke | 0:62a1c91a859a | 453 | if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto SBerror; |
gke | 0:62a1c91a859a | 454 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 455 | |
gke | 0:62a1c91a859a | 456 | F.BaroAltitudeValid = true; |
gke | 0:62a1c91a859a | 457 | return; |
gke | 0:62a1c91a859a | 458 | |
gke | 0:62a1c91a859a | 459 | SBerror: |
gke | 0:62a1c91a859a | 460 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 461 | F.BaroAltitudeValid = F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 462 | return; |
gke | 0:62a1c91a859a | 463 | } // StartBoschBaroADC |
gke | 0:62a1c91a859a | 464 | |
gke | 0:62a1c91a859a | 465 | void ReadBoschBaro(void) { |
gke | 0:62a1c91a859a | 466 | // Possible I2C protocol error - split read of ADC |
gke | 0:62a1c91a859a | 467 | I2CBARO.start(); |
gke | 1:1e3318a30ddd | 468 | if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 469 | if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 470 | I2CBARO.start(); // restart |
gke | 1:1e3318a30ddd | 471 | if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 472 | BaroVal.b1 = I2CBARO.read(I2C_NACK); |
gke | 0:62a1c91a859a | 473 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 474 | |
gke | 0:62a1c91a859a | 475 | I2CBARO.start(); |
gke | 1:1e3318a30ddd | 476 | if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 477 | if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 478 | I2CBARO.start(); // restart |
gke | 1:1e3318a30ddd | 479 | if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto RVerror; |
gke | 0:62a1c91a859a | 480 | BaroVal.b0 = I2CBARO.read(I2C_NACK); |
gke | 0:62a1c91a859a | 481 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 482 | |
gke | 0:62a1c91a859a | 483 | F.BaroAltitudeValid = true; |
gke | 0:62a1c91a859a | 484 | return; |
gke | 0:62a1c91a859a | 485 | |
gke | 0:62a1c91a859a | 486 | RVerror: |
gke | 0:62a1c91a859a | 487 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 488 | |
gke | 0:62a1c91a859a | 489 | F.BaroAltitudeValid = F.HoldingAlt = false; |
gke | 0:62a1c91a859a | 490 | if ( State == InFlight ) { |
gke | 0:62a1c91a859a | 491 | Stats[BaroFailS]++; |
gke | 0:62a1c91a859a | 492 | F.BaroFailure = true; |
gke | 0:62a1c91a859a | 493 | } |
gke | 0:62a1c91a859a | 494 | return; |
gke | 0:62a1c91a859a | 495 | } // ReadBoschBaro |
gke | 0:62a1c91a859a | 496 | |
gke | 0:62a1c91a859a | 497 | #define BOSCH_BMP085_TEMP_COEFF 62L |
gke | 0:62a1c91a859a | 498 | #define BOSCH_SMD500_TEMP_COEFF 50L |
gke | 0:62a1c91a859a | 499 | |
gke | 0:62a1c91a859a | 500 | int24 CompensatedBoschPressure(uint16 BaroPress, uint16 BaroTemp) { |
gke | 0:62a1c91a859a | 501 | static int24 BaroTempComp; |
gke | 0:62a1c91a859a | 502 | |
gke | 0:62a1c91a859a | 503 | if ( BaroType == BaroBMP085 ) |
gke | 0:62a1c91a859a | 504 | BaroTempComp = (BaroTemp * BOSCH_BMP085_TEMP_COEFF + 64L) >> 7; |
gke | 0:62a1c91a859a | 505 | else |
gke | 0:62a1c91a859a | 506 | BaroTempComp = (BaroTemp * BOSCH_SMD500_TEMP_COEFF + 8L) >> 4; |
gke | 0:62a1c91a859a | 507 | |
gke | 0:62a1c91a859a | 508 | return ((int24)BaroPress + BaroTempComp - OriginBaroPressure); |
gke | 0:62a1c91a859a | 509 | |
gke | 0:62a1c91a859a | 510 | } // CompensatedBoschPressure |
gke | 0:62a1c91a859a | 511 | |
gke | 0:62a1c91a859a | 512 | void GetBoschBaroAltitude(void) { |
gke | 0:62a1c91a859a | 513 | static int24 Temp; |
gke | 0:62a1c91a859a | 514 | |
gke | 0:62a1c91a859a | 515 | if ( mSClock() >= mS[BaroUpdate] ) { |
gke | 0:62a1c91a859a | 516 | ReadBoschBaro(); |
gke | 0:62a1c91a859a | 517 | if ( F.BaroAltitudeValid ) |
gke | 0:62a1c91a859a | 518 | if ( AcquiringPressure ) { |
gke | 0:62a1c91a859a | 519 | BaroPressure = (int24)BaroVal.u16; |
gke | 0:62a1c91a859a | 520 | AcquiringPressure = false; |
gke | 0:62a1c91a859a | 521 | } else { |
gke | 0:62a1c91a859a | 522 | BaroTemperature = (int24)BaroVal.u16; |
gke | 0:62a1c91a859a | 523 | AcquiringPressure = true; |
gke | 0:62a1c91a859a | 524 | |
gke | 0:62a1c91a859a | 525 | Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature); |
gke | 0:62a1c91a859a | 526 | CompBaroPressure -= BaroQ.B[BaroQ.Head]; |
gke | 0:62a1c91a859a | 527 | BaroQ.B[BaroQ.Head] = Temp; |
gke | 0:62a1c91a859a | 528 | CompBaroPressure += Temp; |
gke | 0:62a1c91a859a | 529 | BaroQ.Head = (BaroQ.Head + 1) & (BARO_BUFF_SIZE -1); |
gke | 0:62a1c91a859a | 530 | |
gke | 0:62a1c91a859a | 531 | // Pressure queue has 4 entries corresponding to an average delay at 20Hz of 0.1Sec |
gke | 0:62a1c91a859a | 532 | // decreasing pressure is increase in altitude negate and rescale to decimetre altitude |
gke | 0:62a1c91a859a | 533 | |
gke | 0:62a1c91a859a | 534 | BaroRelAltitude = - ( (real32)CompBaroPressure * (real32)P[BaroScale] ) / 1280.0; |
gke | 0:62a1c91a859a | 535 | |
gke | 0:62a1c91a859a | 536 | F.NewBaroValue = F.BaroAltitudeValid; |
gke | 0:62a1c91a859a | 537 | } |
gke | 0:62a1c91a859a | 538 | else { |
gke | 0:62a1c91a859a | 539 | AcquiringPressure = true; |
gke | 0:62a1c91a859a | 540 | Stats[BaroFailS]++; |
gke | 0:62a1c91a859a | 541 | } |
gke | 0:62a1c91a859a | 542 | |
gke | 0:62a1c91a859a | 543 | StartBoschBaroADC(AcquiringPressure); |
gke | 0:62a1c91a859a | 544 | } |
gke | 0:62a1c91a859a | 545 | } // GetBoschBaroAltitude |
gke | 0:62a1c91a859a | 546 | |
gke | 0:62a1c91a859a | 547 | boolean IsBoschBaroActive(void) { // check for Bosch Barometers |
gke | 0:62a1c91a859a | 548 | static uint8 r; |
gke | 0:62a1c91a859a | 549 | |
gke | 0:62a1c91a859a | 550 | I2CBARO.start(); |
gke | 1:1e3318a30ddd | 551 | if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschInactive; |
gke | 0:62a1c91a859a | 552 | if ( I2CBARO.write(BOSCH_TYPE) != I2C_ACK ) goto BoschInactive; |
gke | 0:62a1c91a859a | 553 | I2CBARO.start(); // restart |
gke | 1:1e3318a30ddd | 554 | if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschInactive; |
gke | 0:62a1c91a859a | 555 | r = I2CBARO.read(I2C_NACK); |
gke | 0:62a1c91a859a | 556 | I2CBARO.stop(); |
gke | 0:62a1c91a859a | 557 | |
gke | 0:62a1c91a859a | 558 | if (r == BOSCH_ID_BMP085 ) |
gke | 0:62a1c91a859a | 559 | BaroType = BaroBMP085; |
gke | 0:62a1c91a859a | 560 | else |
gke | 0:62a1c91a859a | 561 | BaroType = BaroSMD500; |
gke | 1:1e3318a30ddd | 562 | |
gke | 0:62a1c91a859a | 563 | TrackMinI2CRate(400000); |
gke | 0:62a1c91a859a | 564 | |
gke | 0:62a1c91a859a | 565 | return(true); |
gke | 0:62a1c91a859a | 566 | |
gke | 0:62a1c91a859a | 567 | BoschInactive: |
gke | 0:62a1c91a859a | 568 | return(false); |
gke | 0:62a1c91a859a | 569 | |
gke | 0:62a1c91a859a | 570 | } // IsBoschBaroActive |
gke | 0:62a1c91a859a | 571 | |
gke | 0:62a1c91a859a | 572 | void InitBoschBarometer(void) { |
gke | 0:62a1c91a859a | 573 | int8 s; |
gke | 0:62a1c91a859a | 574 | int24 Temp, CompBaroPressureP; |
gke | 0:62a1c91a859a | 575 | |
gke | 0:62a1c91a859a | 576 | AltitudeUpdateRate = 1000L / BOSCH_PRESS_TEMP_TIME_MS; |
gke | 0:62a1c91a859a | 577 | |
gke | 0:62a1c91a859a | 578 | F.NewBaroValue = false; |
gke | 0:62a1c91a859a | 579 | CompBaroPressure = 0; |
gke | 0:62a1c91a859a | 580 | |
gke | 0:62a1c91a859a | 581 | TxString("Temp. \tPressure\r\n"); |
gke | 0:62a1c91a859a | 582 | |
gke | 0:62a1c91a859a | 583 | BaroRetries = 0; |
gke | 0:62a1c91a859a | 584 | do { // occasional I2C misread of Temperature so keep doing it until the Origin is stable!! |
gke | 0:62a1c91a859a | 585 | CompBaroPressureP = CompBaroPressure; |
gke | 0:62a1c91a859a | 586 | CompBaroPressure = BaroQ.Head = 0; |
gke | 0:62a1c91a859a | 587 | |
gke | 0:62a1c91a859a | 588 | AcquiringPressure = true; |
gke | 0:62a1c91a859a | 589 | StartBoschBaroADC(AcquiringPressure); // Pressure |
gke | 0:62a1c91a859a | 590 | |
gke | 0:62a1c91a859a | 591 | for ( s = 0; s < 4; s++ ) { |
gke | 0:62a1c91a859a | 592 | while ( mSClock() < mS[BaroUpdate] ); |
gke | 0:62a1c91a859a | 593 | ReadBoschBaro(); // Pressure |
gke | 0:62a1c91a859a | 594 | BaroPressure = BaroVal.u16; |
gke | 0:62a1c91a859a | 595 | |
gke | 0:62a1c91a859a | 596 | AcquiringPressure = !AcquiringPressure; |
gke | 0:62a1c91a859a | 597 | StartBoschBaroADC(AcquiringPressure); // Temperature |
gke | 0:62a1c91a859a | 598 | while ( mSClock() < mS[BaroUpdate] ); |
gke | 0:62a1c91a859a | 599 | ReadBoschBaro(); |
gke | 0:62a1c91a859a | 600 | BaroTemperature = BaroVal.u16; |
gke | 0:62a1c91a859a | 601 | |
gke | 0:62a1c91a859a | 602 | TxVal32(BaroTemperature,0,HT); |
gke | 0:62a1c91a859a | 603 | TxVal32(BaroPressure,0,0); |
gke | 0:62a1c91a859a | 604 | TxNextLine(); |
gke | 0:62a1c91a859a | 605 | |
gke | 0:62a1c91a859a | 606 | Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature); |
gke | 0:62a1c91a859a | 607 | BaroQ.B[s] = Temp; |
gke | 0:62a1c91a859a | 608 | CompBaroPressure += Temp; |
gke | 0:62a1c91a859a | 609 | |
gke | 0:62a1c91a859a | 610 | AcquiringPressure = !AcquiringPressure; |
gke | 0:62a1c91a859a | 611 | StartBoschBaroADC(AcquiringPressure); |
gke | 0:62a1c91a859a | 612 | } |
gke | 0:62a1c91a859a | 613 | |
gke | 0:62a1c91a859a | 614 | } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) && ( abs(CompBaroPressure - CompBaroPressureP) > 12 ) ); // stable within ~0.5M |
gke | 0:62a1c91a859a | 615 | |
gke | 0:62a1c91a859a | 616 | OriginBaroPressure = SRS32(CompBaroPressure, 2); |
gke | 0:62a1c91a859a | 617 | |
gke | 0:62a1c91a859a | 618 | F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES; |
gke | 0:62a1c91a859a | 619 | BaroRelAltitudeP = BaroRelAltitude = 0.0; |
gke | 0:62a1c91a859a | 620 | |
gke | 0:62a1c91a859a | 621 | #ifdef SIMULATE |
gke | 0:62a1c91a859a | 622 | FakeBaroRelAltitude = 0.0; |
gke | 0:62a1c91a859a | 623 | #endif // SIMULATE |
gke | 0:62a1c91a859a | 624 | |
gke | 0:62a1c91a859a | 625 | } // InitBoschBarometer |