Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: baro.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
--- a/baro.c Fri Feb 25 01:35:24 2011 +0000
+++ b/baro.c Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
// = 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 =
+// = http://code.google.com/p/uavp-mods/ =
// ===============================================================================================
// This is part of UAVXArm.
@@ -75,8 +75,8 @@
} // ShowBaro
void BaroTest(void) {
+
TxString("\r\nAltitude test\r\n");
-
TxString("Initialising\r\n");
InitBarometer();
@@ -87,7 +87,9 @@
TxString("\r\nType:\t");
ShowBaroType();
- TxString("Init Retries:\t");
+ TxString("BaroScale:\t");
+ TxVal32(P[BaroScale],0,0);
+ TxString("\r\nInit Retries:\t");
TxVal32((int32)BaroRetries - 2, 0, ' '); // always minimum of 2
if ( BaroRetries >= BARO_INIT_RETRIES )
TxString(" FAILED Init.\r\n");
@@ -95,7 +97,9 @@
TxNextLine();
if ( BaroType == BaroMPX4115 ) {
- TxString("Range :\t");
+ TxString("\r\nAddress :\t0x");
+ TxValH(MCP4725_ID_Actual);
+ TxString("\r\nRange :\t");
TxVal32((int32) BaroDescentAvailable * 10.0, 1, ' ');
TxString("-> ");
TxVal32((int32) BaroClimbAvailable * 10.0, 1, 'M');
@@ -106,15 +110,19 @@
TxNextLine();
}
- if ( !F.BaroAltitudeValid ) goto BAerror;
+ if ( F.BaroAltitudeValid ) {
+
+ while ( !F.NewBaroValue )
+ GetBaroAltitude();
+
+ F.NewBaroValue = false;
- while ( !F.NewBaroValue )
- GetBaroAltitude();
- F.NewBaroValue = false;
+ TxString("Alt.: \t");
+ TxVal32(BaroRelAltitude * 10.0, 1, ' ');
+ TxString("M\r\n");
- TxString("Alt.: \t");
- TxVal32(BaroRelAltitude * 10.0, 1, ' ');
- TxString("M\r\n");
+ } else
+ TxString("Barometer FAILED\r\n");
TxString("\r\nR.Finder: \t");
if ( F.RangefinderAltitudeValid ) {
@@ -128,9 +136,6 @@
TxVal32((int32)AmbientTemperature.i16, 1, ' ');
TxString("C\r\n");
- return;
-BAerror:
- TxString("FAIL\r\n");
} // BaroTest
void GetBaroAltitude(void) {
@@ -167,7 +172,7 @@
Stats[BaroFailS]++;
}
- Temp = Limit( Temp , -BARO_SANITY_CHECK_MPS, BARO_SANITY_CHECK_MPS );
+ Temp = Limit1(Temp, BARO_SANITY_CHECK_MPS);
ROC = ROC * 0.9 + Temp * 0.1;
BaroRelAltitudeP = BaroRelAltitude;
@@ -191,18 +196,17 @@
BaroRelAltitude = BaroRelAltitudeP = CompBaroPressure = OriginBaroPressure = 0;
BaroType = BaroUnknown;
- Comp[Alt] = AltDiffSum = AltDSum = 0;
+ AltComp = AltDiffSum = AltDSum = 0;
F.BaroAltitudeValid= true; // optimistic
-
+
if ( IsFreescaleBaroActive() )
InitFreescaleBarometer();
else
if ( IsBoschBaroActive() )
InitBoschBarometer();
- else {
+ else
F.BaroAltitudeValid = F.HoldingAlt = false;
- Stats[BaroFailS]++;
- }
+
} // InitBarometer
// -----------------------------------------------------------
@@ -222,32 +226,43 @@
void SetFreescaleMCP4725(int16 d) {
static i16u dd;
- static uint8 r;
dd.u16 = d << 4; // left align
I2CBARO.start();
- r = I2CBARO.write(MCP4725_ID_Actual) != I2C_ACK;
- r = I2CBARO.write(MCP4725_CMD) != I2C_ACK;
- r = I2CBARO.write(dd.b1) != I2C_ACK;
- r = I2CBARO.write(dd.b0) != I2C_ACK;
+ if ( I2CBARO.write(MCP4725_ID_Actual) != I2C_ACK ) goto MCP4725Error;
+ if ( I2CBARO.write(MCP4725_CMD) != I2C_ACK ) goto MCP4725Error;
+ if ( I2CBARO.write(dd.b1) != I2C_ACK ) goto MCP4725Error;
+ if ( I2CBARO.write(dd.b0) != I2C_ACK ) goto MCP4725Error;
I2CBARO.stop();
+ return;
+
+MCP4725Error:
+ I2CBARO.stop();
+ I2CError[MCP4725_ID_Actual]++;
+
} // SetFreescaleMCP4725
boolean IdentifyMCP4725(void) {
static boolean r;
-
+
r = true;
- if ( I2CBAROAddressResponds( MCP4725_ID_0xC8 ) )
- MCP4725_ID_Actual = MCP4725_ID_0xC8;
- else
- if ( I2CBAROAddressResponds( MCP4725_ID_0xCC ) )
- MCP4725_ID_Actual = MCP4725_ID_0xCC;
+ MCP4725_ID_Actual = MCP4725_ID_0xCC;
+ if ( I2CBAROAddressResponds( MCP4725_ID_0xCC ) )
+ MCP4725_ID_Actual = MCP4725_ID_0xCC;
+ else
+ if ( I2CBAROAddressResponds( MCP4725_ID_0xC8 ) )
+ MCP4725_ID_Actual = MCP4725_ID_0xC8;
else
r = false;
- return(r);
+
+ return(r);
+
+// MCP4725_ID_Actual = FORCE_BARO_ID;
+// return(true);
+
} // IdentifyMCP4725
void SetFreescaleOffset(void) {
@@ -280,13 +295,14 @@
Delay1mS(100);
ReadFreescaleBaro();
- while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 2) ) {
- BaroOffsetDAC -= 2;
+ while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 1) ) {
+ BaroOffsetDAC -= 1;
SetFreescaleMCP4725(BaroOffsetDAC);
- Delay1mS(10);
+ Delay1mS(10); // 10
ReadFreescaleBaro();
TxVal32(BaroOffsetDAC,0,HT);
- TxVal32(BaroVal.u16,0,' ');
+ TxVal32(BaroVal.u16,0,HT);
+ TxVal32((int32)FreescaleToDM(BaroVal.u16), 1, 0);
TxNextLine();
LEDYellow_TOG;
}
@@ -303,11 +319,13 @@
mS[BaroUpdate] = mSClock() + ADS7823_TIME_MS;
I2CBARO.start(); // start conversion
+ if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto ADS7823Error;
+ if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto ADS7823Error;
+ I2CBARO.stop();
- if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto FSError;
- if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto FSError;
+ if ( I2CBARO.blockread(ADS7823_RD, B, 8) ) goto ADS7823Error;
- I2CBARO.blockread(ADS7823_RD, B, 8); // read block of 4 baro samples
+ // read block of 4 baro samples
B0.b0 = B[1];
B0.b1 = B[0];
@@ -324,15 +342,17 @@
return;
-FSError:
+ADS7823Error:
I2CBARO.stop();
- F.BaroAltitudeValid = F.HoldingAlt = false;
+ I2CError[ADS7823_ID]++;
+
+ // 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
@@ -369,15 +389,15 @@
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
+ if ( P[BaroScale] <= 0 )
+ P[BaroScale] = 56; // failsafe setting
+
BaroPressure = 0;
-
BaroRetries = 0;
do {
BaroPressureP = BaroPressure;
@@ -388,7 +408,7 @@
ReadFreescaleBaro();
BaroPressure = (int24)BaroVal.u16;
} while ( ( ++BaroRetries < BARO_INIT_RETRIES )
- && ( abs((int16)(BaroPressure - BaroPressureP)) > Error ) );
+ && ( fabs( FreescaleToDM(BaroPressure - BaroPressureP) ) > 5 ) );
F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;
@@ -444,54 +464,57 @@
}
I2CBARO.start();
- if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto SBerror;
-
+ if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto BoschError;
// access control register, start measurement
- if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto SBerror;
-
+ if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto BoschError;
// select 32kHz input, measure temperature
- if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto SBerror;
+ if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto BoschError;
I2CBARO.stop();
F.BaroAltitudeValid = true;
return;
-SBerror:
+BoschError:
I2CBARO.stop();
- F.BaroAltitudeValid = F.HoldingAlt = false;
- return;
+
+ I2CError[BOSCH_ID]++;
+ F.BaroAltitudeValid = false;
+
} // StartBoschBaroADC
void ReadBoschBaro(void) {
+
// Possible I2C protocol error - split read of ADC
I2CBARO.start();
- if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto RVerror;
- if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto RVerror;
+ if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
+ if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto BoschError;
I2CBARO.start(); // restart
- if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto RVerror;
+ if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
BaroVal.b1 = I2CBARO.read(I2C_NACK);
I2CBARO.stop();
I2CBARO.start();
- if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto RVerror;
- if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto RVerror;
+ if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
+ if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto BoschError;
I2CBARO.start(); // restart
- if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto RVerror;
+ if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
BaroVal.b0 = I2CBARO.read(I2C_NACK);
I2CBARO.stop();
F.BaroAltitudeValid = true;
return;
-RVerror:
+BoschError:
I2CBARO.stop();
+ I2CError[BOSCH_ID]++;
+
F.BaroAltitudeValid = F.HoldingAlt = false;
if ( State == InFlight ) {
Stats[BaroFailS]++;
F.BaroFailure = true;
}
- return;
+
} // ReadBoschBaro
#define BOSCH_BMP085_TEMP_COEFF 62L
@@ -535,10 +558,8 @@
F.NewBaroValue = F.BaroAltitudeValid;
}
- else {
+ else
AcquiringPressure = true;
- Stats[BaroFailS]++;
- }
StartBoschBaroADC(AcquiringPressure);
}
@@ -565,6 +586,7 @@
return(true);
BoschInactive:
+
return(false);
} // IsBoschBaroActive