Having problem with the wire library on the esp32

[hazee007]

this is a code to count steps using the accelerator/gyroscope sensor (gy 521), this works with arduino boards as well as the esp8266. now i dont know why its not working on esp32.
any help plz

`#include "Wire.h"
#include "SPI.h"
#include "SD.h"

#define MPU6050_ACCEL_XOUT_H 0x3B // R
#define MPU6050_PWR_MGMT_1 0x6B // R/W
#define MPU6050_PWR_MGMT_2 0x6C // R/W
#define MPU6050_WHO_AM_I 0x75 // R

#define MPU6050_I2C_ADDRESS 0x68
typedef union accel_t_gyro_union
{
struct
{
uint8_t x_accel_h;
uint8_t x_accel_l;
uint8_t y_accel_h;
uint8_t y_accel_l;
uint8_t z_accel_h;
uint8_t z_accel_l;
uint8_t t_h;
uint8_t t_l;
uint8_t x_gyro_h;
uint8_t x_gyro_l;
uint8_t y_gyro_h;
uint8_t y_gyro_l;
uint8_t z_gyro_h;
uint8_t z_gyro_l;
} reg;
struct
{
int16_t x_accel;
int16_t y_accel;
int16_t z_accel;
int16_t temperature;
int16_t x_gyro;
int16_t y_gyro;
int16_t z_gyro;
} value;
};

// Use the following global variables and access functions to help store the overall
// rotation angle of the sensor
unsigned long last_read_time;
float last_x_angle; // These are the filtered angles
float last_y_angle;
float last_z_angle;
float last_gyro_x_angle; // Store the gyro angles to compare drift
float last_gyro_y_angle;
float last_gyro_z_angle;

void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro) {
last_read_time = time;
last_x_angle = x;
last_y_angle = y;
last_z_angle = z;
last_gyro_x_angle = x_gyro;
last_gyro_y_angle = y_gyro;
last_gyro_z_angle = z_gyro;
}

inline unsigned long get_last_time() {return last_read_time;}
inline float get_last_x_angle() {return last_x_angle;}
inline float get_last_y_angle() {return last_y_angle;}
inline float get_last_z_angle() {return last_z_angle;}
inline float get_last_gyro_x_angle() {return last_gyro_x_angle;}
inline float get_last_gyro_y_angle() {return last_gyro_y_angle;}
inline float get_last_gyro_z_angle() {return last_gyro_z_angle;}

// Use the following global variables and access functions
// to calibrate the acceleration sensor
float base_x_accel;
float base_y_accel;
float base_z_accel;

float base_x_gyro;
float base_y_gyro;
float base_z_gyro;

int read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) {

accel_t_gyro_union* accel_t_gyro = (accel_t_gyro_union *) accel_t_gyro_ptr;

int error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) accel_t_gyro, sizeof(*accel_t_gyro));

uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap

SWAP ((*accel_t_gyro).reg.x_accel_h, (*accel_t_gyro).reg.x_accel_l);
SWAP ((*accel_t_gyro).reg.y_accel_h, (*accel_t_gyro).reg.y_accel_l);
SWAP ((*accel_t_gyro).reg.z_accel_h, (*accel_t_gyro).reg.z_accel_l);
SWAP ((*accel_t_gyro).reg.t_h, (*accel_t_gyro).reg.t_l);
SWAP ((*accel_t_gyro).reg.x_gyro_h, (*accel_t_gyro).reg.x_gyro_l);
SWAP ((*accel_t_gyro).reg.y_gyro_h, (*accel_t_gyro).reg.y_gyro_l);
SWAP ((*accel_t_gyro).reg.z_gyro_h, (*accel_t_gyro).reg.z_gyro_l);

return error;
}

// The sensor should be motionless on a horizontal surface
// while calibration is happening
void calibrate_sensors() {
int num_readings = 10;
float x_accel = 0;
float y_accel = 0;
float z_accel = 0;
float x_gyro = 0;
float y_gyro = 0;
float z_gyro = 0;
accel_t_gyro_union accel_t_gyro;

//Serial.println("Starting Calibration");

// Discard the first set of values read from the IMU
read_gyro_accel_vals((uint8_t *) &accel_t_gyro);

// Read and average the raw values from the IMU
for (int i = 0; i < num_readings; i++) {
read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
x_accel += accel_t_gyro.value.x_accel;
y_accel += accel_t_gyro.value.y_accel;
z_accel += accel_t_gyro.value.z_accel;
x_gyro += accel_t_gyro.value.x_gyro;
y_gyro += accel_t_gyro.value.y_gyro;
z_gyro += accel_t_gyro.value.z_gyro;
delay(100);
}
x_accel /= num_readings;
y_accel /= num_readings;
z_accel /= num_readings;
x_gyro /= num_readings;
y_gyro /= num_readings;
z_gyro /= num_readings;

// Store the raw calibration values globally
base_x_accel = x_accel;
base_y_accel = y_accel;
base_z_accel = z_accel;
base_x_gyro = x_gyro;
base_y_gyro = y_gyro;
base_z_gyro = z_gyro;

//Serial.println("Finishing Calibration");
}

//const int chipSelect = 4;
void setup()
{
int error;
uint8_t c;

Serial.begin(9600);
// Initialize the 'Wire' class for the I2C-bus.
Wire.begin();

error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);

error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1);

// Clear the 'sleep' bit to start the sensor.
MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);

//Initialize the angles
calibrate_sensors();
set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0);
}

float x,y,z;
int count=0,prev=0;
int threshold=3;

void loop()
{
int error;
double dT;
accel_t_gyro_union accel_t_gyro;
// SaveData();

// Read the raw values.
error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);

// Get the time of reading for rotation computations
unsigned long t_now = millis();

// Convert gyro values to degrees/sec
float FS_SEL = 131;
float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL;
float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL;
float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL;

// Get raw acceleration values
//float G_CONVERT = 16384;
float accel_x = accel_t_gyro.value.x_accel;
float accel_y = accel_t_gyro.value.y_accel;
float accel_z = accel_t_gyro.value.z_accel;

// Get angle values from accelerometer
float RADIANS_TO_DEGREES = 180/3.14159;
//float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;

float accel_angle_z = atan(sqrt(pow(accel_x,2) + pow(accel_y,2))/accel_z)*RADIANS_TO_DEGREES;;
//float accel_angle_z = 0;

// Compute the (filtered) gyro angles
float dt =(t_now - get_last_time())/1000.0;
float gyro_angle_x = gyro_xdt + get_last_x_angle();
float gyro_angle_y = gyro_ydt + get_last_y_angle();
float gyro_angle_z = gyro_z*dt + get_last_z_angle();

// Compute the drifting gyro angles
float unfiltered_gyro_angle_x = gyro_xdt + get_last_gyro_x_angle();
float unfiltered_gyro_angle_y = gyro_ydt + get_last_gyro_y_angle();
float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle();

// Apply the complementary filter to figure out the change in angle - choice of alpha is
// estimated now. Alpha depends on the sampling rate...
float alpha = 0.96;
float angle_x = alpha*gyro_angle_x + (1.0 - alpha)accel_angle_x;
float angle_y = alphagyro_angle_y + (1.0 - alpha)*accel_angle_y;
float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle

// Update the saved data with the latest values
set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);

//Finding the magnitude of acceleration from the combined data from gyroscope and accelerometer.
int mag=sqrt(pow(x-angle_x,2)+pow(y-angle_y,2)+pow(z-angle_z,2));

//If the magnitude is greater than threshold and the previous magnitude is lesser than threshold value increase count.
if(mag>=threshold && prev<threshold)
{
count+=1;
Serial.print("steps= ");
Serial.println(count);
}

prev = mag;
x=angle_x;
y=angle_y;
z=angle_z;

// Delay so we don't swamp the serial port
delay(100);
//Serial.write(10);
}
int MPU6050_read(int start, uint8_t *buffer, int size)
{
int i, n, error;

Wire.beginTransmission(MPU6050_I2C_ADDRESS);
n = Wire.write(start);
if (n != 1)
return (-10);

n = Wire.endTransmission(false); // hold the I2C-bus
if (n != 0)
return (n);

// Third parameter is true: relase I2C-bus after data is read.
Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
i = 0;
while(Wire.available() && i<size)
{
buffer[i++]=Wire.read();
}
if ( i != size)
return (-11);

return (0); // return : no error
}

int MPU6050_write(int start, const uint8_t *pData, int size)
{
int n, error;

Wire.beginTransmission(MPU6050_I2C_ADDRESS);
n = Wire.write(start); // write the start address
if (n != 1)
return (-20);

n = Wire.write(pData, size); // write data bytes
if (n != size)
return (-21);

error = Wire.endTransmission(true); // release the I2C-bus
if (error != 0)
return (error);

return (0); // return : no error
}

int MPU6050_write_reg(int reg, uint8_t data)
{
int error;

error = MPU6050_write(reg, &data, 1);

return (error);
}

`

Originally posted by @hazee007 in #1071 (comment)

[chegewara]

@hazee007
Did you try with this library?
https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

Its much better developed.

[stickbreaker]

int MPU6050_read(int start, uint8_t *buffer, int size)
{
int i, n, error;

Wire.beginTransmission(MPU6050_I2C_ADDRESS);
n = Wire.write(start);
if (n != 1)
return (-10);
// This is where the difference is, wih Release 1.0.0 a ReSTART operation (STOP=FALSE)
// causes the command to be queued (not executed until a STOP is issued,
// so this .endTransmission(false) returns 7 (I2C_ERROR_CONTINUE) to report this queue operation.
// When Release 1.0.1 is generated, this will be changed back to 0 (I2C_ERROR_OK) for compatibility
// with existing libraries.
// For now, change it like this:
n = Wire.endTransmission(false); // hold the I2C-bus
if (! ((n == 0) || (n ==7)) )
return (n);

// Third parameter is true: relase I2C-bus after data is read.
Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
i = 0;
while(Wire.available() && i<size)
{
buffer[i++]=Wire.read();
}
if ( i != size)
return (-11);

return (0); // return : no error
}

Chuck.

[hazee007]

@stickbreaker thank you very much.. works as usual now

[sarwarislammoon]

hi
I am actually having same issue with esp32 , always get '0' while use Wire.read().
Can U please help .I am using ESP 1.0.4

Thanks in advance.

#include <Wire.h>
#define MPU9250_ADDRESS 0x68
#define ACC_FULL_SCALE_2_G 0x00

uint8_t Buf[14];
float yangle;

void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
uint8_t d[16];
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.endTransmission();

// Read Nbytes
Wire.requestFrom(Address,Nbytes);
uint8_t index = 0;
while (Wire.available()){
// Data[index++] = Wire.read();
Serial.print(Wire.available());
Serial.print("->");
Serial.println(Wire.read());
}

}

uint8_t I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.write(Data);
return Wire.endTransmission();
}

void setup() {
Serial.begin(115200);
Wire.begin(19,18,400000); // SDA SCK
I2CwriteByte(MPU9250_ADDRESS, 29, 0x06);
I2CwriteByte(MPU9250_ADDRESS, 28, ACC_FULL_SCALE_2_G);
I2CwriteByte(MPU9250_ADDRESS, 108, 0x2F);

}

void loop() {

getAngle();
//Serial.println(getAngle());

}

int getAngle(){
I2Cread(MPU9250_ADDRESS, 0x3B, 14, Buf);
//Serial.println(Buf[2]);
int16_t ay = -(Buf[2] << 8 | Buf[3]);
yangle = ay / 182.04;
return -yangle;
}