MPU-5060 и "малинка"
В этом месяце технические ночи по "причинческим технинам" разносятся на вторые половины 12-13 и/или (в зависимости от погоды) 16-17 ноября. Уже второй месяц у меня лежит гироскоп MPU-5060, который Гриша предложил прицепить в СПФ, чтобы логгировать реальные вибрации - тогда точно будет понятна причина дрожания изображений (если вибрации не будут наблюдаться, то явно виноват воздух, а не железо; vice versa). Но пока эта железяка упорно отказывается работать. Хотя даташит исчитан вдоль и поперек...
По найденному на интернетских сусеках написано такое чудовище:
#include
#include
#include
#include
#include
#include
#include
#include
#include "MPU6050.h"
int fd;
#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;
}
int32_t x,y,z, ax,ay,az; // hyro & accel data
int32_t xb=0,yb=0,zb=0, axb=0,ayb=0,azb=0; // bias
#define RDHA() do{x = RD16(GYRO_XOUT_H); y = RD16(GYRO_YOUT_H); z = RD16(GYRO_ZOUT_H);\
ax = RD16(ACCEL_XOUT_H); ay = RD16(ACCEL_YOUT_H); az = RD16(ACCEL_ZOUT_H);}while(0)
void get_biases(){ // calibration
printf("Calibrate biases. Wait please..\n");
double t0 = dtime();
int N = 0;
xb=0,yb=0,zb=0, axb=0,ayb=0,azb=0;
do{
while(!(RD8(INT_STATUS) & 1)); // wait next data portion
RDHA();
xb+=x, yb+=y, zb+=z, axb+=ax,ayb+=ay,azb+=az;
++N;
}while(dtime() - t0 < 2.);
xb/=N, yb/=N, zb/=N, axb/=N,ayb/=N,azb/=N;
printf("Got: hyro: %d,%d,%d; accel: %d,%d,%d\n",xb,yb,zb, axb,ayb,azb);
}
void get_average(){ // 1 second average
double t0 = dtime();
int N = 0;
int32_t X=0,Y=0,Z=0, AX=0,AY=0,AZ=0;
do{
while(!(RD8(INT_STATUS) & 1)); // wait next data portion
RDHA();
X+=x, Y+=y, Z+=z, AX+=ax,AY+=ay,AZ+=az;
++N;
}while(dtime() - t0 < 1.);
x=X/N, y=Y/N, z=Z/N, ax=AX/N,ay=AY/N,az=AZ/N;
}
int main()
{
int16_t t;
fd = wiringPiI2CSetup(MPU6050_I2C_ADDRESS);
if (fd == -1){
printf("I2C error\n");
return 1;
}
int answ = RD8(WHO_AM_I_MPU6050);
if(answ != 0x68){
printf("No MPU6050 detected\n");
return 2;
}
WR8(PWR_MGMT_1, 0x80); // device reset
sleep(1);
WR8(CONFIG, 6); // 3 == 44/42Hz, 6 == 5Hz
WR8(SMPLRT_DIV, 9); // 1kHz / 10 = 100Hz
WR16(PWR_MGMT_1, 0);
printf("Perform a self test\n");
WR8(ACCEL_CONFIG, 0xF0); WR8(GYRO_CONFIG, 0xE0); // turn on self-test
sleep(1);
get_average();
int16_t ox=x, oy=y, oz=z, oax=ax, oay=ay, oaz=az;
printf("OLDH: %d,%d,%d, OLDA: %d, %d, %d\n",x,y,z,ax,ay,az);
uint8_t xgt = RD8(SELF_TEST_X), ygt = RD8(SELF_TEST_Y), zgt = RD8(SELF_TEST_Z), sta = RD8(SELF_TEST_A);
int8_t xat = xgt>>3|sta>>4, yat = ygt>>3|((sta>>2)&3), zat = zgt>>3|(sta&3);
xgt &= 0x1f, ygt &= 0x1f, zgt &= 0x1f;
double ftxa = get_fta(xat), ftya = get_fta(yat), ftza = get_fta(zat);
double ftxg = get_ftg(xgt), ftyg = -get_ftg(ygt), ftzg = get_ftg(zgt);
WR8(GYRO_CONFIG, 0); WR8(ACCEL_CONFIG, 0); // turn off self-test
sleep(1);
get_average();
printf("CURH: %d,%d,%d, CURA: %d, %d, %d\n",x,y,z,ax,ay,az);
printf("prec1: %g,%g,%g, CURA: %g, %g, %g\n",(ox-x-ftxg)/ftxg,(oy-y-ftyg)/ftyg,(oz-z-ftzg)/ftzg,
(oax-ax-ftxa)/ftxa,(oay-ay-ftya)/ftya,(oaz-az-ftza)/ftza);
inline double prec(double a, double b){return (a-b)/b;}
printf("FTH: %g, %g, %g; FTA: %g, %g, %g\n", ftxg,ftyg, ftzg, ftxa, ftya, ftza);
printf("GT: %d, %d, %d; AT: %d, %d, %d\n", xgt, ygt, zgt, xat, yat, zat);
printf("changes. H: x=%g%%, y=%g%%, z=%g%%; A: x=%g%%, y=%g%%, z=%g%%\n", prec(ox-x,ftxg), prec(oy-y,ftyg), prec(oz-z,ftzg),
prec(oax-ax,ftxa), prec(oay-ay,ftya), prec(oaz-az, ftza));
get_biases();
// int i;
double t0 = dtime();
//for(i = 0; i < 100; ++i)
while(1) {
t = RD16(TEMP_OUT_H);
RDHA();
printf("T=%g, x=%g y=%g z=%g; t=%g; ", dtime()-t0, (double)(x-xb)/32768.*250.,(double)(y-yb)/32768.*250.,(double)(z-zb)/32768.*250., (double)t/340.0 + 36.53);
// printf("x0=%g y0=%g z0=%g; ", (double)(x)/32768.*250.,(double)(y)/32768.*250.,(double)(z)/32768.*250.);
printf("ax=%g ay=%g az=%g\n", (double)(ax)/32768.*19.6, (double)(ay)/32768.*19.6, (double)(az)/32768.*19.6);
//while(!(RD8(INT_STATUS) & 1)); // wait next data portion
sleep(1);
}
return 0;
}
Вчера результаты были вполне пристойными. А вот сегодня пошел какой-то бред с координаты X аккселерометра. Вот такой бред имею:
make && ./hyrotest
make: Цель <> не требует выполнения команд.
Perform a self test
OLDH: 5929,59760,7200, OLDA: 1714, 2141, 6508
CURH: 65270,75,65351, CURA: 63819, 278, 16051
prec1: -10.6538,-0.00434955,-9.26623, CURA: -30.9477, -0.10164, -4.77104
FTH: 6146.88, -5876.56, 7034.77; FTA: 2073.78, 2073.78, 2530.6
GT: 15, 14, 18; AT: 13, 13, 19
changes. H: x=-10.6538%, y=-0.00434955%, z=-9.26623%; A: x=-30.9477%, y=-0.10164%, z=-4.77104%
Calibrate biases. Wait please..
Got: hyro: 65274,75,65352; accel: 63829,275,16040
T=0.00810504, x=0.1297 y=0.0534058 z=-0.0915527; t=25.9888; ax=38.2095 ay=0.165088 az=9.71865
T=1.0154, x=-0.0305176 y=0.00762939 z=-0.0610352; t=26.0829; ax=38.2621 ay=0.181836 az=9.58945
T=2.02268, x=1.99127 y=-0.106812 z=0.0228882; t=25.9418; ax=38.164 ay=0.117236 az=9.62773
T=3.03001, x=-0.0228882 y=-0.0457764 z=0.038147; t=25.9418; ax=38.231 ay=0.277539 az=9.5751
T=4.03732, x=-1.89972 y=0.0991821 z=0.00762939; t=26.6476; ax=38.1903 ay=0.153125 az=9.63013
^C
Строчка "changes" показывает средние величины отклонений (по даташиту - в процентах, хотя мне казалось, что надо делить на 100, но, судя по бредовым данным далее, делить не надо, надо чуть ли не умножать...) показаний от калибровочных. Как видно, и гироскоп по осям X и Z отлично брешет, и акселерометр по упомянутой оси X (а в т.ч. и по Z) дает какую-то чушь.
Теперь остается гадать: то ли сдох датчик (после отключения и покоя в течение двух-трех минут ситуация возобновляется), то ли карма у меня такая, рукожопая...
В интернетах были найдены только мегакривущие "скетчи" для ардуины, либо какие-то совершенно неинтересные куски кода. В даташите информации куда больше. Но вот железяка что-то подвела...
По найденному на интернетских сусеках написано такое чудовище:
#include
#include
#include
#include
#include
#include
#include
#include
#include "MPU6050.h"
int fd;
#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;
}
int32_t x,y,z, ax,ay,az; // hyro & accel data
int32_t xb=0,yb=0,zb=0, axb=0,ayb=0,azb=0; // bias
#define RDHA() do{x = RD16(GYRO_XOUT_H); y = RD16(GYRO_YOUT_H); z = RD16(GYRO_ZOUT_H);\
ax = RD16(ACCEL_XOUT_H); ay = RD16(ACCEL_YOUT_H); az = RD16(ACCEL_ZOUT_H);}while(0)
void get_biases(){ // calibration
printf("Calibrate biases. Wait please..\n");
double t0 = dtime();
int N = 0;
xb=0,yb=0,zb=0, axb=0,ayb=0,azb=0;
do{
while(!(RD8(INT_STATUS) & 1)); // wait next data portion
RDHA();
xb+=x, yb+=y, zb+=z, axb+=ax,ayb+=ay,azb+=az;
++N;
}while(dtime() - t0 < 2.);
xb/=N, yb/=N, zb/=N, axb/=N,ayb/=N,azb/=N;
printf("Got: hyro: %d,%d,%d; accel: %d,%d,%d\n",xb,yb,zb, axb,ayb,azb);
}
void get_average(){ // 1 second average
double t0 = dtime();
int N = 0;
int32_t X=0,Y=0,Z=0, AX=0,AY=0,AZ=0;
do{
while(!(RD8(INT_STATUS) & 1)); // wait next data portion
RDHA();
X+=x, Y+=y, Z+=z, AX+=ax,AY+=ay,AZ+=az;
++N;
}while(dtime() - t0 < 1.);
x=X/N, y=Y/N, z=Z/N, ax=AX/N,ay=AY/N,az=AZ/N;
}
int main()
{
int16_t t;
fd = wiringPiI2CSetup(MPU6050_I2C_ADDRESS);
if (fd == -1){
printf("I2C error\n");
return 1;
}
int answ = RD8(WHO_AM_I_MPU6050);
if(answ != 0x68){
printf("No MPU6050 detected\n");
return 2;
}
WR8(PWR_MGMT_1, 0x80); // device reset
sleep(1);
WR8(CONFIG, 6); // 3 == 44/42Hz, 6 == 5Hz
WR8(SMPLRT_DIV, 9); // 1kHz / 10 = 100Hz
WR16(PWR_MGMT_1, 0);
printf("Perform a self test\n");
WR8(ACCEL_CONFIG, 0xF0); WR8(GYRO_CONFIG, 0xE0); // turn on self-test
sleep(1);
get_average();
int16_t ox=x, oy=y, oz=z, oax=ax, oay=ay, oaz=az;
printf("OLDH: %d,%d,%d, OLDA: %d, %d, %d\n",x,y,z,ax,ay,az);
uint8_t xgt = RD8(SELF_TEST_X), ygt = RD8(SELF_TEST_Y), zgt = RD8(SELF_TEST_Z), sta = RD8(SELF_TEST_A);
int8_t xat = xgt>>3|sta>>4, yat = ygt>>3|((sta>>2)&3), zat = zgt>>3|(sta&3);
xgt &= 0x1f, ygt &= 0x1f, zgt &= 0x1f;
double ftxa = get_fta(xat), ftya = get_fta(yat), ftza = get_fta(zat);
double ftxg = get_ftg(xgt), ftyg = -get_ftg(ygt), ftzg = get_ftg(zgt);
WR8(GYRO_CONFIG, 0); WR8(ACCEL_CONFIG, 0); // turn off self-test
sleep(1);
get_average();
printf("CURH: %d,%d,%d, CURA: %d, %d, %d\n",x,y,z,ax,ay,az);
printf("prec1: %g,%g,%g, CURA: %g, %g, %g\n",(ox-x-ftxg)/ftxg,(oy-y-ftyg)/ftyg,(oz-z-ftzg)/ftzg,
(oax-ax-ftxa)/ftxa,(oay-ay-ftya)/ftya,(oaz-az-ftza)/ftza);
inline double prec(double a, double b){return (a-b)/b;}
printf("FTH: %g, %g, %g; FTA: %g, %g, %g\n", ftxg,ftyg, ftzg, ftxa, ftya, ftza);
printf("GT: %d, %d, %d; AT: %d, %d, %d\n", xgt, ygt, zgt, xat, yat, zat);
printf("changes. H: x=%g%%, y=%g%%, z=%g%%; A: x=%g%%, y=%g%%, z=%g%%\n", prec(ox-x,ftxg), prec(oy-y,ftyg), prec(oz-z,ftzg),
prec(oax-ax,ftxa), prec(oay-ay,ftya), prec(oaz-az, ftza));
get_biases();
// int i;
double t0 = dtime();
//for(i = 0; i < 100; ++i)
while(1) {
t = RD16(TEMP_OUT_H);
RDHA();
printf("T=%g, x=%g y=%g z=%g; t=%g; ", dtime()-t0, (double)(x-xb)/32768.*250.,(double)(y-yb)/32768.*250.,(double)(z-zb)/32768.*250., (double)t/340.0 + 36.53);
// printf("x0=%g y0=%g z0=%g; ", (double)(x)/32768.*250.,(double)(y)/32768.*250.,(double)(z)/32768.*250.);
printf("ax=%g ay=%g az=%g\n", (double)(ax)/32768.*19.6, (double)(ay)/32768.*19.6, (double)(az)/32768.*19.6);
//while(!(RD8(INT_STATUS) & 1)); // wait next data portion
sleep(1);
}
return 0;
}
Вчера результаты были вполне пристойными. А вот сегодня пошел какой-то бред с координаты X аккселерометра. Вот такой бред имею:
make && ./hyrotest
make: Цель <> не требует выполнения команд.
Perform a self test
OLDH: 5929,59760,7200, OLDA: 1714, 2141, 6508
CURH: 65270,75,65351, CURA: 63819, 278, 16051
prec1: -10.6538,-0.00434955,-9.26623, CURA: -30.9477, -0.10164, -4.77104
FTH: 6146.88, -5876.56, 7034.77; FTA: 2073.78, 2073.78, 2530.6
GT: 15, 14, 18; AT: 13, 13, 19
changes. H: x=-10.6538%, y=-0.00434955%, z=-9.26623%; A: x=-30.9477%, y=-0.10164%, z=-4.77104%
Calibrate biases. Wait please..
Got: hyro: 65274,75,65352; accel: 63829,275,16040
T=0.00810504, x=0.1297 y=0.0534058 z=-0.0915527; t=25.9888; ax=38.2095 ay=0.165088 az=9.71865
T=1.0154, x=-0.0305176 y=0.00762939 z=-0.0610352; t=26.0829; ax=38.2621 ay=0.181836 az=9.58945
T=2.02268, x=1.99127 y=-0.106812 z=0.0228882; t=25.9418; ax=38.164 ay=0.117236 az=9.62773
T=3.03001, x=-0.0228882 y=-0.0457764 z=0.038147; t=25.9418; ax=38.231 ay=0.277539 az=9.5751
T=4.03732, x=-1.89972 y=0.0991821 z=0.00762939; t=26.6476; ax=38.1903 ay=0.153125 az=9.63013
^C
Строчка "changes" показывает средние величины отклонений (по даташиту - в процентах, хотя мне казалось, что надо делить на 100, но, судя по бредовым данным далее, делить не надо, надо чуть ли не умножать...) показаний от калибровочных. Как видно, и гироскоп по осям X и Z отлично брешет, и акселерометр по упомянутой оси X (а в т.ч. и по Z) дает какую-то чушь.
Теперь остается гадать: то ли сдох датчик (после отключения и покоя в течение двух-трех минут ситуация возобновляется), то ли карма у меня такая, рукожопая...
В интернетах были найдены только мегакривущие "скетчи" для ардуины, либо какие-то совершенно неинтересные куски кода. В даташите информации куда больше. Но вот железяка что-то подвела...