Hashan123:
the hard part is for me is get readings from the sensor
See if you can follow this …
RE: reading the raw data from the 6050. That’s done in the loop(), in the last line of the snippet below.
/************
* Main loop *
*************/
void loop()
{
int error;
double dT;
accel_t_gyro_union accel_t_gyro;
float kalAngleX, kalAngleY, kalAngleZ; // Calculate the angle using a Kalman filter
// Read the raw values.
error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);
The reading is done by the function read_gyro_accel_vals(), which seems to return some error indication to determine if the reading was done successfully or not. But where did the data from the 6050 go ? Let’s look at that function to see what it does.
int read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) {
// Read the raw values.
// Read 14 bytes at once,
// containing acceleration, temperature and gyro.
// With the default settings of the MPU-6050,
// there is no filter enabled, and the values
// are not very stable. Returns the error value
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));
// Swap all high and low bytes.
// After this, the registers values are swapped,
// so the structure name like x_accel_l does no
// longer contain the lower byte.
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;
There’s a lot going on there but we can see there’s yet another function called, MPU6050_read().
to do the actual I2C reading. Reading the 6050 datasheet you know the 14 bit raw data values come
across the I2C bus as two separate 7 bit values and those need to be re-combined back into a 14 bit value
stored in a 16 bit word. While the code uses pointers and a structure that may be hard for you to figure
out ATM, you can probably see the high and low bytes being swapped and thus recombined as mentioned. These are stored in memory in a ordered fashion (a structure) so perhaps we’ll look at the structure (strictly speaking a union). It’s named accel_t_gyro. Let’s look at it.
// Declaring an union for the registers and the axis values.
// The byte order does not match the byte order of
// the compiler and AVR chip.
// The AVR chip (on the Arduino board) has the Low Byte
// at the lower address.
// But the MPU-6050 has a different order: High Byte at
// lower address, so that has to be corrected.
// The register part "reg" is only used internally,
// and are swapped in code.
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
{
int x_accel;
int y_accel;
int z_accel;
int temperature;
int x_gyro;
int y_gyro;
int z_gyro;
} value;
};
We can see there’s places for the high and low bytes of the accel, temp and gyro data. You might also
guess that the recombined raw data is stored in the locations named at the bottom. Keep those in mind
and let’s now go back to the main loop(), but down a few lines from the prior snippet.
// 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;
Here we see the raw 16 bit data for the gyro being converted into floating point (FP) variables that have
units of degrees/second. Look at the variable names used. It’s the union/structure name followed by a .value.
and then followed by the names noted above. It’s a fair bet that these variables;
accel_t_gyro.value.x_gyro
accel_t_gyro.value.y_gyro
accel_t_gyro.value.z_gyro
… hold the raw gyro data you’re looking for. Similarly you can find the raw data for the accel X, Y and Z.
Having read all the links I posted you should be able to understand variable names and functions and the general layout of the code. If you want, Google about C/C++ pointers and structures, to better understand the above. But there’s your raw data, if you care to print it out. Though perhaps the scaled (deg/sec) FP values would be easier to understand.