Błędne odczytywanie kątów z MPU-6050

[explosion95]

Dzień dobry!

Słuchajcie, witam na forum Forbota, gdyż mimo, że bywam tu już od dawna jako obserwator, dziś pierwszy raz zmuszony zostałem by prosić sam o pomoc 😉 zajmuję się modułem gyro+acc mpu6050, z którego wyciągam dane i przerabiam, by były nieco bardziej przydatne 😉 poniższy kod opiera się głownie na gotowych przykładach z YT.

Jednakże od samego początku gdzieś w kodzie mam błąd, którego nie potrafię wyłapać. Otóż przeliczone (według mnie logicznie i poprawnie) wartości z żyroskopu na monitorze szeregowym nie ukazują się jako wartości poprawne. Co ciekawe jednak, nie są to wartości oderwane od rzeczywistości, gdyż w miarę kręcenia modułem, kąty narastają bądź maleją, jednakże w mega wolnym tempie. Dla wyjaśnienia, obrót płytką o dany kąt daje proporcjonalnie mniejszy wynik na ekranie (na załączonym screenie widać wartość dla kąta Roll = ~2.74deg przy obrocie płytką o 180deg). Na dole zamieszczam kod programu 😉

Rozumiem chyba całkowicie działanie programu a mimo to nie rozumiem tego zachowania.

Ale jedyna zależność, którą zauważyłem (na screenie po lewej widać ogromną masę funkcji wypisujących na ekran - nie przypadkowo) to to, że dokładanie funkcji Serial.print do funkcji void printData() umieszczonej w pętli głównej wpływa coraz gorzej na wyświetlanie się wartości kątowych pitch, roll i yaw (są jeszcze mniejsze przy tym samym rzeczywistym obrocie płytki).

Pewnie mało zrozumiale napisałem swój problem, ale to pewnie z braku snu 🙁 bardzo proszę Was o pomoc, gdyby tylko ktoś z Was zechciał przeglądnąć kod i wyjaśnić skąd się biorą te błędy... matematyka jest zrobiona chyba poprawnie, a pętla główna ustawiona jest na 250Hz stałego wykonania. Przepraszam za ewentualne błędy albo banalność problemu którego nie dostrzegam.. gdybym mógł jakoś pomóc, to chętnie odpowiem na pytania 😉 pozdrawiam!

/*
*Kod pobierania danych z modulu MPU-6050 bez uzycia zewnetrznych bibliotek
*Jedyna stosowana biblioteka to <Wire.h> do komunikacji I2C z modułem pomiarowym
*
*Zakres pomiarowy zyroskopu ustawiam chyba na +/- 500deg/s
*Zakres pomiarowy akcelerometru ustawiam chyba na +/-8g
*
*Moduł MPU-6050 pozwala też na pomiar i odczytywanie temperatury
*Wartosci są schowane w rejestrach 0x41 i 0x42
*
*Wszelkie dane odnosnie dzialania i inicjowania pracy modulu, mapy rejestrow dostepne
*są w nocie katalogowej MPU-6050
*/


                          ////////////////////////
                          // POCZATEK PROGRAMU  //
                          ////////////////////////


#include <Wire.h>

int kalibracja = 0;                              // zmienna potrzebna do kalibracji zyroskopu
long raw_acc_X, raw_acc_Y, raw_acc_Z;            //surowe dane z rejestrow akcelerometru   
long int raw_gyro_X, raw_gyro_Y, raw_gyro_Z;          //surowe dane z rejestrow gyroskopu
long sum_gyro_X, sum_gyro_Y, sum_gyro_Z;         // zmienne do sumowania odczytow z zyroskopu

long loop_timer;                                 //do mierzenia czasu trwania petli
float gyro_pitch, gyro_roll, gyro_yaw;           // zmienne na katy wyliczone z pomiarow zyroskopu
float acc_pitch, acc_roll, acc_yaw;              // zmienne na katy wyliczone z pomiarow akcelerometru
long acc_vector;                                 // zmienna do liczenia Pitagorasa z skladowych akcelerometru



                         /////////////////////////
                         //  PETLA JEDNORAZOWA  //
                         /////////////////////////


void setup() 
{
Serial.begin(9600);                        // rozpoczecie komunikacji szeregowej
Wire.begin();                              // zainicjowanie komunikacji Wire
setupMPU();                                // zainicjowanie stanu MPU przed praca
GyroCalibration();                         // kalibracja zyroskopu, usrednienie 2000 wynikow
loop_timer = micros();                     // do zmiennej loop_timer zostaje wpisana wartosc czasu
                                          // od załączenia programu
}



                         ////////////////////////
                         //  PROGRAM WLASCIWY  //
                         ////////////////////////


void loop() 
{
recordGyroRegisters();

raw_gyro_X=raw_gyro_X - sum_gyro_X;
raw_gyro_Y=raw_gyro_Y - sum_gyro_Y;
raw_gyro_Z=raw_gyro_Z - sum_gyro_Z;

processGyroData();

recordAccelRegisters();

processAccelData();


printData();

while(micros() - loop_timer < 4000);                                 //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
 loop_timer = micros();                                               //Reset the loop timer
}

void setupMPU()                             // funkcja ustawienia wartosci poczatkowych w MPU
{
 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x6B);                         // ustawiamy dostęp do rejestru 6B by ustawić tryb pracy
 Wire.write(0b00000000);                   // rejestr czuwania wylaczony itp
 Wire.endTransmission();                   // zakonczenie transmisji

 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x1B);                         // rejestr 1B do konfiguracji pracy zyroskopu
 Wire.write(0b00001000);                   // tryb pracy do +/- 500 deg/s
 Wire.endTransmission();                   // zakonczenie transmisji

 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x1C);                         // rejest 1C do konfiguracji pracy akcelerometru
 Wire.write(0b00010000);                   // tryb pracy do +/- 8g
 Wire.endTransmission();                   // zakonczenie transmisji
}


void GyroCalibration()
{
   for (int kalibracja = 0; kalibracja < 2000 ; kalibracja ++)
   {               
   recordGyroRegisters();                  // w każdej petli odczytaj wartosci z zyroskopu
   sum_gyro_X = sum_gyro_X+raw_gyro_X;     // w kazdej petli w zmiennej sum_gyro_X sumuj kolejne pomiary
   sum_gyro_Y = raw_gyro_Y+sum_gyro_Y;     // w kazdej petli w zmiennej sum_gyro_Y sumuj kolejne pomiary
   sum_gyro_Z = raw_gyro_Z+sum_gyro_Z;     // w kazdej petli w zmiennej sum_gyro_Z sumuj kolejne pomiary
   delay(3);                               // Delay 3us to simulate the 250Hz program loop
   }
 sum_gyro_X = sum_gyro_X/2000;             // uśrednij wyniki pomiarow i zapisz do sum_gyro_X
 sum_gyro_Y = sum_gyro_Y/2000;             // uśrednij wyniki pomiarow i zapisz do sum_gyro_X
 sum_gyro_Z = sum_gyro_Z/2000;             // uśrednij wyniki pomiarow i zapisz do sum_gyro_X
}


void recordAccelRegisters()
{
 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x3B);                         // ustawiam rejest 0x3B do danych akcelerometru
 Wire.endTransmission();                   // zakonczenie transmisji
 Wire.requestFrom(0b1101000,6);            // od 0x3B ma sie zaczac pobieranie 6 bajtow danych o acc
 while(Wire.available()<6);
 raw_acc_X=Wire.read()<<8|Wire.read();     // dane acc_x/y/z przechowywane sa w 8bitowych rejestrach
 raw_acc_Y=Wire.read()<<8|Wire.read();     // od adresu 0x3B do 0x40 wlacznie i wlasnie je pobieramy
 raw_acc_Z=Wire.read()<<8|Wire.read();     // i zapisujemy do zmiennych, na kazda os 2 bajty danych

}


void processAccelData()
{                                           //z Pitagorasa przeliczam dlugosc calkowita wektora sily ciezkosci
 acc_vector = sqrt((raw_acc_X*raw_acc_X)+(raw_acc_Y*raw_acc_Y)+(raw_acc_Z*raw_acc_Z));
 acc_pitch = asin((float)raw_acc_Y/acc_vector)* 57.296;
 acc_roll = asin((float)raw_acc_X/acc_vector)* -57.296;       // wzory obok sa by obliczyc kąty pomiedzy skladowymi sil
}                                                              // a te katy to jednoczesnie kąty obrotu modulu


void recordGyroRegisters()
{
 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x43);                         // ustawiam rejest 0x43 do danych zyroskopu
 Wire.endTransmission();                   // zakonczenie transmisji
 Wire.requestFrom(0b1101000,6);            // od 0x3B ma sie zaczac pobieranie 6 bajtow danych o acc
 while(Wire.available()<6);
 raw_gyro_X=Wire.read()<<8|Wire.read();    // dane gyro_x/y/z przechowywane sa w 8bitowych rejestrach
 raw_gyro_Y=Wire.read()<<8|Wire.read();    // od adresu 0x43 do 0x48 wlacznie i wlasnie je pobieramy
 raw_gyro_Z=Wire.read()<<8|Wire.read();    // i zapisujemy do zmiennych, na kazda os 2 bajty danych
}


void processGyroData()                                // 250Hz to czestotliwosc calkowania pomiarow
{                                                     // 65.5 to wartosc dla +/-500deg/s wzieta z dokumentacji
 gyro_pitch=gyro_pitch+(raw_gyro_X * 0.0000611);     // robimy calkowanie by dostac wartosc kąta deg
 gyro_roll=gyro_roll+(raw_gyro_Y * 0.0000611);       // 0.0000611 = 1/(250*65.5)
 gyro_yaw=gyro_yaw+(raw_gyro_Z * 0.0000611);         // wynik bedacy wychyleniem katowym z pomiarow zyroskopu

 gyro_pitch=gyro_pitch+gyro_roll*sin(raw_gyro_Z*0.000001066);  //tymi obliczeniami likwidujemy gimbal lock
 gyro_roll=gyro_roll-gyro_pitch*sin(raw_gyro_Z*0.000001066);
}


void printData()
{                                           // funkcja do wyswietlania wartosci na monitor
 Serial.print("Angle pitch: ");
 Serial.print(gyro_pitch);
 Serial.print("     ");
 Serial.print("Angle roll: ");
 Serial.print(gyro_roll);
 Serial.print("     ");
 Serial.print("Angle yaw: ");
 Serial.print(gyro_yaw);
 Serial.print("     ");
   Serial.println();
   Serial.print("Angle pitch: ");
 Serial.print(gyro_pitch);
 Serial.print("     ");
 Serial.print("Angle roll: ");
 Serial.print(gyro_roll);
 Serial.print("     ");
 Serial.print("Angle yaw: ");
 Serial.print(gyro_yaw);
 Serial.print("     ");
   Serial.println();
   Serial.print("Angle pitch: ");
 Serial.print(gyro_pitch);
 Serial.print("     ");
 Serial.print("Angle roll: ");
 Serial.print(gyro_roll);
 Serial.print("     ");
 Serial.print("Angle yaw: ");
 Serial.print(gyro_yaw);
 Serial.print("     ");
   Serial.println();
   Serial.print("Angle pitch: ");
 Serial.print(gyro_pitch);
 Serial.print("     ");
 Serial.print("Angle roll: ");
 Serial.print(gyro_roll);
 Serial.print("     ");
 Serial.print("Angle yaw: ");
 Serial.print(gyro_yaw);
 Serial.print("     ");


 Serial.println();
}

[slawko_k]

Czy ja dobrze zrozumiałem, że problemem dla Ciebie jest to że przy 180stopniach pokazuje ok 2,74 rad gdy prawidłowa wartość to ok 3,1rad? Wystarczy to przeskalować

Sam zauważyłeś że im więcej na serial wyrzucasz tym gorzej działa.

Ustawiłeś prędkość 9600, wyrzucasz 250r/sek czyli co 4ms dużą pakę. Serial.print z tego co się orientuję jest na przerwaniach więc możliwe że blokuje resztę i pętla wcale nie jest 250Hz tylko wolniej.

Wyświetlaj mniej danych, rób to np 10razy/sek i zwiększ prędkość na 57600 czy 115k

[EasyPeasy_]

Wyświetlaj mniej danych, rób to np 10razy/sek i zwiększ prędkość na 57600 czy 115k

Sorki, że się wtrącę, ale co da zwiększenie BaudRate'u? Że np. jak ja mam GPS 9600 bps i muszę sczytać z niego dane NMEA około 70 znaków, ma jedna paczka, to jakbym jakoś ustawił wyższy BaudRate to szybciej mi się prześle?

[explosion95]

Czy ja dobrze zrozumiałem, że problemem dla Ciebie jest to że przy 180stopniach pokazuje ok 2,74 rad gdy prawidłowa wartość to ok 3,1rad? Wystarczy to przeskalować

Wyświetlana wartość kąta pitch jest już w stopniach, więc poprawnie działający program powinien pokazać na screenie 180deg przy takim samym obrocie płytki. Pokazuje obecnie oczywiście proporcjonalnie mniej, to już zależy ile tych Serial.printów wcisnąłem doświadczalnie do kodu.

Jeśli chodzi o działanie pętli to w niej są wykonywane także obliczenia tych samych kątów roll i pitch ale dla wskazań akcelerometru i tutaj nie ma żadnych błędów - wyniki są bardzo dokładne.. no a to wciąż ta sama pętla przecież. Dlatego tym bardziej nie rozumiem całej sytuacji, a proces przeliczania danych dla gyro sprawdzałem wielokrotnie..

edit, odnośnie czasu trwania pętli zrobiłem testy i trwa ona zawsze 4ms, czyli wykonuje się z poprawną częstotliwością 250Hz 🙂 niezależnie ile jej na potrzeby testu dołożyłem Serial.printów

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[Elvis]

explosion95, może zacznij od upewnienia się że dane z żyroskopu są poprawne? Zamiast przeliczać je i szukać błędu wypisz po prostu to co dostajesz i zobacz czy wartości są jak należy.

Kod który wkleiłeś bardzo przypomina: https://github.com/RalphBacon/MPU6050_Accelerometer_Gyro/blob/master/GryoFunctions.h

Jest tam identyczna wartość dla przeliczania wyników, ale sam żyroskop jest nieco inaczej skonfigurowany - może to ma znaczenie?

[shakixp]

Również witam na forum - postanowiłem nieco się zaktywizować jako że ten układ już przerabiałem.

Pokazuje obecnie oczywiście proporcjonalnie mniej, to już zależy ile tych Serial.printów wcisnąłem doświadczalnie do kodu.

To świadczy, że Serial.print zaburza obliczenia.

Policz, ile masz bajtów do wysłania. Przy baud=9600 mikrokontroler przez serial jest w stanie przesłać 960 bajtów na sekundę (przy kodowaniu 8N1). Przy pętli 4 ms oznacza to raptem 3 bajty + obliczenia.

Żeby nie szukać daleko: https://www.forbot.pl/forum/topics40/czy-serialprint-dziala-jak-delay-vt13609.htm#top

Jako w tym wątku piszą, jak bufor nie jest pusty to funkcja czeka aż się opróżni. Do tego Serial.print też trochę czasu traci na obliczenia - szybszy jest serial.write, ale on wypisuje bajty bez analizy czy to tekst czy liczba.

Jak się zajmowałem tym układem (właściwie MPU-9150, ale to to samo prawie) to dane przesyłałem do dalszej obróbki bezprzewodowo - tam był transfer rzędu kilkanaście - dwadzieścia kilka KB/s. I to same liczby, bez tekstu.

Przy tej ilości danych co w powyższym kodzie (jakieś 200 bajtów) i baud 9600 wychodzi 4-5 Hz. Czyli ograniczyłbym ilość tekstu do minimum.

Sprzwdź, czy poprawnie sprawdziłeś czas wykonywania pętli. Najlepiej wypisz wartość micros() na początku i na końcu. Bo wg powyższego kodu na koniec pętli głównej

while(micros() - loop_timer

da zawsze >4000 (przez Serial.print()).

Sugeruję w ramach testu policzyć, ile w rzeczywistości dostajesz wyników na sekundę i ile jest przetwarzane. Bo obawiam się, że gdzieś uC nie wyrabia.

Z innych wskazówek:

1) Biblioteka Wire.h nie jest zbut szybka. Nie pamiętam ograniczenia, ale ma ona limit szybkości odczytu przez I2C. O ile pamiętam I2C 400 kHz na niej nie działało dobrze.Szybsza jest Fastwire. I na tej drugiej udało mi się puścić komunikację 500kHz (choć wg dokumentacji ten MPU wspiera max. 400) - ale nie sprawdzałem czy na pewno nie ma jakiś błędów.

2) Dobrze jest dać temu układowi kilkanaście sekund na nagrzanie się. Różnica niby niewielka (może z 2-3 stopnie Celsjusza), ale wpływa na wyniki. Jak skalibrujesz układ zanim się ustabilizuje termicznie to wyniki będą nieco zafałszowane.

3) Rejestry 0x3A i 0x38 powinny pozwolić na wygenerowanie przerwania, gdy nowy zestaw danych jest gotowy do odczytania. Dzięki temu unikniesz odczytania tej samej wartości dwukrotnie bądź pominięcia jakiegoś odczytu, gdyby pętla nie wykonywała się dokładnie tyle samo co czas pomiędzy pojawieniem się kolejnych danych w MPU. Ale towszystko pod warunkiem odpowiednio szybkiego wykonywania się pętli programu i odpowiednio szybkiej komunikacji I2C (przy 1000Hz odpada, nawet z fastwire).

4) Gimball locka unikniesz stosując do obliczeń kwaterniony. Ale to dość skomplikowane (przynajmniej dla mnie - bazowałem na gotowym, znalezionym kodzie).

[Elvis]

Prosty test to wywoływanie PrintData() tylko raz na sekundę - dodajesz nową zmienną licznikową, zwiększasz przy każdym obrocie pętli, a jak dojdzie do 250 to wypisujesz dane. Błąd pozostanie, ale będzie znacznie mniejszy. Jeśli próbujesz wypisywać wyniki przy każdym obrocie pętli to na pewno nie uzyskasz 250Hz.

[explosion95]

Ojej, Panowie, bardzo dziękuję za odpowiedzi i pomysły, tyle ich jest że nie odwołam się do wszystkiego odrazu 😉 zabieram się więc do testów! Na szybko kilka spraw:

explosion95, może zacznij od upewnienia się że dane z żyroskopu są poprawne? Zamiast przeliczać je i szukać błędu wypisz po prostu to co dostajesz i zobacz czy wartości są jak należy.

Kod który wkleiłeś bardzo przypomina: https://github.com/RalphBacon/MPU6050_Accelerometer_Gyro/blob/master/GryoFunctions.h

Jest tam identyczna wartość dla przeliczania wyników, ale sam żyroskop jest nieco inaczej skonfigurowany - może to ma znaczenie?

Surowe dane z gyro chyba są poprawne i oczywiście była to pierwsza z rzeczy które sprawdzałem. Nieskalibrowane sięgają ~ +/-32768, a podzielone przez 65.5 -> +/-500deg/s czyli zgodnie z ustawionym zakresem w rejestrze.

Co do kodu, nie dziwię się że jest podobny 😃 gdyż wygląda na wzięty wprost od pewnego pana z YT który zrobił dość popularny DIY quadcopter na arduino 😉 chyba, że było dokładnie odwrotnie, ale to nieważne 😋

Sprzwdź, czy poprawnie sprawdziłeś czas wykonywania pętli. Najlepiej wypisz wartość micros() na początku i na końcu. Bo wg powyższego kodu na koniec pętli głównej

Długość trwania pętli sprawdzałem w każdym obiegu poprzez odjęcie wartości wyrzucanej przez funkcję micros() tuż przed wykonaniem pętli oraz wartości wyrzuconej przez tę funkcję tuż po wykonaniu, no i po odjęciu wychodziło jakoś 4004 microseconds czyli 4ms, wydaje mi się że zrobiłem to poprawnie..

Gimball locka unikniesz stosując do obliczeń kwaterniony. Ale to dość skomplikowane (przynajmniej dla mnie - bazowałem na gotowym, znalezionym kodzie).

niestety z programowania jestem cienki więc chciałem uniknąć kwestii wprowadzania kwaternionów.. jednakże skoro o tym mówisz, czy to znaczy ze sposób na likwidację tego zjawiska jest u mnie w programie zrobiony błędnie?

 
void loop() 
{
recordGyroRegisters();

raw_gyro_X=raw_gyro_X - sum_gyro_X;
raw_gyro_Y=raw_gyro_Y - sum_gyro_Y;
raw_gyro_Z=raw_gyro_Z - sum_gyro_Z;

processGyroData();

recordAccelRegisters();

processAccelData();


printData();

while(micros() - loop_timer < 4000);                                 //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
 loop_timer = micros();                                               //Reset the loop timer
}

Oki, to zanim zabiorę się do analizy Waszych propozycji (mega dziękuję!!!) to jeszcze pytanie (chyba że rozwieje się ono przy czytaniu):

Powyżej cała pętla sterująca. Skoro Serial.print w funkcji printData() zaburza obliczenia to czy wyniki pitch/roll z akcelerometru (funkcja processAccelData() ) tez nie powinna zwracać złych wartości..? Jest wywoływana w pętli sterującej tak samo jak gyro a pięknie w Serial.princie wyrzuca dokładne wartości kątów 😉

Pozdro i dziękuję za pomoc! zabieram się do czytania 🙂

[shakixp]

czy wyniki pitch/roll z akcelerometru (funkcja processAccelData() ) tez nie powinna zwracać złych wartości..?

Nie. Powinna zwracać dobre 😋. Knif jest w tym, że jak obrócisz akcelerometr powiedzmy o 45 stopni to cały czas jest w tej samej pozycji i cały czas będzie zwracać dla dwóch osi ok. 0,7g. Nie ważne, czy będzie to odczyt pierwszy, 200-tny czy 200000-ny. Mierzysz przyspieszenie, które jest stałą wartoscią (w tym przypadku) i ze stosunku 2 stałych wartości liczysz kąt.

Natomiast żyroskop mierzy prędkość kątową. Zwraca wartość różną od 0 tylko, jeżeli się obraca (zakłądam model idealny).Powiedzmy, że ustawisz częstotliwość zbierania danych na 1Hz i będziesz obracać żyroskop o te 45 stopni ale tak, że obrót będzie trwał 0,5 sekundy i będzie wykonywany ze stałą prędkością. Co odczyta żyroskop? Jeżeli odczyt przypadnie na te 0,5 sekundy obrotu, odczyta 90 stopni/sekundę. Super. A jeżeli odczyt przypadnie na pozostałe 0,5 sekundy? Odczyt wyniesie 0. Bo w tym momencie czujnik się nie obraca. I informacja o tym, że właście nastąpił obrót o 45 stopni jest bezpowrotnie stracona.

Odnosząc się do Twojego przykładu. Mierzysz co 4 ms. Zakładając (w miarę) jednostajną szybkość obrotu, powinieneś dostać 250 odczytów na sekundę i z każdego pomnożyć kąt o który się obróci w czasie tych 4 sekund (co robisz). Ale jeżeli np. 200 odczytów z tego nie zostanie wliczonych (bo mikrokontroler nie wyrobi) - 80% informacji idzie się bujać.

w rzeczywistych warunkach obrót nie będzie się odbywał w sposób jednostajny...

Ah. I być może mam rozwiązanie Twojego problemu. Bo twierdzisz, że pętla wykonuje się 4ms, wynik z akcelerometru przeliczasz w radianach do asin (i zakładam, że masz dobry) a wynik z gyro od początku do końca w stopniach (bo tak też MPU te dane podaje). Tylko te wyniki z screenshota są niepokojąco stabilne. Obracasz tym czujnikiem podczas mierzenia? Bo wyglądają jak podczas odczytu bez ruchu...

Żyroskop nie zmierzy wartości bezwzględnej. On mierzy względną. Od początku obrotu do końca.

Wybacz, jeśli piszę jakieś oczywistości dla Ciebie, ale te czujniki działają na innej zasadzie i trzeba to zrozumieć, jeżli chce się z nich wyciągnąć sensowne dane.

czy to znaczy ze sposób na likwidację tego zjawiska jest u mnie w programie zrobiony błędnie?

Nie wiem. Nie zgłębiałem się. Ale sądzę, że błędnie. Za mało obliczeń. Generalnie orientację w przestrzeni możesz liczyć albo za pomocą kątów Eulera albo za pomocą kwaternionów. Nie wiem, czy istnieje inny sposób.

Kąty Eulera mają problem gimball locka. Występuje w jednej z 3 osi jak zbliżysz się w niej do 90 stopni. Da się tak zmodyfikować obliczenia, że możesz sobie tą oś wybrać. Jak wiesz, że nigdy w okolice 85-90 stopni w tej osi się czujnik nie zbliży, możesz spróbować pozostać przy tej metodzie.

Kwaterniony tej wady są pozbawione (po to się je używa), ale bardziej skomplikowane w zrozumieniu. Mocy obliczeniowej dużo więcej nie potrzebują (abstrahując od tego, że liczenie trygonometrii na Atmedze to zabójstwo - brak jednostki zmiennoprzecinkowej i tyko 8-bit - ale się da).

Dlaczego uważam, że że błędnie pozbywasz się gimball locka? Bo po pierwsze nie wiem, czy w ogóle ten problem w Twoich obliczeniach wystąpi. Wydaje mi się, że nie i że po prostu jak poobracasz czujnikiem jednocześnie w 3 osiach to dostaniesz śmieci.

Po drugie, zanalizuj taki przykład:

Obróć czujnik o 90 stopni w jedną stronę w osi x (osi x dla czujnika ), następnie w dowolną stroną w osi y (znowu - dla czujnika - jego osie są teraz obrócone!) o 90 stopni i następnie w osi x (dla czujnika) w stronę odwrotną co na początku o 90 stopni. Jakie masz odczyty? Wszędzie 0 stopni (w przybliżeniu). Tylko dziwnym trafem czujnik leży obrócony o 90 stopni w osi z...

I po to są właśnie kąty Eulera i kwaterniony. Żeby przeliczyć dane z czujnika (który może być dowolnie obrócony wokół którejś z osi) na jakiś punkt odniesienia (względem Ziemi, biegunów magnetycznych, gwiazd, etc.).

Bez tego możesz opierać się na odczycie z żyroskopu tylko wtedy, gdy obrót będzie w jedenj osi naraz i przed obrotem w kolejnej osi nastąpi powrót do pozycji wyjściowej.

Na początek proponuję zerknąć np. tutaj:

http://www.chrobotics.com/shop/um7-lt-orientation-sensor

pooglądać animacje na YT i zrozumieć na czym polegają kąty Eulera i skąd się bierze gimball lock. Weź sobie ten czujnik albo jakikolwiek inny przedmiot i poobracaj nim w powietrzu i porównaj, jakie w danym momencie są osie obrotu dla czujnika i dla Ciebie.

Ja w moim projekcie jakieś 2-3 tygodnie próbowałem rozwiązać problem najpierw od zera a potem za pomocą kątów Eulera. W końcu dowiedziałem się co to kwaterniony i to rozwiązało wszystkie moje problemy w tym względzie.

A potem doszedł problem kalibracji tego czujnika... ale nie będę Cię zniechęcał 😋 - być może to Ci nie będzie potrzebne.

[explosion95]

shakixp, na początek kurczę no, muszę to napisać: otóż nawet nie wiesz jak mi miło, że chce Ci się w sobotę przed północą pomagać jakiemuś laikowi na forum pytającemu o banały 😉 ja chyba jeszcze nawet nie wiem ile nie wiem 😃 :D 😃 a skoro o tym mowa, wielu rzeczy wciąż nie rozumiem.. jeśli mogę nagiąć choć raz jeszcze Twoją uprzejmość... 🙂

A tak poważnie, wracając do propozycji Elvis, by rozrzedzić częstotliwość wysyłania danych przez UART na monitor, zrobiłem to (1Hz) i nagle okazało się, że odczyty dla żyroskopu stały się w porządku. Ponadto zapoznałem się z problemem zapychania bufora portu szeregowego z podanego wcześniej linku do innego tematu (warto było, dziękuję 😉 ) i moim wnioskiem jest, że wypisywanie danych na monitor w moim programie musiało zapchać bufor portu szeregowego praktycznie od razu... czy to znaczy że wystarczy nie wysyłać tych danych, bądź wysyłać je stopniowo? Ot, cała sztuczka? Chciałem je monitorować by widzieć czy wszystko gra, ich ciągłe wypisywanie nie jest mi potrzebne

Jeśli udowadniam teraz swoją ignorancję bądź niewiedzę - ehh trudno:

Ale jeżeli np. 200 odczytów z tego nie zostanie wliczonych (bo mikrokontroler nie wyrobi) - 80% informacji idzie się bujać.

Rozumiem w takim razie, że to właśnie dość szybko zapchany bufor powoduje, że program zawiesza działanie do czasu każdorazowego zwolnienia się w nim jakiegoś miejsca. Z tego powodu radykalnie zmniejsza się częstotliwość pobierania danych z gyro, a więc te nieliczne wzięte do obliczeń są pomnożone przez "deltaT" (swoją drogą o wiele mniejsze od rzeczywistego czasu pomiędzy pobranymi pomiarami) i ostatecznie dają znacznie mniejszy wynik w całce. Tak to działa? Nawet jeśli pętla dalej trwa przez ślicznie ustalone 250ms to po prostu nie w każdej zostaje obliczony nowy kąt do zsumowania, tak?

Tylko te wyniki z screenshota są niepokojąco stabilne. Obracasz tym czujnikiem podczas mierzenia? Bo wyglądają jak podczas odczytu bez ruchu..

Trudno mi powiedzieć, do zrobienia screena oczywiście obracałem modułem ale chyba zrobiłem zrzut dla płytki postawionej do góry nogami więc mogła się już nie ruszać (dla podkreślenia, jak proporcjonalnie mniejsze są wyniki odczytane od obrotu rzeczywistego). Niepokojąco stabilne? Do czego dążysz? bo chyba jest tu kolejna rzecz której najwyraźniej nie widzę (zaczyna mi być głupio tu pisać) ale skoro gyro liczy prędkość względną, a poprzez całkowanie sumuję drobne (względne) kąty wykonane co "deltaT" miedzy odczytami, to nawet jeśli gyro się nie rusza ( 0deg/s*dT =0) to kąty wyświetlają się poprawnie (pomijam dryft). Wiem że piszę Ci coś oczywistego, ale może akurat wyjdzie to czego nie rozumiem

Wybacz, jeśli piszę jakieś oczywistości dla Ciebie, ale te czujniki działają na innej zasadzie i trzeba to zrozumieć, jeżli chce się z nich wyciągnąć sensowne dane.

Troche offtopic, ale każdy błąd który mi wytkniesz jest dla mnie bardzo cenny 😉 a na pewno jest ich wiele, tylko coraz bardziej stracham się wykorzystywać Twoją uprzejmość

Na początek proponuję zerknąć np. tutaj:

http://www.chrobotics.com/shop/um7-lt-orientation-sensor

pooglądać animacje na YT i zrozumieć na czym polegają kąty Eulera i skąd się bierze gimbal lock.

Dzięki za link. Jak od dłuższej chwili staram się zrozumieć to zjawisko to tak sobie myślę, że te proste obliczenia (niby na gimbal locka właśnie) w moim kodzie wzięte prosto od gościa z YT, który z powodzeniem (!) wprowadził to do swojego quadcoptera, są "troszkę" śmieszne. Przyznaję, wziąłem je bez przemyślenia... facet to nawet tłumaczy skąd je wziął, więc gdybyś był ciekaw, podam link 😉 na pewno więcej Ci to powie, niż mi

Co ciekawsze (i dla mnie trochę zaskakujące) to to, że kąty Eulera myślałem, że są mi znane z studiów automatyki i robotyki gdzie przerabialiśmy różne obroty względem układów lokalnych lub układu bazowego, jednakże... po 1) nijak mi to nie pomaga zrozumieć GL (ale już się za to biorę - szanuję bardzo Twoje powyższe wytłumaczenie) po 2) nikt nam o tym nie powiedział a sam obracając wiele układów współrzędnych nie zauważyłem tego problemu. Sam już nie wiem jak źle to świadczy o mojej (nie)wiedzy.

Aha, przedłużam już ale kurczę, Ty tu shakixp do mnie górnolotne tematy a ja zwróciłem dopiero uwagę czy ja w ogóle odbieram dobre dane z modułu...? Poniżej screen przy leżącym płasko MPU i odczyty z acc:

Troszkę duże te liczby dla zakresu +/-8g (4096), szczególnie dla osi X, gdzie siły w ogóle nie ma... nie wiem jak bardzo przymrużyć na to oko, jednakże nigdzie na YT nie widziałem takich błędów u ludzi w tutorialach.

Pozdrawiam i wielkie dzięki za cierpliwość 😃

[shakixp]

bądź wysyłać je stopniowo

Bądź szybciej. Nie bój się prędkości 57600 i 115200. Tylko upewnij się, że nie ma błędów podczas transmisji. (W Arduino raczej nie będzie). Gdybyś dysponował Atmegą na płytce drukowanej i odpowiednim modułem UART-USB, można się pokusić o jeszcze wyższe prędkości (np. 500000). Ale przy wyższych prędkościach mogą wystąpić zakłócenia z powodu indukcyjności, pojemności i oporu linii transmisyjnych - zależy głównie od długości kabli i zastosowanych oporników (tzn. sygnał narasta zbyt wolno lub zbyt wolno opada). W przypadku Arduino za transmisję UART-> USB odpowiada Atmega 32u4 - trzeba by zerknąć w kod źródłowy tego chipu i zmienić w nim prędkość UART. I jednocześnie w Atmega 328. Trochę roboty z tym może być i nie radzę, jeśli nie masz zapasowego mikrokontrolera.

A jak chcesz móc przejrzeć dane z seriala przy większej szybkości to przechwyć je do pliku - nic Ci nie ucieknie wtedy.

Nawet jeśli pętla dalej trwa przez ślicznie ustalone 250ms

Z 4 ms zrobiło się nagle 250? 😉

dość szybko zapchany bufor powoduje, że program zawiesza

Na to wygląda. Nie pasuje mi w tym tylko mierzone przez Ciebie 4000us - powinno być więcej.

Swoją drogą liczysz zakładając na sztywno czas między kolejnymi pomiarami i obliczeniami. Spróbuj zmienić

gyro_pitch=gyro_pitch+(raw_gyro_X * 0.0000611);     // robimy calkowanie by dostac wartosc kąta deg
 gyro_roll=gyro_roll+(raw_gyro_Y * 0.0000611);       // 0.0000611 = 1/(250*65.5)
 gyro_yaw=gyro_yaw+(raw_gyro_Z * 0.0000611);         // wynik bedacy wychyleniem katowym z pomiarow zyroskopu 

tak, by do obliczeń użyć zmierzonej (micros()) ilości czasu, jaka upłynęła od ostatniego liczenia. Raz, że może wyniki będą lepsze a dwa, że łatwiej będzie Ci zmienić częstotliwość próbkowania z MPU.

Niepokojąco stabilne

Tak u mnie wyglądały jak czujnik był w bezruchu, stąd zwróciłem na to uwagę. Przy okazji Arduino ma chyba od jakiegoś czasu funkcję rysowania wykresów wartości z portu szeregowego (ta druga pozycja w menu dot. portu).

obliczenia (niby na gimbal locka właśnie) w moim kodzie wzięte prosto od gościa z YT, który z powodzeniem (!) wprowadził to do swojego quadcoptera

Może i działały. Nie twierdzę, że nie. Tylko, że quadcopter na boki przechyla się w niewielkim stopniu, więc błąd może nie być duży. Poza tym pewnie kompensował go pomiarami z akcelerometru.

gyro_pitch=gyro_pitch+gyro_roll*sin(raw_gyro_Z*0.000001066);  //tymi obliczeniami likwidujemy gimbal lock

Wybacz, że nie chce mi się tego analizować dokładnie, ale nawet jeżli jest tu dla osi X kompensacja z osi Z to dlaczego nie z osi Y? Po prostu uważam, że jak zrobisz to do początu poprawnie to później nie będziesz się doszukiwał błędów tam, gdzie nie powinieneś. W quadcopterze liczenie z kątów Eulera powinno wystarczyć - przy założeniu, że nie będzie latał bokiem 😉.

są mi znane z studiów automatyki i robotyki

O, to w takim razie bierz moje słowa z dużą dozą sceptycyzmu, bo ja takich rzeczy na studiach nie miałem - wiem tyle co się nauczyłem przy zabawie z tym czujnikiem (czyli trochę z teorii, trochę z praktyki).

Troszkę duże te liczby dla zakresu +/-8g (4096), szczególnie dla osi X, gdzie siły w ogóle nie ma...

A skąd wiesz, że nie ma ? :->

A na poważnie. W osi Z jest około 1g.

W osi X i Y... paradoksalnie też masz dobre odczyty.

Primo: czy jesteś absolutnie pewien, że czujnik leży idealnie płasko i oś Z jest idealnie równoległa do siły grawitacji? Odchylenia od siły grawitacji o 1 stopień w osi Z nie zauważysz, ale w osi X już tak. Co więcej... tak naprawdę nie jest aż tak istotne czy jest dobrze zorientowany sam moduł z układem MPU tylko czujnik wewnątrz tego układu...

Secundo: Jest taka ciekawa pozycja w tabelce w dokumentacji. "Cross axis sensitivity" dla której dopuszcza się tolerancję 2 %. Co ona oznacza? Otóż producent nie daje gwarancji, że kąt między osiami Z, Y i X czujników wynosi dokładnie 90 stopni. Co znaczy, że z powodów konstrukcyjnych czujnik osi X może odczuwać nieco siły z osi Z (można tu podstawić dowolną kombinację osi).

Kolejna pozycja: "Zero-G Initial Calibration Tolerance" (to akurat z MPU-9250). Czyli nawet jak masz idealnie ustawiony czujnik to może odczytywać z odchyleniem 60mg. Dla zakresu 8g odpowiada to około 246. A więc nawet wiecej niż odczytujesz. Ale rozumiem, że to co odczytujesz jest już po kalibracji. Zatem skomplikuję sytuację (w kontekście tego co napisałem wcześniej o nagrzaniu się układu przed kalibracją) - ten błąd kalibracji jest jeszcze zależny od temperatury i ta zalezność niekoniecznie jest liniowa...

Żeby nie być gołosłownym, poniżej pomiar, który kiedyś popełniłem. Pierwsze 7 wykresów to odpowiednio osie X, Y, Z akcelerometru, X, Y, Z żyroskopu i temperatura (gdzieś od ok. 0 do 30 stopni - robione w zimie):

Generalnie błędy kalibracji przypominają odchylenia z przetwornika ADC (pewnie jednym z powodów jest to, że wewnętrznie napięcie na postać cyfrową jest przetwarzane właśnie przez ADC).

Tertio: Na razie się tym za badzo nie martw, bo jak będziesz to chciał zrobić bardzo dokładnie to osiwiejesz, stwierdzisz, że nie warto albo za drogo (albo uznasz to za ciekawe wyzwanie), ale to jest kwestia kalibracji. O prostą możesz się względnie łatwo postarać. Dokładniejsza wymaga więcej zachodu. O dokładną.... jak Ci to coś powie to urządzenia do kalibracji IMU kosztują tysiące dolarów 😋.

[explosion95]

shakixp, przepraszam, że odpisuję Ci dopiero po takim czasie jednak życie i obowiązki skutecznie mnie zatrzymało a chciałem przeanalizować Twoje uwagi z należytą starannością 😉 dzięki za obszerny post!

Po pierwsze, niewiele umiem informacji wyciągnąć z wykresów, które wrzuciłeś, choć ogólnie zdaje mi się, że 1) wraz z wzrostem temperatury jest ogólna tendencja do lepszych wyników, 2) baaardzo ogólnie to w okolicach 20-25st C chyba te wyniki ogółem są najbardziej poprawne. Swoją drogą słusznie ciekawe, że wartości LSB z czujnika temperatury nie rosną liniowo, proporcjonalnie do wzrostu temperatury rzeczywistej.

Po drugie i bardzo ważne:

Na to wygląda. Nie pasuje mi w tym tylko mierzone przez Ciebie 4000us - powinno być więcej.

Muszę oddać honor i przyznać się do błędu - ponownie zrobiłem test na czas trwania pętli sterowania i nie jest to 4ms /250Hz... Pętla w istocie zawiesza działanie na czas zwolnienia miejsca w buforze UARTa. Poniżej kod pętli dla baud rate = 9600baud:

 
void loop() 
{
czas1=micros();

recordGyroRegisters();

raw_gyro_X=raw_gyro_X - sum_gyro_X;
raw_gyro_Y=raw_gyro_Y - sum_gyro_Y;
raw_gyro_Z=raw_gyro_Z - sum_gyro_Z;

processGyroData();

processAccelData();

recordAccelRegisters();

Serial.println(czas3);
while(micros() - loop_timer < 4000);                                 //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
czas2=micros();
czas3=czas2-czas1;
 loop_timer = micros();                                               //Reset the loop timer
}

Trochę wstyd, że twierdziłem wyżej inaczej, a wszystko z błędnego pomiaru. Przy prędkości 9600baud pierwsze kilkanaście pętli robiło się faktycznie w 4000us, po czym czas ten wzrastał na stałą wartość ~6200us. Chwilę zajęło mi dojście do tego, że po prostu bufor dość szybko się zapycha, stąd większy czas w późniejszych obiegach. Przy innych prędkościach przesyłu jak również przy innym "obciążeniu" portu wysyłanymi danymi ta tendencja również jest widoczna. Przepraszam za pomyłkę 😉

Spróbuj zmienić (...) tak, by do obliczeń użyć zmierzonej (micros()) ilości czasu, jaka upłynęła od ostatniego liczenia. Raz, że może wyniki będą lepsze a dwa, że łatwiej będzie Ci zmienić częstotliwość próbkowania z MPU.

Zastanawiałem się, czy może dobrym pomysłem byłoby zmniejszenie częstotliwości działania pętli sterującej? Na przykład do 200Hz/5ms? Jaki to może mieć wpływ na szybkość reakcji konstrukcji drona w dalekiej przyszłości? Funkcji w pętli będzie tylko dochodzić, czas jej trwania będzie się tylko wydłużać, to może większy bufor czasowy na obliczenia się przyda...?

Drugą z opcji, którą rozważam jest to co wyżej fajnie mi zaproponowałeś, czyli każdorazowy pomiar czasu trwania każdego obiegu, tak by czas wykonania każdej pojedynczej pętli był uwzględniany jako chwilowe "deltaT" w całkowaniu. Muszę jednak sprawdzić, jak funkcja micros() obciąża mikroprocesor, bo nie chciałbym by okazało się, że mierzenie w pętli tego czasu na jej początku, końcu i odejmowanie wartości było cięższe od uzupełniania czasu do 4000us poprzez pętlę while. shakixp, byłbyś w stanie ocenić oba rozwiązania? Swoją drogą jeszcze sprawdzę, czy jest jakaś różnica czasowa, jeśli pętla sterująca wywołuje kolejno funkcje obliczające, czy jeśli usunę te funkcje a ich treści wsadzę bezpośrednio do kodu pętli głównej.

Pozdrawiam! 😉

[shakixp]

wraz z wzrostem temperatury jest ogólna tendencja do lepszych wyników

Powiedziałbym raczej, że nie tyle lepszych co innych. Bo lepsze to bardziej odpowiadające wielkości mierzonego parametru a tego bez kalibracji nie wiadomo. Nie wiem też czy i w jakiej temperaturze były te czujniki kalibrowane fabrycznie.

A więcej informacji raczej z tych wykresów nie wyciągniesz - mierzyłem po prostu zmienność wyników w bezruchu przy zmianie temperatury.

W tych układach jest DMP - podobnoż potrafi w pewnym stopniu kompensować wpływ temperatury. Ale początkowo nie wszystkie jego funkcje były znane i obsługiwane. Swoją drogą pan Jeff Rowberg napisał kilka lat temu bibliotekę do obsługi MPU-6050 / 9150 i to obsługującą funkcje nieopisane w dokumentacji (reverse engineering). Ta biblioteka ma również funkcje do liczenia katów Eulera, kwaternionów - możesz sobie zerknąć. Ja akurat z tego nie korzystałem.

czy może dobrym pomysłem byłoby zmniejszenie częstotliwości działania pętli sterującej? Na przykład do 200Hz/5ms?

Zasadniczo (choć mogę się mylić), większa częstotliwość próbkowania to większa dokładność, ale jednocześnie większe szumy (co z kolei może wymagać filtrowania). Z tym, że ten układ ma filtr dolnoprzepustowy (DLFP). Myślę, że na początek możesz próbować nawet 100Hz (aż będziesz miał poprawne obliczenia) a jak się okaże później za mało to wtedy zwiększyć.

Funkcji w pętli będzie tylko dochodzić, czas jej trwania będzie się tylko wydłużać

Niewątpliwie... Może być tak, że układ się wyrobi a może być tak, że nie wyrobi - zależy od tego co będziesz chciał liczyć, z jaką częstotliwością i jak zoptymalizujesz kod (i czy będziesz wykorzystywał w dużej ilości obliczenia zmiennoprzecinkowe). Ja miałem taką konfigurację: Atmega328@8MHz odczytywała dane z MPU (1000Hz) i przesyłała bezprzewodowo do Arduino Uno, które wszystko liczyło.

Jak zabraknie mocy mimo dobrej optymalizacji to albo będziesz musiał zmniejszyć częstotliwość, albo użyć 2 układów, albo jednego a szybszego (np. Teensy - od wersji 3.5 ma fpu).

funkcja micros() obciąża mikroprocesor

Nie sądzę, żeby mocno. Pewnie nie bardziej niż rozdzielczość czasowa tej funkcji (czyli 4us).

Jak nie chcesz tyle razy wywoływać tej funkcji to proponuję

delayMicroseconds(4000-(micros() - loop_timer));

Tym sposobem wywołasz ją raz a nie przy każdej iteracji pętli.

każdorazowy pomiar czasu trwania każdego obiegu

Jeszcze jedną szpilkę wbiję: micros() ma dokładność 4 us co znaczy, że może w niewielkim stopniu wpływać na dokładnośc obliczeń (nie więcej niż 0,1%, ale błąd będzie się kumulował). Z tym, że w Arduino prostego sposobu na poprawę tej dokładności chyba nie ma. Może gdyby ręcznie napisać taką funkcję z użyciem timera 16MHz? Ale 1 us nie przeskoczysz raczej.

Mimo wszystko uważam, że to oraz mierzenie czasu trwania na bazie przerwań z MPU to dokładniejszy pomiar. Chociażby dlatego, że dokładność wewnętrznego zegara MPU (wg którego synchronizuje sobie pomiary) jest określana na +/- 1%. Ale to tak na przyszłość.

czy jest jakaś różnica czasowa, jeśli pętla sterująca wywołuje kolejno funkcje obliczające, czy jeśli usunę te funkcje a ich treści wsadzę bezpośrednio do kodu pętli głównej.

Myślę, że pomijalna w stosunku do czasu potrzebnego na Serial.print, obliczenia zmiennoprzecinkowe etc. Obstawiam zakres 1-3us. Poszukaj, ile czasu zajmuje na Arduino mnożenie float*float albo liczenie sin() to się zdziwisz 😋. Zresztą możesz sam sprawdzić.

Możesz oznaczyć funkcje jako inline, ale nie wiem ile to da.

[explosion95]

Ta biblioteka ma również funkcje do liczenia katów Eulera, kwaternionów - możesz sobie zerknąć.

Taaak, spotkałem się z nią, bardzo popularna, szkoda tylko że na moim arduino nie chciała się kompilować... ale nic straconego, więcej się nauczyłem 😉

Mimo wszystko uważam, że to oraz mierzenie czasu trwania na bazie przerwań z MPU to dokładniejszy pomiar.

Ooo, to chyba nie jest mi znane. Na bazie przerwań z MPU? to nie są one regulowane linią zegarową SCL wychodzącą z arduino? czyli tożsamą z częstotliwością wykonywania pętli? Jeśli mowa o dodatkowym wyjściu modułu INT to zwarłem go do masy. Obecny kod mam rozpisany na zmienny czas wykonania pętli i na obecną chwilę po obliczeniach kątów i filtracji wychodzi około 2600us, czyli mam jeszcze sporo zapasu na następne obliczenia 🙂

Myślę, że pomijalna w stosunku do czasu potrzebnego na Serial.print, obliczenia zmiennoprzecinkowe etc. Obstawiam zakres 1-3us.

Jak zwykle masz rację, różnica między stosowaniem funkcji a niestosowaniem jest znikoma, właściwie żadna 😃 a obliczenia na floatach czy zmiennych double istotnie trwa dość długo, lepiej kiedy tylko się da to takich obliczeń unikać 😉

 void ProcessGyroData()                                                    // deltaT to czas probkowania ~2500us = 2.5ms = 0.0025s
{             
                                                                         // 65.5 to wartosc dla +/-500deg/s wzieta z dokumentacji
 gyro_pitch=gyro_pitch+(raw_gyro_X * (deltaT/65500000));                 // robimy calkowanie by dostac wartosc kąta deg             //sprawdz wzory uwzgledniajac deltaT!!!!
 gyro_roll=gyro_roll+(raw_gyro_Y * (deltaT/65500000));                   // 
 gyro_yaw=gyro_yaw+(raw_gyro_Z * (deltaT/65500000));                     // wyniki bedace wychyleniem katowym z pomiarow zyroskopu

gyro_pitch=gyro_pitch+gyro_roll*sin(raw_gyro_Z*deltaT*liczba);          //          
gyro_roll=gyro_roll-gyro_pitch*sin(raw_gyro_Z*deltaT*liczba);           // tymi obliczeniami likwidujemy gimbal lock
}

void ComplementaryFilter()
{
 if (first_cycle==true)            // warunek spełniony wylacznie raz
 {                                 // w pierwszym obiegu sterowania
   gyro_pitch = acc_pitch;         // zeby wpisac do gyro polozenie poczatkowe
   gyro_roll = acc_roll;           // z pomiarow akcelerometru
   first_cycle = false;            

   output_pitch = gyro_pitch;      // w pierwszej petli wpisujemy to
   output_roll = gyro_roll;        // bezposrednio na wyjscie
 }

// FILTR KOMPLEMENTARNY DLA KĄTÓW PITCH I ROLL

     output_pitch = 0.97 * output_pitch  + 0.03 * acc_pitch;
     output_roll = 0.97 *output_roll + 0.03 * acc_roll;
     output_yaw = gyro_yaw;
}


void PrintData()
{                                           // funkcja do wyswietlania wartosci na monitor


 Serial.print(acc_pitch);
 Serial.print("\t");
 Serial.print(acc_roll);
 Serial.print("\t");
 Serial.print(output_roll);
   Serial.print("\t");
 Serial.print(output_pitch);

Powyżej zawarty jest fragment odpowiedzialny za filtr komplementarny. Zamieszczam screeny z kreślarki, bo chciałbym spytać o jakość filtracji wyników, które widać na wykresach:

Kąt Roll dla układu w bezruchu

Kąt Roll dla układu poruszanego

Kąt Pitch dla układu w bezruchu

Kąt Pitch dla układu poruszanego

Dla mnie jako amatora filtracja wydaje się być całkiem ładna choć są opóźnienia w reakcji filtra, nie wiem tylko jak bardzo szkodliwe 😉 na ogół im bardziej zwiększam wpływ acc w filtrze, tym odpowiedź jest szybsza ale bardziej zaszumiona, a obecne nastawy (0.97 gyro, 0.03 acc) dobrałem doświadczalnie 😃

[shakixp]

void ComplementaryFilter()
{
 if (first_cycle==true)

Oczywiście, że możesz tak zrobić, jeśli nie chcesz, żeby startować od wartości 0. Drobna wskazówka optymalizacji: Gdyby pierwsy odczyt zrobić w setup(), unikasz sprawdzania warunku if() w każdym wywołaniu tej funkcji.

Co do filtrowania - mam w tym nie wielkie doświadczenie praktyczne, więc nie doradzę więcej poza doświadczalnym dobraniem parametrów 😋. No, może poza tym, że możesz poszukać jakiegoś przykładu filtra Kalmana - może się lepiej nadaje (ale za to trudniejszy do zaprogramowania i dobrania parametrów).

to nie są one regulowane linią zegarową SCL wychodzącą z arduino?

Na ile ja rozumiem dokumentację to nie. Bo po co byłby zegar wewnetrzny?. Jak na mój rozum to zegar SCL służy tylko do komunikacji po i2c a pracą układu steruje jesgo wewnętrzny (bądź zewnętrzny z kwarcu, jak się taki wybierze) zegar.

Uważam, że dopóki nie będziesz miał działającej całości to nie warto tracić czas nad takimi kwestiami tym bardziej, że może to w przypadku quadcoptera nie mieć tak istotnego znaczenia. Ale warto o tym wiedzieć na wypadek, gdzybyś potrzebował na późniejszym etapie dokładniejszych oblicznień.

Na dzień dzisiejszy ustaw tylko zegar wewnętrzny np. z żyroskopu. Nie wiem, który jest domyślny, ale przynajmniej jeden z nich ma dokładność 5% chyba.

czyli tożsamą z częstotliwością wykonywania pętli?

Hm? Nie wiem co miałeś na myśli dokładnie, ale pętlę wykonujesz z częstością 250Hz a zegar SCL wynosi pewnie 100KHz i to jeszcze jest generowany sprzętowo (o ile używasz domyślnych pinów).