Создание собственной системы стабилизации
нахаляву досталась платка flip32 по случаю затестил горизонт cleanflight визуально вроде бы не валится в ускорениях
по случаю затестил горизонт cleanflight визуально вроде бы не валится в ускорениях
Каким образом испытывал? Каруселькой?
линейно перемещал вдоль поверхности
линейно перемещал вдоль поверхности
Линейно перемещать с ускорением долго нельзя, если постоянная времени коррекции большая, то завал будет не заметен. Лучше все таки крутить.
ну в арду даже при резких линейных перемещениях можно зафиксировать приличные отклонения
сложилось мнение что нужно интегральную составляющую корекции гироскопов добавлять а пропорциональную от акселя уменьшить
линейно перемещал вдоль поверхности
Только “каруселькой”…
сложилось мнение что нужно интегральную составляющую корекции гироскопов добавлять а пропорциональную от акселя уменьшить
У кого сложилось ? если можно поподробнее… как то, пока не встречал рабочего решения этой проблемы…
тема длинная но интересная 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?
так что мелкая мне вполне интересна ) За ссылки спасибо!
он кривой - мама не горюй - сразу проверяй
Так мне нужно только распределение ресурсов, вручную лень собирать. Решил немного поупражняться со скуки, ибо на крыло до вступления в силу закона вряд ли успею. Щас всё “пилю” на виртуалке, жаль адекватной модели коптера в ней нету…