Skip to content

Instantly share code, notes, and snippets.

@JGrossholtz
Created January 23, 2020 15:59
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save JGrossholtz/24e427b62c2e5dbbec6546c55c8979d8 to your computer and use it in GitHub Desktop.
Save JGrossholtz/24e427b62c2e5dbbec6546c55c8979d8 to your computer and use it in GitHub Desktop.
/*
* 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;
}
@pragad-t
Copy link

@Dbrit I am running into the exact same issue. Were you able to find a workaround?

@Dbrit
Copy link

Dbrit commented Sep 26, 2022

@pragad-t I don't believe I was able to get this code to work. It's been a while since this project but I think this worked:

void i2c_read(int fd, uint8_t address, uint8_t* dest, size_t len){
    dest[0] = address;
    if(read(fd, dest, len) != len){
        std::cerr << "Error reading" << std::endl;
    }
}

void i2c_write(int fd, uint8_t address, uint8_t* dest, size_t len){
    uint8_t buf[len + 1];
    buf[0] = address;
    memcpy(buf + 1, dest, len);
    if(write(fd, dest, len + 1) != len + 1){
        std::cerr << "Error writing" << std::endl;
    }
}

uint16_t i2c_read_16(int fd, uint8_t address){
    uint8_t buf[2];
    i2c_read(fd, address, buf, 1);
    i2c_read(fd, address + 1, buf + 1, 1);
    return (buf[0] << 8) | buf[1];
}

And to use these functions:

    if((this->fd = open(FILENAME, O_RDWR)) < 0){
        std::cerr << "bad" << std::endl;
        return;
    }


    if(ioctl(this->fd, I2C_SLAVE, addr) < 0){
        std::cerr << "no address " << addr << std::endl;
    }

    i2c_write_8(this->fd, SMPLRT_DIV, 7);
    i2c_write_8(this->fd, PWR_MGMT_1, 1);
    i2c_write_8(this->fd, CONFIG, 0);
    i2c_write_8(this->fd, GYRO_CONFIG, 24);
    i2c_write_8(this->fd, INT_ENABLE, 1);`
```

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