Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: baro.c
- Revision:
- 0:62a1c91a859a
- Child:
- 1:1e3318a30ddd
--- /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