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

Исправляюсь. Теперь можно и домой идти...

Как и ожидалось, причиной всему — рукожопость. Мне вот только оператором термоядерных станций работать (пусть это фантастика, но будем считать, что все-таки в будущем "холодный термояд" появится): к середине смены ничего в радиусе пары сотен километров вокруг станции не останется ☺
Такие как я забывают наручные часы/скальпели/зажимы/etc внутри пациентов во время операции, не доворачивают гайки на колесах при "переобувке", выезжают в гололед на летней резине и просыпают время посадки на свой рейс...

Ошибка была элементарной: зачем-то int16_t я поменял на int32_t, в результате чего вся котовасия и началась!
Теперь код выглядит так:

#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;
}

int16_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 reset_brd(){
	printf("RESET!\n");
    WR8(PWR_MGMT_1, 0x80); // device reset
    sleep(1);
	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
}

void get_biases(){ //  calibration
	printf("Calibrate biases. Wait please..\n");
	double t0 = dtime(), t;
	int N = 0;
	xb=0,yb=0,zb=0, axb=0,ayb=0,azb=0;
	do{
		if(!(RD8(INT_STATUS) & 1)) continue; // wait next data portion
		RDHA();
		xb+=x, yb+=y, zb+=z, axb+=ax,ayb+=ay,azb+=az;
		++N;
	}while((t = dtime() - t0) < 2.);
	if(!N) return;
	xb/=N, yb/=N, zb/=N,  axb/=N,ayb/=N,azb/=N;
	printf("Got bias: hyro: %d,%d,%d; accel: %d,%d,%d\n",xb,yb,zb, axb,ayb,azb);
}

void get_average(){ // 1 second average
	double t0 = dtime(), t;
	int N = 0;
	int32_t X=0,Y=0,Z=0, AX=0,AY=0,AZ=0;
	do{
		if(!(RD8(INT_STATUS) & 1)) continue; // wait next data portion
		RDHA();
		X+=x, Y+=y, Z+=z, AX+=ax,AY+=ay,AZ+=az;
		++N;
	}while((t = dtime() - t0) < 1.);
	if(!N) return; 
	x=X/N, y=Y/N, z=Z/N,  ax=AX/N,ay=AY/N,az=AZ/N;
}

int ctr = 0;

int print_h(){
	static int ox = 0, oy = 0, oz = 0;
	static double oxv=0., oyv=0., ozv=0.;
	int r = 0;
	inline void ph(const char s, int32_t c, int32_t b, double *o){
		double val = (double)(c-b)/32768.*250.;
		if(fabs(val - *o) < 0.5) return;
		printf("%c=%.1f ", s, val);
		++r;
		*o = val;
	}
	ph('x', x, xb, &oxv); ph('y', y, yb, &oyv); ph('z', z, zb, &ozv);
	if(x == ox && y == oy && z == oz && ++ctr == 5){ // board hangs?
		ctr = 0; reset_brd();
	}
	return r;
}

int print_a(){
	static int ox = 0, oy = 0, oz = 0;
	static double oxv=0., oyv=0., ozv=0.;
	int r = 0;
	inline void pa(const char *s, int32_t a, int32_t b, double *o){
		(void) b;
		double val = (double)(a)/32768.*19.6;
		if(fabs(val - *o) < 0.2) return;
		printf("%s=%.1f ", s, val);
		++r;
		*o = val;
	}
	pa("ax", ax, axb, &oxv);
	pa("ay", ay, ayb, &oyv);
	pa("az", az, azb, &ozv);
	if(ax == ox && ay == oy && az == oz && ++ctr == 5){ // board hangs?
		ctr = 0; reset_brd();
	}
	return r;
}

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;
    }
    reset_brd();
	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);
	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);
        get_average();
        printf("%g, t=%.0f ", dtime() - t0, (double)t/340.0 + 36.53);
        if(print_h()) printf("\t");
        print_a();
        printf("\n");
        //while(!(RD8(INT_STATUS) & 1)); // wait next data portion
        //sleep(1);
    }
    return 0;
}

Я дополнительно добавил детектирование зависания датчика (сейчас он на соплях подключен, поэтому при интенсивном его колебании периодически пропадает питание и датчик зависает) для перезаписи настроек.

Теперь выхлоп выглядит вполне прилично:
make && ./hyrotest 
gcc -Wall -Werror -Wextra  -D_XOPEN_SOURCE=1111   -c -o test.o test.c
gcc -Wall -Werror -Wextra  -D_XOPEN_SOURCE=1111 test.o -lwiringPi -lm -o hyrotest
RESET!
Perform a self test
changes. H: x=0.00782801%, y=-0.00366888%, z=0.0496433%; A: x=0.314991%, y=1.12752%, z=-4.71018%
Calibrate biases. Wait please..
Got bias: hyro: -278,64,-173; accel: -790,-3130,15859
1.00669, t=29 ax=-0.5 ay=-1.9 az=9.5 
2.01383, t=29 
3.01945, t=29 
4.02735, t=29 
5.03276, t=29 
6.0399, t=29 
7.0482, t=29 
8.05222, t=29 
9.0574, t=29 
10.0598, t=29 
11.0651, t=29 
12.0725, t=29 x=-39.0 y=25.5 z=31.5 	ax=-1.8 az=9.0 
13.0784, t=29 x=-75.7 y=40.9 z=28.7 	ax=-5.0 ay=-4.1 az=-5.9 
14.0842, t=29 x=10.0 y=16.7 z=23.1 	ax=-5.4 ay=-1.5 az=-7.8 
15.0906, t=29 x=-0.3 y=6.1 z=10.1 	ax=-1.0 ay=0.3 az=-9.7 
16.0958, t=29 x=0.3 y=-0.8 z=1.0 	ax=-1.4 az=-9.9 
17.1033, t=29 y=-0.1 z=0.1 	
18.1106, t=29 
19.1169, t=29 
20.1236, t=29 
^C

Все отклонения входят в рамки по даташиту (±14%). Но точность просто "поражает": температура явно врет как минимум градусов на пять (при заявленом отклонении от реальных показаний максимум в 1 градус)! Акселерометр тоже врет значительно сильней заявленых ±0.5%. В ту же степь и гироскоп, показывающий вообще непонятные вещи.
Хотя, по-человечески, надо бы это все дело откалибровать. Придумать бы только более-менее нормальный стенд!

В принципе, для нашей задачи измерения колебаний СПФ БТА гироскоп не нужен: достаточно лишь показаний акселерометра, чтобы выявить толчки (ускорения во время гидирования значительно меньше установленного мной порога в 0.2м/с²). Остается допилить утилиту сбора данных, и можно будет с 12 ноября начать сбор данных (технической ночи скорей всего не будет, судя по прогнозу, а вот 16, возможно, что-нибудь понаблюдать получится — заодно, если позволит небо, сниму волновые фронты Шаком-Гартманном, давненько этого не делали).
Tags: c, raspberry pi, железяки, рукожопие
Subscribe

  • Опять Подорванка смыла мост

    В четверг лило настолько, что, похоже, опять на подорванке забилась стремнина бревнами, а потом внезапно это все прорвало. МЧСовсцы перетаскивали…

  • Аж коробит…

    Уже который раз вижу (даже в прессе) выражение: "ставить прививку". Это что за странное такое словоблудие? Либо эпохи царской России, либо даже…

  • Упаковочка

    Получил сегодня пару посылок с алиэкспресса. Одна из них была с новым для меня способом доставки (на посылке не указывался адрес, а лишь "до…

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
  • 5 comments