Odczytanie danych z żyroskopu.

[sith114]

Witam,

Jestem w trakcie budowy platformy samopoziomującej. Wykorzystuje czujnik MinIMU-9 v5, jeśli chodzi o akcelerometr szybko udało mi się uzyskać kąty przechylenia i pochylenia:

#include <Wire.h>
#include <LSM6.h>

LSM6 imu;


float gyrox=0;
float gyroy=0;
float gyroz=0;
float akcelx=0;
float akcely=0;
float akcelz=0;

void setup()
{
 Serial.begin(9600);
 Wire.begin();

 if (!imu.init())
 {
   Serial.println("Failed to detect and initialize IMU!");
   while (1);
 }
 imu.enableDefault();
}

void loop()
{
 imu.read();
akcelx=imu.a.x;
akcely=imu.a.y;
akcelz=imu.a.z;

gyrox = imu.g.x;
gyroy = imu.g.y;
gyroz = imu.g.z;

int pitchAcc = (atan2(akcelx, akcelz)*180.0)/M_PI;
int rollAcc  = (atan2(akcely, akcelz)*180.0)/M_PI;

Serial.print(rollAcc);
Serial.print("\t");
Serial.print(pitchAcc);
Serial.print("\t");  
Serial.println();
delay(100);
}

Problem pojawia się przy próbie uzyskania danych z żyroskopu, przeglądałem różne gotowe programy. Kąt uzyskuję się po przez całkowanie prędkości kątowej uzyskanej w wyniku pomiarów żyroskopu.

alfa=alfa+gyrox*dt

gdzie:

alfa - kąt

gyrox prędkość obrotowa w okół osi x

dt - czas próbkowania

W przypadku akcelerometru nie potrzebowałem konwersji danych z wartości binarnych na m/s^2 ponieważ i tak parametr czułości czujnika się skraca. Jak dobrze rozumiem w przypadku żyroskopu powinienem uzyskać przed całkowaniem wartość w °/s w dokumentacji znalazłem takie o to wartości:

Używam zakresu +-245°

Nie wiem czy dobrze rozumiem ale przeliczenie próbowałem zrealizować na zasadzie:

gyrox= imu.g.x*8,75/1000;

Czy popełniam jakieś błędy w rozumowaniu samej zasady?

Używam biblioteki przygotowane przez producenta, wcześniej nie miałem styczności z czujnikami z których dane odczytuje się po I2C.

Docelowo chciałbym wykorzystać filtr komplementarny do fuzji danych z obu czujników.