Переполнение дополнительного фильтра STM32F103

Question

Я программирую контроллер полета для своего квадроцикла. Пока он отлично работает в режиме скорости, поэтому я решил добавить режим углов. Для этого требуется слияние данных акселерометра и гироскопа, чтобы получить истинное положение квадроцикла в пространстве. Так что я был в состоянии интегрировать градусов/сек с гироскопом в угол и получить угол от Accel, как показано ниже:

Микроконтроллер: STM32F103

Ps: Все переменные Struct {двойные х, у , г; }, dt является uint64_t

//getting angle from gyro rate 
gyroAngle.x += gyroRate.x * dt/1000; 
gyroAngle.y += gyroRate.y * dt/1000; 
gyroAngle.z += gyroRate.z * dt/1000; 

//getting angle from accel 
accelAngle.x = atan2(accel.y, sqrt(pow(accel.x,2) + pow(accel.z,2))) * (180/M_PI); 
accelAngle.y = -atan2(accel.x, sqrt(pow(accel.y,2) + pow(accel.z,2))) * (180/M_PI); 

//combines the two into true rotational position 
angle.x = .85 * (angle.x + gyroAngle.x * (dt/1000)) + (.15 * accelAngle.x); 
angle.y = .85 * (angle.y + gyroAngle.y * (dt/1000)) + (.15 * accelAngle.y); 

Проблема: у меня это на петле 250 Гц. К сожалению, через минуту или два gyroAngle начинают возвращать тарабарщину (я думаю, что она переполняется), в результате угол также возвращает габадж, так как в нем используется gyroAngle. Ускорение не вызвало у меня никаких проблем.

Это какой-то выход из последовательного монитора:

angle: -1  3  /* 
angle: -1  3  */normal until here 
----------------------------------------------------------- 
angle: -98  406  /* 
angle: -338  582  * 
angle: -710  646  * 
angle: -1113  716  * starts giving me garbage 
angle: -1616  849  * 
angle: -2327  1016  * 
angle: -3047  1377  */ 

Я совершенно озадачен, почему это происходит. Любая помощь приветствуется.

Answer 1

Скорее всего, вы идете в переполнение интеграции. Это вполне нормально и объясняет «дрейф» во многих подобных ситуациях.

Что происходит, так это то, что ваши измерения являются конечными, а не бесконечно точными (предел разрешения вашего датчика). Со временем дробные биты ниже 10 или 12 бит, которые вы читаете, складываются.

Очень скоро это приведет к переполнению регистров или к значению, отсутствующему номером ошибки *, который вы взяли. Вибрация вашего квадроцикла усугубит эту проблему.

Решение состоит в том, чтобы просто сбрасывать интеграцию в ноль каждый раз в то время или когда оно превышает некоторый предел (значение или итерации).

Пытается сохранить абсолютный результат из-за долгого, ограниченного прецизионного численного интегрирования.

the/1000, который у вас есть в нескольких местах, также приводит к утери точности при усечении - вы всегда округляете. Я бы не попытался исправить это путем округления, но, вероятно, это объясняет много ошибок.

Плавающей точкой может быть опция? возможно, даже компонент F4 для замены F103 - вы можете получить совместимый с штырьком. Возьмите чип в свою местную мастерскую по ремонту сотовых телефонов, если у вас нет опыта работы с SMD.

Возможно, вам понадобится добавить магнитометр, чтобы вы могли сбросить его до лучшей оценки.