Создание собственной системы стабилизации
тема длинная но интересная github.com/cleanflight/cleanflight/issues/791
сложилось мнение что нужно интегральную составляющую корекции гироскопов добавлять а пропорциональную от акселя уменьшить
Не понял, интегральную коррекцию гиры от акселя или чего?
У меня например аксель управляет ТОЛЬКО смещением ДУСа, т.е. исправляет интегральную ошибку, никакой пропорциональной составляющей.
смещение ДУС - через пропорциональный коэф между нормалью и горизонтом системы,
а постоянная составляющая - долгосрочное среднее значение от этой компенсации, источником которой является характерное уплывание гироскопов
в результате за счет уменьшения влияния от моментальной нормали можно достичь мньшей пропорции смещения чтоз пропорциональную и как следствие меньшее отклонение в перегрузках
Борис запилил raceflight под F4 проц. интересно а cleanflight-подобные прошивки пойдут на F4BY? просто по набору датчиков она более продвинута чем KISSFC)))
пойдут - драйвера для датчиков можно взять тут github.com/SergDoc/…/Release
Борис запилил raceflight под F4 проц
там всё есть только подправить прошивку под железо…
Публикую свой “велосипед”, может кому пригодиться.
А нельзя коротко пояснить - как ЭТО работает и что может ? (в чём “изюминка”, так сказать… )
А нельзя коротко пояснить
Коротко уже объяснил, это движок программных прерываний с приоритетами, по сути замена замена надстройка над штатной системой прерываний. Т.е. у меня есть набор функций-обработчиков с набором вычислений, которые должны быть выполнены по какому-то событию. Приведу свой пример, нужно сделать следующее:
Получить данные ДУС - Рассчитать новое положение ИНС - Рассчитать стабилизатор моментов - Рассчитать микшер - Вывалить в ШИМ.
Можно сделать так:
ISR_DUS()
{
Получить данные ДУС();
Рассчитать новое положение ИНС();
Рассчитать стабилизатор моментов();
Рассчитать микшер ();
Вывалить в ШИМ();
}
Всё хорошо, если больше ничего делать не нужно, но если еще параллельно идет опрос других устройств, то это недопустимо.
Поэтому лучше сделать так:
ISR_DUS()
{
Получить данные ДУС();
ВыставимФлаг(ДУСГотов);
}Loop{
if (ДУСГотов) {
Рассчитать новое положение ИНС();
Рассчитать стабилизатор моментов();
Рассчитать микшер ();
Вывалить в ШИМ();
}
}
Это способ разделить “железные” прерывания, с “быстрыми” обработчиками, и “медленные” обработчики-вычислители. Собственно для СТМ32 можно было использовать незадействованные “железные” вектора, и вызывать их поднятием бита в регистре прерываний соответствующего устройства. Но хотелось получить более универсальный, простой и управляемый механизм. Собственно в этом и изюминка.
)) уже не очень коротко. Работает просто, запрос вызывает планировщик и ставит флажок, вызывается прерывание, которое последовательно в соответствии с приоритетами выбирает флажки и запускает обработчики. Всё.
запрос вызывает планировщик
А “запрос” кто, когда и как формирует ?? Это “конечный автомат на прерываниях” получается что ль ??
А “запрос” кто, когда и как формирует ??
ROSA_CallEvent(0);
У меня запросы формируют обработчики “железных” прерываний, а дальше, допустим, обработчик расчета ИНС, вызывает обработчик микширов ШИМ, которые имееют более низкий приоритет относительно ИНС и стабилизатора. Так что если микшеры еще не досчитаются, а ДУС даст новые данные, то ИНС их не пропустит, прервет обсчет микширов.
Собственно я пробные проекты выкладывал, под АВРстудио можно “поиграться” в софтовом симуляторе не прошивая кристал, как впрочем и в проекте под СТМ32Ф1 и кейл
можно “поиграться” в софтовом симуляторе
крайний вопрос - реальные испытания были ?? А то у меня щас “КоОС” начала внезапно безбожно виснуть по непонятным причинам и надежды нарыть глюк мало… короче вопрос выкинуть ее нафиг остро назрел…
крайний вопрос - реальные испытания были ??
Да, “виртуалка” СТМ32Ф4 и полетник на Ф3. На Ф1 испытывал только на демопроекте, но с ним всё прощще, не нужно контекст ФПУ сохранять. Так что будет работать.
А то у меня щас “КоОС” начала внезапно безбожно виснуть по непонятным причинам и надежды нарыть глюк мало… короче вопрос выкинуть ее нафиг остро назрел…
Роса как бы не заменяет типовую ртос, она не может выполнять независимые ЦИКЛИЧЕСКИЕ задачи (хотя возможно зациклить две задачи, которые будут вызывать друг друга).
2oleg70
Дополню кусками из своего проекта:
hw_config.с - массив ссылок на обработчики и массив приоритетов (макс 254).
const Event_TypeDef _Event_List[] = {
DUSCalc,
AccCalc,
Rdy_PWM1,
Rdy_PWM2,
MagmCalc,
BaroCalc,
GNSSCalc,
Rdy_RC,
FrSky_Process,
};
const Event_Priorety_TypeDef _Ev_Prior[] = {
0,1,2,2,3,4,4,5,6 };
обработчики вызываются функцией ROSA_CallEvent(<номер обработчика в массиве 0-254>); чтоб не запоминать
вносим в хедер hw_config.h перечислитель обработчиков,
typedef enum {
evDUS,
evAcc,
evPWM1,
evPWM2,
evMag,
evBaro,
evGNSS,
evRC,
evFRSKY,
evRXTerm,
} ROSA_Events;
после чего используем вместо номеров названия событий 😉
/*--------- вызовы методов по готовности данных датчиков ---------- */
/* Аксель */
void AccDataRdy(void)
{
AP.ГотовДанныхАкселя();
ROSA_CallEvent(evAcc);
#ifndef ACC_FILTER
Mag_Zapros();
#endif
}
void AccCalc(void)
{
AP.РасчетПоАкселю();
}
/* ДУС */
void DUSDataRdy(void)
{
AP.ГотовДанныхДУС();
ROSA_CallEvent(evDUS);
}
void DUSCalc(void)
{
AP.РасчетПоДУС();
ROSA_CallEvent(evPWM1); /* обновим приводы */
}
/* магнитометр */
void MagmDataRdy(void)
{
AP.ГотовДанныхМагнит();
AP.инс.Барометр.ЗапросДанных();
ROSA_CallEvent(evMag);
}
void MagmCalc(void)
{
AP.РасчетПоМагнит();
}
/* барометр */
void BaroDataRdy(void)
{
AP.ГотовДанныхБаро();
ROSA_CallEvent(evBaro);
}
void BaroCalc(void)
{
AP.РасчетПоБаро();
}
/* ГНСС */
void GNSSCalc(void)
{
glns.FrameRdy();
}
/* ППМ получен*/
void Rdy_RC(void)
{
ppm.rasch();
}
/* вывод кадра ШИМ завершен */
void Rdy_PWM1(void)
{
/* суммируем управление на приводы */
mixINS1.rasch(AP.инс.Крен*1024.0f,AP.инс.Тангаж*1024.0f,AP.инс.Рыск*1024.0f,AP.цУпрТяга);
mixPID1.rasch(AP.цУпрКрен,AP.цУпрТанг,AP.цУпрРыск,AP.цУпрТяга);
ROSA_CallEvent(evPWM2);
}
/* вывод кадра ШИМ завершен */
void Rdy_PWM2(void)
{
/* суммируем управление на приводы */
mixINS2.rasch(AP.инс.Крен*1024.0f,AP.инс.Тангаж*1024.0f,AP.инс.Рыск*1024.0f,AP.инс.векРасстоянияИНС.z);
mixPID2.rasch(AP.цУпрКрен,AP.цУпрТанг,AP.цУпрРыск,AP.цУпрТяга);
}
В консоли для контроля пропусков обработки использую :
const char* str_hndlr[] = { "ДУС","Аксель","ШИМ1","ШИМ2","Магн.","Баро","ГНСС","Р/У","Телем.","Связь"};
.........
t = millis()+1002;
while (getchar() == EOF){
if (millis()-t > 1000){
printf("\r\nСистемное время: %d",millis());
printf("\r\nОбработчик \tСост. \tСч.пропусков\r\n");
for (Event_Number_TypeDef i = 0;i<Event_Count;i++)
{
//char c*[] = {"Стп","Раб","Жд"};
char s;
switch(ROSA_GetStatEvent(i)){
case EVENT_WAIT: s= 'W'; break;
case EVENT_READY:
case EVENT_RUN: s= 'R'; break;
case EVENT_BLOCK:
case EVENT_BLOCK_WAIT: s= 'B'; break;
default: s= 'S'; break;
}
ROSA_InterruptDisable();
int c = ROSA_GetFailCount(i);
ROSA_ResetFailCount(i);
ROSA_InterruptEnable();
printf("%s:\t\t%c\t%d\r\n",str_hndlr[i],s,c);
}
printf("Обработчиков в ожидании: %d\r\n",ROSA_GetWaitingEvent());
t = millis();
}
}
Вывод:
Системное время: 412922264
Обработчик Сост. Сч.пропусков
ДУС: S 0
Аксель: S 0
ШИМ1: S 0
ШИМ2: S 0
Магн.: S 0
Баро: S 0
ГНСС: S 0
Р/У: S 0
Телем.: S 40
Обработчиков в ожидании: 0
2SergDoc Сергей, поделись кодом для настройки МПУ9150, ну и особенностями, если не жалко )
Решил тут попробовать новый кодогенератор для СТМ32, низы под ТауЛавс Спарки переделываю, ибо на эроплан у меня Ф4Бы не лезет.
низы под ТауЛавс Спарки переделываю
мелкую не дождался (
дык у таулабсов и возьми - там довольно красиво драйвер расписан - спарки 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х мерном пространстве не даст никогда! Ну разве что в случае вращения идеально вокруг одной оси. Интегрирование необходимо выполнять через матрицу поворота или кватернион.