Создание собственной системы стабилизации

oleg70
mahowik:

версию аксель+гпс

Ломаю голову, как бы разделить линейные ускорения и крен у акселя… пока глухо.

mahowik
oleg70:

Ломаю голову, как бы разделить линейные ускорения и крен у акселя… пока глухо.

если абстрагироваться от ИМУ, то вот матрица поворота, только знаки надо подобрать правильно (тот еще ребус!):

  #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) играетесь. Там проще по идее, т.к. в кватернионах калькуляции…

Alexsis1109
Sir_Alex:

Я уже писал вам, одного 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 прерываний? Или есть кто делает на атмеге?

rual

Ну понаписали!!! Вот куда время уходит!!!😛

SergDoc:

Народ мне тут на ухо нашептали, грамотные программеры меня поправя если что не так, по прерываниям, первое что надо сделать в прерывании - это запретить все прерывания

Сергей, неверно тебе нашептали, но АВР по умолчанию именно так и делает, SirAlex ниже написал про эту особенность. В СТМ32 всё гораздо лучше, в нем оптимизирована смена контекста работы проца (переход в подпрограммы), есть векторный контроллер прерываний, вы бирай сам что для тебя более важно. Главное не забыть выделить достаточное место под стек! И не надо париться что там что прерывает, у меня по 7 прерываний “одновременно” обрабатываются:)

mahowik
rual:

Ну понаписали!!! Вот куда время уходит!!!

ну да, ну да 😃
у нас завтра демо по проекту, а я через translit.ru, расписался тут 😃

вообще надеюсь поможет, ибо имея даже самый быстрый и точный IMU, можно получить хреновый результат, если выходной пид-регуль “слаб” 😃

Geniok
Alexsis1109:

в обычном приемнике, от Futaba 7C например, РРМ сигнал ведь разложен по 7 канала, на каждой ножке свой канал…

в Atmega128 есть только 2 прерывания с режимом захвата, это 1 и 3 NIT, т.е. я могу только 2 канала с приемника аппаратно обработать. а что делать с остальными? на них нужно программный захват организовывать с помощью простых INTов?

Я так понял все тут делают на STM32, а у него сколько аппаратных ICP прерываний? Или есть кто делает на атмеге?

Погоди, так PPM приходит же единым пакетом. Все каналы сразу. Значит тебе для его обработки нужна всего 1 ножка. И с помощью этой 1 ножки ты и обрабатываешь все свои 7 каналов. Тебе нужно ловить только переходы из 0 в 1 и из 1 в 0 и все, как написали выше.

Alexsis1109
Geniok:

Погоди, так PPM приходит же единым пакетом. Все каналы сразу. Значит тебе для его обработки нужна всего 1 ножка. И с помощью этой 1 ножки ты и обрабатываешь все свои 7 каналов. Тебе нужно ловить только переходы из 0 в 1 и из 1 в 0 и все, как написали выше.

да, согласен, РРМ это 1 канал. Но откуда его взять в обычном приемнике? я так понимаю, приемник ловит РРМ и разделяет его на все 7 ножек в отдельности…

rual
SergDoc:

вместо ифк датчиков прислали 4 пирометра, поругаться или с пирометрами что-нибудь замутить?

Это подстава, “прорекламируй” поставщика.

Razek:

А поделитесь ссылкой на математику?

ищем по “расстояние между точками на поверхности земли”, там мног всего, для начала gis-lab.info/qa/great-circles.html см. реализацию для эсель (наиболее близко к Си). Из этого просто нужно сделать 2мерный вектор ХУ.

mahowik:

а по чем там X,Y корректится? гпс? optflow?

нет, это чистый горизонт, линейные перемещения пока сам планирю считать. Просто более простой и понятной (мне) реализации не встречал. Только у него все лишние члены векторных произведений типа (1,0,0)х(0,0,1) исключены, я же всё делал “академически” ибо изобретал “велосипед” с нуля (т.е. с высоты своих познаний)

mahowik:

глянул… не потянет авр-ка похоже…

А ты попробуй, ДЦМ помоему шибко хуже.

mahowik:

Вот код ПД-части по гире:

Я как раз пор это, ПИ-регулятор комплементарного филтра. Тут палка о двух концах, сделаешь большой И - будет перекомпенсация (горизонт будет крыльями махать), при маленьком И будет нескомпенсированый завал в одну из сторон.

mahowik:

в крутых контроллерах типа немец, наза, а также от vis.asta так и сделано…

Тут по другому никак, более оперативных перемещения датчиков нет

mahowik:

он кстати про mpu6050

нет, он кстати про МПУ6000 на ПХ4м контроллере.

mahowik:

вот вариант красивый, но дороговато

Да, это сказка, то что нужно для конкурса (только тссс!), вот только стоил бы он баксов 70. А так я сам такой собиру, нужно только дождаться цифровой камеры от китайцких товарищей. Я видел где-то на датчике от мыши, просто туда был прикручен объектив от камеры.

oleg70:

Ломаю голову, как бы разделить линейные ускорения и крен у акселя… пока глухо.

“Академически” делается предельно просто: поворачиваем вектор акселя в мировую СК и вычитаем вектор нормали (0,0,-1) или (0,0,1) зависит от “полярности” акселя. В остатке все остальные ускорения 😁, в т.ч. ошибка горизонта. В моей вылолженной проге вроде есть функции в класcе INS.

Geniok
Alexsis1109:

да, согласен, РРМ это 1 канал. Но откуда его взять в обычном приемнике? я так понимаю, приемник ловит РРМ и разделяет его на все 7 ножек в отдельности…

Ааа, так у вас обычный приемник…
Пришла тут идея, если свести все каналы в один, соединив их между собой после выхода из приемника?
Так как импулься между собой разделены, то они перекрываться не будут, тогда все 7 каналов можно пустить по 1 проводу и подцепить к одному каналу таймера.

SergDoc
rual:

Это подстава, “прорекламируй” поставщика.

dx 😦 я писал раньше что посылка потерялась, а потом ещё не мог найти их (датчики) чтоб показать, а оказывается их уже нет, вот они и переслали новую с датчиками движения на пирометрах 😦 я один сегодня на работе уже сломал 😃 ладно фиг с ними, невелика потеря, сейчас уныло смотрю в код AeroQad сколько всего переделать то надо - ууу, никак не насмелюсь пока даже порты переписывать, а надо ещё gps от арду впихнуть (он там есть но надо правильно библиотеки подключать научится), удержание высоты сделано чисто на баро 😦 сонара нет, короче пахать и пахать, а хочется сделать cli, дабы не компилить постоянно для изменения аппарата или там подключения модулей (gps, сонар…) ну и всяких там расширеных возможностей по настройке, а без мативатора (платы) как-то ничего не хочется делать…

Geniok
SergDoc:

dx 😦

Один раз связывался с dx, больше я туда ни ногой…

SergDoc
Geniok:

Пришла тут идея, если свести все каналы в один, соединив их между собой?

это не панацея, и где гарантии что вы не убъёте приёмник, есть простое решение о котором уже писал, сам с такой штукой летаю, правда просто потому, что куча проводов задолбала…

Alexsis1109
SergDoc:

есть простое решение о котором уже писал

напишите еще раз, пожалуйста) а то искать замучаюсь…

SergDoc

на синенькое тыкаем и ждём… пост 49

mahowik
rual:

Я как раз пор это, ПИ-регулятор комплементарного филтра. Тут палка о двух концах, сделаешь большой И - будет перекомпенсация (горизонт будет крыльями махать), при маленьком И будет нескомпенсированый завал в одну из сторон.

вот вот! я когда с MARG-ом игрался, потратил кучу времени на настройку пид параметров компл. филтра (КФ), но так и не получил стабильного результата…
Но решение есть. Я както с Олегом (Syberian) общался по этому поводу и он наткнулся на туже проблему… Если пральна помню, то выкидываешь ПИД регуль из КФ и оставляешь, класический КФ, где дрейф быстрого/высокочастотного вектора гиры компенсируется НЧ вектором акселя:
Vg = Vg * (1 - 1/k) + Va * 1/k
где Vg - вектор гиры, Va - вектор акселя, на твой цикл (380 герц вроде) к=400…600

rual:

нет, он кстати про МПУ6000 на ПХ4м контроллере.

на его контроллере совсем не mpu6050/6000… а вот когда он писал порт на PX4, то сильно его ругал

Geniok
SergDoc:

это не панацея, и где гарантии что вы не убъёте приёмник,

Согласен, тут гарантий нет, есть 1 приемник, надо будет попробовать…

Alexsis1109
SergDoc:

на синенькое тыкаем и ждём… пост 49

49 пост в этой ветке??