Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
Diff: baro.c
- Revision:
- 0:62a1c91a859a
- Child:
- 1:1e3318a30ddd
diff -r 000000000000 -r 62a1c91a859a baro.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/baro.c Fri Feb 18 22:28:05 2011 +0000 @@ -0,0 +1,613 @@ +// =============================================================================================== +// = UAVXArm Quadrocopter Controller = +// = Copyright (c) 2008 by Prof. Greg Egan = +// = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = +// = http://code.google.com/p/uavp-mods/ http://uavp.ch = +// =============================================================================================== + +// This is part of UAVXArm. + +// UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU +// General Public License as published by the Free Software Foundation, either version 3 of the +// License, or (at your option) any later version. + +// UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without +// even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +// See the GNU General Public License for more details. + +// You should have received a copy of the GNU General Public License along with this program. +// If not, see http://www.gnu.org/licenses/ + +#include "UAVXArm.h" + +// Barometers Freescale TI ADC and Bosch BMP085 3.8MHz, Bosch SMD500 400KHz + +#define BARO_MIN_CLIMB 150.0 // M minimum available barometer climb from origin +#define BARO_MIN_DESCENT -50.0 //M minimum available barometer descent from origin + +void GetBaroAltitude(void); +void InitBarometer(void); + +void ShowBaroType(void); +void BaroTest(void); + +#define BaroROCFilter HardFilter + +uint16 BaroPressure, BaroTemperature; +boolean AcquiringPressure; +int16 BaroOffsetDAC; + +#define BARO_BUFF_SIZE 4 + +struct { + uint8 Head, Tail; + int24 B[BARO_BUFF_SIZE]; +} BaroQ; + +int32 OriginBaroPressure, CompBaroPressure; +real32 BaroRelAltitude, BaroRelAltitudeP; +i16u BaroVal; +int8 BaroType; +int16 BaroClimbAvailable, BaroDescentAvailable; +int16 AltitudeUpdateRate; +int8 BaroRetries; + +real32 FakeBaroRelAltitude; +int8 SimulateCycles = 0; + +void ShowBaroType(void) { + switch ( BaroType ) { + case BaroMPX4115: + TxString("MPX4115\r\n"); + break; + case BaroSMD500: + TxString("SMD500\r\n"); + break; + case BaroBMP085: + TxString("BMP085\r\n"); + break; + case BaroUnknown: + TxString("None\r\n"); + break; + default: + break; + } +} // ShowBaro + +void BaroTest(void) { + TxString("\r\nAltitude test\r\n"); + + TxString("Initialising\r\n"); + + InitBarometer(); + + while ( F.BaroAltitudeValid && ! F.NewBaroValue ) + GetBaroAltitude(); + + TxString("\r\nType:\t"); + ShowBaroType(); + + TxString("Init Retries:\t"); + TxVal32((int32)BaroRetries - 2, 0, ' '); // always minimum of 2 + if ( BaroRetries >= BARO_INIT_RETRIES ) + TxString(" FAILED Init.\r\n"); + else + TxNextLine(); + + if ( BaroType == BaroMPX4115 ) { + TxString("Range :\t"); + TxVal32((int32) BaroDescentAvailable * 10.0, 1, ' '); + TxString("-> "); + TxVal32((int32) BaroClimbAvailable * 10.0, 1, 'M'); + TxString(" {Offset "); + TxVal32((int32)BaroOffsetDAC, 0,'}'); + if (( BaroClimbAvailable < BARO_MIN_CLIMB ) || (BaroDescentAvailable > BARO_MIN_DESCENT)) + TxString(" Bad climb or descent range - offset adjustment?"); + TxNextLine(); + } + + if ( !F.BaroAltitudeValid ) goto BAerror; + + while ( !F.NewBaroValue ) + GetBaroAltitude(); + F.NewBaroValue = false; + + TxString("Alt.: \t"); + TxVal32(BaroRelAltitude * 10.0, 1, ' '); + TxString("M\r\n"); + + TxString("\r\nR.Finder: \t"); + if ( F.RangefinderAltitudeValid ) { + GetRangefinderAltitude(); + TxVal32(RangefinderAltitude * 100.0, 2, ' '); + TxString("M\r\n"); + } else + TxString("no rangefinder\r\n"); + + TxString("\r\nAmbient :\t"); + TxVal32((int32)AmbientTemperature.i16, 1, ' '); + TxString("C\r\n"); + + return; +BAerror: + TxString("FAIL\r\n"); +} // BaroTest + +void GetBaroAltitude(void) { + static real32 Temp, AltChange; + + if ( BaroType == BaroMPX4115 ) + GetFreescaleBaroAltitude(); + else + GetBoschBaroAltitude(); + + if ( F.NewBaroValue ) { +#ifdef SIMULATE + if ( State == InFlight ) { + if ( ++SimulateCycles >= AltitudeUpdateRate ) { + FakeBaroRelAltitude += ( DesiredThrottle - CruiseThrottle ) + Comp[Alt]; + if ( FakeBaroRelAltitude < -5.0 ) + FakeBaroRelAltitude = 0.0; + + SimulateCycles = 0; + + ROC = FakeBaroRelAltitude - BaroRelAltitudeP; + BaroRelAltitudeP = FakeBaroRelAltitude; + } + BaroRelAltitude = FakeBaroRelAltitude; + } +#else + + AltChange = BaroRelAltitude - BaroRelAltitudeP; + Temp = AltChange * AltitudeUpdateRate; + + if ( fabs( Temp ) > BARO_SANITY_CHECK_MPS ) { + BaroRelAltitude = BaroRelAltitudeP; // use previous value + Temp = 0; + Stats[BaroFailS]++; + } + + Temp = Limit( Temp , -BARO_SANITY_CHECK_MPS, BARO_SANITY_CHECK_MPS ); + ROC = ROC * 0.9 + Temp * 0.1; + BaroRelAltitudeP = BaroRelAltitude; + +#endif // SIMULATE + + if ( State == InFlight ) { + if ( ROC > Stats[MaxROCS] ) + Stats[MaxROCS] = ROC; + else + if ( ROC < Stats[MinROCS] ) + Stats[MinROCS] = ROC; + + if ( BaroRelAltitude > Stats[BaroRelAltitudeS] ) + Stats[BaroRelAltitudeS] = BaroRelAltitude; + } + } + +} // GetBaroAltitude + +void InitBarometer(void) { + BaroRelAltitude = BaroRelAltitudeP = CompBaroPressure = OriginBaroPressure = 0; + BaroType = BaroUnknown; + + Comp[Alt] = AltDiffSum = AltDSum = 0; + F.BaroAltitudeValid= true; // optimistic + + if ( IsFreescaleBaroActive() ) + InitFreescaleBarometer(); + else + if ( IsBoschBaroActive() ) + InitBoschBarometer(); + else { + F.BaroAltitudeValid = F.HoldingAlt = false; + Stats[BaroFailS]++; + } +} // InitBarometer + +// ----------------------------------------------------------- + +// Freescale ex Motorola MPX4115 Barometer with ADS7823 12bit ADC + +void SetFreescaleMCP4725(int16); +void SetFreescaleOffset(void); +void ReadFreescaleBaro(void); +real32 FreescaleToDM(int24); +void GetFreescaleBaroAltitude(void); +boolean IsFreescaleBaroActive(void); +void InitFreescaleBarometer(void); + +void SetFreescaleMCP4725(int16 d) { + static i16u dd; + static uint8 r; + + dd.u16 = d << 4; // left align + + I2CBARO.start(); + r = I2CBARO.write(MCP4725_WR) != I2C_ACK; + r = I2CBARO.write(MCP4725_CMD) != I2C_ACK; + r = I2CBARO.write(dd.b1) != I2C_ACK; + r = I2CBARO.write(dd.b0) != I2C_ACK; + I2CBARO.stop(); + +} // SetFreescaleMCP4725 + +void SetFreescaleOffset(void) { + // Steve Westerfeld + // 470 Ohm, 1uF RC 0.47mS use 2mS for settling? + + TxString("\r\nOffset \tPressure\r\n"); + + BaroOffsetDAC = MCP4725_MAX; + + SetFreescaleMCP4725(BaroOffsetDAC); + + Delay1mS(20); // initial settling + ReadFreescaleBaro(); + + while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*7L)/10L) ) + && (BaroOffsetDAC > 20) ) { // first loop gets close + BaroOffsetDAC -= 20; // approach at 20 steps out of 4095 + SetFreescaleMCP4725(BaroOffsetDAC); + Delay1mS(20); + ReadFreescaleBaro(); + TxVal32(BaroOffsetDAC,0,HT); + TxVal32(BaroVal.u16,0,' '); + TxNextLine(); + LEDYellow_TOG; + } + + BaroOffsetDAC += 20; // move back up to come at it a little slower + SetFreescaleMCP4725(BaroOffsetDAC); + Delay1mS(100); + ReadFreescaleBaro(); + + while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 2) ) { + BaroOffsetDAC -= 2; + SetFreescaleMCP4725(BaroOffsetDAC); + Delay1mS(10); + ReadFreescaleBaro(); + TxVal32(BaroOffsetDAC,0,HT); + TxVal32(BaroVal.u16,0,' '); + TxNextLine(); + LEDYellow_TOG; + } + + Delay1mS(200); // wait for caps to settle + F.BaroAltitudeValid = BaroOffsetDAC > 0; + +} // SetFreescaleOffset + +void ReadFreescaleBaro(void) { + static char B[8]; + static i16u B0, B1, B2, B3; + + mS[BaroUpdate] = mSClock() + ADS7823_TIME_MS; + + I2CBARO.start(); // start conversion + + if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto FSError; + if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto FSError; + + I2CBARO.read(ADS7823_RD, B, 8); // read block of 4 baro samples + + B0.b0 = B[1]; + B0.b1 = B[0]; + B1.b0 = B[3]; + B1.b1 = B[2]; + B2.b0 = B[5]; + B2.b1 = B[4]; + B3.b0 = B[7]; + B3.b1 = B[6]; + + BaroVal.u16 = (uint16)16380 - ( B0.u16 + B1.u16 + B2.u16 + B3.u16 ); + + F.BaroAltitudeValid = true; + + return; + +FSError: + I2CBARO.stop(); + + F.BaroAltitudeValid = F.HoldingAlt = false; + if ( State == InFlight ) { + Stats[BaroFailS]++; + F.BaroFailure = true; + } + return; +} // ReadFreescaleBaro + +real32 FreescaleToDM(int24 p) { // decreasing pressure is increase in altitude negate and rescale to metre altitude + return( -( (real32)p * 0.8 ) / (real32)P[BaroScale] ); +} // FreescaleToDM + +void GetFreescaleBaroAltitude(void) { + static int24 BaroPressure; + + if ( mSClock() >= mS[BaroUpdate] ) { + ReadFreescaleBaro(); + if ( F.BaroAltitudeValid ) { + BaroPressure = (int24)BaroVal.u16; // sum of 4 samples + + BaroRelAltitude = FreescaleToDM(BaroPressure - OriginBaroPressure); + + F.NewBaroValue = F.BaroAltitudeValid; + } + } + +} // GetFreescaleBaroAltitude + +boolean IsFreescaleBaroActive(void) { // check for Freescale Barometer + + I2CBARO.start(); + if ( I2CBARO.write(ADS7823_ID) != I2C_ACK ) goto FreescaleInactive; + + BaroType = BaroMPX4115; + I2CBARO.stop(); + + TrackMinI2CRate(400000); + + return(true); + +FreescaleInactive: + I2CBARO.stop(); + return(false); + +} // IsFreescaleBaroActive + +void InitFreescaleBarometer(void) { + static int16 BaroOriginAltitude, MinAltitude; + real32 Error; + static int24 BaroPressureP; + + AltitudeUpdateRate = 1000L/ADS7823_TIME_MS; + + BaroTemperature = 0; + Error = ( (int16)P[BaroScale] * 20 ) / 16; // 0.2M + BaroPressure = 0; + + BaroRetries = 0; + do { + BaroPressureP = BaroPressure; + + SetFreescaleOffset(); + + while ( mSClock() < mS[BaroUpdate] ) {}; + ReadFreescaleBaro(); + BaroPressure = (int24)BaroVal.u16; + } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) + && ( abs((int16)(BaroPressure - BaroPressureP)) > Error ) ); + + F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES; + + OriginBaroPressure = BaroPressure; + + BaroRelAltitudeP = BaroRelAltitude = 0.0; + + MinAltitude = FreescaleToDM((int24)ADS7823_MAX*4); + BaroOriginAltitude = FreescaleToDM(OriginBaroPressure); + BaroDescentAvailable = MinAltitude - BaroOriginAltitude; + BaroClimbAvailable = -BaroOriginAltitude; + + //F.BaroAltitudeValid &= (( BaroClimbAvailable >= BARO_MIN_CLIMB ) + // && (BaroDescentAvailable <= BARO_MIN_DESCENT)); + +#ifdef SIMULATE + FakeBaroRelAltitude = 0; +#endif // SIMULATE + +} // InitFreescaleBarometer + +// ----------------------------------------------------------- + +// Bosch SMD500 and BMP085 Barometers + +void StartBoschBaroADC(boolean); +int24 CompensatedBoschPressure(uint16, uint16); + +void GetBoschBaroAltitude(void); +boolean IsBoschBaroActive(void); +void InitBoschBarometer(void); + +// SMD500 9.5mS (T) 34mS (P) +// BMP085 4.5mS (T) 25.5mS (P) OSRS=3 +#define BOSCH_TEMP_TIME_MS 11 // 10 increase to make P+T acq time ~50mS +//#define BMP085_PRESS_TIME_MS 26 +//#define SMD500_PRESS_TIME_MS 34 +#define BOSCH_PRESS_TIME_MS 38 +#define BOSCH_PRESS_TEMP_TIME_MS 50 // pressure and temp time + overheads + +void StartBoschBaroADC(boolean ReadPressure) { + static uint8 TempOrPress; + + if ( ReadPressure ) { + TempOrPress = BOSCH_PRESS; + mS[BaroUpdate] = mSClock() + BOSCH_PRESS_TIME_MS; + } else { + mS[BaroUpdate] = mSClock() + BOSCH_TEMP_TIME_MS; + if ( BaroType == BaroBMP085 ) + TempOrPress = BOSCH_TEMP_BMP085; + else + TempOrPress = BOSCH_TEMP_SMD500; + } + + I2CBARO.start(); + if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto SBerror; + + // access control register, start measurement + if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto SBerror; + + // select 32kHz input, measure temperature + if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto SBerror; + I2CBARO.stop(); + + F.BaroAltitudeValid = true; + return; + +SBerror: + I2CBARO.stop(); + F.BaroAltitudeValid = F.HoldingAlt = false; + return; +} // StartBoschBaroADC + +void ReadBoschBaro(void) { + // Possible I2C protocol error - split read of ADC + I2CBARO.start(); + if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto RVerror; + if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto RVerror; + I2CBARO.start(); // restart + if ( I2CBARO.write(BOSCH_ID+1) != I2C_ACK ) goto RVerror; + BaroVal.b1 = I2CBARO.read(I2C_NACK); + I2CBARO.stop(); + + I2CBARO.start(); + if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto RVerror; + if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto RVerror; + I2CBARO.start(); // restart + if ( I2CBARO.write(BOSCH_ID+1) != I2C_ACK ) goto RVerror; + BaroVal.b0 = I2CBARO.read(I2C_NACK); + I2CBARO.stop(); + + F.BaroAltitudeValid = true; + return; + +RVerror: + I2CBARO.stop(); + + F.BaroAltitudeValid = F.HoldingAlt = false; + if ( State == InFlight ) { + Stats[BaroFailS]++; + F.BaroFailure = true; + } + return; +} // ReadBoschBaro + +#define BOSCH_BMP085_TEMP_COEFF 62L +#define BOSCH_SMD500_TEMP_COEFF 50L + +int24 CompensatedBoschPressure(uint16 BaroPress, uint16 BaroTemp) { + static int24 BaroTempComp; + + if ( BaroType == BaroBMP085 ) + BaroTempComp = (BaroTemp * BOSCH_BMP085_TEMP_COEFF + 64L) >> 7; + else + BaroTempComp = (BaroTemp * BOSCH_SMD500_TEMP_COEFF + 8L) >> 4; + + return ((int24)BaroPress + BaroTempComp - OriginBaroPressure); + +} // CompensatedBoschPressure + +void GetBoschBaroAltitude(void) { + static int24 Temp; + + if ( mSClock() >= mS[BaroUpdate] ) { + ReadBoschBaro(); + if ( F.BaroAltitudeValid ) + if ( AcquiringPressure ) { + BaroPressure = (int24)BaroVal.u16; + AcquiringPressure = false; + } else { + BaroTemperature = (int24)BaroVal.u16; + AcquiringPressure = true; + + Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature); + CompBaroPressure -= BaroQ.B[BaroQ.Head]; + BaroQ.B[BaroQ.Head] = Temp; + CompBaroPressure += Temp; + BaroQ.Head = (BaroQ.Head + 1) & (BARO_BUFF_SIZE -1); + + // Pressure queue has 4 entries corresponding to an average delay at 20Hz of 0.1Sec + // decreasing pressure is increase in altitude negate and rescale to decimetre altitude + + BaroRelAltitude = - ( (real32)CompBaroPressure * (real32)P[BaroScale] ) / 1280.0; + + F.NewBaroValue = F.BaroAltitudeValid; + } + else { + AcquiringPressure = true; + Stats[BaroFailS]++; + } + + StartBoschBaroADC(AcquiringPressure); + } +} // GetBoschBaroAltitude + +boolean IsBoschBaroActive(void) { // check for Bosch Barometers + static uint8 r; + + I2CBARO.start(); + if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto BoschInactive; + if ( I2CBARO.write(BOSCH_TYPE) != I2C_ACK ) goto BoschInactive; + I2CBARO.start(); // restart + if ( I2CBARO.write(BOSCH_ID+1) != I2C_ACK ) goto BoschInactive; + r = I2CBARO.read(I2C_NACK); + I2CBARO.stop(); + + if (r == BOSCH_ID_BMP085 ) + BaroType = BaroBMP085; + else + BaroType = BaroSMD500; + + TrackMinI2CRate(400000); + + return(true); + +BoschInactive: + return(false); + +} // IsBoschBaroActive + +void InitBoschBarometer(void) { + int8 s; + int24 Temp, CompBaroPressureP; + + AltitudeUpdateRate = 1000L / BOSCH_PRESS_TEMP_TIME_MS; + + F.NewBaroValue = false; + CompBaroPressure = 0; + + TxString("Temp. \tPressure\r\n"); + + BaroRetries = 0; + do { // occasional I2C misread of Temperature so keep doing it until the Origin is stable!! + CompBaroPressureP = CompBaroPressure; + CompBaroPressure = BaroQ.Head = 0; + + AcquiringPressure = true; + StartBoschBaroADC(AcquiringPressure); // Pressure + + for ( s = 0; s < 4; s++ ) { + while ( mSClock() < mS[BaroUpdate] ); + ReadBoschBaro(); // Pressure + BaroPressure = BaroVal.u16; + + AcquiringPressure = !AcquiringPressure; + StartBoschBaroADC(AcquiringPressure); // Temperature + while ( mSClock() < mS[BaroUpdate] ); + ReadBoschBaro(); + BaroTemperature = BaroVal.u16; + + TxVal32(BaroTemperature,0,HT); + TxVal32(BaroPressure,0,0); + TxNextLine(); + + Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature); + BaroQ.B[s] = Temp; + CompBaroPressure += Temp; + + AcquiringPressure = !AcquiringPressure; + StartBoschBaroADC(AcquiringPressure); + } + + } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) && ( abs(CompBaroPressure - CompBaroPressureP) > 12 ) ); // stable within ~0.5M + + OriginBaroPressure = SRS32(CompBaroPressure, 2); + + F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES; + BaroRelAltitudeP = BaroRelAltitude = 0.0; + +#ifdef SIMULATE + FakeBaroRelAltitude = 0.0; +#endif // SIMULATE + +} // InitBoschBarometer