Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

baro.c@2:90292f8bd179, 2011-04-26 (annotated)

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
1:1e3318a30ddd 
Not flightworthy. Posted for others to make use of the I2C SW code.

Who changed what in which revision?

UserRevisionLine numberNew 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 2:90292f8bd179 5 // = http://code.google.com/p/uavp-mods/ =
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 2:90292f8bd179 78
gke 0:62a1c91a859a 79 TxString("\r\nAltitude test\r\n");
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 2:90292f8bd179 90 TxString("BaroScale:\t");
gke 2:90292f8bd179 91 TxVal32(P[BaroScale],0,0);
gke 2:90292f8bd179 92 TxString("\r\nInit Retries:\t");
gke 0:62a1c91a859a 93 TxVal32((int32)BaroRetries - 2, 0, ' '); // always minimum of 2
gke 0:62a1c91a859a 94 if ( BaroRetries >= BARO_INIT_RETRIES )
gke 0:62a1c91a859a 95 TxString(" FAILED Init.\r\n");
gke 0:62a1c91a859a 96 else
gke 0:62a1c91a859a 97 TxNextLine();
gke 0:62a1c91a859a 98
gke 0:62a1c91a859a 99 if ( BaroType == BaroMPX4115 ) {
gke 2:90292f8bd179 100 TxString("\r\nAddress :\t0x");
gke 2:90292f8bd179 101 TxValH(MCP4725_ID_Actual);
gke 2:90292f8bd179 102 TxString("\r\nRange :\t");
gke 0:62a1c91a859a 103 TxVal32((int32) BaroDescentAvailable * 10.0, 1, ' ');
gke 0:62a1c91a859a 104 TxString("-> ");
gke 0:62a1c91a859a 105 TxVal32((int32) BaroClimbAvailable * 10.0, 1, 'M');
gke 0:62a1c91a859a 106 TxString(" {Offset ");
gke 0:62a1c91a859a 107 TxVal32((int32)BaroOffsetDAC, 0,'}');
gke 0:62a1c91a859a 108 if (( BaroClimbAvailable < BARO_MIN_CLIMB ) || (BaroDescentAvailable > BARO_MIN_DESCENT))
gke 0:62a1c91a859a 109 TxString(" Bad climb or descent range - offset adjustment?");
gke 0:62a1c91a859a 110 TxNextLine();
gke 0:62a1c91a859a 111 }
gke 0:62a1c91a859a 112
gke 2:90292f8bd179 113 if ( F.BaroAltitudeValid ) {
gke 2:90292f8bd179 114
gke 2:90292f8bd179 115 while ( !F.NewBaroValue )
gke 2:90292f8bd179 116 GetBaroAltitude();
gke 2:90292f8bd179 117
gke 2:90292f8bd179 118 F.NewBaroValue = false;
gke 0:62a1c91a859a 119
gke 2:90292f8bd179 120 TxString("Alt.: \t");
gke 2:90292f8bd179 121 TxVal32(BaroRelAltitude * 10.0, 1, ' ');
gke 2:90292f8bd179 122 TxString("M\r\n");
gke 0:62a1c91a859a 123
gke 2:90292f8bd179 124 } else
gke 2:90292f8bd179 125 TxString("Barometer FAILED\r\n");
gke 0:62a1c91a859a 126
gke 0:62a1c91a859a 127 TxString("\r\nR.Finder: \t");
gke 0:62a1c91a859a 128 if ( F.RangefinderAltitudeValid ) {
gke 0:62a1c91a859a 129 GetRangefinderAltitude();
gke 0:62a1c91a859a 130 TxVal32(RangefinderAltitude * 100.0, 2, ' ');
gke 0:62a1c91a859a 131 TxString("M\r\n");
gke 0:62a1c91a859a 132 } else
gke 0:62a1c91a859a 133 TxString("no rangefinder\r\n");
gke 0:62a1c91a859a 134
gke 1:1e3318a30ddd 135 TxString("\r\nUAVXArm Shield:\t");
gke 0:62a1c91a859a 136 TxVal32((int32)AmbientTemperature.i16, 1, ' ');
gke 0:62a1c91a859a 137 TxString("C\r\n");
gke 0:62a1c91a859a 138
gke 0:62a1c91a859a 139 } // BaroTest
gke 0:62a1c91a859a 140
gke 0:62a1c91a859a 141 void GetBaroAltitude(void) {
gke 0:62a1c91a859a 142 static real32 Temp, AltChange;
gke 0:62a1c91a859a 143
gke 0:62a1c91a859a 144 if ( BaroType == BaroMPX4115 )
gke 0:62a1c91a859a 145 GetFreescaleBaroAltitude();
gke 0:62a1c91a859a 146 else
gke 0:62a1c91a859a 147 GetBoschBaroAltitude();
gke 0:62a1c91a859a 148
gke 0:62a1c91a859a 149 if ( F.NewBaroValue ) {
gke 0:62a1c91a859a 150 #ifdef SIMULATE
gke 0:62a1c91a859a 151 if ( State == InFlight ) {
gke 0:62a1c91a859a 152 if ( ++SimulateCycles >= AltitudeUpdateRate ) {
gke 0:62a1c91a859a 153 FakeBaroRelAltitude += ( DesiredThrottle - CruiseThrottle ) + Comp[Alt];
gke 0:62a1c91a859a 154 if ( FakeBaroRelAltitude < -5.0 )
gke 0:62a1c91a859a 155 FakeBaroRelAltitude = 0.0;
gke 0:62a1c91a859a 156
gke 0:62a1c91a859a 157 SimulateCycles = 0;
gke 0:62a1c91a859a 158
gke 0:62a1c91a859a 159 ROC = FakeBaroRelAltitude - BaroRelAltitudeP;
gke 0:62a1c91a859a 160 BaroRelAltitudeP = FakeBaroRelAltitude;
gke 0:62a1c91a859a 161 }
gke 0:62a1c91a859a 162 BaroRelAltitude = FakeBaroRelAltitude;
gke 0:62a1c91a859a 163 }
gke 0:62a1c91a859a 164 #else
gke 0:62a1c91a859a 165
gke 0:62a1c91a859a 166 AltChange = BaroRelAltitude - BaroRelAltitudeP;
gke 0:62a1c91a859a 167 Temp = AltChange * AltitudeUpdateRate;
gke 0:62a1c91a859a 168
gke 0:62a1c91a859a 169 if ( fabs( Temp ) > BARO_SANITY_CHECK_MPS ) {
gke 0:62a1c91a859a 170 BaroRelAltitude = BaroRelAltitudeP; // use previous value
gke 0:62a1c91a859a 171 Temp = 0;
gke 0:62a1c91a859a 172 Stats[BaroFailS]++;
gke 0:62a1c91a859a 173 }
gke 0:62a1c91a859a 174
gke 2:90292f8bd179 175 Temp = Limit1(Temp, BARO_SANITY_CHECK_MPS);
gke 0:62a1c91a859a 176 ROC = ROC * 0.9 + Temp * 0.1;
gke 0:62a1c91a859a 177 BaroRelAltitudeP = BaroRelAltitude;
gke 0:62a1c91a859a 178
gke 0:62a1c91a859a 179 #endif // SIMULATE
gke 0:62a1c91a859a 180
gke 0:62a1c91a859a 181 if ( State == InFlight ) {
gke 0:62a1c91a859a 182 if ( ROC > Stats[MaxROCS] )
gke 0:62a1c91a859a 183 Stats[MaxROCS] = ROC;
gke 0:62a1c91a859a 184 else
gke 0:62a1c91a859a 185 if ( ROC < Stats[MinROCS] )
gke 0:62a1c91a859a 186 Stats[MinROCS] = ROC;
gke 0:62a1c91a859a 187
gke 0:62a1c91a859a 188 if ( BaroRelAltitude > Stats[BaroRelAltitudeS] )
gke 0:62a1c91a859a 189 Stats[BaroRelAltitudeS] = BaroRelAltitude;
gke 0:62a1c91a859a 190 }
gke 0:62a1c91a859a 191 }
gke 0:62a1c91a859a 192
gke 0:62a1c91a859a 193 } // GetBaroAltitude
gke 0:62a1c91a859a 194
gke 0:62a1c91a859a 195 void InitBarometer(void) {
gke 0:62a1c91a859a 196 BaroRelAltitude = BaroRelAltitudeP = CompBaroPressure = OriginBaroPressure = 0;
gke 0:62a1c91a859a 197 BaroType = BaroUnknown;
gke 0:62a1c91a859a 198
gke 2:90292f8bd179 199 AltComp = AltDiffSum = AltDSum = 0;
gke 0:62a1c91a859a 200 F.BaroAltitudeValid= true; // optimistic
gke 2:90292f8bd179 201
gke 0:62a1c91a859a 202 if ( IsFreescaleBaroActive() )
gke 0:62a1c91a859a 203 InitFreescaleBarometer();
gke 0:62a1c91a859a 204 else
gke 0:62a1c91a859a 205 if ( IsBoschBaroActive() )
gke 0:62a1c91a859a 206 InitBoschBarometer();
gke 2:90292f8bd179 207 else
gke 0:62a1c91a859a 208 F.BaroAltitudeValid = F.HoldingAlt = false;
gke 2:90292f8bd179 209
gke 0:62a1c91a859a 210 } // InitBarometer
gke 0:62a1c91a859a 211
gke 0:62a1c91a859a 212 // -----------------------------------------------------------
gke 0:62a1c91a859a 213
gke 0:62a1c91a859a 214 // Freescale ex Motorola MPX4115 Barometer with ADS7823 12bit ADC
gke 0:62a1c91a859a 215
gke 0:62a1c91a859a 216 void SetFreescaleMCP4725(int16);
gke 1:1e3318a30ddd 217 boolean IdentifyMCP4725(void);
gke 0:62a1c91a859a 218 void SetFreescaleOffset(void);
gke 0:62a1c91a859a 219 void ReadFreescaleBaro(void);
gke 0:62a1c91a859a 220 real32 FreescaleToDM(int24);
gke 0:62a1c91a859a 221 void GetFreescaleBaroAltitude(void);
gke 0:62a1c91a859a 222 boolean IsFreescaleBaroActive(void);
gke 0:62a1c91a859a 223 void InitFreescaleBarometer(void);
gke 0:62a1c91a859a 224
gke 1:1e3318a30ddd 225 uint8 MCP4725_ID_Actual;
gke 1:1e3318a30ddd 226
gke 0:62a1c91a859a 227 void SetFreescaleMCP4725(int16 d) {
gke 0:62a1c91a859a 228 static i16u dd;
gke 0:62a1c91a859a 229
gke 0:62a1c91a859a 230 dd.u16 = d << 4; // left align
gke 0:62a1c91a859a 231
gke 0:62a1c91a859a 232 I2CBARO.start();
gke 2:90292f8bd179 233 if ( I2CBARO.write(MCP4725_ID_Actual) != I2C_ACK ) goto MCP4725Error;
gke 2:90292f8bd179 234 if ( I2CBARO.write(MCP4725_CMD) != I2C_ACK ) goto MCP4725Error;
gke 2:90292f8bd179 235 if ( I2CBARO.write(dd.b1) != I2C_ACK ) goto MCP4725Error;
gke 2:90292f8bd179 236 if ( I2CBARO.write(dd.b0) != I2C_ACK ) goto MCP4725Error;
gke 0:62a1c91a859a 237 I2CBARO.stop();
gke 0:62a1c91a859a 238
gke 2:90292f8bd179 239 return;
gke 2:90292f8bd179 240
gke 2:90292f8bd179 241 MCP4725Error:
gke 2:90292f8bd179 242 I2CBARO.stop();
gke 2:90292f8bd179 243 I2CError[MCP4725_ID_Actual]++;
gke 2:90292f8bd179 244
gke 0:62a1c91a859a 245 } // SetFreescaleMCP4725
gke 0:62a1c91a859a 246
gke 1:1e3318a30ddd 247 boolean IdentifyMCP4725(void) {
gke 1:1e3318a30ddd 248
gke 1:1e3318a30ddd 249 static boolean r;
gke 2:90292f8bd179 250
gke 1:1e3318a30ddd 251 r = true;
gke 2:90292f8bd179 252 MCP4725_ID_Actual = MCP4725_ID_0xCC;
gke 2:90292f8bd179 253 if ( I2CBAROAddressResponds( MCP4725_ID_0xCC ) )
gke 2:90292f8bd179 254 MCP4725_ID_Actual = MCP4725_ID_0xCC;
gke 2:90292f8bd179 255 else
gke 2:90292f8bd179 256 if ( I2CBAROAddressResponds( MCP4725_ID_0xC8 ) )
gke 2:90292f8bd179 257 MCP4725_ID_Actual = MCP4725_ID_0xC8;
gke 1:1e3318a30ddd 258 else
gke 1:1e3318a30ddd 259 r = false;
gke 2:90292f8bd179 260
gke 2:90292f8bd179 261 return(r);
gke 2:90292f8bd179 262
gke 2:90292f8bd179 263 // MCP4725_ID_Actual = FORCE_BARO_ID;
gke 2:90292f8bd179 264 // return(true);
gke 2:90292f8bd179 265
gke 1:1e3318a30ddd 266 } // IdentifyMCP4725
gke 1:1e3318a30ddd 267
gke 0:62a1c91a859a 268 void SetFreescaleOffset(void) {
gke 0:62a1c91a859a 269 // Steve Westerfeld
gke 0:62a1c91a859a 270 // 470 Ohm, 1uF RC 0.47mS use 2mS for settling?
gke 0:62a1c91a859a 271
gke 0:62a1c91a859a 272 TxString("\r\nOffset \tPressure\r\n");
gke 0:62a1c91a859a 273
gke 0:62a1c91a859a 274 BaroOffsetDAC = MCP4725_MAX;
gke 0:62a1c91a859a 275
gke 0:62a1c91a859a 276 SetFreescaleMCP4725(BaroOffsetDAC);
gke 0:62a1c91a859a 277
gke 0:62a1c91a859a 278 Delay1mS(20); // initial settling
gke 0:62a1c91a859a 279 ReadFreescaleBaro();
gke 0:62a1c91a859a 280
gke 0:62a1c91a859a 281 while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*7L)/10L) )
gke 0:62a1c91a859a 282 && (BaroOffsetDAC > 20) ) { // first loop gets close
gke 0:62a1c91a859a 283 BaroOffsetDAC -= 20; // approach at 20 steps out of 4095
gke 0:62a1c91a859a 284 SetFreescaleMCP4725(BaroOffsetDAC);
gke 0:62a1c91a859a 285 Delay1mS(20);
gke 0:62a1c91a859a 286 ReadFreescaleBaro();
gke 0:62a1c91a859a 287 TxVal32(BaroOffsetDAC,0,HT);
gke 0:62a1c91a859a 288 TxVal32(BaroVal.u16,0,' ');
gke 0:62a1c91a859a 289 TxNextLine();
gke 0:62a1c91a859a 290 LEDYellow_TOG;
gke 0:62a1c91a859a 291 }
gke 0:62a1c91a859a 292
gke 0:62a1c91a859a 293 BaroOffsetDAC += 20; // move back up to come at it a little slower
gke 0:62a1c91a859a 294 SetFreescaleMCP4725(BaroOffsetDAC);
gke 0:62a1c91a859a 295 Delay1mS(100);
gke 0:62a1c91a859a 296 ReadFreescaleBaro();
gke 0:62a1c91a859a 297
gke 2:90292f8bd179 298 while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 1) ) {
gke 2:90292f8bd179 299 BaroOffsetDAC -= 1;
gke 0:62a1c91a859a 300 SetFreescaleMCP4725(BaroOffsetDAC);
gke 2:90292f8bd179 301 Delay1mS(10); // 10
gke 0:62a1c91a859a 302 ReadFreescaleBaro();
gke 0:62a1c91a859a 303 TxVal32(BaroOffsetDAC,0,HT);
gke 2:90292f8bd179 304 TxVal32(BaroVal.u16,0,HT);
gke 2:90292f8bd179 305 TxVal32((int32)FreescaleToDM(BaroVal.u16), 1, 0);
gke 0:62a1c91a859a 306 TxNextLine();
gke 0:62a1c91a859a 307 LEDYellow_TOG;
gke 0:62a1c91a859a 308 }
gke 0:62a1c91a859a 309
gke 0:62a1c91a859a 310 Delay1mS(200); // wait for caps to settle
gke 0:62a1c91a859a 311 F.BaroAltitudeValid = BaroOffsetDAC > 0;
gke 0:62a1c91a859a 312
gke 0:62a1c91a859a 313 } // SetFreescaleOffset
gke 0:62a1c91a859a 314
gke 0:62a1c91a859a 315 void ReadFreescaleBaro(void) {
gke 0:62a1c91a859a 316 static char B[8];
gke 0:62a1c91a859a 317 static i16u B0, B1, B2, B3;
gke 0:62a1c91a859a 318
gke 0:62a1c91a859a 319 mS[BaroUpdate] = mSClock() + ADS7823_TIME_MS;
gke 0:62a1c91a859a 320
gke 0:62a1c91a859a 321 I2CBARO.start(); // start conversion
gke 2:90292f8bd179 322 if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto ADS7823Error;
gke 2:90292f8bd179 323 if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto ADS7823Error;
gke 2:90292f8bd179 324 I2CBARO.stop();
gke 0:62a1c91a859a 325
gke 2:90292f8bd179 326 if ( I2CBARO.blockread(ADS7823_RD, B, 8) ) goto ADS7823Error;
gke 0:62a1c91a859a 327
gke 2:90292f8bd179 328 // read block of 4 baro samples
gke 0:62a1c91a859a 329
gke 0:62a1c91a859a 330 B0.b0 = B[1];
gke 0:62a1c91a859a 331 B0.b1 = B[0];
gke 0:62a1c91a859a 332 B1.b0 = B[3];
gke 0:62a1c91a859a 333 B1.b1 = B[2];
gke 0:62a1c91a859a 334 B2.b0 = B[5];
gke 0:62a1c91a859a 335 B2.b1 = B[4];
gke 0:62a1c91a859a 336 B3.b0 = B[7];
gke 0:62a1c91a859a 337 B3.b1 = B[6];
gke 0:62a1c91a859a 338
gke 0:62a1c91a859a 339 BaroVal.u16 = (uint16)16380 - ( B0.u16 + B1.u16 + B2.u16 + B3.u16 );
gke 0:62a1c91a859a 340
gke 0:62a1c91a859a 341 F.BaroAltitudeValid = true;
gke 0:62a1c91a859a 342
gke 0:62a1c91a859a 343 return;
gke 0:62a1c91a859a 344
gke 2:90292f8bd179 345 ADS7823Error:
gke 0:62a1c91a859a 346 I2CBARO.stop();
gke 0:62a1c91a859a 347
gke 2:90292f8bd179 348 I2CError[ADS7823_ID]++;
gke 2:90292f8bd179 349
gke 2:90292f8bd179 350 // F.BaroAltitudeValid = F.HoldingAlt = false;
gke 0:62a1c91a859a 351 if ( State == InFlight ) {
gke 0:62a1c91a859a 352 Stats[BaroFailS]++;
gke 0:62a1c91a859a 353 F.BaroFailure = true;
gke 0:62a1c91a859a 354 }
gke 2:90292f8bd179 355
gke 0:62a1c91a859a 356 } // ReadFreescaleBaro
gke 0:62a1c91a859a 357
gke 0:62a1c91a859a 358 real32 FreescaleToDM(int24 p) { // decreasing pressure is increase in altitude negate and rescale to metre altitude
gke 0:62a1c91a859a 359 return( -( (real32)p * 0.8 ) / (real32)P[BaroScale] );
gke 0:62a1c91a859a 360 } // FreescaleToDM
gke 0:62a1c91a859a 361
gke 0:62a1c91a859a 362 void GetFreescaleBaroAltitude(void) {
gke 0:62a1c91a859a 363 static int24 BaroPressure;
gke 0:62a1c91a859a 364
gke 0:62a1c91a859a 365 if ( mSClock() >= mS[BaroUpdate] ) {
gke 0:62a1c91a859a 366 ReadFreescaleBaro();
gke 0:62a1c91a859a 367 if ( F.BaroAltitudeValid ) {
gke 0:62a1c91a859a 368 BaroPressure = (int24)BaroVal.u16; // sum of 4 samples
gke 0:62a1c91a859a 369 BaroRelAltitude = FreescaleToDM(BaroPressure - OriginBaroPressure);
gke 0:62a1c91a859a 370 F.NewBaroValue = F.BaroAltitudeValid;
gke 0:62a1c91a859a 371 }
gke 0:62a1c91a859a 372 }
gke 0:62a1c91a859a 373
gke 0:62a1c91a859a 374 } // GetFreescaleBaroAltitude
gke 0:62a1c91a859a 375
gke 0:62a1c91a859a 376 boolean IsFreescaleBaroActive(void) { // check for Freescale Barometer
gke 0:62a1c91a859a 377
gke 1:1e3318a30ddd 378 static boolean r;
gke 0:62a1c91a859a 379
gke 1:1e3318a30ddd 380 r = I2CBAROAddressResponds( ADS7823_ID );
gke 1:1e3318a30ddd 381 if ( r ) {
gke 1:1e3318a30ddd 382 BaroType = BaroMPX4115;
gke 1:1e3318a30ddd 383 r = IdentifyMCP4725();
gke 1:1e3318a30ddd 384 TrackMinI2CRate(400000);
gke 1:1e3318a30ddd 385 }
gke 1:1e3318a30ddd 386 return (r);
gke 0:62a1c91a859a 387
gke 0:62a1c91a859a 388 } // IsFreescaleBaroActive
gke 0:62a1c91a859a 389
gke 0:62a1c91a859a 390 void InitFreescaleBarometer(void) {
gke 0:62a1c91a859a 391 static int16 BaroOriginAltitude, MinAltitude;
gke 0:62a1c91a859a 392 static int24 BaroPressureP;
gke 0:62a1c91a859a 393
gke 0:62a1c91a859a 394 AltitudeUpdateRate = 1000L/ADS7823_TIME_MS;
gke 0:62a1c91a859a 395
gke 0:62a1c91a859a 396 BaroTemperature = 0;
gke 2:90292f8bd179 397 if ( P[BaroScale] <= 0 )
gke 2:90292f8bd179 398 P[BaroScale] = 56; // failsafe setting
gke 2:90292f8bd179 399
gke 0:62a1c91a859a 400 BaroPressure = 0;
gke 0:62a1c91a859a 401 BaroRetries = 0;
gke 0:62a1c91a859a 402 do {
gke 0:62a1c91a859a 403 BaroPressureP = BaroPressure;
gke 0:62a1c91a859a 404
gke 0:62a1c91a859a 405 SetFreescaleOffset();
gke 0:62a1c91a859a 406
gke 0:62a1c91a859a 407 while ( mSClock() < mS[BaroUpdate] ) {};
gke 0:62a1c91a859a 408 ReadFreescaleBaro();
gke 0:62a1c91a859a 409 BaroPressure = (int24)BaroVal.u16;
gke 0:62a1c91a859a 410 } while ( ( ++BaroRetries < BARO_INIT_RETRIES )
gke 2:90292f8bd179 411 && ( fabs( FreescaleToDM(BaroPressure - BaroPressureP) ) > 5 ) );
gke 0:62a1c91a859a 412
gke 0:62a1c91a859a 413 F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;
gke 0:62a1c91a859a 414
gke 0:62a1c91a859a 415 OriginBaroPressure = BaroPressure;
gke 0:62a1c91a859a 416
gke 0:62a1c91a859a 417 BaroRelAltitudeP = BaroRelAltitude = 0.0;
gke 0:62a1c91a859a 418
gke 0:62a1c91a859a 419 MinAltitude = FreescaleToDM((int24)ADS7823_MAX*4);
gke 0:62a1c91a859a 420 BaroOriginAltitude = FreescaleToDM(OriginBaroPressure);
gke 0:62a1c91a859a 421 BaroDescentAvailable = MinAltitude - BaroOriginAltitude;
gke 0:62a1c91a859a 422 BaroClimbAvailable = -BaroOriginAltitude;
gke 0:62a1c91a859a 423
gke 0:62a1c91a859a 424 //F.BaroAltitudeValid &= (( BaroClimbAvailable >= BARO_MIN_CLIMB )
gke 0:62a1c91a859a 425 // && (BaroDescentAvailable <= BARO_MIN_DESCENT));
gke 0:62a1c91a859a 426
gke 0:62a1c91a859a 427 #ifdef SIMULATE
gke 0:62a1c91a859a 428 FakeBaroRelAltitude = 0;
gke 0:62a1c91a859a 429 #endif // SIMULATE
gke 0:62a1c91a859a 430
gke 0:62a1c91a859a 431 } // InitFreescaleBarometer
gke 0:62a1c91a859a 432
gke 0:62a1c91a859a 433 // -----------------------------------------------------------
gke 0:62a1c91a859a 434
gke 0:62a1c91a859a 435 // Bosch SMD500 and BMP085 Barometers
gke 0:62a1c91a859a 436
gke 0:62a1c91a859a 437 void StartBoschBaroADC(boolean);
gke 0:62a1c91a859a 438 int24 CompensatedBoschPressure(uint16, uint16);
gke 0:62a1c91a859a 439
gke 0:62a1c91a859a 440 void GetBoschBaroAltitude(void);
gke 0:62a1c91a859a 441 boolean IsBoschBaroActive(void);
gke 0:62a1c91a859a 442 void InitBoschBarometer(void);
gke 0:62a1c91a859a 443
gke 0:62a1c91a859a 444 // SMD500 9.5mS (T) 34mS (P)
gke 0:62a1c91a859a 445 // BMP085 4.5mS (T) 25.5mS (P) OSRS=3
gke 0:62a1c91a859a 446 #define BOSCH_TEMP_TIME_MS 11 // 10 increase to make P+T acq time ~50mS
gke 0:62a1c91a859a 447 //#define BMP085_PRESS_TIME_MS 26
gke 0:62a1c91a859a 448 //#define SMD500_PRESS_TIME_MS 34
gke 0:62a1c91a859a 449 #define BOSCH_PRESS_TIME_MS 38
gke 0:62a1c91a859a 450 #define BOSCH_PRESS_TEMP_TIME_MS 50 // pressure and temp time + overheads
gke 0:62a1c91a859a 451
gke 0:62a1c91a859a 452 void StartBoschBaroADC(boolean ReadPressure) {
gke 0:62a1c91a859a 453 static uint8 TempOrPress;
gke 0:62a1c91a859a 454
gke 0:62a1c91a859a 455 if ( ReadPressure ) {
gke 0:62a1c91a859a 456 TempOrPress = BOSCH_PRESS;
gke 0:62a1c91a859a 457 mS[BaroUpdate] = mSClock() + BOSCH_PRESS_TIME_MS;
gke 0:62a1c91a859a 458 } else {
gke 0:62a1c91a859a 459 mS[BaroUpdate] = mSClock() + BOSCH_TEMP_TIME_MS;
gke 0:62a1c91a859a 460 if ( BaroType == BaroBMP085 )
gke 0:62a1c91a859a 461 TempOrPress = BOSCH_TEMP_BMP085;
gke 0:62a1c91a859a 462 else
gke 0:62a1c91a859a 463 TempOrPress = BOSCH_TEMP_SMD500;
gke 0:62a1c91a859a 464 }
gke 0:62a1c91a859a 465
gke 0:62a1c91a859a 466 I2CBARO.start();
gke 2:90292f8bd179 467 if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto BoschError;
gke 0:62a1c91a859a 468 // access control register, start measurement
gke 2:90292f8bd179 469 if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto BoschError;
gke 0:62a1c91a859a 470 // select 32kHz input, measure temperature
gke 2:90292f8bd179 471 if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto BoschError;
gke 0:62a1c91a859a 472 I2CBARO.stop();
gke 0:62a1c91a859a 473
gke 0:62a1c91a859a 474 F.BaroAltitudeValid = true;
gke 0:62a1c91a859a 475 return;
gke 0:62a1c91a859a 476
gke 2:90292f8bd179 477 BoschError:
gke 0:62a1c91a859a 478 I2CBARO.stop();
gke 2:90292f8bd179 479
gke 2:90292f8bd179 480 I2CError[BOSCH_ID]++;
gke 2:90292f8bd179 481 F.BaroAltitudeValid = false;
gke 2:90292f8bd179 482
gke 0:62a1c91a859a 483 } // StartBoschBaroADC
gke 0:62a1c91a859a 484
gke 0:62a1c91a859a 485 void ReadBoschBaro(void) {
gke 2:90292f8bd179 486
gke 0:62a1c91a859a 487 // Possible I2C protocol error - split read of ADC
gke 0:62a1c91a859a 488 I2CBARO.start();
gke 2:90292f8bd179 489 if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
gke 2:90292f8bd179 490 if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto BoschError;
gke 0:62a1c91a859a 491 I2CBARO.start(); // restart
gke 2:90292f8bd179 492 if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
gke 0:62a1c91a859a 493 BaroVal.b1 = I2CBARO.read(I2C_NACK);
gke 0:62a1c91a859a 494 I2CBARO.stop();
gke 0:62a1c91a859a 495
gke 0:62a1c91a859a 496 I2CBARO.start();
gke 2:90292f8bd179 497 if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
gke 2:90292f8bd179 498 if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto BoschError;
gke 0:62a1c91a859a 499 I2CBARO.start(); // restart
gke 2:90292f8bd179 500 if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
gke 0:62a1c91a859a 501 BaroVal.b0 = I2CBARO.read(I2C_NACK);
gke 0:62a1c91a859a 502 I2CBARO.stop();
gke 0:62a1c91a859a 503
gke 0:62a1c91a859a 504 F.BaroAltitudeValid = true;
gke 0:62a1c91a859a 505 return;
gke 0:62a1c91a859a 506
gke 2:90292f8bd179 507 BoschError:
gke 0:62a1c91a859a 508 I2CBARO.stop();
gke 0:62a1c91a859a 509
gke 2:90292f8bd179 510 I2CError[BOSCH_ID]++;
gke 2:90292f8bd179 511
gke 0:62a1c91a859a 512 F.BaroAltitudeValid = F.HoldingAlt = false;
gke 0:62a1c91a859a 513 if ( State == InFlight ) {
gke 0:62a1c91a859a 514 Stats[BaroFailS]++;
gke 0:62a1c91a859a 515 F.BaroFailure = true;
gke 0:62a1c91a859a 516 }
gke 2:90292f8bd179 517
gke 0:62a1c91a859a 518 } // ReadBoschBaro
gke 0:62a1c91a859a 519
gke 0:62a1c91a859a 520 #define BOSCH_BMP085_TEMP_COEFF 62L
gke 0:62a1c91a859a 521 #define BOSCH_SMD500_TEMP_COEFF 50L
gke 0:62a1c91a859a 522
gke 0:62a1c91a859a 523 int24 CompensatedBoschPressure(uint16 BaroPress, uint16 BaroTemp) {
gke 0:62a1c91a859a 524 static int24 BaroTempComp;
gke 0:62a1c91a859a 525
gke 0:62a1c91a859a 526 if ( BaroType == BaroBMP085 )
gke 0:62a1c91a859a 527 BaroTempComp = (BaroTemp * BOSCH_BMP085_TEMP_COEFF + 64L) >> 7;
gke 0:62a1c91a859a 528 else
gke 0:62a1c91a859a 529 BaroTempComp = (BaroTemp * BOSCH_SMD500_TEMP_COEFF + 8L) >> 4;
gke 0:62a1c91a859a 530
gke 0:62a1c91a859a 531 return ((int24)BaroPress + BaroTempComp - OriginBaroPressure);
gke 0:62a1c91a859a 532
gke 0:62a1c91a859a 533 } // CompensatedBoschPressure
gke 0:62a1c91a859a 534
gke 0:62a1c91a859a 535 void GetBoschBaroAltitude(void) {
gke 0:62a1c91a859a 536 static int24 Temp;
gke 0:62a1c91a859a 537
gke 0:62a1c91a859a 538 if ( mSClock() >= mS[BaroUpdate] ) {
gke 0:62a1c91a859a 539 ReadBoschBaro();
gke 0:62a1c91a859a 540 if ( F.BaroAltitudeValid )
gke 0:62a1c91a859a 541 if ( AcquiringPressure ) {
gke 0:62a1c91a859a 542 BaroPressure = (int24)BaroVal.u16;
gke 0:62a1c91a859a 543 AcquiringPressure = false;
gke 0:62a1c91a859a 544 } else {
gke 0:62a1c91a859a 545 BaroTemperature = (int24)BaroVal.u16;
gke 0:62a1c91a859a 546 AcquiringPressure = true;
gke 0:62a1c91a859a 547
gke 0:62a1c91a859a 548 Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature);
gke 0:62a1c91a859a 549 CompBaroPressure -= BaroQ.B[BaroQ.Head];
gke 0:62a1c91a859a 550 BaroQ.B[BaroQ.Head] = Temp;
gke 0:62a1c91a859a 551 CompBaroPressure += Temp;
gke 0:62a1c91a859a 552 BaroQ.Head = (BaroQ.Head + 1) & (BARO_BUFF_SIZE -1);
gke 0:62a1c91a859a 553
gke 0:62a1c91a859a 554 // Pressure queue has 4 entries corresponding to an average delay at 20Hz of 0.1Sec
gke 0:62a1c91a859a 555 // decreasing pressure is increase in altitude negate and rescale to decimetre altitude
gke 0:62a1c91a859a 556
gke 0:62a1c91a859a 557 BaroRelAltitude = - ( (real32)CompBaroPressure * (real32)P[BaroScale] ) / 1280.0;
gke 0:62a1c91a859a 558
gke 0:62a1c91a859a 559 F.NewBaroValue = F.BaroAltitudeValid;
gke 0:62a1c91a859a 560 }
gke 2:90292f8bd179 561 else
gke 0:62a1c91a859a 562 AcquiringPressure = true;
gke 0:62a1c91a859a 563
gke 0:62a1c91a859a 564 StartBoschBaroADC(AcquiringPressure);
gke 0:62a1c91a859a 565 }
gke 0:62a1c91a859a 566 } // GetBoschBaroAltitude
gke 0:62a1c91a859a 567
gke 0:62a1c91a859a 568 boolean IsBoschBaroActive(void) { // check for Bosch Barometers
gke 0:62a1c91a859a 569 static uint8 r;
gke 0:62a1c91a859a 570
gke 0:62a1c91a859a 571 I2CBARO.start();
gke 1:1e3318a30ddd 572 if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschInactive;
gke 0:62a1c91a859a 573 if ( I2CBARO.write(BOSCH_TYPE) != I2C_ACK ) goto BoschInactive;
gke 0:62a1c91a859a 574 I2CBARO.start(); // restart
gke 1:1e3318a30ddd 575 if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschInactive;
gke 0:62a1c91a859a 576 r = I2CBARO.read(I2C_NACK);
gke 0:62a1c91a859a 577 I2CBARO.stop();
gke 0:62a1c91a859a 578
gke 0:62a1c91a859a 579 if (r == BOSCH_ID_BMP085 )
gke 0:62a1c91a859a 580 BaroType = BaroBMP085;
gke 0:62a1c91a859a 581 else
gke 0:62a1c91a859a 582 BaroType = BaroSMD500;
gke 1:1e3318a30ddd 583
gke 0:62a1c91a859a 584 TrackMinI2CRate(400000);
gke 0:62a1c91a859a 585
gke 0:62a1c91a859a 586 return(true);
gke 0:62a1c91a859a 587
gke 0:62a1c91a859a 588 BoschInactive:
gke 2:90292f8bd179 589
gke 0:62a1c91a859a 590 return(false);
gke 0:62a1c91a859a 591
gke 0:62a1c91a859a 592 } // IsBoschBaroActive
gke 0:62a1c91a859a 593
gke 0:62a1c91a859a 594 void InitBoschBarometer(void) {
gke 0:62a1c91a859a 595 int8 s;
gke 0:62a1c91a859a 596 int24 Temp, CompBaroPressureP;
gke 0:62a1c91a859a 597
gke 0:62a1c91a859a 598 AltitudeUpdateRate = 1000L / BOSCH_PRESS_TEMP_TIME_MS;
gke 0:62a1c91a859a 599
gke 0:62a1c91a859a 600 F.NewBaroValue = false;
gke 0:62a1c91a859a 601 CompBaroPressure = 0;
gke 0:62a1c91a859a 602
gke 0:62a1c91a859a 603 TxString("Temp. \tPressure\r\n");
gke 0:62a1c91a859a 604
gke 0:62a1c91a859a 605 BaroRetries = 0;
gke 0:62a1c91a859a 606 do { // occasional I2C misread of Temperature so keep doing it until the Origin is stable!!
gke 0:62a1c91a859a 607 CompBaroPressureP = CompBaroPressure;
gke 0:62a1c91a859a 608 CompBaroPressure = BaroQ.Head = 0;
gke 0:62a1c91a859a 609
gke 0:62a1c91a859a 610 AcquiringPressure = true;
gke 0:62a1c91a859a 611 StartBoschBaroADC(AcquiringPressure); // Pressure
gke 0:62a1c91a859a 612
gke 0:62a1c91a859a 613 for ( s = 0; s < 4; s++ ) {
gke 0:62a1c91a859a 614 while ( mSClock() < mS[BaroUpdate] );
gke 0:62a1c91a859a 615 ReadBoschBaro(); // Pressure
gke 0:62a1c91a859a 616 BaroPressure = BaroVal.u16;
gke 0:62a1c91a859a 617
gke 0:62a1c91a859a 618 AcquiringPressure = !AcquiringPressure;
gke 0:62a1c91a859a 619 StartBoschBaroADC(AcquiringPressure); // Temperature
gke 0:62a1c91a859a 620 while ( mSClock() < mS[BaroUpdate] );
gke 0:62a1c91a859a 621 ReadBoschBaro();
gke 0:62a1c91a859a 622 BaroTemperature = BaroVal.u16;
gke 0:62a1c91a859a 623
gke 0:62a1c91a859a 624 TxVal32(BaroTemperature,0,HT);
gke 0:62a1c91a859a 625 TxVal32(BaroPressure,0,0);
gke 0:62a1c91a859a 626 TxNextLine();
gke 0:62a1c91a859a 627
gke 0:62a1c91a859a 628 Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature);
gke 0:62a1c91a859a 629 BaroQ.B[s] = Temp;
gke 0:62a1c91a859a 630 CompBaroPressure += Temp;
gke 0:62a1c91a859a 631
gke 0:62a1c91a859a 632 AcquiringPressure = !AcquiringPressure;
gke 0:62a1c91a859a 633 StartBoschBaroADC(AcquiringPressure);
gke 0:62a1c91a859a 634 }
gke 0:62a1c91a859a 635
gke 0:62a1c91a859a 636 } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) && ( abs(CompBaroPressure - CompBaroPressureP) > 12 ) ); // stable within ~0.5M
gke 0:62a1c91a859a 637
gke 0:62a1c91a859a 638 OriginBaroPressure = SRS32(CompBaroPressure, 2);
gke 0:62a1c91a859a 639
gke 0:62a1c91a859a 640 F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;
gke 0:62a1c91a859a 641 BaroRelAltitudeP = BaroRelAltitude = 0.0;
gke 0:62a1c91a859a 642
gke 0:62a1c91a859a 643 #ifdef SIMULATE
gke 0:62a1c91a859a 644 FakeBaroRelAltitude = 0.0;
gke 0:62a1c91a859a 645 #endif // SIMULATE
gke 0:62a1c91a859a 646
gke 0:62a1c91a859a 647 } // InitBoschBarometer