diff --git a/Raspberry/MPU-6050-log/MPU6050.h b/Raspberry/MPU-6050-log/MPU6050.h new file mode 100644 index 0000000..f98e169 --- /dev/null +++ b/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 diff --git a/Raspberry/MPU-6050-log/Makefile b/Raspberry/MPU-6050-log/Makefile new file mode 100644 index 0000000..56e813a --- /dev/null +++ b/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) diff --git a/Raspberry/MPU-6050-log/README b/Raspberry/MPU-6050-log/README new file mode 100644 index 0000000..8e7448c --- /dev/null +++ b/Raspberry/MPU-6050-log/README @@ -0,0 +1 @@ +Simple data logger for 6-axis gyroscope/accelerometer MPU6050 \ No newline at end of file diff --git a/Raspberry/MPU-6050-log/main.c b/Raspberry/MPU-6050-log/main.c new file mode 100644 index 0000000..0652ed5 --- /dev/null +++ b/Raspberry/MPU-6050-log/main.c @@ -0,0 +1,141 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +}