Создание собственной системы стабилизации
Мне тут подумалось, а триммирование акселя не делал, у меня LSM по акселю тянет вперёд и вправо, в Акро - тоесть на ДУСах одних ни куда не ведёт, что означает что и аппарат не кривой и ц.т. правильный (можно почесаться если ветра особого нет), а вот при задействовании акселя после калибровки надо ещё триммировать - есть такая полезная штука в Вие, иначе полетит сам…
// **** PITCH & ROLL & YAW PID ****
dT = cycleTime * 1e-6;
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
Может еще с калманом поколдовать? Не бывает же что так сразу и все работет =)))
Там колдовать особо не с чем, весь АПИ
void iNEMO_AHRS_Init(iNEMO_SENSORDATA* pExtSensorData,
iNEMO_EULER_ANGLES* pExtAngle,
iNEMO_QUAT* pExtQuat);
/**
********************************************************************************
* @brief Update AHRS variables
@param pSensorData : Sensor Data
* @param pAngle : Roll, Pitch and Yaw Angles
* @param pQuat : Quaternion
* @retval None
* @par Functions called:
* @ref iNEMO_PropagateP
* @ref iNEMO_CalculateK
* @ref iNEMO_UpdateP
* @ref iNEMO_fMatCopy
********************************************************************************
*/
void iNEMO_AHRS_Update(iNEMO_SENSORDATA* pExtSensorData,
iNEMO_EULER_ANGLES* pExtAngle,
iNEMO_QUAT* pExtQuat);
void iNEMO_AHRS_DeInit(iNEMO_SENSORDATA* pSensorData,
iNEMO_EULER_ANGLES* pAngle,
iNEMO_QUAT* pQuat);
А до МАРГА что было ДСМ?
Нее 😃, с ДЦМ не разбирался и не жалею. Было собственное rcopen.com/forum/f134/topic224458/1048(более близкое к МАРГу как оказалось), но с принципиальными отличиями: марг в каждой итерации корректирует градиент по ДУСу, после чего добавляет его в кватернион бортовой СК, у меня же бортовая СК корретируется акселем и компасом, и тот МАРГ который на оф.сайте не корректируется по компасу в перевернутом состоянии ( это я опытным путем вычислил, т.к. в коде не совсем понятна математика коррекции, лишние члены алгебраически сокращенны для оптимизации).
Мне тут подумалось, а триммирование акселя не делал, у меня LSM по акселю тянет вперёд и вправо, в Акро - тоесть на ДУСах одних ни куда не ведёт, что означает что и аппарат не кривой и ц.т. правильный (можно почесаться если ветра особого нет), а вот при задействовании акселя после калибровки надо ещё триммировать - есть такая полезная штука в Вие, иначе полетит сам…
Сергей, имхо, тримирование это мертвому припарка, если усреднённый аксель при эволюциях (если исключить влияние центростремительного ускор) не даёт четкий вектор G, значит не какой фильтр уже не поможет. Значит что шум акселя не белый, и фильтр уведёт горизонт за неправильным средним, а положениее среднего будет зависить от многих факторов: оборотов движков, текущего положения Ск и т.п. Т.е. оттримируешь ты аксель только под определённые условия, и если они изменятся это смещение может сделать ещё хуже.
Триммирование не влияет на фильтр, только на ПИДы при условии, что используется аксель, а “ведёт” его скорей всего из-за нелинейности самого датчика, хотя можно попробовать сделать калибровку по всем осям? (и с холодильником 😃 )
Триммирование не влияет на фильтр, только на ПИДы при условии, что используется аксель, а “ведёт” его скорей всего из-за нелинейности самого датчика, хотя можно попробовать сделать калибровку по всем осям? (и с холодильником )
Видимо в нелинейности дело, а “триммирование” я руками делаю, жостик немного право держу. Ручное управление меня особо не волнует, а вот при автоматическом ИНС является единственным источником информации о положении, от этого положения отсчитываются и все прочие вектора, в т.ч. суммарные, ну и качество этой информации соответствует качеству определения положения в пространстве. Вот это и крайне печально… ((
Там колдовать особо не с чем, весь АПИ
Мне ДСМ тоже не очень понравился очень к вибрациям чувствителен. Хотя и было это на заре, сейчас то понимаю что если гомно на входе то как не фильтруй оно гомно на выходе и будет. Вопрос только в стабильности этого гомна =)) Есть тропа к хорошему описанию этой либы?
можно попробовать сделать калибровку по всем осям?
Минимум по всем осям
Мне ДСМ тоже не очень понравился очень к вибрациям чувствителен.
У меня есть старый арду, брал его для получения опыта и изучения ПО, покрутил его пару раз на столе и бросил в ящик, ибо горизонт при эволюциях абсолютно не адекватный. Возможно там надо было что то поднастроить, но не думаю что получилось бы сильно лучше. В итоге он у меня так и не летал, летаю только на своём. Щас думаю что с ним делать, даже если ПО улучшели с тех пор, то уменя нет времени и желания прикручивать его на раму и настроить для продажи…
Есть тропа к хорошему описанию этой либы?
Идёш по ссылке , скачиваешь сюит, устанавливаешь, в появившейся папке фирмваре находишь АХРС_ЛИБ, там две подпапки заголовки и исходники, в исходниках цепляешь к проекту сишный файл с единичкой на конце (с 2кой для РТОС), в загловках находишь либ, это и есть код екф, прицепляешь его к проекту на линковку, заголовочный файл включаешь в свой исходник инклюдом, в своём исходнике размещаешь структуры с типом из заголовка.
В итоге твоя прога должна инциализировать калман в начале, потом должна заполнять периодически структуру данных сенсоров, там же заполняешь ДТ, поле счётчика оставляешь нулём, если все данные поставляешь разом, если нет ( как у меня), при НЕнулевом счетчике коррекция не происходит ( у мну гира считается 400 раз без коррекции, а корекция происходит 50 раз по готовности акселя), вектор ДУСа подаёшь умноженный на ДТ, компас и аксель либо нормализуешь, либо приводишь к единице в покое, в выходных структурах получаешь углы и кватернион.
Да, ешё… Код под лицензией, посему для “чистоты” не забуть купить у Стшников соответсвующую девоплату и можешь пользоваться для своих исследований без ограничений )))
Либа писана под 103й проц и 303м не идёт, возможно пойдёт на 405/407, но не проверял. Всё что написано найдено экспериментально, по этому не буду клясться что всё именно так и есть, но у меня работает.
АХРС_ЛИБ уже нашел даже, примеры кое какие есть, пока первый уровень понятности достигнут осталось прикрутить посмотреть что да как, либа под 407 заведтся?
407 заведтся?
вот
возможно пойдёт на 405/407, но не проверял.
Чтоб сильно не вникать, подсунь ИНициализации обнуленные структуры под отладчиком (либо маячёк поставь после функции), если пройдёт, значит надежда есть, если зависнет - занчит сушите вёсла.
Да, ешё… Код под лицензией, посему для “чистоты” не забуть купить у Стшников соответсвующую девоплату и можешь пользоваться для своих исследований без ограничений )))
Вот блин, да я вообще преступником №1 стану, мало того что к ихним продуктам мне доступ заказан, так ещё и ПО трогать нельзя 😃
так ещё и ПО трогать нельзя
Ну зачем же так?😃 Нужно верить в доброту и справедливость, джентельмены верят другдругу на слово! Честным людям можно, но это уже офтоп, т.к. вопрос философский.
Да ещё, ковыряюсь с хренью аля CC3D - забыл блин про инициализацию УСБ, повесил пока на проводочках, всё равно на этой плате не летать (хотя кто его знает, она вместо мелкоплаты один в один становится), осталось MPUшку запаять и можно пробовать…
и вообще летать хочу, а тут то снег ,то дождь, то всё вместе, надо наверно к другу в гости на новоселье с коптером припереться, у него лестничная площядка Огромная, можно даже по кругу летать 😃 и от роботов ползающих его отучить, хай тоже летающие делает 😃
ковыряюсь с хренью аля CC3D
Там же вроде тоже екф?
Я хотел на МПУ посмотреть, но по всей видимости у меня проц дохлый (с прошлой платы выдран) даже бутлоадер не стартанул, думаю и датчики паять не буду…
Пока не получается подменить бутлоадер СС на СС3D …
Загнал бутлоадер, загнал прошивку, светодиод моргает правильно, коннекта нет 😦
Выпаял 25P16 а она не мычит не телится (на программаторе), другой нет, чувствую будет ещё один Wii…
чувствую будет ещё один Wii…
Сергей, а ЦЦ без неё жить не будет?
Вообщем наигрался я с стшным калманом, вещь сугубо демонстрационная, работет до 100 Гц (так написано в доке), демо проект 50 и у мну тоже 50… Память берёт динамическую, причём при каждой итерации забирает и отдаёт, на этом только скорость теряет, вообщем “полный Пэ”. Тем кто не наигрался могу посоветую расширить кучу не менее 1000 хексов, и это только если ваш код в ней не барахтается
Heap_Size EQU 0x00002000
AREA HEAP, NOINIT, READWRITE, ALIGN=3
__heap_base
Heap_Mem SPACE Heap_Size
__heap_limit
Сергей, а ЦЦ без неё жить не будет?
Там много впринципе на неё завязано, но есть порт на Naze32 (с MPU6050), так что думаю это возможно, да и много переделок по i2c значит всётаки плохо у ОР с этой шиной, я же думаю ради эксперимента зацепить SPI датчик в Wii, надо ж уже когда-то программировать учится 😃 (слова правильные нашел для изгнания OP-ских бутлоадеров и то хорошо,)
Вообщем наигрался я с стшным калманом
Мда печально, теперь понятно почему его никто не юзает =)
Не совсем, Олег Syberian Кальман использует rcopen.com/blogs/39391/15187
Он же не СТшный калман юзает