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

little MPU6050 changes

Browse Source
This commit is contained in:
eddyem 2016-11-11 16:01:35 +03:00
parent 567d5b7aa7
commit e8a0b47452
1 changed files with 37 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
Raspberry/MPU-6050-log/main.c
View File

@ -12,7 +12,9 @@
int fd, ctr=0; 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}; 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 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 RD8(reg) wiringPiI2CReadReg8(fd, reg)
#define RD16(reg) (RD8(reg)<<8|RD8(reg+1)) #define RD16(reg) (RD8(reg)<<8|RD8(reg+1))
@ -40,18 +42,18 @@ double dtime(){
void reset_brd(){ void reset_brd(){
fprintf(stderr, "RESET!\n"); fprintf(stderr, "RESET!\n");
WR8(PWR_MGMT_1, 0x80); // device reset 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(CONFIG, 3); // 3 == 44/42Hz, 6 == 5Hz
WR8(SMPLRT_DIV, 9); // 1kHz / 10 = 100Hz 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); usleep(100000);
} }
void print_ag(){ void print_ag(){
static int16_t oldvals[6]; static int16_t oldvals[6];
int16_t dat[6]; int16_t dat[6];
static double doldvals[6]; static double doldvals[6] = {-1000.,-1000.,-1000.,-1000.,-1000.,-1000.};
double vals[6]; double vals[6];
int i, ctr = 0; int i, ctr = 0;
double t0 = dtime(); double t0 = dtime();
@ -77,21 +79,26 @@ void print_ag(){
} }
} }
if(ctr){ // there was changes if(ctr){ // there was changes
int16_t t = RD16(TEMP_OUT_H);
printf("%.3f\t", dtime()); printf("%.3f\t", dtime());
for(i = 0; i < 6; ++i) printf("%.1f\t", vals[i]); 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(){ void get_stat(){
double t0 = dtime(), scds = 1.; 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; int i;
int16_t r;
for(i = 0; i < 6; ++i){ // fill initial values 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{ do{
double t1 = dtime(); double t1 = dtime();
while(!(RD8(INT_STATUS) & 1) && dtime() - t1 < 1.); // wait next data portion while(!(RD8(INT_STATUS) & 1) && dtime() - t1 < 1.); // wait next data portion
@ -101,24 +108,35 @@ void get_stat(){
return; return;
} }
for(i = 0; i < 6; ++i){ for(i = 0; i < 6; ++i){
d = ((double)RD16(addrs[i])) * scales[i]; r = RD16(addrs[i]);
d = r * scales[i];
mean[i] += d; mean[i] += d;
s2[i] += d*d;
if(max[i] < d) max[i] = d; if(max[i] < d) max[i] = d;
if(min[i] > d) min[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.; N += 1.;
if(dtime() - t0 > scds){ if(dtime() - t0 > scds){
printf("%.0f\b", scds); printf("\r%.0f ", scds);
scds += 1.; scds += 1.;
} }
}while(dtime() - t0 < 10.); }while(dtime() - t0 < 30.);
printf("\nAX, AY, AZ, GX, GY, GZ. [ min, max, mean ]\n"); printf("\nAX, AY, AZ, GX, GY, GZ, T. [ min, max, mean, max-min, std ]\n");
for(i = 0; i < 6; ++i) for(i = 0; i < 7; ++i){
printf("[ %.1f, %.1f, %.1f ] ", min[i], max[i], mean[i] / N); 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"); printf("\n\n");
} }
int main(int argc, char **argv){ int main(int argc, char **argv){
// int i;
fd = wiringPiI2CSetup(MPU6050_I2C_ADDRESS); fd = wiringPiI2CSetup(MPU6050_I2C_ADDRESS);
setbuf(stdout, NULL); setbuf(stdout, NULL);
if (fd == -1){ if (fd == -1){
@ -131,10 +149,12 @@ int main(int argc, char **argv){
return 2; return 2;
} }
reset_brd(); reset_brd();
// printf("got first 32 regs:\n");
// for(i = 0; i < 32; ++i) printf("R[%x]=%u ", i, RD8(i));
(void) argv; (void) argv;
if(argc == 2) get_stat(); if(argc == 2) get_stat();
else{ 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(); while(1) print_ag();
} }
return 0; return 0;