diff --git a/Raspberry/MPU-6050-log/main.c b/Raspberry/MPU-6050-log/main.c index 0652ed5..230265a 100644 --- a/Raspberry/MPU-6050-log/main.c +++ b/Raspberry/MPU-6050-log/main.c @@ -12,7 +12,9 @@ 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.}; +// threshold based on an accelerometer statistics for 30sec; gyro isn't mean +//const double thres[6] = {0.6, 0.6, 0.9, 5., 5., 5.}; +const double thres[6] = {0.2, 0.2, 0.3, 1., 1., 1.}; #define RD8(reg) wiringPiI2CReadReg8(fd, reg) #define RD16(reg) (RD8(reg)<<8|RD8(reg+1)) @@ -40,18 +42,18 @@ double dtime(){ void reset_brd(){ fprintf(stderr, "RESET!\n"); WR8(PWR_MGMT_1, 0x80); // device reset - usleep(100000); + sleep(1); + WR16(PWR_MGMT_1, 0); 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 + 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]; + static double doldvals[6] = {-1000.,-1000.,-1000.,-1000.,-1000.,-1000.}; double vals[6]; int i, ctr = 0; double t0 = dtime(); @@ -77,21 +79,26 @@ void print_ag(){ } } if(ctr){ // there was changes + int16_t t = RD16(TEMP_OUT_H); printf("%.3f\t", dtime()); for(i = 0; i < 6; ++i) printf("%.1f\t", vals[i]); - printf("\n"); + printf("%.0f\n", t/340.+36.53); } } -// get statistics for 10 seconds +// get statistics for 30 seconds void get_stat(){ double t0 = dtime(), scds = 1.; - double mean[6]={0.}, max[6], min[6], d, N = 0.; + double mean[7]={0.}, max[7], min[7], s2[7]={0.}, d, N = 0.; int i; + int16_t r; for(i = 0; i < 6; ++i){ // fill initial values - max[i] = min[i] = ((double)RD16(addrs[i])) * scales[i]; + r = RD16(addrs[i]); + max[i] = min[i] = r * scales[i]; } - printf("Wait for 10 seconds, please\n"); + r = RD16(TEMP_OUT_H); + max[6] = min[6] = r/340.+36.53; + printf("Wait for 30 seconds, please\n"); do{ double t1 = dtime(); while(!(RD8(INT_STATUS) & 1) && dtime() - t1 < 1.); // wait next data portion @@ -101,24 +108,35 @@ void get_stat(){ return; } for(i = 0; i < 6; ++i){ - d = ((double)RD16(addrs[i])) * scales[i]; + r = RD16(addrs[i]); + d = r * scales[i]; mean[i] += d; + s2[i] += d*d; if(max[i] < d) max[i] = d; if(min[i] > d) min[i] = d; } + r = RD16(TEMP_OUT_H); + d = r/340.+36.53; + mean[6] += d; s2[6] += d*d; + if(max[6] < d) max[6] = d; + if(min[6] > d) min[6] = d; + N += 1.; if(dtime() - t0 > scds){ - printf("%.0f\b", scds); + printf("\r%.0f ", 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); + }while(dtime() - t0 < 30.); + printf("\nAX, AY, AZ, GX, GY, GZ, T. [ min, max, mean, max-min, std ]\n"); + for(i = 0; i < 7; ++i){ + double m = mean[i]/N; + printf("[ %.1f, %.1f, %.1f, %.1f, %.4f ] ", min[i], max[i], m, max[i]-min[i], sqrt(s2[i]/N - m*m)); + } printf("\n\n"); } int main(int argc, char **argv){ +// int i; fd = wiringPiI2CSetup(MPU6050_I2C_ADDRESS); setbuf(stdout, NULL); if (fd == -1){ @@ -131,10 +149,12 @@ int main(int argc, char **argv){ return 2; } reset_brd(); +// printf("got first 32 regs:\n"); +// for(i = 0; i < 32; ++i) printf("R[%x]=%u ", i, RD8(i)); (void) argv; if(argc == 2) get_stat(); else{ - printf("Unix time\tAX\tAY\tAZ\tGX\tGY\tGZ\n"); + printf("Unix time\tAX\tAY\tAZ\tGX\tGY\tGZ\tT\n"); while(1) print_ag(); } return 0;