mpu6060.c

/*
 * 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 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);`
```