Создание собственной системы стабилизации
низы под ТауЛавс Спарки переделываю
мелкую не дождался (
дык у таулабсов и возьми - там довольно красиво драйвер расписан - спарки 2 на 9250 или у тебя 1-я на ф3?
github.com/SergDoc/TauLabs/blob/…/pios_mpu9150.c
github.com/SergDoc/TauLabs/…/pios_mpu9250_spi.c
ну и хедеры там же:
github.com/SergDoc/TauLabs/blob/…/pios_mpu9150.h
github.com/SergDoc/TauLabs/blob/…/pios_mpu9250.h
только там ещё pios_mpu60x0.h прикручен, в нём дописаны регистры работы с акселем 6500
github.com/SergDoc/TauLabs/blob/…/pios_mpu60x0.h
как им управлять из мелкой глянь:
github.com/SergDoc/TauLabs/blob/…/pios_board.c#L42…
Решил тут попробовать новый кодогенератор для СТМ32
он кривой - мама не горюй - сразу проверяй, чтобы все порты прописал, теряет порты из конфига…
мелкую не дождался (
Та не… я ж спарки ещё около 2х лет назад купил. Да и
или у тебя 1-я на ф3?
так что мелкая мне вполне интересна ) За ссылки спасибо!
он кривой - мама не горюй - сразу проверяй
Так мне нужно только распределение ресурсов, вручную лень собирать. Решил немного поупражняться со скуки, ибо на крыло до вступления в силу закона вряд ли успею. Щас всё “пилю” на виртуалке, жаль адекватной модели коптера в ней нету…
кстати смотрю, а в 9150 один регистр отвечает за фильтры, а не 2 как в 9250…
так что мелкая мне вполне интересна )
пошел дорисовывать)))
Есть вопрос. У меня датчик MPU6050. Гира углы считает отлично. Если наклонять контроллер ( свой ) по осям в разные стороны, углы совпадают с углом акселерометра ( коррекции нет)
Если вращать вокруг оси x,y на 360 град. и назад то погрешность 1-2 градуса. При движении вдоль стола по 2-м осям нули. Но стоит наклонять плату постепенно
меняя ось , как бы по сфере но без вращения по оси z , то гиры начинают врать. Угол уходит прилично на 10-20 град. Это влияние осей друг на друга ?
меняя ось , как бы по сфере но без вращения по оси z , то гиры начинают врать. Угол уходит прилично на 10-20 град. Это влияние осей друг на друга ?
Нет, влияние осей очень незначительно может доли градуса ошибки даст. У вас скорее всего ошибка вычислений (округление или фильтр какой-нибудь), который отбрасывает низкие скорости, или более серьезная ошибка в алгоритме.
Нет, влияние осей очень незначительно может доли градуса ошибки даст. У вас скорее всего ошибка вычислений (округление или фильтр какой-нибудь), который отбрасывает низкие скорости, или более серьезная ошибка в алгоритме.
Ошибки нет. Также есть влияние оси z на y. Заметил что коптер при развороте вокруг оси уносит в сторону. Положил плату на стол. Углы по акселю и гирам 0.
Вращаю плату вокруг оси и угол Y по гирам уходит на 5 градусов. Расчитал зависимость и внес в формулу. И теперь при вращении коптер висит в одной точке.
Угол_гир_х= Угол_гир_х + данные_гир_х *0.00008;
Угол_гир_y= Угол_гир_y + ( данные_гир_z*0.02 + данные_гир_y) * 0.00008;
И какое время ставите на коррекцию гир по акселю. У меня стоит примерно 5 сек.
При резких движениях платы вдоль стола гиры держат углы , если меньше то влияние есть.
Просмотрев множество вариантов систем стабилизации полёта, www.kkmulticopter.com/index.p...tics&Itemid=65 - эту штуку собрал сам, немецкие бутерброды не по карману, есть мысли по созданию собственного УНИВЕРСАЛЬНОГО контроллера для многомоторных систем, как пообещал знакомый программист не без исключения зачатков искуственного интеллекта (надеюсь). Преследую цель бесперебойной работы при потере связи с управляющей системой, выполнение задания и возврат на точку старта. Повторяюсь - бюджет очень урезан, каждое устройство будет собиратся вручную. Если есть мнения прошу кидаться помидорами. Старт разработок Июль 2011г.
ИИ здесь вреден. Интеллект и так есть - это интеллект пилота. Ну ка, качество ручной стабилизации по крену при обычном полёте и когда будете находу выводить формулы вычисления углов отклонения элеронов? А отталкиваться надо от многоканального астатического регулятора. Разнообразные регуляторы и системы автоматического управления - моя профессия и на сколько они на самом деле просты могу рассказать, как специалист. Для универсальности делайте автопилот адаптивным.
Угол_гир_х= Угол_гир_х + данные_гир_х *0.00008;
а что это?
я так понимаю интегрирование должно быть, тогда что такое 0.00008 - яко бы время между отсчётами? но оно может быть разным…
ИИ здесь вреден. Интеллект и так есть - это интеллект пилота.
не, не вреден, только в данном узле он не нужен )))
а что это?
я так понимаю интегрирование должно быть, тогда что такое 0.00008 - яко бы время между отсчётами? но оно может быть разным…
Время одинаковое. В секунду у меня проходит 380 считываний датчика. И в зависимости от диапазона измерений гиры это число будет разное.
Проделал эксперимент. Плата на столе. Поднимаю и медленно вращаю по х на 360 градусов , кладу. Насчитал 365. Далее в обратную сторону и на стол , угол 1 гр.
И за 4 полных таких цикла угол 0 градусов. Коррекции по акселю убрал.
Время одинаковое. В секунду у меня проходит 380 считываний датчика.
в поликлинике тоже может быть средняя температура 36,6 но у одних она еще 40 а у других комнатная.
нужно брать реальное дельта времени за период измерения а не среднее за секунду
в поликлинике тоже может быть средняя температура 36,6 но у одних она еще 40 а у других комнатная.
нужно брать реальное дельта времени за период измерения а не среднее за секунду
Это не влияет абсолютно на результат. Есть у кого конкретные примеры со
своего железа.
Угол_гир_х= Угол_гир_х + данные_гир_х *0.00008;
Угол_гир_y= Угол_гир_y + ( данные_гир_z*0.02 + данные_гир_y) * 0.00008;
Методический косяк: линейное интегрирование каждой оси в отдельности правильной ориентации в 3х мерном пространстве не даст никогда! Ну разве что в случае вращения идеально вокруг одной оси. Интегрирование необходимо выполнять через матрицу поворота или кватернион.
Ошибки нет. Также есть влияние оси z на y. Заметил что коптер при развороте вокруг оси уносит в сторону. Положил плату на стол. Углы по акселю и гирам 0.
У вас нерабочий алгоритм определения ориентации, а вы уже коптером рулите??? Есть же методика разработки, разбиение на изолированные функции. Такой подход как у вас, когда по наблюдению за работой сложной системы, пытаться найти ошибку в ее подсистеме, не даст хороших результатов.
У вас нерабочий алгоритм определения ориентации, а вы уже коптером рулите??? .
Ошибочное мнение. Коптер отлично держит горизонт, получше всяких готовых прошивок . Могу продемонстрировать , живем в одном городе.
Для полета задаем угол по оси , с пульта. Коптер наклоняется и автоматом добавляет газ обратно пропорционально COS угла наклона.
Доб_газ= 70/(COS(угол_наклона)) -70 , например при 30 град. добавляем 11 единиц шим
Также есть автомат увеличения газа по измерению напряжения батареи , при снижении напряжения на 2 V добавляется примерно 20 единиц. У меня был тока один недочет. При горизонтальном полете ,когда сбрасывал угол , коптер уходил вверх метров на 5-7. На регуляторах стояла прошивка с неработающим торможением Damped и при выравнивании коптера 1-1.5 сек оставались повышенные обороты, больше чем надо для висения.
Прошил более старой версией и торможение заработало. Пока не проверил , погода у нас фиговая. Ну а вообще управление на 3DR Radio модемах.
Пульт свой( модернизированная Turnigy 9x ), протокол передачи свой. На пульт приходит информация с коптера ( GPS данные, напряжение батареи) и выводится на олед дисплей 20х4 .
При разряде батареи на коптере , в пульте пищит зумер.
Но стоит наклонять плату постепенно
меняя ось , как бы по сфере но без вращения по оси z , то гиры начинают врать. Угол уходит прилично на 10-20 град. Это влияние осей друг на друга ?
А мне кажется, что ошибка при медленном вращении платы это результат банального шума гиры (так и должно быть), а вот как раз коррекция по акселю для устранения этого шума и нужна, НО не раз в пять секунд !, а каждую итерацию с частотой расчета ИМУ… (А формула связывающая разные оси через линейный коэф. приведет к непредсказуемым результатам на разных режимах полета…)
а вот как раз коррекция по акселю для устранения этого шума и нужна, НО не раз в пять секунд !, а каждую итерацию с частотой расчета ИМУ…
Так и происходит формула :
угол_гир_х = угол_гир_х * 0.999 + угол_акселя_х * 0.001
Каждое измерение приближает значение угла гиры к углу акселя. Если у меня 380 измерений в сек. то для коррекции за 3 сек надо коэф. 1/(3*380 ) примерно.
Никакого шума гир нет. Я калибрую их при включении питания. Делаю 4000 измерений , нахожу среднее для каждой оси , и этот коэф. вычитаю при каждом считывании данных.
Тут все просто. Если мы по Х наклоним на 45 градусов , а потом по Y на 45гр.( относительно оси X ) то для оси земли это уже не 45 гр. Более лучше это понять если наклоним в право или лево на 90 градусов. Тогда ось Y вообще теряет смысл как ось , которая показывает наклон относительно земли., она превращается в Z. Но для отличной стабилизации хватает по PID регулятору на каждую ось.
угол_гир_х = угол_гир_х * 0.999 + угол_акселя_х * 0.001
Всё правильно, если так, и больше ничего не нужно… (старый добрый комплементарный фильтр") ищите косяк в другом месте… начиная с “сырых” данных датчиков и до выходных показаний PIDa…
Так уже сказали где косяк:
Так и происходит формула : угол_гир_х = угол_гир_х * 0.999 + угол_акселя_х * 0.001
- нельзя интегрировать гироскоп раздельно по осям. К примеру, наклони его на 90 вбок, поверни по азимуту на 90 и верни обратно. Проинтегрировался угол по одной оси, а реально сенсор развернулся по другой.
и больше ничего не нужно…
Да, еще матрицы или кватернионы естественно… ))
Спасает то что летаем при небольшом угле , до 30 градусов и погрешность при таком алгоритме небольшая.
И в основном летают одной осью вперед ,на которой камера.
Можно прикинуть допустимый угол наклона для полета.
488Гц частота шим ардуины ,и максимальное время шим 2049 мкс.
2049/255=8.035 мкс один шаг. шима
Регулятор можно запрограмировать на диапазон от 1000 до 2020 мкс.
Тогда минимальное значение шим для регулятора 1000/8.035=124 ед , даем при сетапе
analogWrite( 9 , 124);
Вот тут могут быть проблемы. 1000мкс при программировании регулятора могут не совпадать с нашими 1000 в ардуине.
Могут отличатся немного частоты кварца, и не все регуляторы инициализируются.
Нужно менять число на другое 123 или 125 к примеру. ( калибровка от пульта у меня отключена )
Максимальный уровень шим 2020/8.035=251 ед
Если будем давать больше 251 , регулятор не воспримет этот диапазом, а иногда даже может быть провал оборотов.
Ограничиваем диапазон шим на регулятор
M1=constrain( gaz + DM1a + DRZ + DRH , 124 , 251);
analogWrite( 9 , M1 );
Висим на 50 % газа , это 124+ (251-124)/2=188 ед
При проседании питания на 2V добавить надо 20 ед.
Еще запас на диапазон регулировок PID регуляторов -20 +20
Получаем 251-188-20-20=23 ед. Это число шим которое пойдет на коррекцию газа при наклоне.
Тяга_при_наклоне = Тяга_висения/cos(угла)
Коптер когда летит с наклоном ,его к земле прижимает встречный поток и тяга должна быть еще больше.
Вычислил и эксп. проверил формулу для своего коптера
шим_коррекции = 70/Cos(угол)-70
отсюда при максимальной коррекции в 23 ед угол может быть 41 градус
Ошибочное мнение. Коптер отлично держит горизонт, получше всяких готовых прошивок .
Спасает то что летаем при небольшом угле , до 30 градусов и погрешность при таком алгоритме небольшая.
Вот собственно ответ, почему летает - коррекция от акселя перетягивает интегральную ошибку.
Предлагаю немного подкорректировать Ваши формулы, Сергей, и получить “дубовый” (в хорошем смысле слова) алгоритм, но БЕЗ ОПРЕДЕЛЕНИЯ ОРИЕНТАЦИИ. Предполагаю, судя по поведению, аналогичный алгоритм используется в китайских игрушках.
Делаем так:
Сумм_крен +=ось_акселя_y * Кки;
Сумм_танг += ось_акселя_x * Кти;
шим_крен= угол_гир_х * Kкд + ось_акселя_y * Kкп +Сумм_крен + ось_РУ_КРЕН*Ккру;
шим_танг= угол_гир_y * Ктд + ось_акселя_x * Kтп+Сумм_танг + ось_РУ_ТАНГ*Ктру;
шим_рыск= угол_гир_z * Крд + ось_РУ_рыск*КРру;;
шим_газ = шим_газ* 0.999 + (ось_акселя_z- 1) * 0.001 + ось_РУ_рыск*Кгаз_ру;;
Надо только нормировать аксель до 1 (единица =1G), либо можно всё делать в целочисленном формате. И работать будет на “ура” даже на MCS-51.
Осталось только смешать это всё в сумматоре, для конфигурации “+” будет выглядеть вот так:
мотор_перед = -шим_танг+ шим_газ;
мотор_задний = шим_танг+ шим_газ;мотор_левый = -шим_крен+ шим_газ;
мотор_правый = шим_крен+ шим_газ;
Летать будет как по рельсам.
Если интересно, могу дорисовать компас.