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

[eddy_em]

В этом месяце технические ночи по "причинческим технинам" разносятся на вторые половины 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) дает какую-то чушь.

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

Comments

[sevasat.livejournal.com]

Берешь ардуинку (с али за 1.5 бакса), хреначиш туда скетч, по УАРТУ подключаешь к малинке. Грязно, криво, избыточно, но быстро, просто и работает.

[eddy-em.livejournal.com]

Ублюдочное решение. Тем более, что по коду этого "скетча" я отчасти пытался процедуру самотестирования воспроизвести. Нашел кучу ошибок. Скетч — говно.

[sevasat.livejournal.com]

Ардуино-сектч не надо проверять. Его надо залить, перекреститься, сплюнуть и включить. Ардуина состоит из костылей чуть менее чем полностью но тем не менее даже работает.

[eddy-em.livejournal.com]

Ардуйня удобна для одноразовых сиюминутных решений — спора нет. Скажем, купил датчик, и надо проверить, работает ли он. На ардуйне за 10 минут проверил. Далее — пишем код под нормальный микроконтроллер и работаем с датчиком ☺

А проблема оказалась в моей невнимательности: я зачем-то int16_t у типов считываемых данных поменял на int32_t, в результате чего отрицательные значения стали положительными симметрично относительно 32768...

Пишу следующий пост с обновленным кодом.

[ex0_planet]

И, если скетч не заработал, то что?

[eddy-em.livejournal.com]

Плакать горючими слезами и умолять аврщиков написать нужный "скетч", понятное дело.

[sevasat.livejournal.com]

Искать другой сектч, спрашивать на стековерфлоу и форуме, если все это не помогает - править рандомные куски от балды и пытаться собрать.

[ex0_planet]

Мне кажется, именно это и называется "доебаться до мышей".

[t-mike.livejournal.com]

так вроде ядерный драйвер есть

[eddy-em.livejournal.com]

И действительно ☺

Ну да черт с ним. Оно у меня и в юзерспейсе нормально работает.

P.S. Почитал readme к модулю ведра. Ничего интересного: тупой sysfs-интерфейс для работы из-под баш-скриптов.
Мне никакой разницы: на сях писать простыню, или на баше. На сях даже как-то приятней…

[t-mike.livejournal.com]

ну всё равно скажу своё имхо :)
в споре i2c-dev vs WiringPi у меня первое побеждает, ибо универсальней

[eddy-em.livejournal.com]

Оба кривые. Кстати, что-то я вот не могу нагуглить примера использования I2C или SPI без wiringPi и костылей вроде sysfs…

[t-mike.livejournal.com]

ну так первоисточник ж
https://www.kernel.org/doc/Documentation/i2c/dev-interface
https://www.kernel.org/doc/Documentation/spi/spidev
https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/tools/spi/spidev_test.c
https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/tools/spi/spidev_fdx.c
ну и тут https://github.com/groeck/i2c-tools

что кривое? i2c-dev/spidev или wiringPi (который тупо обёртка к i2c-dev/spidev)

кстати, почему sysfs костыль? идеология такая, наравится или нет другой вопрос.

[eddy-em.livejournal.com]

Да все кривое. У "ядреных" модулей мне не нравится то, что запись/чтение идет не сисвызовами read/write (что логично), а ioctl'ами (что непривычно как-то).

А sysfs удобен лишь при работе из баша. В сях он — костыль похлеще ioctl'ов.

[t-mike.livejournal.com]

э... так так и сделано через read/write, ioctl это специфичные вещи, типа скорости, адреса и т.д. и т.п.

почему sysfs в сях костыль?
чем открыть/прочитать/записать/закрыть /sysfs/XXX отличается от открыть/прочитать/записать/закрыть /dev/XXX? ах, ну да в последнем варианте получим сырые данные которые ещё надо обработать

[eddy-em.livejournal.com]

Нет, чтение-запись через ioctl:

	ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
	if (ret < 1)
		pabort("can't send spi message");

> почему sysfs в сях костыль?
Потому что вместо работы с одним-единственным файлом и сисвызовами read/write приходится работать с толпой файлов!

[t-mike.livejournal.com]

опять же в доках
Normal open() and close() operations on /dev/spidevB.D files work as you would expect.
Standard read() and write() operations are obviously only half-duplex, and the chipselect is deactivated between those operations. Full-duplex access, and composite operation without chipselect de-activation, is available using the SPI_IOC_MESSAGE(N) request.

>Потому что вместо работы с одним-единственным файлом и сисвызовами read/write приходится работать с толпой файлов!
а это уж на совести разрабочика, sysfs тут не причём

[eddy-em.livejournal.com]

> опять же в доках
Через задницу же! По-человечески, в памяти ведра должны быть постоянно выделены два буфера. Сисвызов write должен записывать указанные данные в буфер записи и (если файл открыт в блокирующем режиме) ждать, пока буфер не будет передан. Последующий сисвызов read просто считывает данные из принимающего буфера.
Могли бы сделать именно так. А чтобы не городить слишком больших буферов, можно было бы как в v4l2 при помощи сисвызова подсовывать буферы из userspace.