Okay so i'm trying to read data from the gy521 sensor for arduino. I'm at the stage where i am reading data from the FIFO register, but when i try and assign the data to an array so i can calculate position, the array writes 20 values instead of the 10 i defined at the start. Here is my work in progress code:
#include <Wire.h>
#define MPU_6050 0x68
#define PWR_MGMT_1 0x6B
#define FIFO_EN 0x23
#define FIFO_COUNTH 0x72
#define FIFO_COUNTL 0x73
#define FIFO_R_W 0x74
#define SMPLRT_DIV 0x19
#define USER_CTRL 0x6A
#define GY_CONFIG 0x1B
#define SELF_TEST_X 0x0D
#define SELF_TEST_Y 0x0E
#define SELF_TEST_Z 0x0F
#define INT_EN 0x38
#define INT_STAT 0x3A
#define ARRAY_SIZE 10
int FIFO_OVERFLOW_STAT;
int FIFO_DATA_X;
int FIFO_DATA_Y;
int FIFO_DATA_Z;
int x0;
int y0;
int z0;
int FIFO_COUNT;
int x_val[ARRAY_SIZE];
int y_val[ARRAY_SIZE];
int z_val[ARRAY_SIZE];
void setup() {
Serial.begin(115200); //Start a serial interface
Wire.begin(); //set up i2c
Wire.beginTransmission(MPU_6050); //start transmission to slave
Wire.write(PWR_MGMT_1);
Wire.write(0); //Wake up
Wire.endTransmission(true); // Send byte in buffer
Wire.beginTransmission(MPU_6050);
Wire.write(SMPLRT_DIV);
Wire.write(5); //set the sample rate to 250hz (because 8000/1+31=250)
Wire.endTransmission(true);
disable_FIFO();
Wire.beginTransmission(MPU_6050);
Wire.write(USER_CTRL);
Wire.write(68); //enable the FIFO Line and Clear it
Wire.endTransmission(true);
check_FIFO_size();
Wire.beginTransmission(MPU_6050);
Wire.write(SELF_TEST_X);
Wire.write(7); //set up self tests
Wire.endTransmission(true);
Wire.beginTransmission(MPU_6050);
Wire.write(SELF_TEST_Y);
Wire.write(7); //set up self tests
Wire.endTransmission(true);
Wire.beginTransmission(MPU_6050);
Wire.write(SELF_TEST_Z);
Wire.write(7); //set up self tests
Wire.endTransmission(true);
Wire.beginTransmission(MPU_6050);
Wire.write(GY_CONFIG);
Wire.write(224); //Self test gyro and set Full Scale range to 250*/s
Wire.endTransmission(true);
Wire.beginTransmission(MPU_6050);
Wire.write(INT_EN);
Wire.write(16); //set up interrupt overflow of FIFO to prevent loss of data
Wire.endTransmission(true);
enable_FIFO();
get_0_vals();
}
void enable_FIFO(){
Wire.beginTransmission(MPU_6050);
Wire.write(FIFO_EN);
Wire.write(112); //enable the FIFO line for GY x,y and z
Wire.endTransmission(true);
}
void disable_FIFO(){
Wire.beginTransmission(MPU_6050);
Wire.write(FIFO_EN);
Wire.write(0); //enable the FIFO line for GY x,y and z
Wire.endTransmission(true);
}
void check_FIFO_overflow_stat(){
Wire.beginTransmission(MPU_6050);
Wire.write(INT_STAT);
Wire.endTransmission(false);
Wire.requestFrom(MPU_6050, 1, true);
if(Wire.available() >= 1){
FIFO_OVERFLOW_STAT = Wire.read() >> 4;
}
Serial.println(FIFO_OVERFLOW_STAT);
}
void check_FIFO_size(){
Wire.beginTransmission(MPU_6050);
Wire.write(FIFO_COUNTH);
Wire.endTransmission(false);
Wire.requestFrom(MPU_6050, 2, true);
if(Wire.available() >=2){
FIFO_COUNT = Wire.read() << 8;
FIFO_COUNT |= Wire.read();
}
Wire.endTransmission();
Serial.print("FIFO count: ");
Serial.println(FIFO_COUNT);
}
void get_0_vals(){
Wire.beginTransmission(MPU_6050);
Wire.write(FIFO_R_W);
Wire.endTransmission(false);
Wire.requestFrom(MPU_6050, 6, true);
if(Wire.available() >= 6){
x0 = Wire.read() << 8;
x0 |= Wire.read();
y0 = Wire.read() << 8;
y0 |= Wire.read();
z0 = Wire.read() << 8;
z0 |= Wire.read();
}
Wire.endTransmission();
}
void read_FIFO_xyz(){
Wire.beginTransmission(MPU_6050);
Wire.write(FIFO_R_W);
Wire.endTransmission(false);
Wire.requestFrom(MPU_6050, 6, true);
if(Wire.available() >=6){
FIFO_DATA_X = Wire.read() << 8;
FIFO_DATA_X |= Wire.read();
FIFO_DATA_Y = Wire.read() << 8;
FIFO_DATA_Y |= Wire.read();
FIFO_DATA_Z = Wire.read() << 8;
FIFO_DATA_Z |= Wire.read();
}
Wire.endTransmission(true);
}
void zero_FIFO_data(){
FIFO_DATA_X = FIFO_DATA_X - x0;
FIFO_DATA_Y = FIFO_DATA_Y - y0;
FIFO_DATA_Z = FIFO_DATA_Z - z0;
}
void fill_arrays(){
for(int i = 0; i<10; i++){
should_FIFO_be_on();
if(FIFO_COUNT > 6){
read_FIFO_xyz();
zero_FIFO_data();
x_val[i] = FIFO_DATA_X;
y_val[i] = FIFO_DATA_Y;
z_val[i] = FIFO_DATA_Z;
}
}
}
void should_FIFO_be_on(){
check_FIFO_size();
if(FIFO_COUNT > 900){
disable_FIFO();
}
else{
enable_FIFO();
}
}
void loop() {
Serial.print("Size of x_val: ");
Serial.println(sizeof(x_val));
Serial.println("Start of Main");
check_FIFO_size();
should_FIFO_be_on();
if(FIFO_COUNT > 60){
fill_arrays();
}
Serial.print("Size of x_val: ");
Serial.println(sizeof(x_val));
for(int i = 0; i<10; i++){
Serial.print("Element ");
Serial.print(i);
Serial.print(" of x_val: ");
Serial.println(x_val[i]);
}
}
I have defined x_val as being of size ARRAY_SIZE which is defined as 10.
After calling my fill array function, Serial.print(sizeof(x_val)); returns 20. I have a feeling this is related to the fact that the values of FIFO_DATA_X are 2 Bytes, but that doesnt make sense to me because an int can be 2 or 4 byte.
Sorry in advance, my code is nowhere near perfect, but i can't use the MPU_6050 library so ive had to do my best from the data sheet and register map, plus my understanding of C++ and arduino is pretty poor (as is probably evident).
You are using sizeof operator, which does not return the number of elements in the array.
Returns size in bytes of the object representation of type.
Here's a solution:
sizeof(x_val) / sizeof(x_val[0])
This returns number of elements in the array. More on this here.