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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.