Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
/*
* This file is an MPU6050 demonstration.
* https://openest.io/en/2020/01/21/mpu6050-accelerometer-on-raspberry-pi/
* Copyright (c) 2020 Julien Grossholtz - https://openest.io.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#define MPU6050_I2C_ADDR 0x68
#define REG_ACCEL_ZOUT_H 0x3F
#define REG_ACCEL_ZOUT_L 0x40
#define REG_PWR_MGMT_1 0x6B
#define REG_ACCEL_CONFIG 0x1C
#define REG_SMPRT_DIV 0x19
#define REG_CONFIG 0x1A
#define REG_FIFO_EN 0x23
#define REG_USER_CTRL 0x6A
#define REG_FIFO_COUNT_L 0x72
#define REG_FIFO_COUNT_H 0x73
#define REG_FIFO 0x74
#define REG_WHO_AM_I 0x75
int file = -1;
// Please note, this is not the recommanded way to write data
// to i2c devices from user space.
void i2c_write(__u8 reg_address, __u8 val) {
char buf[2];
if(file < 0) {
printf("Error, i2c bus is not available\n");
exit(1);
}
buf[0] = reg_address;
buf[1] = val;
if (write(file, buf, 2) != 2) {
printf("Error, unable to write to i2c device\n");
exit(1);
}
}
// Please note, this is not thre recommanded way to read data
// from i2c devices from user space.
char i2c_read(uint8_t reg_address) {
char buf[1];
if(file < 0) {
printf("Error, i2c bus is not available\n");
exit(1);
}
buf[0] = reg_address;
if (write(file, buf, 1) != 1) {
printf("Error, unable to write to i2c device\n");
exit(1);
}
if (read(file, buf, 1) != 1) {
printf("Error, unable to read from i2c device\n");
exit(1);
}
return buf[0];
}
uint16_t merge_bytes( uint8_t LSB, uint8_t MSB) {
return (uint16_t) ((( LSB & 0xFF) << 8) | MSB);
}
// 16 bits data on the MPU6050 are in two registers,
// encoded in two complement. So we convert those to int16_t
int16_t two_complement_to_int( uint8_t LSB, uint8_t MSB) {
int16_t signed_int = 0;
uint16_t word;
word = merge_bytes(LSB, MSB);
if((word & 0x8000) == 0x8000) { // negative number
signed_int = (int16_t) -(~word);
} else {
signed_int = (int16_t) (word & 0x7fff);
}
return signed_int;
}
int main(int argc, char *argv[]) {
int adapter_nr = 1; /* probably dynamically determined */
char bus_filename[250];
char accel_x_h,accel_x_l,accel_y_h,accel_y_l,accel_z_h,accel_z_l,temp_h,temp_l;
uint16_t fifo_len = 0;
int16_t x_accel = 0;
int16_t y_accel = 0;
int16_t z_accel = 0;
int16_t temp = 0;
float x_accel_g, y_accel_g, z_accel_g, temp_f;
snprintf(bus_filename, 250, "/dev/i2c-1", adapter_nr);
file = open(bus_filename, O_RDWR);
if (file < 0) {
/* ERROR HANDLING; you can check errno to see what went wrong */
exit(1);
}
if (ioctl(file, I2C_SLAVE, MPU6050_I2C_ADDR) < 0) {
/* ERROR HANDLING; you can check errno to see what went wrong */
exit(1);
}
i2c_write(REG_PWR_MGMT_1, 0x01);
i2c_write(REG_ACCEL_CONFIG, 0x00);
i2c_write(REG_SMPRT_DIV, 0x07);
i2c_write(REG_CONFIG, 0x00);
i2c_write(REG_FIFO_EN, 0x88);
i2c_write(REG_USER_CTRL, 0x44);
while(fifo_len != 1024) {
accel_x_h = i2c_read(REG_FIFO_COUNT_L);
accel_x_l = i2c_read(REG_FIFO_COUNT_H);
fifo_len = merge_bytes(accel_x_h,accel_x_l);
if(fifo_len == 1024) {
printf("fifo overflow !\n");
i2c_write(REG_USER_CTRL, 0x44);
continue;
}
if(fifo_len >= 8) {
accel_x_h = i2c_read(REG_FIFO);
accel_x_l = i2c_read(REG_FIFO);
accel_y_h = i2c_read(REG_FIFO);
accel_y_l = i2c_read(REG_FIFO);
accel_z_h = i2c_read(REG_FIFO);
accel_z_l = i2c_read(REG_FIFO);
temp_h = i2c_read(REG_FIFO);
temp_l= i2c_read(REG_FIFO);
x_accel= two_complement_to_int(accel_x_h,accel_x_l);
x_accel_g = ((float) x_accel)/16384;
y_accel= two_complement_to_int(accel_y_h,accel_y_l);
y_accel_g = ((float) y_accel)/16384;
z_accel= two_complement_to_int(accel_z_h,accel_z_l);
z_accel_g = ((float) z_accel)/16384;
temp = two_complement_to_int(temp_h, temp_l);
temp_f = (float)temp/340 + 36.53; // calculated as described in the MPU60%) register map document
printf("x_accel %.3fg y_accel %.3fg z_accel %.3fg temp=%.1fc \r", x_accel_g, y_accel_g, z_accel_g, temp_f);
} else {
usleep(10000);
}
}
return 0;
}
@Dbrit

This comment has been minimized.

Copy link

@Dbrit Dbrit commented Feb 7, 2021

When I run your code, it outputs a single data point and then I instantly get a FIFO overflow. The only change I made was the i2c address. Do you have any idea what could cause this overflow?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment