Создание собственной системы стабилизации
Тогда что ж мы рассчитываем? , подаем на ПИД не их ли ?
Я то думал что как раз на ПИД подаются углы “уставки” с RC приемника и углы текущего положения “тела”, а на выходе получаем заветное значение управляющего воздействия на моторы…
вы описали частный случай. для режима удержания, например, вобще не нужны ни кватернионы ни матрицы, стики задают желаемую угловую скорость. управление аппаратом не ограничивается висящими в горизонте коптерами 😉
а кватернион (либо матрица) всего лишь более компактный (либо менее ресурсоемкий) метод хранения текущего положения.
по моему вот для начала что мне надо переписать под себя:
Сергей, найди функцию которая получает в параметре тип stm32_pin_info , вот она должна настраивать порты и перефирию.
Начал с нуля копать родные библиотеки и понял, что написаны они довольно в произвольной форме даже на уровне “дефайнов”, и нахаляву взять кусок кода из одного проекта и присобачить к своему - проблема.
Не факт, мне очень помогли с Ф1 на Ф3 перелезть.
А теперь по теме ветки: не кажется ли Вам странным что при использовании кватернионов мы на входе (функции расчета) имеем углы с гиры и акса и на выходе тоже получаем углы…??? В чем смысл то ?
Не так, ДУС (гира) даёт мгновенную угловую скорость, аксель дает вектор суммы всех ускорений.
Углы ДУСа и углы на выходе кватерниона не одно и тоже, представьте что самоль летит левым крылом к земле и перекладывает рули высоты на кабрирование, что измениться тангаж или курс?
вы описали частный случай. для режима удержания, например, вобще не нужны ни кватернионы ни матрицы, стики задают желаемую угловую скорость. управление аппаратом не ограничивается висящими в горизонте коптерами
Всё правильно, вот только в этом этом режиме АППАРАТ не знает своё положение в пространстве, а просто пытается удерживать заданные Р/У угловые скорости.
найди функцию которая получает в параметре тип stm32_pin_info , вот она должна настраивать порты и перефирию.
она там же, кстати не смотрел раньше есть ли такая беда в кеил, но в эклипсе - выделяешь функцию или дефайн какой-нибудь, жмёшь f3 и опля нашлась 😃
Всё правильно, вот только в этом этом режиме АППАРАТ не знает своё положение в пространстве,.
Предлагаю тогда поделиться общими соображениями об организации основного рабочего цикла программы стабилизации по типу:
0.чтение Р/У
1.чтение ДУС.
2.Чтение Акселя.
3.Усреднение ДУС (по желанию)
4.Вычисление углов
и т.д. ,
а то кажется есть разночтения на этот счет.
в этом этом режиме АППАРАТ не знает своё положение в пространстве
так ему и не нужно его знать )
общими соображениями об организации основного рабочего цикла программы стабилизации по типу
вы какие величины стабилизировать хотите и на каком аппарате? а то пример только один напрашивается:
- читаем датчики
- ничего не делаем
- конец цикла
Предлагаю тогда поделиться общими соображениями об организации основного рабочего цикла программы стабилизации по типу:
у меня в основном цикле только терминальный ввод-вывод, всё остальное вычисляется “параллельно” в прерываниях:
- готовность ДУС->чтение ДУС -> вычисление текущего положения (цикл 400Гц)
- готовность акселя -> чтение акселя -> чтение компаса -> коррекция положения по акселю и компасу
- прерывание от ШИМ-> вычисление разностей между задающим и текущим положением -> вычисление ПИД стабилизатора-> расшивка управления на геометрию рамы
- прерывание от входного ППМ-> чтение получение параметров от каналов РУ -> формирование задающего кватерниона\
И ещё несколько менее приорететных потоков (АЦП, УСАРТ и пр.).
Поправьте если не прав:
Коррекция положения происходит все же по акселю (компас опционально) и при ручном управлении скорректированная ориентация рамы стремится к ориентации заданной Р/У (приведенной например к диапазону 0-45 грд. по крену и тангажу)
И еще, Александр, как реализовали оцифровку ППМ (аппаратно, программно)?? Хочу попробовать на Capture от таймеров.
так примерно если по простому habrahabr.ru/post/118192/ и ПИД тоже доспупно we.easyelectronics.ru/…/pid-regulyatory--dlya-chay…
Коррекция положения происходит все же по акселю (компас опционально) и при ручном управлении скорректированная ориентация рамы стремится к ориентации заданной Р/У (приведенной например к диапазону 0-45 грд. по крену и тангажу)
Да. Да, я не делал отдельный АКРО режим , смысла в этом не увидел, аппрат всегда знает своё положение (в рамках допустимой ошибки 😃), при желании можно дать РУ вращать заданое положение по всем осям на 360 градусов.
как реализовали оцифровку ППМ (аппаратно, программно)?? Хочу попробовать на Capture от таймеров.
Всё работает в прерывании, в режиме захвата таймером
uint16_t ppm1_buf[4]; /* буфер для значений каналов ППМ */
uint8_t ppm1_nimp[4]; /* значения счетчиков пропусков ППМ */
/* прерывание по ППМ сигналу */
void TIM_PPM1_IRQHandler(void) /* ППМ 1-4 */
{
static uint16_t ccr_buf[4]; /* буфер для временного хранения начала импульса */
uint16_t port = TIM_PPM1->CCER;
uint16_t ccr;
if (TIM_GetFlagStatus(TIM_PPM1,TIM_FLAG_CC1) == SET) /* 1 канал */
{
ccr = TIM_GetCapture1(TIM_PPM1);
if((port & TIM_CCER_CC1P) == 0) {
ccr_buf[0] = ccr;
TIM_OC1PolarityConfig(TIM_PPM1,TIM_ICPolarity_Falling);
} else {
TIM_OC1PolarityConfig(TIM_PPM1,TIM_ICPolarity_Rising);
if (TIM_GetFlagStatus(TIM_PPM1,TIM_FLAG_CC1OF) == RESET)
ppm1_buf[0] = ccr - ccr_buf[0];
else TIM_ClearFlag(TIM_PPM1,TIM_FLAG_CC1OF);
}
ppm1_nimp[0] = 0;
TIM_ClearITPendingBit(TIM_PPM1, TIM_IT_CC1);
}
if (TIM_GetFlagStatus(TIM_PPM1,TIM_FLAG_CC2) == SET) /* 2 канал */
{
ccr = TIM_GetCapture2(TIM_PPM1);
if ((port & TIM_CCER_CC2P) == 0) {
ccr_buf[1] = ccr;
TIM_OC2PolarityConfig(TIM_PPM1,TIM_ICPolarity_Falling);
} else {
TIM_OC2PolarityConfig(TIM_PPM1,TIM_ICPolarity_Rising);
if (TIM_GetFlagStatus(TIM_PPM1,TIM_FLAG_CC2OF) == RESET)
ppm1_buf[1] = ccr - ccr_buf[1];
else TIM_ClearFlag(TIM_PPM1,TIM_FLAG_CC2OF);
}
ppm1_nimp[1] = 0;
TIM_ClearITPendingBit(TIM_PPM1, TIM_IT_CC2);
}
if (TIM_GetFlagStatus(TIM_PPM1,TIM_FLAG_CC3) == SET) /* 3 канал */
{
ccr = TIM_GetCapture3(TIM_PPM1);
if((port & TIM_CCER_CC3P) == 0) {
ccr_buf[2] = ccr;
TIM_OC3PolarityConfig(TIM_PPM1,TIM_ICPolarity_Falling);
} else {
TIM_OC3PolarityConfig(TIM_PPM1,TIM_ICPolarity_Rising);
if (TIM_GetFlagStatus(TIM_PPM1,TIM_FLAG_CC3OF) == RESET)
ppm1_buf[2] = ccr - ccr_buf[2];
else TIM_ClearFlag(TIM_PPM1,TIM_FLAG_CC3OF);
}
ppm1_nimp[2] = 0;
TIM_ClearITPendingBit(TIM_PPM1, TIM_IT_CC3);
}
if (TIM_GetFlagStatus(TIM_PPM1,TIM_FLAG_CC4) == SET) /* 4 канал */
{
ccr = TIM_GetCapture4(TIM_PPM1);
if((port & TIM_CCER_CC4P) == 0) {
ccr_buf[3] = ccr;
TIM_OC4PolarityConfig(TIM_PPM1,TIM_ICPolarity_Falling);
} else {
TIM_OC4PolarityConfig(TIM_PPM1,TIM_ICPolarity_Rising);
if (TIM_GetFlagStatus(TIM_PPM1,TIM_FLAG_CC4OF) == RESET)
ppm1_buf[3] = ccr - ccr_buf[3];
else TIM_ClearFlag(TIM_PPM1,TIM_FLAG_CC4OF);
}
ppm1_nimp[3] = 0;
TIM_ClearITPendingBit(TIM_PPM1, TIM_IT_CC4);
}
if (TIM_GetFlagStatus(TIM_PPM1,TIM_FLAG_Update) == SET)
{
if (ppm1_nimp[0]< PPM_NCNT) ppm1_nimp[0]++;
if (ppm1_nimp[1]< PPM_NCNT) ppm1_nimp[1]++;
if (ppm1_nimp[2]< PPM_NCNT) ppm1_nimp[2]++;
if (ppm1_nimp[3]< PPM_NCNT) ppm1_nimp[3]++;
Rdy_PPM1();
TIM_ClearITPendingBit(TIM_PPM1, TIM_IT_Update);
}
}
Охеренный перевод, особенно точность терминов радует)
Блин совсем запутался, порты выведеные обозначены как D-тра-ля-ля, а порты не выведеные обозначены цыфирками - а вот где они так обозначаются, наверно одним разрабам известно. Что пишут:
To add a new board type, add a new pair of files to
/wirish/boards/, update the section below with a new “BOARD” type,
and update /wirish/rules.mk to include your boards/your_board.cpp
file in the top-level Makefile build.
тоесть переписать всё под свою плату(создать новую) но всё равно мне надо знать где прячутся порты под цыфрами…
ага, пофиг как, все можно просто цыфирками обозвать или d+цыфирка - без разницы и убрать ненужные добавить нужные, эх понеслась душа в рай 😃
вот как оно это делается:
typedef struct stm32_pin_info {
gpio_dev *gpio_device; /**< Maple pin's GPIO device */
timer_dev *timer_device; /**< Pin's timer device, if any. */
adc_dev* adc_device; /**< ADC device, if any. */
uint8 gpio_bit; /**< Pin's GPIO port bit. */
uint8 timer_channel; /**< Timer channel, or 0 if none. */
uint8 adc_channel; /**< Pin ADC channel, or ADCx if none. */
} stm32_pin_info;
т. е. ненужные заменить нужными, но не удалять, а то нумерация испортится…
d+цифирка эта маплавское (аля ардуинское) обозачение, полагаю такая ботва без маловского ИДЕ реткосный геморой, если только из ИДЕ соответсвующую функцию выдрать.
Добрый день, друзья! На сколько я понимаю, все автопилоты и системы стабилизации с ними, работают по следующему алгоритму: опрос РРМ сигнала с приемника аппаратуры, опрос всех датчиков, коррекция РРМ сигнала и передача его на двигатели. Поскольку изачально частота обновления РРМ сигнала 50Гц (в футабе 7 вроде 68Гц), то с учетом потери времени на опрос датчиков, на выходе мы будем иметь как максимум 25Гц обновления РРМ сигнала с приемника, а то и меньше. Я все правильно понимаю?
Радио задаёт либо угол наклона, либо угловую скорость, ну и т.д. - ну в общем знать, что пилот от аппарата хочет, а к обсчёту ИНС или как это ИМУ, ну как кому больше нравится, никакого отношения не имеет, это КУК там да нопами загружен - ждёт прерывания со входа, потом опрашивает гиры и высчитывает выходы на моторы 50Гц ему за глаза для обсчёта…
d+цифирка эта маплавское (аля ардуинское) обозачение,
да вот с этим и воюю, зато порты перепишу без особых напрягов под свою плату, хотя г. редкостное, но на начальной стадии от этого не смогу отказаться - потренируюсь на том, что есть… а да онож можно писать как уже говорил, и просто цыфирку, и Dцыфирка, и порт сам прописать аля PAкакойнибудь - всё переварит…
50Гц ему за глаза для обсчёта…
но важно как можно быстрее передать изменение РРМ сигнала с радио на выход платы. В идеальном случае, если просто транслировать сигнал с радио на выход через плату стабилизации, то все изменения можно передавать так же с частотой 50Гц. но стоит только отвлечься на обработку датчиков, так сразу как минимум одну пачку импульсов РРМ придется пропустить, соответственно на выход передавать изменение РРМ через раз, с частотой 25Гц. просто хочется понять, это нормальное явление? и с какой минимальной частотой можно передавать изменения РРМ с радио на выход?
относительно КУКа - простой самый, работает на 8-ми МГц. и большую часть времени, между прерываниями курит:
void delay_us(uint8_t time) /* time delay for us */
{
while(time--)
{
asm volatile ("NOP"); asm volatile ("NOP");
asm volatile ("NOP"); asm volatile ("NOP");
asm volatile ("NOP"); asm volatile ("NOP");
asm volatile ("NOP");
}
}
гы пока копался нашел радио (всмысле входы)
//PIN assignment
#define THROTTLEPIN 2
#define ROLLPIN 4
#define PITCHPIN 5
#define YAWPIN 6
#define AUX1PIN 7
// alias for RC
#define ROLL 4
#define PITCH 2
#define YAW 3
#define THROTTLE 1
#define MODE 5
#define AUX1 6
#define AUX2 7
#define AUX3 8
😃
APM_RC_MP32.h - если кому интересно, выходы там же 😃
а да онож можно писать как уже говорил, и просто цыфирку, и Dцыфирка, и порт сам прописать аля PAкакойнибудь - всё переварит…
Я бы всёж посмотрел на ту функцию, которая данный тип переваривает.
Поскольку изачально частота обновления РРМ сигнала 50Гц (в футабе 7 вроде 68Гц), то с учетом потери времени на опрос датчиков, на выходе мы будем иметь как максимум 25Гц обновления РРМ сигнала с приемника, а то и меньше. Я все правильно понимаю?
Ну это ведь как напишите код, если тупить в ожидании окончания чтения всех каналов РУ, то может обновление выхода и реже 25Гц будет. У меня эти процессы друг с другом не связаны, обновление горизонта 400Гц с запуском по готовности ДУС, обновление ПИДов и ШИМ с запуском от таймера ШИМ 400Гц, получение ППМ по фронту-спаду на входах захвата таймера (примерно 50Гц), зависит от сигнала. И абсолютно не важно сколько раз обновляется ППМ, да хоть 1 раз в секунду, обновление выхода всё равно будет 400Гц (т.е. каждый период ШИМ) и стабилизация будет работать с той же частотой, а вот задача на углы будет менятся раз в секунду.
Я бы всёж посмотрел на ту функцию, которая данный тип переваривает.
В ардуине, на сколько помню, тоже можно порты на прямую обзывать PA… PB… но вот то, что цифирка или Dцифирка меня тоже убила 😃
И абсолютно не важно сколько раз обновляется ППМ, да хоть 1 раз в секунду, обновление выхода всё равно будет 400Гц (т.е. каждый период ШИМ) и стабилизация будет работать с той же частотой, а вот задача на углы будет менятся раз в секунду.
можно вот тут поподробнее… не совсем понял как Вы это реализуете. Расскажу как я это делаю, а Вы скажите что не так. Имеется обычный приемник от Futtaba 7. У него на выходе, ШИМ каждого канала обновляется примерно с частой 68ГЦ. И имеются гироскопы. И вот какой алгоритм программы: захват “1” ШИМа первого канала приемника - подсчет его длительности, как только на первом канале “0”, переходим к подсчету длительности второго канала, и так далее все каналы по подряд. Потом опрос гироскопа - коррекция ШИМа и передача РРМ сигнала на двигатели. Получается, что когда я перехожу к опросу гироскопа, я пропускаю следующую пачку РРМ сигнала которая может уже быть с другими длительностями ШИМов каналов. Вот главная проблема и как ее решить? Или например время получения данных с барометра BMP085 в самом точном режиме составляет 30мс. А ШИМ с приемника обовляется с частотой 68Гц, т.е. примерно 15мс. Получается пока я буду сидеть в цикле опроса барометра, успеет пройти аж две новых пачки РРМ сигнала?((((
если тупить в ожидании окончания чтения всех каналов РУ, то может обновление выхода и реже 25Гц будет.
разве можно между чтением двух каналов РУ еще что-то делать? допустим если первый канал в самом минимуме а это 1мс, то до второго канала остается время не более 1мс, это в лучшем случае, а то и меньше. Вы хотите сказать что в это время нужно считывать, домустим, инфу с датчиков? разве спасет 1мс? или я не так понял?