void DoCompassTest()
{
uint16 v;
TxString("\r\nCompass test\r\n");
I2CStart();
if( SendI2CByte(COMPASS_I2C_ID+1) != I2C_ACK ) goto CTerror;
v = ((uint16)RecvI2CByte(I2C_ACK)*256) | RecvI2CByte(I2C_NACK);
I2CStop();
TxVal32((int32)v, 1, 0);
TxString("deg\r\n");
return;
CTerror:
I2CStop();
TxString("FAIL\r\n");
} // DoCompassTest
// read temp & pressure values from baro sensor
// return value= niltemp;
// returns 1 if value is available
uns8 ReadValueFromBaro(void)
{
#ifdef NOT_PORTED_FOR18F // use gregs code when ready
// test if conversion is ready
I2CStart();
if( SendI2CByte(BARO_ADDR) != I2C_ACK ) goto RVerror;
// access control register
if( SendI2CByte(0xf4) != I2C_ACK ) goto RVerror;
I2CStart(); // restart
if( SendI2CByte(BARO_ADDR+1) != I2C_ACK ) goto RVerror;
// read control register
niltemp.low8 = RecvI2CByte(I2C_NACK);
I2CStop();
if( (niltemp.low8 & 0b0010.0000) == 0 )
{ // conversion is ready, read it!
I2CStart();
if( SendI2CByte(BARO_ADDR) != I2C_ACK ) goto RVerror;
// access A/D registers
if( SendI2CByte(0xf6) != I2C_ACK ) goto RVerror;
I2CStart(); // restart
if( SendI2CByte(BARO_ADDR+1) != I2C_ACK ) goto RVerror;
niltemp.high8 = RecvI2CByte(I2C_ACK);
niltemp.low8 = RecvI2CByte(!I2C_NACK);
I2CStop();
return(1);
}
return(0);
RVerror:
I2CStop();
#endif
_UseBaro = 0; // read error, disable baro
return(0);
}
// initialize compass sensor
void InitDirection(void)
{
#ifdef NOT_PORTED_FOR18F // use gregs code when ready
// set Compass device to Compass mode
I2CStart();
if( SendI2CByte(COMPASS_ADDR) != I2C_ACK ) goto IDerror;
// this initialization is no longer needed, is done by testsoftware!
#if 0
if( SendI2CByte('G') != I2C_ACK ) goto IDerror;
if( SendI2CByte(0x74) != I2C_ACK ) goto IDerror;
// select operation mode, continuous mode, 20 Hz
if( SendI2CByte(0b0.11.1.00.10) != I2C_ACK ) goto IDerror;
I2CStop();
I2CStart();
if( SendI2CByte(COMPASS_ADDR) != I2C_ACK ) goto IDerror;
if( SendI2CByte('r') != I2C_ACK ) goto IDerror;
if( SendI2CByte(0x06) != I2C_ACK ) goto IDerror;
I2CStop();
// read multiple read count to avoid wear-and-tear on EEPROM
I2CStart();
if( SendI2CByte(COMPASS_ADDR+1) != I2C_ACK ) goto IDerror;
if( RecvI2CByte(!I2C_ACK) != 16 )
{ // not correctly set, set it up.
I2CStop();
I2CStart();
if( SendI2CByte(COMPASS_ADDR) != I2C_ACK ) goto IDerror;
if( SendI2CByte('w') != I2C_ACK ) goto IDerror;
if( SendI2CByte(0x06) != I2C_ACK ) goto IDerror;
if( SendI2CByte(16) != I2C_ACK ) goto IDerror;
}
#endif
_UseCompass = 1;
IDerror:
I2CStop();
#endif
_UseCompass = 0;
}
// Read direction, convert it to 2 degrees unit
// and store result in variable AbsDirection.
// The current heading correction is stored in CurDeviation
void GetDirection(void)
{
#ifdef NOT_PORTED_FOR18F // use gregs code when ready
int16 DirVal, temp;
// set Compass device to Compass mode
I2CStart();
if( SendI2CByte(COMPASS_ADDR+1) != I2C_ACK )
{
I2CStop();
CurDeviation = 0; // no sensor present, deviation = 0
return;
}
DirVal.high8 = RecvI2CByte(I2C_ACK);
DirVal.low8 = RecvI2CByte(!I2C_ACK);
I2CStop();
// DirVal has 1/10th degrees
// convert to set 360.0 deg = 240 units
DirVal /= 15;
// must use pre-decrement, because of dumb compiler
if( AbsDirection > COMPASS_MAX )
{
CurDeviation = 0;
AbsDirection--;
}
else
{
// setup desired heading (AbsDirection)
if( AbsDirection == COMPASS_MAX ) // no heading stored yet
{
AbsDirection = DirVal; // store current heading
CurDeviation = 0;
}
// calc deviation and direction of deviation
DirVal = AbsDirection - DirVal;
// handle wraparound
if( DirVal <= -240/2 )
DirVal += 240;
if( DirVal > 240/2 )
DirVal -= 240;
// positive means ufo is left off-heading
// negative means ufo is right off-heading
if( DirVal > 20 ) // limit to give soft reaction
DirVal = 20;
if( DirVal < -20 )
DirVal = -20;
// Empirical found :-)
// New_CurDev = ((3*Old_CurDev)+DirVal) / 4
//
temp = (long)CurDeviation; // the previous value!
temp *= 3;
temp += DirVal; // add the new value
temp <<= 2; // = 16* NewCurDev
temp *= CompassFactor;
CurDeviation = temp.high8;
}
#endif
}
