Add MPU-6050 management for RPI

Raspberry/MPU-6050-log/MPU6050.h

@@ -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

Raspberry/MPU-6050-log/Makefile

@@ -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)

Raspberry/MPU-6050-log/README

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

Raspberry/MPU-6050-log/main.c

@@ -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;
+}