Создание собственной системы стабилизации
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х мерном пространстве не даст никогда! Ну разве что в случае вращения идеально вокруг одной оси. Интегрирование необходимо выполнять через матрицу поворота или кватернион.
Ошибки нет. Также есть влияние оси z на y. Заметил что коптер при развороте вокруг оси уносит в сторону. Положил плату на стол. Углы по акселю и гирам 0.
У вас нерабочий алгоритм определения ориентации, а вы уже коптером рулите??? Есть же методика разработки, разбиение на изолированные функции. Такой подход как у вас, когда по наблюдению за работой сложной системы, пытаться найти ошибку в ее подсистеме, не даст хороших результатов.
У вас нерабочий алгоритм определения ориентации, а вы уже коптером рулите??? .
Ошибочное мнение. Коптер отлично держит горизонт, получше всяких готовых прошивок . Могу продемонстрировать , живем в одном городе.
Для полета задаем угол по оси , с пульта. Коптер наклоняется и автоматом добавляет газ обратно пропорционально COS угла наклона.
Доб_газ= 70/(COS(угол_наклона)) -70 , например при 30 град. добавляем 11 единиц шим
Также есть автомат увеличения газа по измерению напряжения батареи , при снижении напряжения на 2 V добавляется примерно 20 единиц. У меня был тока один недочет. При горизонтальном полете ,когда сбрасывал угол , коптер уходил вверх метров на 5-7. На регуляторах стояла прошивка с неработающим торможением Damped и при выравнивании коптера 1-1.5 сек оставались повышенные обороты, больше чем надо для висения.
Прошил более старой версией и торможение заработало. Пока не проверил , погода у нас фиговая. Ну а вообще управление на 3DR Radio модемах.
Пульт свой( модернизированная Turnigy 9x ), протокол передачи свой. На пульт приходит информация с коптера ( GPS данные, напряжение батареи) и выводится на олед дисплей 20х4 .
При разряде батареи на коптере , в пульте пищит зумер.
Но стоит наклонять плату постепенно
меняя ось , как бы по сфере но без вращения по оси z , то гиры начинают врать. Угол уходит прилично на 10-20 град. Это влияние осей друг на друга ?
А мне кажется, что ошибка при медленном вращении платы это результат банального шума гиры (так и должно быть), а вот как раз коррекция по акселю для устранения этого шума и нужна, НО не раз в пять секунд !, а каждую итерацию с частотой расчета ИМУ… (А формула связывающая разные оси через линейный коэф. приведет к непредсказуемым результатам на разных режимах полета…)
а вот как раз коррекция по акселю для устранения этого шума и нужна, НО не раз в пять секунд !, а каждую итерацию с частотой расчета ИМУ…
Так и происходит формула :
угол_гир_х = угол_гир_х * 0.999 + угол_акселя_х * 0.001
Каждое измерение приближает значение угла гиры к углу акселя. Если у меня 380 измерений в сек. то для коррекции за 3 сек надо коэф. 1/(3*380 ) примерно.
Никакого шума гир нет. Я калибрую их при включении питания. Делаю 4000 измерений , нахожу среднее для каждой оси , и этот коэф. вычитаю при каждом считывании данных.
Тут все просто. Если мы по Х наклоним на 45 градусов , а потом по Y на 45гр.( относительно оси X ) то для оси земли это уже не 45 гр. Более лучше это понять если наклоним в право или лево на 90 градусов. Тогда ось Y вообще теряет смысл как ось , которая показывает наклон относительно земли., она превращается в Z. Но для отличной стабилизации хватает по PID регулятору на каждую ось.
угол_гир_х = угол_гир_х * 0.999 + угол_акселя_х * 0.001
Всё правильно, если так, и больше ничего не нужно… (старый добрый комплементарный фильтр") ищите косяк в другом месте… начиная с “сырых” данных датчиков и до выходных показаний PIDa…
Так уже сказали где косяк:
Так и происходит формула : угол_гир_х = угол_гир_х * 0.999 + угол_акселя_х * 0.001
- нельзя интегрировать гироскоп раздельно по осям. К примеру, наклони его на 90 вбок, поверни по азимуту на 90 и верни обратно. Проинтегрировался угол по одной оси, а реально сенсор развернулся по другой.
и больше ничего не нужно…
Да, еще матрицы или кватернионы естественно… ))