// Read the ITG3200 Gyroscope
void Read_Gyro() {
// Point to the first data register
I2C->beginTransmission(GyroAddress);
I2C->write(0x1B); // point to the first data register
I2C->endTransmission();
// read 8 byte, from address 4 (Data Registers)
I2C->beginTransmission(GyroAddress);
I2C->requestFrom(GyroAddress, 8);
if (I2C->available() >= 8) {
//Just confirming that the data order is Temp, X, Y, Z
Gyro_T = (I2C->read() * 256) + I2C->read(); // Temp MSB * 256 + Temp LSB
Gyro_X = (I2C->read() * 256) + I2C->read(); // X axis MSB * 256 + X axis LSB
Gyro_Y = (I2C->read() * 256) + I2C->read(); // Y axis MSB * 256 + Y axis LSB
Gyro_Z = (I2C->read() * 256) + I2C->read(); // Z axis MSB * 256 + Z axis LSB
}
// Incorrent number of returned bytes
else {
Serial.println("Recieving incorrect amount of bytes from Gyroscope");
while(I2C->available()) {
Serial.print("data byte = ");
Serial.println(I2C->read(), DEC); //print the returned number as a decimal
}
}
I2C->endTransmission();
}
// Initialise the device
void Init_Dev(TwoWire *I2C_Ptr) {
I2C = I2C_Ptr; // Pointer to the "Wire" I2C object
// Setup the ADXL345 Accelerometer
Serial.print("Setting up ADXL345 Accelerometer...");
//Put the sensor into measurement mode. The data sheet recommends that you first Reset, then Sleep mode then Measurement mode.
I2C->beginTransmission(AccelAddress);
I2C->write(0x2D); //Reset the POWER_CTL (0x2D) register.
I2C->write(0b00000000);
I2C->endTransmission();
I2C->beginTransmission(AccelAddress);
I2C->write(0x2D); //Put the ADXL345 into StandBy mode by writing 0x10 to the POWER_CTL (0x2D) register.
I2C->write(0b00010000);
I2C->endTransmission();
I2C->beginTransmission(AccelAddress);
I2C->write(0x2D); //Put the ADXL345 into Measurement Mode by writing 0x08 to the POWER_CTL (0x2D) register.
I2C->write(0b00001000);
I2C->endTransmission();
// force a calibration event
Accel_X_Center_Ave = 99;
//Set the G force range the sensor will work in
I2C->beginTransmission(AccelAddress);
I2C->write(0x31); //Put the ADXL345 into +/- 4G range by writing the value 0x01 to the DATA_FORMAT (0x31) register.
I2C->write(0b00000001); //(Default is +/-2G range
I2C->endTransmission();
Serial.println("Done");
delay(100);
}
/* The "train" requested by servo motors is
the minimal of 20ms. Our PWM allow us to have
41.7Hz that means 23.98 ms.
However the cypress does not offer a good angle
resolution on this frequency and the user
has option to operates the servos in 188Hz */
void Servo::prepare_pin(uint8_t pin)
{
extern TwoWire Wire;
Wire.begin();
// let's use this function only to select the bit port
// the datasheet is a little confusing regading this set
analogWrite(pin, 1);
// Select programmable PWM CLK source to 367.7 Hz
Wire.beginTransmission(CYPRESS_I2C_ADDRESS);
Wire.write(0x29);
Wire.write(0x04);
Wire.endTransmission();
// Rising edge register
Wire.beginTransmission(CYPRESS_I2C_ADDRESS);
Wire.write(0x2a);
Wire.write(0xff);
Wire.endTransmission();
// Set divider to get 47.4Hz freq.
Wire.beginTransmission(CYPRESS_I2C_ADDRESS);
Wire.write(0x2C);
if (this->is188hz)
Wire.write(0x02);
else
Wire.write(0x09);
Wire.endTransmission();
}
/** Read multiple bytes from an 8-bit device register.
* @param useSPI true : use SPI
* @param devAddr I2C slave device address
* @param regAddr First register regAddr to read from
* @param length Number of bytes to read
* @param data Buffer to store read data in
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Number of bytes read (0 indicates failure)
*/
int8_t I2Cdev::readBytes(bool useSPI, uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(useSPI ? "SPI (0x" : "I2C 0x");
Serial.print(devAddr, HEX);
Serial.print(") reading ");
Serial.print(length, DEC);
Serial.print(" bytes from 0x");
Serial.print(regAddr, HEX);
Serial.print("...");
#endif
int8_t count = 0;
// I2C
if (!useSPI) {
Wire.beginTransmission(devAddr);
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.send(regAddr);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.write(regAddr);
#endif
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, length);
uint32_t t1 = millis();
for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
data[count] = Wire.receive();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
data[count] = Wire.read();
#endif
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
#endif
}
if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout
Wire.endTransmission();
} else {
digitalWrite(devAddr, LOW);
byte Addr = regAddr | 0x80;
SPI.transfer(Addr);
for (uint8_t cnt=0; cnt < length; cnt++) {
data[cnt] = SPI.transfer(0);
count++;
}
digitalWrite(devAddr, HIGH);
}
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(". Done (");
Serial.print(count, DEC);
Serial.println(" read).");
#endif
return count;
}
/** Read multiple bytes from an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr First register regAddr to read from
* @param length Number of bytes to read
* @param data Buffer to store read data in
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Number of bytes read (0 indicates failure)
*/
int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print("I2C (0x");
Serial.print(devAddr, HEX);
Serial.print(") reading ");
Serial.print(length, DEC);
Serial.print(" bytes from 0x");
Serial.print(regAddr, HEX);
Serial.print("...");
#endif
int8_t count = 0;
Wire.beginTransmission(devAddr);
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.send(regAddr);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.write(regAddr);
#endif
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, length);
uint32_t t1 = millis();
for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
data[count] = Wire.receive();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
data[count] = Wire.read();
#endif
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
#endif
}
if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout
Wire.endTransmission();
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(". Done (");
Serial.print(count, DEC);
Serial.println(" read).");
#endif
return count;
}
void MCP23018_Init_IOCON(void)
{
delayMicroseconds(1000u);
i2c.beginTransmission((uint8_t)MISC_ADDR);
i2c.write(0x0Au); // select IOCON
i2c.write(0xA0u); // BANK=1, ADDR pointer NOT incremented
i2c.endTransmission();
delayMicroseconds(1000u);
i2c.beginTransmission((uint8_t)VEH_IO_ADDR);
i2c.write(0x0Au); // select IOCON
i2c.write(0xA0u); // BANK=1, ADDR pointer NOT incremented
i2c.endTransmission();
}
void MCP23018_WRITE(uint8_t device, uint8_t reg, uint8_t data)
{
uint8_t addr;
if(DEV_MISC == device){ addr = MISC_ADDR; }
else{ addr = VEH_IO_ADDR; }
i2c.beginTransmission(addr);
i2c.write(reg);
i2c.write(data);
i2c.endTransmission();
}
void PCF8574::i2cSend(){
PCFBUFFER = i2cRead(0x00);
for(int i=0;i<8;i++){
if(bitRead(PCFTRISA,i) == INPUT) bitWrite(PCFPORTA,i,bitRead(PCFBUFFER,i));
if(PCFPULL[i] == 1) bitWrite(PCFPORTA,i,1); // Required for unblock pull level
if(PCFPULL[i] == 2) bitWrite(PCFPORTA,i,0); // Required for unblock pull level
}
Wire1.beginTransmission(PCFaddress);
Wire1.write((uint8_t )PCFPORTA);
Wire1.endTransmission();
}
static void writeCommand(byte reg, byte value)
{
Wire1.beginTransmission((uint8_t)BMP085_ADDRESS);
#if ARDUINO >= 100
Wire1.write((uint8_t)reg);
Wire1.write((uint8_t)value);
#else
Wire1.send(reg);
Wire1.send(value);
#endif
Wire1.endTransmission();
}
static bool NunchuckInitialize(TwoWire& interface)
{
interface.begin(); // join i2c bus as master
interface.beginTransmission(0x52);// transmit to device 0x52
interface.write((uint8_t)0xF0);// sends a zero.
interface.write((uint8_t)0x55);// sends a zero.
interface.write((uint8_t)0xFB);// sends a zero.
interface.write((uint8_t)0x00);// sends a zero.
interface.endTransmission();// stop transmitting
return 1;
}
// Initialise the device
void Init_Dev(TwoWire *I2C_Ptr) {
I2C = I2C_Ptr; // Pointer to the "Wire" I2C object
// Setup the HMC5883L Magnetometer
Serial.print("Setting up HMC5883L Magnetometer...");
I2C->beginTransmission(CompassAddress);
I2C->write(0x00); // point to register address 1 (Configuration Register A)
I2C->write(0b01110000); // set to 8 samples per reading @ 15Hz
I2C->endTransmission();
I2C->beginTransmission(CompassAddress);
I2C->write(0x01); // point to register address 1 (Configuration Register B)
I2C->write(0b00100000); // Set gain to 1 (default - sensor bandwidth +/-1.3 Guass & Scale 0.92)
I2C->endTransmission();
I2C->beginTransmission(CompassAddress);
I2C->write(0x02); // point to register address 2 (Mode Register)
I2C->write(0b00000000); // Set continuous measurement mode
I2C->endTransmission();
// Help start the calibration process - especially through the initial cycles - with some rough initial data
// These are a bit smaller than what is ultimatly expected by about 1/4. But will help until the ultimate
// Min's and Max's are finally found through device rotation.
Compass_Max_X = 450;
Compass_Min_X = -450;
Compass_Max_Y = 450;
Compass_Min_Y = -450;
Compass_Max_Z = 450;
Compass_Min_Z = -450;
// Set declination angle for this location
Compass_Declination_Angle = 0.20595; // 11 deg 48' = Cranbourne, Victoria, Australia
Serial.println("Done");
delay(100);
}
// Initialise the device
void Init_Dev(TwoWire *I2C_Ptr) {
I2C = I2C_Ptr; // Pointer to the "Wire" I2C object
// Setup the ITG3200 Gyroscope
Serial.print("Setting up ITG3200 Gyroscope...");
//Set the sample rate to 100 hz
I2C->beginTransmission(GyroAddress);
I2C->write(0x15); // point to Sample Rate Divider register address
// I2C->write(0x09); // (internal sample rate of 8 kHz) / (*9* + 1) = 800 per second (800Hz)
I2C->write(0b01001111); // (internal sample rate of 8 kHz) / (*79* + 1) = 100 per second (100Hz)
I2C->endTransmission();
//Set the gyroscope scale for +/-2000 degrees per second
I2C->beginTransmission(GyroAddress);
I2C->write(0x16); // point to DLPF and FS register address (0x16)
I2C->write(0b00011000); // set Full Scale range (+/-2000deg/sec) and DLPF (LPF=256Hz and Internal Sample Rate = 8kHz)
I2C->endTransmission();
Serial.println("Done");
delay(100);
}
// Read the ADXL345 Accelerometer
void Read_Accel() {
I2C->beginTransmission(AccelAddress); // start transmission to device
I2C->write(0x32); // point to the first data register DATAX0
I2C->endTransmission(); // end transmission
// read 6 byte, from address 32 (Data Registers)
I2C->beginTransmission(AccelAddress); // start transmission to device
I2C->requestFrom(AccelAddress, 6);
if (I2C->available() >= 6) {
Accel_X = I2C->read() + (I2C->read() * 256); // X axis LSB + X axis MSB * 256
Accel_Y = I2C->read() + (I2C->read() * 256); // Y axis LSB + Y axis MSB * 256
Accel_Z = I2C->read() + (I2C->read() * 256); // Z axis LSB + Z axis MSB * 256
}
// Incorrent number of returned bytes
else {
Serial.println("Recieving incorrect amount of bytes from Accelerometer");
while(I2C->available()) {
Serial.print("data byte = ");
Serial.println(I2C->read(), DEC); //print the returned number as a decimal
}
}
I2C->endTransmission();
}
void MCP342X::selectChannel(byte channel, byte gain)
{
//configuration register, assuming 16 bit resolution
// Initiate one shot conversion
// 16 bits, 66.6 ms later the data should be available.
byte reg = 1 << BIT_RDY |
channel << BIT_C0 |
0 << BIT_OC |
1 << BIT_S1 |
gain;
Wire1.beginTransmission(I2C_ADDRESS);
Wire1.write(reg);
Wire1.endTransmission();
}
static void read16(byte reg, uint16_t *value)
{
Wire1.beginTransmission((uint8_t)BMP085_ADDRESS);
#if ARDUINO >= 100
Wire1.write((uint8_t)reg);
#else
Wire1.send(reg);
#endif
Wire1.endTransmission();
Wire1.requestFrom((uint8_t)BMP085_ADDRESS, (byte)2);
#if ARDUINO >= 100
*value = (Wire1.read() << 8) | Wire1.read();
#else
*value = (Wire1.receive() << 8) | Wire1.receive();
#endif
Wire1.endTransmission();
}
uint8_t MCP23018_READ(uint8_t device, uint8_t reg)
{
uint8_t addr;
uint8_t result;
if(DEV_MISC == device){ addr = MISC_ADDR; }
else{ addr = VEH_IO_ADDR; }
i2c.beginTransmission(addr);
i2c.write(reg); // IODIRA.BANK1 Address
i2c.endTransmission();
i2c.requestFrom(addr,(uint8_t)1u);
while (i2c.available() < 1u);
result = i2c.receive();
return result;
}
/** Read multiple words from a 16-bit device register.
* @param useSPI true : use SPI
* @param devAddr I2C slave device address or Slave Select pin if SPI
* @param regAddr First register regAddr to read from
* @param length Number of words to read
* @param data Buffer to store read data in
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Number of words read (0 indicates failure)
*/
int8_t I2Cdev::readWords(bool useSPI, uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) {
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(useSPI ? "SPI (0x" : "I2C (0x");
Serial.print(devAddr, HEX);
Serial.print(") reading ");
Serial.print(length, DEC);
Serial.print(" words from 0x");
Serial.print(regAddr, HEX);
Serial.print("...");
#endif
int8_t count = 0;
Wire.beginTransmission(devAddr);
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.send(regAddr);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.write(regAddr);
#endif
if (!useSPI) {
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
uint32_t t1 = millis();
bool msb = true; // starts with MSB, then LSB
for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
if (msb) {
// first byte is bits 15-8 (MSb=15)
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
data[count] = Wire.receive() << 8;
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
data[count] = Wire.read() << 8;
#endif
} else {
// second byte is bits 7-0 (LSb=0)
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
data[count] |= Wire.receive();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
data[count] |= Wire.read();
#endif
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
#endif
count++;
}
msb = !msb;
}
if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout
Wire.endTransmission();
} else {
uint8_t _byteCnt = (uint8_t)(length * 2);
byte Addr = regAddr | 0x80;
digitalWrite(devAddr, LOW);
SPI.transfer(Addr);
bool msb = true;
for (uint8_t cnt=0; cnt < _byteCnt; cnt++) {
if (msb) {
data[cnt] = SPI.transfer(0) << 8;
}
else
{
data[cnt] |= SPI.transfer(0);
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
#endif
count++;
}
msb = !msb;
}
digitalWrite(devAddr, HIGH);
}
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(". Done (");
Serial.print(count, DEC);
Serial.println(" read).");
#endif
return count;
}
static bool NunchuckReadData(TwoWire& interface, NunchuckData& data)
{
interface.requestFrom(0x52, 6);
data.jX = interface.read();
data.jY = interface.read();
data.accX = interface.read();
data.accY = interface.read();
data.accZ = interface.read();
data.buttons = interface.read();
data.accZ<<=2;
data.accZ|=data.buttons>>6;
data.accY<<=2;
data.accY|=(data.buttons>>4)&0x3;
data.accX<<=2;
data.accX|=(data.buttons>>2)&0x3;
data.buttons&=0x3;
SERIAL_PORT_MONITOR.print("Joy : ");
SERIAL_PORT_MONITOR.print(data.jX);
SERIAL_PORT_MONITOR.print(", ");
SERIAL_PORT_MONITOR.print(data.jY);
SERIAL_PORT_MONITOR.print("\tAcc : ");
SERIAL_PORT_MONITOR.print(data.accX);
SERIAL_PORT_MONITOR.print(", ");
SERIAL_PORT_MONITOR.print(data.accY);
SERIAL_PORT_MONITOR.print(", ");
SERIAL_PORT_MONITOR.print(data.accZ);
SERIAL_PORT_MONITOR.print("\tBtn : ");
SERIAL_PORT_MONITOR.print(" [");
SERIAL_PORT_MONITOR.print(data.buttons);
SERIAL_PORT_MONITOR.print("] ");
switch(data.buttons)
{
case 0x0ul:
SERIAL_PORT_MONITOR.println("C + Z");
break;
case 0x1ul:
SERIAL_PORT_MONITOR.println("C");
break;
case 0x2ul:
SERIAL_PORT_MONITOR.println("Z");
break;
case 0x3ul:
SERIAL_PORT_MONITOR.println("No key");
break;
default:
break;
}
interface.beginTransmission(0x52);// transmit to device 0x52
interface.write((uint8_t)0x00);// sends a zero.
interface.endTransmission();// stop transmitting
return 1;
}
// Read the HMC5883L Compass
void Read_Compass() {
// Point to the first data register
I2C->beginTransmission(CompassAddress);
I2C->write(0x03); // point to the first data register
I2C->endTransmission();
// read 6 byte, from address 3 (Data Registers)
I2C->beginTransmission(CompassAddress);
I2C->requestFrom(CompassAddress, 6);
if (I2C->available() >= 6) {
//Just confirming that the data register order is X Z Y (ie NOT X Y Z)
Compass_Raw_X = (I2C->read() * 256) + I2C->read(); // X axis MSB * 256 + X axis LSB
Compass_Raw_Z = (I2C->read() * 256) + I2C->read(); // Z axis MSB * 256 + Z axis LSB
Compass_Raw_Y = (I2C->read() * 256) + I2C->read(); // Y axis MSB * 256 + Y axis LSB
}
// Incorrent number of returned bytes
else {
Serial.println("Recieving incorrect amount of bytes from Compass");
while(I2C->available()) {
Serial.print("data byte = ");
Serial.println(I2C->read(), DEC); //print the returned number as a decimal
}
}
I2C->endTransmission();
// Scale the Raw readings based on the sensor scale (Gauss = 1.3 & Scale = 0.92)
Compass_Raw_X *= 0.92;
Compass_Raw_Y *= 0.92;
Compass_Raw_Z *= 0.92;
// Update the Max Min limit readings
if (Compass_Raw_X > Compass_Max_X) Compass_Max_X = Compass_Raw_X;
if (Compass_Raw_X < Compass_Min_X) Compass_Min_X = Compass_Raw_X;
if (Compass_Raw_Y > Compass_Max_Y) Compass_Max_Y = Compass_Raw_Y;
if (Compass_Raw_Y < Compass_Min_Y) Compass_Min_Y = Compass_Raw_Y;
if (Compass_Raw_Z > Compass_Max_Z) Compass_Max_Z = Compass_Raw_Z;
if (Compass_Raw_Z < Compass_Min_Z) Compass_Min_Z = Compass_Raw_Z;
// Update the offset
Compass_Offset_X = (Compass_Max_X + Compass_Min_X) / 2;
Compass_Offset_Y = (Compass_Max_Y + Compass_Min_Y) / 2;
Compass_Offset_Z = (Compass_Max_Z + Compass_Min_Z) / 2;
// Calculate calibrated readings
Compass_Calib_X = Compass_Raw_X - Compass_Offset_X;
Compass_Calib_Y = Compass_Raw_Y - Compass_Offset_Y;
Compass_Calib_Z = Compass_Raw_Z - Compass_Offset_Z;
//--- Calculate the X Y plane heading ---
// Calculate heading (radians) using the X, Y plane - Assuming magnetometer is level
Compass_Heading = atan2(Compass_Calib_Y, Compass_Calib_X);
// Adjust with Declination error (magnetic north vs true north)
Compass_Heading += Compass_Declination_Angle;
// Correct for when signs are reversed.
if(Compass_Heading < 0) Compass_Heading += 2*PI;
// Check for wrap due to addition of declination.
if(Compass_Heading > 2*PI) Compass_Heading -= 2*PI;
// Convert radians to degrees for readability.
Compass_Heading *= 180/M_PI;
//--- Calculate the X Z plane heading ---
// Calculate heading (radians) using the X, Z plane
Compass_Heading_XZ = atan2(Compass_Calib_Z, Compass_Calib_X);
// Adjust with Declination error (magnetic north vs true north)
Compass_Heading_XZ += Compass_Declination_Angle;
// Correct for when signs are reversed.
if(Compass_Heading_XZ < 0) Compass_Heading_XZ += 2*PI;
// Check for wrap due to addition of declination.
if(Compass_Heading_XZ > 2*PI) Compass_Heading_XZ -= 2*PI;
// Convert radians to degrees for readability.
Compass_Heading_XZ *= 180/M_PI;
//--- Calculate the Y Z plane heading ---
// Calculate heading (radians) using the Y, Z plane
Compass_Heading_YZ = atan2(Compass_Calib_Z, Compass_Calib_Y);
// Adjust with Declination error (magnetic north vs true north)
Compass_Heading_YZ += Compass_Declination_Angle;
// Correct for when signs are reversed.
if(Compass_Heading_YZ < 0) Compass_Heading_YZ += 2*PI;
// Check for wrap due to addition of declination.
if(Compass_Heading_YZ > 2*PI) Compass_Heading_YZ -= 2*PI;
// Convert radians to degrees for readability.
Compass_Heading_YZ *= 180/M_PI;
}
/* set I2C register of CY8C9540A to specified value */
static void _cy_set_register(uint8_t reg, uint8_t value) {
Wire.beginTransmission(CY_I2C_ADDR);
Wire.write(reg);
Wire.write(value);
Wire.endTransmission();
}