Определение угла наклона акселерометром
Использую cortex m3, пишу на C, стандартная математическая библиотека math.h. Для atan2 знаки обоих аргументов используются для вычисления квадранта результата.
Михаил, ты так предлагаешь?
void to_angle(void){
if (fYA>0|fZA>0){
roll = atan(fYA/fZA);
return;
}
if (fYA>0|fZA<0){
roll = atan(fZA/fYA);
return;
}
if (fYA<0|fZA>0){
roll = -atan(fYA/fZA);
return;
}
if (fYA<0|fZA<0){
roll = -atan(fZA/fYA);
return;
}
}
Использую cortex m3
чей кортекс, если не секрет? а к m4 вроде как добавили FPU как отдельный сопроцессор плавающей запятой.
Александр, roll - это глобальная переменная, кот. где-то потом используется? не очень ясен смысл ф-ии… всмысле матем-ий ясен, а практический не очень.
Если у тебя Арктангенс2 такой умный, можешь попробовать:
roll = atan2(-ZA,YA);
pitch= atan2(-ZA,-XA);
Это я пишу исходя из осей акселерометра. Они не совпадают с принятыми осями в навигации. Там по-другому.
А если только тангенс, то
void to_angle(void)
{
if ((fYA<=0)&&(fZA>0))
{
roll = atan(-fYA/fZA) - pi();
return;
}
if (fZA<0)
{
roll = atan(-fYA/fZA);
return;
}
if (fYA>=0)&&(fZA>0)
{
roll = atan(-fYA/fZA) + pi();
return;
}
if (fYA<0)&&(fZA=0)
{
roll = -pi()/2;
return;
}
if (fYA>0)&&(fZA=0)
{
roll = pi()/2;
return;
}
}
для Петруччо: LPC1768, STM32F100C4T6B. Мощи много после AVR. FPU для этого проекта вряд-ли нужен будет. roll глобальная, используется пока только для визуализации угла.
в ответ на #9
в обеих случаях работает если не крутить по оси X, иначе перевороты в районе ±90гр как на первом рисунке #1
в обеих случаях работает если не крутить по оси X, иначе перевороты в районе ±90гр как на первом рисунке #1
Там все верно. “Переворот” по тангажу – это правильно. Вот гляди, два случая:
- Летим в прямом полете. Тангаж +2 градуса. Крен 1 градус. Начинаем вертеться по крену не меняя тангажа. Получается, что тангаж будет положительный. И так мы крутимся до крена 89 градусов. Все вроде бы правильно.
- Летим в перевернутом положении. Тангаж, при той же проекции силы тяжести на ось X, уже будет не +2, а -2. Соответственно и по крену тоже показания. Будет не +1, а +179. Начинаем вертеться по крену от 179 до 91 градуса. И тут тоже тангаж остается отрицательным. Как и положено.
А теперь посмотри в каком положении самолет при тангаже +2, крене 89, и тангаже -2, крене 91? Да это же почти одно и то же. Отличие на 2 градуса по крену.
Вот и получается, что в 90 градусов по крену тангаж меняет знак. Он начинает показывать переход от прямого к перевернутому полету.
(Сорри. Может, конечно, немного попутал. И по крену тоже углы будут менять знак, но принцип понятен.)
Относительно меня или относительно вас 😃
Я стою на земле, смотрю на самолёт Михаила, и вижу, что он делает бочку, никакого переворота по тангажу не вижу, если- бы увидел как хвост поменялся с носом в момент перехода по крену 90гр. очень бы удивился 😃
Относительно меня или относительно вас 😃
Относительно того, что измеряешь. Можешь, конечно аксель и к земле прикрутить, а не к модели. Дело хозяйское.
Я стою на земле, смотрю на самолёт Михаила, и вижу, что он делает бочку, никакого переворота по тангажу не вижу
Вот. Красным обозначены оси акселерометра (X, Z). Черным – горизонтальная ось земли (Xз). Сила тяжести - F.
Если вертеть вдоль оси Х, то видно, что в нормальном полете угол между X и Хз - положительный. В перевернутом – отрицательный. Т.к. относительно перевернутой оси Z, связанной с моделью, модель зарывается как бы ниже оси Хз.
Михаил, ты поворачиваешь систему отсчёта вместе с самолётом, я же хочу получить углы относительно земли.
я же хочу получить углы относительно земли.
А смысл? Захотелось посвязываться с кватернионами? (от одного названия волосы дыбом) На самом деле углы относительно земли, на мой взгляд, достаточно бесполезны. Может я чего-то не вижу?? В любом случае, данные нужны для того, чтобы удерживать модель на определенном крене/тангаже. Так? И в этом случае автопилот будет работать в каком-то ограниченном пределе их изменения, думаю не более чем +/-45 градусов по тангажу и +/-30 по крену. Т.е. НЕ автоматизировать перевернутый полет. Разве что определить, что модель летит кверхтормашками и попытаться ее вывести в нормальный режим полета. А в нормальном режиме полета указания по крену и тангажу будут совпадать с наблюдаемыми с земли.
Михаил, ты поворачиваешь систему отсчёта вместе с самолётом
Я ее не поворачиваю. Она в микросхеме акселерометра жестко прошита, и будет крутиться вместе с микросхемой.
Кстати, как вариант, можешь проверку на знак fZA сделать. И если fZA>0, то в расчете тангажа подставлять (-fZA).
Я сделал компенсацию по компасу, но это довольно сложный алгоритм… Именно по этому большенство народа для определения горизонта используют аксели.
code.google.com/p/imumargalgorithm30042010sohm/ 2010 год, MARG на кватернионах. Гарантировано работает, сам проверял - крутить можно вверх ногами, вращать на ребре и т.д. Повторяемость углов положения поразительная. Коррекция ДУС по магнетометру все 3 оси, по акселерометру все 3 оси. В конце кода есть небольшая ошибочка с присвоениями =)
На базе этого алгоритма построена библиотечка www.varesano.net/projects/hardware/FreeIMU, но в ней есть косяк, который дает дрифт Yaw. Эту библиотеку удобно использовать для опроса датчиков, а МАРГ переписать. После этого получается конфетка весом около 20 кб, которая дает кватернион/готовые углы/сырье.
Гарантировано работает, сам проверял - крутить можно вверх ногами, вращать на ребре и т.д. Повторяемость углов положения поразительная. Коррекция ДУС по магнетометру все 3 оси, по акселерометру все 3 оси. В конце кода есть небольшая ошибочка с присвоениями =)
А как себя этот фильтр ведет, если нет данных магнитометра?
И (по крайней мере на первый взгляд на код) ошибок не нашел.
Эту библиотеку удобно использовать для опроса датчиков, а МАРГ переписать. После этого получается конфетка весом около 20 кб, которая дает кватернион/готовые углы/сырье
Со второй частью что-то совсем не понял, где там искать библиотеку?..
А как себя этот фильтр ведет, если нет данных магнитометра?
И (по крайней мере на первый взгляд на код) ошибок не нашел.Со второй частью что-то совсем не понял, где там искать библиотеку?..
под версиями железа “FreeIMU library”
www.varesano.net/…/libraries_20120118_0959.zip
А как себя этот фильтр ведет, если нет данных магнитометра?
- mx=0; my=0; mz=0; Коррекция ДУС только по акселерометру. Работает, но сильнее “ловит” линейные ускорения и дает несуществующие наклоны.
- ax=0; ay=0; az=0; Коррекция ДУС только по магнетометру. Работает, но заторможеннее, чем в комплексе с акселерометром.
- mx=0; my=0; mz=0; ax=0; ay=0;az=0; Коррекции ДУС не происходит. Работает, но по курсу плывет.
Опаснее критические значения с датчиков, а не нулевые.
Нужно как-то “ловить” опасные показания, например, когда магнетометр находится в сильном ЭМ поле, то коррекция по компасу приведет к аварии. Можно даже “раскрутить” ИМУ по любой оси, действуя на магнетометр. Аналогично, акселерометр нужно фильтровать, отсекать “глюки” неверной калибровки диапазона, к примеру.