![[personal profile]](https://www.dreamwidth.org/img/silk/identity/user.png){kind=small}
Entry tags:
Исправляюсь. Теперь можно и домой идти...
Как и ожидалось, причиной всему — рукожопость. Мне вот только оператором термоядерных станций работать (пусть это фантастика, но будем считать, что все-таки в будущем "холодный термояд" появится): к середине смены ничего в радиусе пары сотен километров вокруг станции не останется ☺
Такие как я забывают наручные часы/скальпели/зажимы/etc внутри пациентов во время операции, не доворачивают гайки на колесах при "переобувке", выезжают в гололед на летней резине и просыпают время посадки на свой рейс...
Ошибка была элементарной: зачем-то int16_t я поменял на int32_t, в результате чего вся котовасия и началась!
Теперь код выглядит так:
Я дополнительно добавил детектирование зависания датчика (сейчас он на соплях подключен, поэтому при интенсивном его колебании периодически пропадает питание и датчик зависает) для перезаписи настроек.
Теперь выхлоп выглядит вполне прилично:
Все отклонения входят в рамки по даташиту (±14%). Но точность просто "поражает": температура явно врет как минимум градусов на пять (при заявленом отклонении от реальных показаний максимум в 1 градус)! Акселерометр тоже врет значительно сильней заявленых ±0.5%. В ту же степь и гироскоп, показывающий вообще непонятные вещи.
Хотя, по-человечески, надо бы это все дело откалибровать. Придумать бы только более-менее нормальный стенд!
В принципе, для нашей задачи измерения колебаний СПФ БТА гироскоп не нужен: достаточно лишь показаний акселерометра, чтобы выявить толчки (ускорения во время гидирования значительно меньше установленного мной порога в 0.2м/с²). Остается допилить утилиту сбора данных, и можно будет с 12 ноября начать сбор данных (технической ночи скорей всего не будет, судя по прогнозу, а вот 16, возможно, что-нибудь понаблюдать получится — заодно, если позволит небо, сниму волновые фронты Шаком-Гартманном, давненько этого не делали).
Такие как я забывают наручные часы/скальпели/зажимы/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, возможно, что-нибудь понаблюдать получится — заодно, если позволит небо, сниму волновые фронты Шаком-Гартманном, давненько этого не делали).