Определение угла наклона акселерометром
Михаил, ты поворачиваешь систему отсчёта вместе с самолётом, я же хочу получить углы относительно земли.
я же хочу получить углы относительно земли.
А смысл? Захотелось посвязываться с кватернионами? (от одного названия волосы дыбом) На самом деле углы относительно земли, на мой взгляд, достаточно бесполезны. Может я чего-то не вижу?? В любом случае, данные нужны для того, чтобы удерживать модель на определенном крене/тангаже. Так? И в этом случае автопилот будет работать в каком-то ограниченном пределе их изменения, думаю не более чем +/-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; Коррекции ДУС не происходит. Работает, но по курсу плывет.
Опаснее критические значения с датчиков, а не нулевые.
Нужно как-то “ловить” опасные показания, например, когда магнетометр находится в сильном ЭМ поле, то коррекция по компасу приведет к аварии. Можно даже “раскрутить” ИМУ по любой оси, действуя на магнетометр. Аналогично, акселерометр нужно фильтровать, отсекать “глюки” неверной калибровки диапазона, к примеру.