void BaroTest(void)
{
uint8 r;
TxString("\r\nBarometer test\r\n");
if ( !_UseBaro ) goto BAerror;
if ( BaroType == BARO_ID_BMP085 )
TxString("Type:\tBMP085\r\n");
else
TxString("Type:\tSMD500\r\n");
if( !StartBaroADC(BARO_PRESS) ) goto BAerror;
Delay1mS(BARO_PRESS_TIME);
r = ReadValueFromBaro();
TxString("Press: \t");
TxVal32((int32)BaroVal, 0, 0);
if( !StartBaroADC(BaroTemp) ) goto BAerror;
Delay1mS(BARO_TEMP_TIME);
r = ReadValueFromBaro();
TxString("\tTemp: ");
TxVal32((int32)BaroVal, 0, 0);
TxNextLine();
TxNextLine();
return;
BAerror:
I2CStop();
TxString("FAIL\r\n");
} // BaroTest
// initialize compass sensor
void InitAltimeter(void)
{
#ifdef NOT_PORTED_FOR18F // use gregs code when ready
// read temperature once to get base value
// set SMD500 device to start temperature conversion
if( !StartBaroADC(0xee) ) goto BAerror;
// wait 40ms
for( Vtmp=40; Vtmp!=0; Vtmp--)
{
T0IF=0;
while(T0IF == 0);
}
ReadValueFromBaro();
BaseTemp = niltemp; // save start value
// read pressure once to get base value
// set SMD500 device to start pressure conversion
if( !StartBaroADC(0xf4) ) goto BAerror;
// wait 40ms
for( Vtmp=40; Vtmp!=0; Vtmp--)
{
T0IF=0;
while(T0IF == 0);
}
ReadValueFromBaro();
BasePressure = niltemp;
_UseBaro = 1;
// prepare for next run
// if( !StartBaroADC(0xf4) ) goto BAerror;
return;
BAerror:
I2CStop();
#endif
_UseBaro = 0;
}
void ComputeBaroComp(void)
{
#ifdef NOT_PORTED_FOR18F // use gregs code when ready
if( ReadValueFromBaro() ) // returns niltemp as value
{ // successful
if( !_BaroTempRun )
{ // current measurement was "pressure"
if( ThrDownCount ) // while moving throttle stick
{
BasePressure = niltemp; // current read value is the new level
BaroCompSum = 0;
}
else
{ // while holding altitude
niltemp -= BasePressure;
//SendComChar('B');
// the uncorrected relative height
// SendComValH(niltemp.high8);
// SendComValH(niltemp.low8);
// SendComValH(TempCorr.high8);
// SendComValH(TempCorr.low8);
// niltemp1 has -400..+400 approx
niltemp1 = TempCorr * BaroTempCoeff;
niltemp1 += 16;
niltemp1 /= 32;
niltemp += niltemp1; // compensating temp
// the corrected relative height, the higher alti, the lesser value
// New Baro = (3*BaroSum + New_Baro)/4
niltemp1 = niltemp; // because of bank bits
niltemp = BaroCompSum; // remember old value for delta
BaroCompSum *= 3;
BaroCompSum += niltemp1;
BaroCompSum += 2; // rounding
BaroCompSum >>= 2; // div by 4
niltemp1 = BaroCompSum - niltemp; // subtract new height to get delta
#ifdef INTTEST
SendComChar('a');
SendComValH(BaroCompSum.high8);
SendComValH(BaroCompSum.low8); // current height
SendComChar(';');
SendComValH(TempCorr.high8);
SendComValH(TempCorr.low8); // current temp
SendComChar(';');
SendComValH(niltemp1.low8); // delta height
SendComChar(';');
#endif
// was: +10 and -5
if( BaroCompSum > 8 ) // zu tief: ordentlich Gas geben
BaroCompSum = 8;
if( BaroCompSum < -3 ) // zu hoch: nur leicht nachlassen
BaroCompSum = -3;
// weiche Regelung (proportional)
// nitemp kann nicht überlaufen (-3..+8 * PropFact)
nitemp = (int)BaroCompSum.low8 * BaroThrottleProp;
if( VBaroComp > nitemp )
VBaroComp--;
else
if( VBaroComp < nitemp )
VBaroComp++;
if( VBaroComp > nitemp )
VBaroComp--;
else
if( VBaroComp < nitemp )
VBaroComp++;
// Differentialanteil
if( niltemp1 > 8 )
niltemp1.low8 = 8;
else
if( niltemp1 < -8 )
niltemp1.low8 = -8;
nitemp = (int)niltemp1.low8 * BaroThrottleDiff;
VBaroComp += nitemp;
if( VBaroComp > 15 )
VBaroComp = 15;
if( VBaroComp < -5 )
VBaroComp = -5;
#ifdef INTTEST
SendComValH(VBaroComp);
SendComChar(0x0d);
SendComChar(0x0a);
#endif
}
StartBaroADC(0xee); // next is temp
}
else
{
if( ThrDownCount )
BaseTemp = niltemp; // current read value
else // TempCorr: The warmer, the higher
{
TempCorr = niltemp - BaseTemp;
// TempCorr += 4; // compensate rounding error later /8
}
StartBaroADC(0xf4); // next is pressure
}
}