// 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();
}
int PCF8574::i2cRead(uint8_t value){
Wire1.requestFrom((uint8_t )PCFaddress, (uint8_t )1);
if (Wire1.available()) PCFPORTA = (int) Wire1.read();
else PCFPORTA = (int)value; //error condition
//return value;
return PCFPORTA;
}
/** 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 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();
}
/** 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;
}
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;
}
void PCF8574::i2cRead(){
uint8_t value = 0x00;
Wire1.requestFrom((uint8_t )PCFaddress, (uint8_t )1);
if (Wire1.available()) PCFPORTA = (int) Wire1.read();
else PCFPORTA = (int)value; //error condition
}
// 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;
}