Najważniejszą rzeczą w autopilocie jest jego jednostka inercyjna, będąca układem odniesienia położenia autopilota względem Ziemi. Czujnikami zasilającymi jednostkę danymi pomiarowymi są przede wszystkim żyroskopy oraz akcelerometry i magnetometry. Aby łatwiej można było sobie wyobrazić proporcję wykorzystania czujników, przyjmijmy, że żyroskopy dostarczają 97% potrzebnych danych, akcelerometry 2% a magnetometr 1% danych używanych do obliczenia kątów potrzebnych do orientacji w w przestrzeni. To pokazuje, że najważniejszym czujnikiem jest żyroskop. Używane w małym lotnictwie żyroskopy MEMS działają na zasadzie pomiaru siły Coriolisa i przeliczają ją na prędkość kątową. Jest to trudny pomiar, niemający naturalnego punktu odniesienia do zjawiska fizycznego, tak jak to jest w przypadku akcelerometrów, które mierzą bardzo silne przyspieszenie ziemskie, albo magnetometrów mierzących dużo słabsze, ale wyczuwalne np. igłą kompasu pole magnetyczne. Dlatego trzeba w sposób arbitralny wyznaczyć, że dla statycznego czujnika dana wartość jest prędkością zerową.
Dodatkowo stopień skomplikowania metody pomiaru wpływa na jego podatność na wpływ temperatury. O ile akcelerometr i magnetometr są praktycznie niepodatne albo inaczej mówiąc wpływ temperatury jest mniejszy, niż dokładność pomiaru jaką potrzebujemy osiągnąć, to żyroskop jest wyraźnie podatny.
Po trzecie, inny jest sposób wykorzystania danych przez wymienione 3 grupy czujników. O ile dane akcelerometru i magnetometru wchodzą do obliczeń wprost, jako arkus tangens poszczególnych składowych XYZ, to dane żyroskopu są całkowane. Załóżmy, że każdy z tych czujników ma 1% błędu w skali rozpiętości swojego pomiaru. Dla akcelerometru i żyroskopu w najgorszym przypadku błąd wyznaczania kąta będzie wynosił 2%. Natomiast w przypadku żyroskopu, ten 1% błędu prędkości kątowej wyrażonej w [°/s] będzie sumowany w każdym cyklu pomiarowym, co spowoduje, że błąd kąta będzie narastał z prędkością 1% na sekundę i już po krótkiej chwili spowoduje kumulację do katastrofalnie dużej wartości.
Z tymi problemami można sobie radzić na kilka sposobów. Niezbędnym minimum jest aby przy każdym starcie wyznaczyć zero prędkości kątowych dla zastałych warunków. To jest wystarczające do latania blisko i nisko, gdzie nie ma istotnych różnic temperatury. Chcąc lecieć wyżej trzeba mieć na uwadze że na każde 100m wysokości temperatura spada średnio o 0,6°C. Aby poradzić sobie z wpływem temperatury na pomiar, stosuje się termostatowanie, czyli nagrzanie czujników do stałej temperatury i kalibrację zera w tak ustalonych warunkach. Ze znanych autopilotów tak robi Pixhawk Cube. Wadą tego rozwiązania jest czas potrzebny na nagrzanie się czujnika, zwłaszcza w zimie. Innym podejściem jest zbadanie kształtu charakterystyki dryftu temperaturowego po to, aby w warunkach zmieniającej się w locie temperatury móc wyznaczyć współczynnik korekcji tego dryftu. Tak robi się w sprzedawanych tutaj AP opracowanych przez Zbiga. Daje to dobre efekty ale kalibracja jest bardziej pracochłonna i trudniej jest ją opracować.
W APL3 próbuję połączyć te dwie metody. Płytka modułu IMU ma wbudowana grzałkę zrobioną z kilku oporników. Czujniki IMU mają swoje termometry, więc można będzie uruchomić regulator termostatu, utrzymujący stałą temperaturę czujników. Pierwsze próby nagrzewania modułu jeszcze poza autopilotem wyglądają zadowalająco. Na termografie po lewej stronie widać grzałkę umieszczoną na spodniej stronie płytki modułu. Być może trzeba będzie trochę przedłużyć ścieżkę grzejącą, bo dół modułu gdzie siedzi ND130 jest niedogrzany. Na górnej stronie płytki widać jedno miejsce gdzie utworzył się mostek termiczny i jest wyraźnie cieplej. Na zdjęciach poniżej pokazano spód i wierzch płytki modułu, po ponad 3,5 minuty grzania pełną mocą.
Na pewno trzeba będzie położyć na grzałkę plasterek maty termoprzewodzącej, aby ciepło rozchodziło się równomiernie i oraz modułowi dać przepuszczający powietrze dla czujników ciśnienia kubraczek, aby nie tracił ciepła i pozwolił zmniejszyć różnice temperatury części płytki. Zastanawiam się nad takim lub takim. Który waszym zdaniem będzie lepszy? :-)
Jeżeli chodzi o drugą część zagadnienia, czyli wyznaczenie charakterystyki dryftu i odtwarzanie na bieżąco wartości korygującej, to jestem już po pierwszym podejściu. Zostało utworzone osobne menu z kalibracjami, gdzie na razie w dosyć prymitywny sposób została obsłużona kalibracja w 3 temperaturach: pokojowej 25+/-5°C, zimnej 10+/-5°C i gorącej 40+/-5°C.
Z uzyskanych danych wyznaczam charakterystykę, na razie liniową, osobną dla niskiej temperatury osobną dla wysokiej. W trakcie normalnej pracy z charakterystyki odczytuję wartość korekcji dryftu dla bieżącej temperatury, który odejmuję od wskazań. Pierwsze rezultaty może nie rzucają na kolana, ale są jakimś punktem odniesienia do dalszych prac. Przykładowe, surowe dane z nieskalibrowanych żyroskopów wyglądają tak jak na obrazku niżej. Dane z indeksem 1 pochodzą z układu ICM42688, z indeksem 2 z układu LSM6DSV. Dostaję zakres prędkości kątowych od +6 do -5°/s dla zakresu +-1000°/s, czyli błąd nieskalibrowanego czujnika jest w okolicach 0,5%.
Po kalibracji w 3 temperaturach autopilot został włożony do lodówki na kilka godzin, potem wyjęty i uruchomiono rejestrację trwającą godzinę. Temperatura rejestrowana przez termometry czujników wyglądała tak jak na obrazku poniżej. Mamy zmianę ok. 24°C. Czujniki rozgrzewają się do ponad 30°, ponieważ znajdują się pomiędzy ciepłym procesorem a jeszcze cieplejszym wyświetlaczem. Grzałka na razie jest wyłączona.
Wykres prędkości kątowych pierwszego skalibrowanego żyroskopu w funkcji czasu przy powyższej zmianie temperatury wygląda tak jak niżej. Na żółtym wykresie prędkości kątowej R widać załamanie charakterystyki przy przejściu z charakterystyki „zimnej” na „gorącą”. To może świadczyć, że liniowa charakterystyka to za mało, aby dobrze odwzorować kształt charakterystyki temperaturowej. Jednak potrzeba więcej pomiarów, aby to jednoznacznie potwierdzić. Trzeba też zbadać, jaki jest rozrzut kolejnych kalibracji.
Dla drugiego czujnika wykres prędkości kątowych wygląda podobnie. Ząbek jest trochę mniejszy, ale dla prędkości Q jest zauważalny.
Takie wykresy niewiele mówią, więc wykorzystajmy największą słabość metody wyznaczania kąta, czy konieczność całkowania danych. Im więcej średnia wartość będzie różniła się od zera, tym szybciej całka będzie przyrastała, czyli będzie gorzej. W czujniku 1 prędkość P daje błąd kata przechylenia Phi relatywnie niewielki, natomiast kąt pochylenia Theta urósł w ciągu godziny do -485°, kąt odchylenia Psi do 625,5°.
Podobnie drugi czujnik. Dla jednej osi udało się trafić ze współczynnikiem do -0,2°/h a maksymalnie do -10°/4h. Najgorzej skalibrowana oś dała błąd o wartości 354,8°/h co jest połową błędu pierwszego czujnika. Jest zbyt wcześnie, aby wyciągać wnioski o przewadze jednego czujnika nad drugim, raczej to są błędy mojej metody i rozrzut kalibracji, który trzeba będzie dopiero oszacować.
Następnego dnia rozbudowałem loger, tak aby móc rejestrować surowe dane z żyroskopów, żeby wyznaczyć ich charakterystykę temperaturową. W dotychczasowej próbie lodówkowej, czyli narastaniu temperatury od 8 do 34°C wyszła mi taka charakterystyka. Wyższe temperatury są liniowe, ale niskie zachowują się podejrzanie. Mam za krótką charakterystykę, aby zrozumieć, na czym polega problem.
Na szczęście wczoraj był piątek a na obiad frytki z paluszkami rybnymi, co pozwoliło zwolnić trochę miejsca w zamrażalniku, więc autopilot powędrował do zamrażalnika, który ma moc rozciągania charakterystyki w dół. Tym razem temperatura wyglądała tak jak poniżej.
Odpowiednio ta sama charakterystyka dryftu temperaturowego wygląda tak jak niżej i wszystko stało się jasne. W chwili przejścia temperatury przez zero charakterystyka skacze do nieskończoności, żeby później ponownie wrócić do liniowej. Jedynym parametrem przechodzącym przez zero jest temperatura, więc to ona jest winna a w zasadzie skala Celsjusza, gdyż temperatura jest w mianowniku, a jeżeli mianownik osiąga zero to z matematycznego punktu widzenia, jest to brzydka sprawa. No cóż profesorowi Celsjuszowi musimy powiedzieć „Sorry Winnetou” i przeprosić się z lordem Kelvinem.
Kalibracja wzmocnienia
W trakcie testów okazało się, że kalibracja zera to ma za mało. Dwu równolegle ułożonym żyroskopom podczas poruszania powoli rozjeżdża się całka prędkości kątowych, dlatego trzeba było wprowadzić dodatkową kalibrację skalowania zakresu sygnału żyroskopu. Kiedyś, gdy pracowałem z APL2, specjalnie do tego celu kupiłem gramofon ze stabilizowaną prędkością obrotową talerza aby wykorzystać go jako wzorzec. To był zły pomysł, bo gramofon dosyć słabo trzymał swoją prędkość, a sam talerz uginał się pod niesymetrycznym obciążeniem powodując bicie. Teraz wymyśliłem znacznie prostszy test: policzenie całki z prędkości kątowej.
Napisałem procedurę, gdzie na początku całka jest zerowana, następnie należy obrócić autopilot dookoła kalibrowanej osi o 3 pełne obroty i procedura kalibracyjna oblicza proporcję wartości 6*Pi do scałkowanego kąta. Żeby uniknąć błędów nieosiowości obrotu na ekranie wyświetla się poziomica. Zbyt duża odchyłka od pionu lub wartość całki zbytnio odbiegająca od normatywnych 6*Pi nie pozwoli na zapisanie wyników kalibracji.
Termostat
Skoro artykuł rozpoczął się od termogramu, warto przedstawić ciąg dalszy. Mając wolną chwilę, napisałem oprogramowanie termostatu, działającego jako regulator proporcjonalny sterujący programowym PWM włączającym grzałkę. Programowy PWM polega na tym że za okres PWM przyjęto 100 obiegów pętli głównej (0,5 s) i jeżeli odpowiedź regulatora P, czyli uchyb * wzmocnienie wynosi np. 20%, to znaczy, że grzałka jest włączana w każdym cyklu 0 a wyłączana w cyklu 20. Bezwładność cieplna jest na tyle duża, że temperatura mierzona po drugiej stronie płytki nie pulsuje. Taki regulator jest banalnie prosty. Tak wygląda przykładowy przebieg regulacji po włączeniu. Temperaturę zadana jest ustawiona trochę wyższa niż temperatura otoczenia. Na początku grzałka jest włączona z pełną mocą, a gdy uchyb się zmniejsza, wypełnienie PWM stabilizuje się w okolicach 40% czasu grzania.
Na nie bardzo było wiadomo, jakie przyjąć wzmocnienie, ale to jest proste. Wzór regulatora P wygada tak:
Wypełnienie PWM = błąd_regulacji * wzmocnienie
Minimalne wzmocnienie określa zadowalający uchyb regulacji. Załóżmy, że zadowala nas uchyb nieprzekraczający 2°C. Jeżeli jest większy, to ma grzać z pełną mocą, więc wzmocnienie będzie wynosiło 100 / 2 = 50. Zwykle maksymalne wzmocnienie dobiera się tak, aby nie było oscylacji regulowanego parametru, ale wydaje się, że nie ma potrzeby stabilizować temperatury do ułamka stopnia. Ponieważ regulator proporcjonalny nigdy nie osiąga wartości zadanej, zawsze jest jakiś uchyb regulacji, więc spotkałem się z rozwiązaniem użycia regulatora proporcjonalno-całkującego. Wydaje mi się, że jest to przerost formy nad treścią. Obecnie regulator utrzymuje temperaturę ok. 0,2°C niższą niż zadana. Na razie spróbuję zwiększyć wartość zadaną o te 0,2°C i będę obserwował. Na razie wydaje się, że to wystarczy do osiągnięcia celu bez zwiększania komplikacji algorytmu.