Создание собственной системы стабилизации
версию аксель+гпс
Ломаю голову, как бы разделить линейные ускорения и крен у акселя… пока глухо.
Ломаю голову, как бы разделить линейные ускорения и крен у акселя… пока глухо.
если абстрагироваться от ИМУ, то вот матрица поворота, только знаки надо подобрать правильно (тот еще ребус!):
#define deg2rad(x) (float)(x*PI/180.0f)
float cr = cos(deg2rad(angle[ROLL]));
float cp = cos(deg2rad(angle[PITCH]));
float cy = cos(deg2rad(angle[YAW]));
float sr = sin(deg2rad(angle[ROLL]));
float sp = sin(deg2rad(angle[PITCH]));
float sy = sin(deg2rad(angle[YAW]));
float earth_acc_x = (cp*cy) * acc_x + (sr*sp*cy - cr*sy) * acc_y + (sr*sy + cr*sp*cy) * acc_z;
float earth_acc_y = (cp*sy) * acc_x + (cr*cy + sr*sp*sy) * acc_y + (-sr*cy + cr*sp*sy) * acc_z;
float earth_acc_z = (-sp) * acc_x + (sr*cp) * acc_y + (cr*cp) * acc_z;
на выходе earth_acc_x/y/z - это ускорения не зависимые от наклонов…
Вы вроде с инерциалкой Александра (rual) играетесь. Там проще по идее, т.к. в кватернионах калькуляции…
Я уже писал вам, одного ICP(Input Capture Pin) хватит для обработки PPM(CPPM) сигнала. Для этого достаточно ловить только Rising Edge (из 0 в 1). Если вам надо каждый канал по отдельности обрабатывать, то разумеется вам надо столько ICP пинов, сколько у вас каналов и в этом случае надо его переключать из 0 в 1 и 1 в 0.
в обычном приемнике, от Futaba 7C например, РРМ сигнал ведь разложен по 7 канала, на каждой ножке свой канал…
в Atmega128 есть только 2 прерывания с режимом захвата, это 1 и 3 NIT, т.е. я могу только 2 канала с приемника аппаратно обработать. а что делать с остальными? на них нужно программный захват организовывать с помощью простых INTов?
Я так понял все тут делают на STM32, а у него сколько аппаратных ICP прерываний? Или есть кто делает на атмеге?
Ну понаписали!!! Вот куда время уходит!!!😛
Народ мне тут на ухо нашептали, грамотные программеры меня поправя если что не так, по прерываниям, первое что надо сделать в прерывании - это запретить все прерывания
Сергей, неверно тебе нашептали, но АВР по умолчанию именно так и делает, SirAlex ниже написал про эту особенность. В СТМ32 всё гораздо лучше, в нем оптимизирована смена контекста работы проца (переход в подпрограммы), есть векторный контроллер прерываний, вы бирай сам что для тебя более важно. Главное не забыть выделить достаточное место под стек! И не надо париться что там что прерывает, у меня по 7 прерываний “одновременно” обрабатываются:)
Ну понаписали!!! Вот куда время уходит!!!
ну да, ну да 😃
у нас завтра демо по проекту, а я через translit.ru, расписался тут 😃
вообще надеюсь поможет, ибо имея даже самый быстрый и точный IMU, можно получить хреновый результат, если выходной пид-регуль “слаб” 😃
в обычном приемнике, от Futaba 7C например, РРМ сигнал ведь разложен по 7 канала, на каждой ножке свой канал…
в Atmega128 есть только 2 прерывания с режимом захвата, это 1 и 3 NIT, т.е. я могу только 2 канала с приемника аппаратно обработать. а что делать с остальными? на них нужно программный захват организовывать с помощью простых INTов?
Я так понял все тут делают на STM32, а у него сколько аппаратных ICP прерываний? Или есть кто делает на атмеге?
Погоди, так PPM приходит же единым пакетом. Все каналы сразу. Значит тебе для его обработки нужна всего 1 ножка. И с помощью этой 1 ножки ты и обрабатываешь все свои 7 каналов. Тебе нужно ловить только переходы из 0 в 1 и из 1 в 0 и все, как написали выше.
Погоди, так PPM приходит же единым пакетом. Все каналы сразу. Значит тебе для его обработки нужна всего 1 ножка. И с помощью этой 1 ножки ты и обрабатываешь все свои 7 каналов. Тебе нужно ловить только переходы из 0 в 1 и из 1 в 0 и все, как написали выше.
да, согласен, РРМ это 1 канал. Но откуда его взять в обычном приемнике? я так понимаю, приемник ловит РРМ и разделяет его на все 7 ножек в отдельности…
вместо ифк датчиков прислали 4 пирометра, поругаться или с пирометрами что-нибудь замутить?
Это подстава, “прорекламируй” поставщика.
А поделитесь ссылкой на математику?
ищем по “расстояние между точками на поверхности земли”, там мног всего, для начала gis-lab.info/qa/great-circles.html см. реализацию для эсель (наиболее близко к Си). Из этого просто нужно сделать 2мерный вектор ХУ.
а по чем там X,Y корректится? гпс? optflow?
нет, это чистый горизонт, линейные перемещения пока сам планирю считать. Просто более простой и понятной (мне) реализации не встречал. Только у него все лишние члены векторных произведений типа (1,0,0)х(0,0,1) исключены, я же всё делал “академически” ибо изобретал “велосипед” с нуля (т.е. с высоты своих познаний)
глянул… не потянет авр-ка похоже…
А ты попробуй, ДЦМ помоему шибко хуже.
Вот код ПД-части по гире:
Я как раз пор это, ПИ-регулятор комплементарного филтра. Тут палка о двух концах, сделаешь большой И - будет перекомпенсация (горизонт будет крыльями махать), при маленьком И будет нескомпенсированый завал в одну из сторон.
в крутых контроллерах типа немец, наза, а также от vis.asta так и сделано…
Тут по другому никак, более оперативных перемещения датчиков нет
он кстати про mpu6050
нет, он кстати про МПУ6000 на ПХ4м контроллере.
вот вариант красивый, но дороговато
Да, это сказка, то что нужно для конкурса (только тссс!), вот только стоил бы он баксов 70. А так я сам такой собиру, нужно только дождаться цифровой камеры от китайцких товарищей. Я видел где-то на датчике от мыши, просто туда был прикручен объектив от камеры.
Ломаю голову, как бы разделить линейные ускорения и крен у акселя… пока глухо.
“Академически” делается предельно просто: поворачиваем вектор акселя в мировую СК и вычитаем вектор нормали (0,0,-1) или (0,0,1) зависит от “полярности” акселя. В остатке все остальные ускорения 😁, в т.ч. ошибка горизонта. В моей вылолженной проге вроде есть функции в класcе INS.
Я видел где-то на датчике от мыши, просто туда был прикручен объектив от камеры.
да, согласен, РРМ это 1 канал. Но откуда его взять в обычном приемнике? я так понимаю, приемник ловит РРМ и разделяет его на все 7 ножек в отдельности…
Ааа, так у вас обычный приемник…
Пришла тут идея, если свести все каналы в один, соединив их между собой после выхода из приемника?
Так как импулься между собой разделены, то они перекрываться не будут, тогда все 7 каналов можно пустить по 1 проводу и подцепить к одному каналу таймера.
ищем по “расстояние между точками на поверхности земли”, там мног всего
вот еще ссылочка www.movable-type.co.uk/scripts/latlong.html
Это подстава, “прорекламируй” поставщика.
dx 😦 я писал раньше что посылка потерялась, а потом ещё не мог найти их (датчики) чтоб показать, а оказывается их уже нет, вот они и переслали новую с датчиками движения на пирометрах 😦 я один сегодня на работе уже сломал 😃 ладно фиг с ними, невелика потеря, сейчас уныло смотрю в код AeroQad сколько всего переделать то надо - ууу, никак не насмелюсь пока даже порты переписывать, а надо ещё gps от арду впихнуть (он там есть но надо правильно библиотеки подключать научится), удержание высоты сделано чисто на баро 😦 сонара нет, короче пахать и пахать, а хочется сделать cli, дабы не компилить постоянно для изменения аппарата или там подключения модулей (gps, сонар…) ну и всяких там расширеных возможностей по настройке, а без мативатора (платы) как-то ничего не хочется делать…
dx 😦
Один раз связывался с dx, больше я туда ни ногой…
Пришла тут идея, если свести все каналы в один, соединив их между собой?
это не панацея, и где гарантии что вы не убъёте приёмник, есть простое решение о котором уже писал, сам с такой штукой летаю, правда просто потому, что куча проводов задолбала…
есть простое решение о котором уже писал
напишите еще раз, пожалуйста) а то искать замучаюсь…
на синенькое тыкаем и ждём… пост 49
Я как раз пор это, ПИ-регулятор комплементарного филтра. Тут палка о двух концах, сделаешь большой И - будет перекомпенсация (горизонт будет крыльями махать), при маленьком И будет нескомпенсированый завал в одну из сторон.
вот вот! я когда с MARG-ом игрался, потратил кучу времени на настройку пид параметров компл. филтра (КФ), но так и не получил стабильного результата…
Но решение есть. Я както с Олегом (Syberian) общался по этому поводу и он наткнулся на туже проблему… Если пральна помню, то выкидываешь ПИД регуль из КФ и оставляешь, класический КФ, где дрейф быстрого/высокочастотного вектора гиры компенсируется НЧ вектором акселя:
Vg = Vg * (1 - 1/k) + Va * 1/k
где Vg - вектор гиры, Va - вектор акселя, на твой цикл (380 герц вроде) к=400…600
нет, он кстати про МПУ6000 на ПХ4м контроллере.
на его контроллере совсем не mpu6050/6000… а вот когда он писал порт на PX4, то сильно его ругал
это не панацея, и где гарантии что вы не убъёте приёмник,
Согласен, тут гарантий нет, есть 1 приемник, надо будет попробовать…
на синенькое тыкаем и ждём… пост 49
49 пост в этой ветке??
49 пост в этой ветке??