/* Read a byte from I2C bus */
unsigned char i2c_read_byte(int nack)
{
unsigned char byte = 0;
unsigned bit;
for (bit = 0; bit < 8; bit++)
byte = (byte << 1) | i2c_read_bit();
i2c_write_bit(nack);
return byte;
}
// Read a byte from I2C bus
unsigned char i2cReadByte(unsigned char nack, unsigned char send_stop) {
unsigned char byte = 0;
unsigned bit;
for (bit = 0; bit < 8; bit++) {
byte = (byte << 1) | i2c_read_bit();
}
i2c_write_bit(nack);
if (send_stop) {
i2c_stop();
}
return byte;
}
static unsigned int i2c_write(unsigned char byte)
{
unsigned int bit;
unsigned int ack;
for(bit = 0; bit < 8; bit++) {
i2c_write_bit(byte & 0x80);
byte <<= 1;
}
ack = !i2c_read_bit();
return ack;
}
// Read a byte from I2C bus
unsigned char i2c_read_byte(bool nack, bool send_stop) {
unsigned char byte = 0;
unsigned bit;
for (bit = 0; bit < 8; bit++) {
byte = (byte << 1) | i2c_read_bit();
}
i2c_write_bit(nack);
if (send_stop) {
i2c_stop_cond();
}
return byte;
}
static unsigned char i2c_read(int ack)
{
unsigned char byte = 0;
unsigned int bit;
for(bit = 0; bit < 8; bit++) {
byte <<= 1;
byte |= i2c_read_bit();
}
i2c_write_bit(!ack);
return byte;
}
/*
nack must be 0 if the data reading continues
nack should be 1 after the last byte. send stop after this
*/
unsigned __attribute__ ((noinline)) i2c_read_byte(unsigned nack)
{
unsigned i = 8;
unsigned b = 0;
do
{
b <<= 1;
b |= i2c_read_bit();
i--;
} while ( i != 0 );
i2c_write_bit(nack);
return b;
}
/* Write a byte to I2C bus. Return 0 if ack by the slave */
int i2c_write_byte(unsigned char byte)
{
unsigned bit;
int nack;
for (bit = 0; bit < 8; bit++) {
i2c_write_bit((byte & 0x80) != 0);
byte <<= 1;
}
nack = i2c_read_bit();
return nack;
}
void i2c_write(unsigned char byte)
{
TRISCbits.TRISC2 = 0; // writing data
for (size_t i=0; i < 8; ++i) {
i2c_write_bit((byte&0x80) == 0x80);
byte <<= 1;
}
TRISCbits.TRISC2 = 1; // reading the nack
i2c_read_bit();
}
unsigned __attribute__ ((noinline)) i2c_write_byte(unsigned b)
{
unsigned i = 8;
do
{
i2c_write_bit(b & 128);
b <<= 1;
i--;
} while ( i != 0 );
/* read ack from client */
/* 0: ack was given by client */
/* 1: nothing happend during ack cycle */
return i2c_read_bit();
}
uint8_t i2c_write_byte(uint8_t b)
{
uint8_t i = 8;
do
{
i2c_write_bit(b & 128);
b <<= 1;
i--;
} while ( i != 0 );
/* read ack from client */
/* 0: ack was given by client */
/* 1: nothing happend during ack cycle */
return i2c_read_bit();
}
/*
* update quaternion
*/
void mpu6050_updateQuaternion(char address) {
short ax = 0;
short ay = 0;
short az = 0;
short gx = 0;
short gy = 0;
short gz = 0;
double axg = 0;
double ayg = 0;
double azg = 0;
double gxrs = 0;
double gyrs = 0;
double gzrs = 0;
//get raw data
while(1) {
i2c_read_bit(address, MPU6050_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, (unsigned char *)buffer);
if(buffer[0])
break;
delay_us(10);
}
i2c_read_bytes(address, MPU6050_ACCEL_XOUT_H, 14, (unsigned char *)buffer);
ax = (((short)buffer[0]) << 8) | buffer[1];
ay = (((short)buffer[2]) << 8) | buffer[3];
az = (((short)buffer[4]) << 8) | buffer[5];
gx = (((short)buffer[8]) << 8) | buffer[9];
gy = (((short)buffer[10]) << 8) | buffer[11];
gz = (((short)buffer[12]) << 8) | buffer[13];
#if MPU6050_CALIBRATEDACCGYRO == 1
axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN;
ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN;
azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN;
gxrs = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN*0.01745329; //degree to radians
gyrs = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN*0.01745329; //degree to radians
gzrs = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN*0.01745329; //degree to radians
#else
axg = (double)(ax)/MPU6050_AGAIN;
ayg = (double)(ay)/MPU6050_AGAIN;
azg = (double)(az)/MPU6050_AGAIN;
gxrs = (double)(gx)/MPU6050_GGAIN*0.01745329; //degree to radians
gyrs = (double)(gy)/MPU6050_GGAIN*0.01745329; //degree to radians
gzrs = (double)(gz)/MPU6050_GGAIN*0.01745329; //degree to radians
#endif
//compute data
mpu6050_mahonyUpdate(gxrs, gyrs, gzrs, axg, ayg, azg);
}
static uint8_t i2c_write_byte(uint8_t b)
{
i2c_write_bit(b & 128);
i2c_write_bit(b & 64);
i2c_write_bit(b & 32);
i2c_write_bit(b & 16);
i2c_write_bit(b & 8);
i2c_write_bit(b & 4);
i2c_write_bit(b & 2);
i2c_write_bit(b & 1);
/* read ack from client */
/* 0: ack was given by client */
/* 1: nothing happend during ack cycle */
return i2c_read_bit();
}
// Write a byte to I2C bus. Return 0 if ack by the slave.
unsigned char i2cWriteByte(unsigned char send_start, unsigned char send_stop, unsigned char byte) {
unsigned char bit;
unsigned char nack;
if (send_start) {
i2c_start();
}
for (bit = 0; bit < 8; bit++) {
i2c_write_bit((byte & 0x80) != 0);
byte <<= 1;
}
nack = i2c_read_bit();
if (send_stop) {
i2c_stop();
}
return nack;
}
//*----------------------------------------------------------------------------
//* Name : i2c_write_byte()
//* Brief : Write a byte on I2C bus
//* Argument : Data value which is 8bits
//* Return : ACK = 0, NACK = -1
//*----------------------------------------------------------------------------
int I2C_write_byte(uint8 c) {
int i;
GPIOR_CFGIO_OUTPUT(I2C_PORT, I2C_SDA_PIN);
for( i = 0; i < 8; i++) {
i2c_write_bit((c << i) & 0x80);
}
GPIOR_CFGIO_INPUT(I2C_PORT, I2C_SDA_PIN);
// check ack
if(i2c_read_bit()) {
// no ack
return -1;
} else {
// ack
return 0;
}
}
// Write a byte to I2C bus. Return 0 if ack by the slave.
bool i2c_write_byte(bool send_start,
bool send_stop,
unsigned char byte) {
unsigned bit;
bool nack;
if (send_start) {
i2c_start_cond();
}
for (bit = 0; bit < 8; bit++) {
i2c_write_bit((byte & 0x80) != 0);
byte <<= 1;
}
nack = i2c_read_bit();
if (send_stop) {
i2c_stop_cond();
}
return nack;
}
//*----------------------------------------------------------------------------
//* Name : i2c_read_byte()
//* Brief : Read a byte from I2C bus
//* Argument : FALSE = No ACK, TRUE = ACK
//* Return : Data read from I2C bus
//*----------------------------------------------------------------------------
int I2C_read_byte(int ACK) {
uint8 buf = 0;
int i;
GPIOR_CFGIO_INPUT(I2C_PORT, I2C_SDA_PIN);
for( i = 0; i < 8; i ++) {
buf <<= 1;
if(i2c_read_bit()) {
buf |= 1;
}
}
// ack or not
GPIOR_CFGIO_OUTPUT(I2C_PORT, I2C_SDA_PIN);
if(ACK) {
i2c_write_bit(0);
} else {
i2c_write_bit(1);
}
return buf;
}
unsigned char i2c_read_with_response(enum i2c_ack ack)
{
unsigned char byte=0;
TRISCbits.TRISC2 = 1; // reading data
for (size_t i=0; i < 8; ++i) {
byte <<= 1;
if (i2c_read_bit() != 0)
byte |= 0x1;
}
TRISCbits.TRISC2 = 0; // write an ACK
if (ack == I2C_ACK)
i2c_write_bit(0); // send ack
else
i2c_write_bit(1);
return byte;
}