eddy/eddys_snippets
1
0
Fork 0
mirror of https://github.com/eddyem/eddys_snippets.git synced 2025-12-06 10:45:12 +03:00
Code Issues Packages Projects Releases Wiki Activity

Add MPU-6050 management for RPI

Browse Source
This commit is contained in:
eddyem 2016-11-10 23:08:10 +03:00
parent 001e61af6e
commit acab855069
4 changed files with 285 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
Raspberry/MPU-6050-log/MPU6050.h Normal file
View File

@ -0,0 +1,119 @@
#pragma once
// Registers stolen from https://courses.cs.washington.edu/courses/cse466/14au/labs/l4/MPU6050BasicExample.ino
#define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD
#define YGOFFS_TC 0x01
#define ZGOFFS_TC 0x02
#define X_FINE_GAIN 0x03 // [7:0] fine gain
#define Y_FINE_GAIN 0x04
#define Z_FINE_GAIN 0x05
#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
#define XA_OFFSET_L_TC 0x07
#define YA_OFFSET_H 0x08
#define YA_OFFSET_L_TC 0x09
#define ZA_OFFSET_H 0x0A
#define ZA_OFFSET_L_TC 0x0B
#define SELF_TEST_X 0x0D
#define SELF_TEST_Y 0x0E
#define SELF_TEST_Z 0x0F
#define SELF_TEST_A 0x10
#define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050?
#define XG_OFFS_USRL 0x14
#define YG_OFFS_USRH 0x15
#define YG_OFFS_USRL 0x16
#define ZG_OFFS_USRH 0x17
#define ZG_OFFS_USRL 0x18
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define FF_THR 0x1D // Free-fall
#define FF_DUR 0x1E // Free-fall
#define MOT_THR 0x1F // Motion detection threshold bits [7:0]
#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
#define FIFO_EN 0x23
#define I2C_MST_CTRL 0x24
#define I2C_SLV0_ADDR 0x25
#define I2C_SLV0_REG 0x26
#define I2C_SLV0_CTRL 0x27
#define I2C_SLV1_ADDR 0x28
#define I2C_SLV1_REG 0x29
#define I2C_SLV1_CTRL 0x2A
#define I2C_SLV2_ADDR 0x2B
#define I2C_SLV2_REG 0x2C
#define I2C_SLV2_CTRL 0x2D
#define I2C_SLV3_ADDR 0x2E
#define I2C_SLV3_REG 0x2F
#define I2C_SLV3_CTRL 0x30
#define I2C_SLV4_ADDR 0x31
#define I2C_SLV4_REG 0x32
#define I2C_SLV4_DO 0x33
#define I2C_SLV4_CTRL 0x34
#define I2C_SLV4_DI 0x35
#define I2C_MST_STATUS 0x36
#define INT_PIN_CFG 0x37
#define INT_ENABLE 0x38
#define DMP_INT_STATUS 0x39 // Check DMP interrupt
#define INT_STATUS 0x3A
#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 EXT_SENS_DATA_00 0x49
#define EXT_SENS_DATA_01 0x4A
#define EXT_SENS_DATA_02 0x4B
#define EXT_SENS_DATA_03 0x4C
#define EXT_SENS_DATA_04 0x4D
#define EXT_SENS_DATA_05 0x4E
#define EXT_SENS_DATA_06 0x4F
#define EXT_SENS_DATA_07 0x50
#define EXT_SENS_DATA_08 0x51
#define EXT_SENS_DATA_09 0x52
#define EXT_SENS_DATA_10 0x53
#define EXT_SENS_DATA_11 0x54
#define EXT_SENS_DATA_12 0x55
#define EXT_SENS_DATA_13 0x56
#define EXT_SENS_DATA_14 0x57
#define EXT_SENS_DATA_15 0x58
#define EXT_SENS_DATA_16 0x59
#define EXT_SENS_DATA_17 0x5A
#define EXT_SENS_DATA_18 0x5B
#define EXT_SENS_DATA_19 0x5C
#define EXT_SENS_DATA_20 0x5D
#define EXT_SENS_DATA_21 0x5E
#define EXT_SENS_DATA_22 0x5F
#define EXT_SENS_DATA_23 0x60
#define MOT_DETECT_STATUS 0x61
#define I2C_SLV0_DO 0x63
#define I2C_SLV1_DO 0x64
#define I2C_SLV2_DO 0x65
#define I2C_SLV3_DO 0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET 0x68
#define MOT_DETECT_CTRL 0x69
#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
#define PWR_MGMT_2 0x6C
#define DMP_BANK 0x6D // Activates a specific bank in the DMP
#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
#define DMP_REG_1 0x70
#define DMP_REG_2 0x71
#define FIFO_COUNTH 0x72
#define FIFO_COUNTL 0x73
#define FIFO_R_W 0x74
#define WHO_AM_I_MPU6050 0x75 // Should return 0x68
#define MPU6050_I2C_ADDRESS (0x68) // I2C

24
Raspberry/MPU-6050-log/Makefile Normal file
View File

@ -0,0 +1,24 @@
# run `make DEF=...` to add extra defines
PROGRAM = hyrolog
LDFLAGS = -lwiringPi -lm
SRCS = $(wildcard *.c)
CC = gcc
DEFINES = $(DEF) -D_XOPEN_SOURCE=1111 -D_GNU_SOURCE=11
CFLAGS = -Wall -Werror -Wextra $(DEFINES)
OBJS = $(SRCS:.c=.o)
all : $(PROGRAM)
$(PROGRAM) : $(OBJS)
$(CC) $(CFLAGS) $(OBJS) $(LDFLAGS) -o $(PROGRAM)
# some addition dependencies
# %.o: %.c
# $(CC) $(CFLAGS) $< -o $@
#$(SRCS) : %.c : %.h $(INDEPENDENT_HEADERS)
# @touch $@
#
#%.h: ;
clean:
/bin/rm -f *.o *~
depend:
$(CC) -MM $(SRCS)

1
Raspberry/MPU-6050-log/README Normal file
View File

@ -0,0 +1 @@
Simple data logger for 6-axis gyroscope/accelerometer MPU6050

141
Raspberry/MPU-6050-log/main.c Normal file
View File

@ -0,0 +1,141 @@
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include <stdint.h>
#include <math.h>
#include <sys/time.h>
#include "MPU6050.h"
int fd, ctr=0;
uint8_t addrs[6] = {ACCEL_XOUT_H,ACCEL_YOUT_H,ACCEL_ZOUT_H, GYRO_XOUT_H,GYRO_YOUT_H,GYRO_ZOUT_H};
const double scales[6] = {19.6/32768., 19.6/32768., 19.6/32768., 250/32768., 250/32768., 250/32768.};
const double thres[6] = {0.5, 0.5, 0.5, 5., 5., 5.};
#define RD8(reg) wiringPiI2CReadReg8(fd, reg)
#define RD16(reg) (RD8(reg)<<8|RD8(reg+1))
#define WR8(r,d) do{wiringPiI2CWriteReg8(fd, r,d);}while(0)
#define WR16(r,d) do{WR8(r,(d>>8)&0xff); WR8(r+1, d&0xff);}while(0)
double get_fta(int8_t at){
if(!at) return 0.;
return 4096.*0.34*pow((0.92/0.34), ((double)at-1.)/30.);
}
double get_ftg(int8_t gt){
if(!gt) return 0.;
return 25.*131.*pow(1.046, (double)gt - 1.);
}
double dtime(){
double t;
struct timeval tv;
gettimeofday(&tv, NULL);
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
return t;
}
void reset_brd(){
fprintf(stderr, "RESET!\n");
WR8(PWR_MGMT_1, 0x80); // device reset
usleep(100000);
WR8(CONFIG, 3); // 3 == 44/42Hz, 6 == 5Hz
WR8(SMPLRT_DIV, 9); // 1kHz / 10 = 100Hz
WR16(PWR_MGMT_1, 0);
WR8(GYRO_CONFIG, 0); WR8(ACCEL_CONFIG, 0); // turn off self-test, max precision
usleep(100000);
}
void print_ag(){
static int16_t oldvals[6];
int16_t dat[6];
static double doldvals[6];
double vals[6];
int i, ctr = 0;
double t0 = dtime();
while(!(RD8(INT_STATUS) & 1) && dtime() - t0 < 1.); // wait next data portion
if(dtime() - t0 > 1.){
reset_brd();
return;
}
for(i = 0; i < 6; ++i){
dat[i] = RD16(addrs[i]);
if(oldvals[i] == dat[i]) ++ctr;
else oldvals[i] = dat[i];
}
if(ctr == 6){ // hang
reset_brd();
return;
}
ctr = 0;
for(i = 0; i < 6; ++i){
vals[i] = ((double)dat[i]) * scales[i];
if(fabs(vals[i] - doldvals[i]) > thres[i]){
++ctr; doldvals[i] = vals[i];
}
}
if(ctr){ // there was changes
printf("%.3f\t", dtime());
for(i = 0; i < 6; ++i) printf("%.1f\t", vals[i]);
printf("\n");
}
}
// get statistics for 10 seconds
void get_stat(){
double t0 = dtime(), scds = 1.;
double mean[6]={0.}, max[6], min[6], d, N = 0.;
int i;
for(i = 0; i < 6; ++i){ // fill initial values
max[i] = min[i] = ((double)RD16(addrs[i])) * scales[i];
}
printf("Wait for 10 seconds, please\n");
do{
double t1 = dtime();
while(!(RD8(INT_STATUS) & 1) && dtime() - t1 < 1.); // wait next data portion
if(dtime() - t1 > 1.){
reset_brd();
printf("ERROR!!! System hangs!\n");
return;
}
for(i = 0; i < 6; ++i){
d = ((double)RD16(addrs[i])) * scales[i];
mean[i] += d;
if(max[i] < d) max[i] = d;
if(min[i] > d) min[i] = d;
}
N += 1.;
if(dtime() - t0 > scds){
printf("%.0f\b", scds);
scds += 1.;
}
}while(dtime() - t0 < 10.);
printf("\nAX, AY, AZ, GX, GY, GZ. [ min, max, mean ]\n");
for(i = 0; i < 6; ++i)
printf("[ %.1f, %.1f, %.1f ] ", min[i], max[i], mean[i] / N);
printf("\n\n");
}
int main(int argc, char **argv){
fd = wiringPiI2CSetup(MPU6050_I2C_ADDRESS);
setbuf(stdout, NULL);
if (fd == -1){
printf("I2C error\n");
return 1;
}
int answ = RD8(WHO_AM_I_MPU6050);
if(answ != MPU6050_I2C_ADDRESS){
printf("No MPU6050 detected\n");
return 2;
}
reset_brd();
(void) argv;
if(argc == 2) get_stat();
else{
printf("Unix time\tAX\tAY\tAZ\tGX\tGY\tGZ\n");
while(1) print_ag();
}
return 0;
}