96 lines
2.2 KiB
C
Executable File
96 lines
2.2 KiB
C
Executable File
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <sys/ioctl.h>
|
|
#include <fcntl.h>
|
|
#include <errno.h>
|
|
|
|
#include "i2c-dev.h"
|
|
|
|
#define SMPLRT_DIV 0x19
|
|
#define CONFIG 0x1A
|
|
#define GYRO_CONFIG 0x1B
|
|
#define ACCEL_CONFIG 0x1C
|
|
#define ACCEL_XOUT_H 0x3B
|
|
#define ACCEL_XOUT_L 0x3C
|
|
#define ACCEL_YOUT_H 0x3D
|
|
#define ACCEL_YOUT_L 0x3E
|
|
#define ACCEL_ZOUT_H 0x3F
|
|
#define ACCEL_ZOUT_L 0x40
|
|
#define TEMP_OUT_H 0x41
|
|
#define TEMP_OUT_L 0x42
|
|
#define GYRO_XOUT_H 0x43
|
|
#define GYRO_XOUT_L 0x44
|
|
#define GYRO_YOUT_H 0x45
|
|
#define GYRO_YOUT_L 0x46
|
|
#define GYRO_ZOUT_H 0x47
|
|
#define GYRO_ZOUT_L 0x48
|
|
#define PWR_MGMT_1 0x6B
|
|
|
|
void swap_int16(short *val)
|
|
{
|
|
*val = (*val << 8) | (*val >> 8);
|
|
}
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
int fd;
|
|
int ret;
|
|
short accelx, accely, accelz;
|
|
short temp;
|
|
short gyrox, gyroy, gyroz;
|
|
unsigned char *p;
|
|
|
|
fd = open("/dev/i2c-5", O_RDWR);
|
|
if (fd == -1)
|
|
goto fail;
|
|
|
|
if (ioctl(fd, I2C_SLAVE, 0x68) < 0)
|
|
goto fail;
|
|
|
|
i2c_smbus_write_byte_data(fd, PWR_MGMT_1, 0x80);
|
|
usleep(200000);
|
|
i2c_smbus_write_byte_data(fd, PWR_MGMT_1, 0x40);
|
|
i2c_smbus_write_byte_data(fd, PWR_MGMT_1, 0x00);
|
|
|
|
i2c_smbus_write_byte_data(fd, SMPLRT_DIV, 0x7);
|
|
i2c_smbus_write_byte_data(fd, CONFIG, 0x6);
|
|
i2c_smbus_write_byte_data(fd, GYRO_CONFIG, 0x3 << 3);
|
|
i2c_smbus_write_byte_data(fd, ACCEL_CONFIG, 0x3 << 3);
|
|
|
|
while (1) {
|
|
accelx = i2c_smbus_read_word_data(fd, ACCEL_XOUT_H);
|
|
swap_int16(&accelx);
|
|
accely = i2c_smbus_read_byte_data(fd, ACCEL_YOUT_H);
|
|
swap_int16(&accely);
|
|
accelz = i2c_smbus_read_byte_data(fd, ACCEL_ZOUT_H);
|
|
swap_int16(&accelz);
|
|
|
|
printf("accelx: %.2f\n", accelx / 2048.0);
|
|
printf("accely: %.2f\n", accely / 2048.0);
|
|
printf("accelz: %.2f\n", accelz / 2048.0);
|
|
|
|
temp = i2c_smbus_read_word_data(fd, TEMP_OUT_H);
|
|
swap_int16(&temp);
|
|
printf("temp: %.2f\n", temp / 340.0 + 36.53);
|
|
|
|
gyrox = i2c_smbus_read_word_data(fd, GYRO_XOUT_H);
|
|
swap_int16(&gyrox);
|
|
gyroy = i2c_smbus_read_byte_data(fd, GYRO_YOUT_H);
|
|
swap_int16(&gyroy);
|
|
gyroz = i2c_smbus_read_byte_data(fd, GYRO_ZOUT_H);
|
|
swap_int16(&gyroz);
|
|
|
|
printf("gyrox: %.2f\n", gyrox / 16.4);
|
|
printf("gyroy: %.2f\n", gyroy / 16.4);
|
|
printf("gyroz: %.2f\n", gyroz / 16.4);
|
|
|
|
sleep(1);
|
|
}
|
|
|
|
fail:
|
|
perror("i2c test");
|
|
exit(EXIT_FAILURE);
|
|
}
|