Емельянов Эдуард Владимирович (eddy_em) wrote,
Емельянов Эдуард Владимирович
eddy_em

MPU-5060 и "малинка"

В этом месяце технические ночи по "причинческим технинам" разносятся на вторые половины 12-13 и/или (в зависимости от погоды) 16-17 ноября. Уже второй месяц у меня лежит гироскоп MPU-5060, который Гриша предложил прицепить в СПФ, чтобы логгировать реальные вибрации — тогда точно будет понятна причина дрожания изображений (если вибрации не будут наблюдаться, то явно виноват воздух, а не железо; vice versa). Но пока эта железяка упорно отказывается работать. Хотя даташит исчитан вдоль и поперек...

По найденному на интернетских сусеках написано такое чудовище:
#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;
#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) дает какую-то чушь.

Теперь остается гадать: то ли сдох датчик (после отключения и покоя в течение двух-трех минут ситуация возобновляется), то ли карма у меня такая, рукожопая...
В интернетах были найдены только мегакривущие "скетчи" для ардуины, либо какие-то совершенно неинтересные куски кода. В даташите информации куда больше. Но вот железяка что-то подвела...
Tags: c, железяки, рукожопие
Subscribe

  • Темы-2

    Некоторые испугались, прочитав предыдущие темы. Повторяю: темы для работы в течение всей школы (три года). А вот — их части, которые можно осилить за…

  • Темы для творческих работ школьников

    В связи с возможным проведением очной весенней школы АФШ (детей набрали еще прошлым летом, но пока очно не было возможности встретиться из-за…

  • Бюджетная читалка с алиэкспресса

    Долго искал вменяемые электронные читалки, но за формат примерно А4 просят около $900, вообще озверели. ОК, решил взять мелкую дешевую читалку, чтобы…

promo eddy_em august 17, 2019 12:33 3
Buy for 10 tokens
Юра намедни напечатал корпус для хронометра. Для первого блина получилось неплохо: И еще немного фотографий:
  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your reply will be screened

  • 18 comments

  • Темы-2

    Некоторые испугались, прочитав предыдущие темы. Повторяю: темы для работы в течение всей школы (три года). А вот — их части, которые можно осилить за…

  • Темы для творческих работ школьников

    В связи с возможным проведением очной весенней школы АФШ (детей набрали еще прошлым летом, но пока очно не было возможности встретиться из-за…

  • Бюджетная читалка с алиэкспресса

    Долго искал вменяемые электронные читалки, но за формат примерно А4 просят около $900, вообще озверели. ОК, решил взять мелкую дешевую читалку, чтобы…