Создание собственной системы стабилизации
не Алексей, фильтруется оно
//ITG3200 and ITG3205 Gyro LPF setting
#if defined(ITG3200_LPF_256HZ) || defined(ITG3200_LPF_188HZ) || defined(ITG3200_LPF_98HZ) || defined(ITG3200_LPF_42HZ) || defined(ITG3200_LPF_20HZ) || defined(ITG3200_LPF_10HZ)
#if defined(ITG3200_LPF_256HZ)
#define ITG3200_SMPLRT_DIV 0 //8000Hz
#define ITG3200_DLPF_CFG 0
#endif
#if defined(ITG3200_LPF_188HZ)
#define ITG3200_SMPLRT_DIV 0 //1000Hz
#define ITG3200_DLPF_CFG 1
#endif
#if defined(ITG3200_LPF_98HZ)
#define ITG3200_SMPLRT_DIV 0
#define ITG3200_DLPF_CFG 2
#endif
#if defined(ITG3200_LPF_42HZ)
#define ITG3200_SMPLRT_DIV 0
#define ITG3200_DLPF_CFG 3
#endif
#if defined(ITG3200_LPF_20HZ)
#define ITG3200_SMPLRT_DIV 0
#define ITG3200_DLPF_CFG 4
#endif
#if defined(ITG3200_LPF_10HZ)
#define ITG3200_SMPLRT_DIV 0
#define ITG3200_DLPF_CFG 5
#endif
т.е. если Sampling Rate = F = 1KHz то на выходе LPF_188HZ - что вий вполне успеет прочитать…
на f4 реально взять с датчика по полной…
Кстати, а чё все к фильтру привязались то?
Вот именно эту мысль я и толкаю… Работы в плане доводки датчиков + ПИД и алгоритмов работы с ними не початый край, а этот аспект как то все “перескакивают” и сразу в “калманы”…
Из своих наблюдений (в теме меньше года) могу сообщить:
Попытки обработать данные с гиры/акселя до подачи в ИМУ особых реальных улучшений не дали…(применял усреднение, “медиану” без разн…) только тормозили цикл, ради смеха брал усреднение аж по 70 выборкам - бесполезно…
Правда оговорюсь сразу - это злополучный LSM, гира от F3 вполне адекватная. Таким образом шумы акселя + гира при запущенных моторах не позволяют увеличить коэф. усиления (а равно о быстродействие) ИМУ и повысить качество стабилизации аппарата…
Ставил свою платку на самолет (схема классика), - да - картина лучше, но при “кривом” винте (трава налипла), один раз была явная потеря контроля, чего уж говорить о четырех моторах, создающих в рабочем диапазоне МАССУ резонансов !
Я вот ПИДы вообще не меняю,
кстати, да, ПИД скорее сильней привязан непосредственно к свойствам датчиков + настройки их усиления…
т.е. если Sampling Rate = F = 1KHz то на выходе LPF_188HZ - что вий вполне успеет прочитать… на f4 реально взять с датчика по полной…
Честно говоря, у меня нету всего кода, т.к. это в Сашиной прошивке (mahowii), но строка которая должна была залить тот самый делитель, закоменчена:
// i2c_writeReg(ITG3200_ADDRESS, 0x15, ITG3200_SMPLRT_DIV); //register: Sample Rate Divider -- default value = 0: OK
А по умолчанию, делитель = 0, т.е. 1KHz
P.S. Ха, так в твоем куске кода, делитель всегда равен 0 ))))
так смотри ITG3200_SMPLRT_DIV 0 и ITG3200_DLPF_CFG 0 это максимум датчика т.е. 8КГц считывание и 256Гц на выходе это при времени цикла 3906 вполне успеваемая цыфра 😃 я не работал с этим датчиком, но так понимаю, эти два регистра связаны между собой…
Калманов
Калман
калман
калмана
Калмана
Калмана
😃)))
Напомню 7-го числа начался отличный курс по цифровой обработке сигналов www.coursera.org/course/dsp я записался, но пока из за проблем со временем не начал…
далее через 2 дня начнется курс Machine Learning от Стэнфорда! www.coursera.org/course/ml
- кореш мой проходит сейчас Artificial Intelligence for Robotics www.udacity.com/course/cs373
Очень доволен, что с курсом есть необходимый материал по азам линейной алгебры. И кстати там есть урок по Калману. Друг сказал что сперва не въехал, а потом допер и все как на ладони стало. Курс этот от фрица, препода из Стэнфорда, который участвует в проекте по автономным гугломобилям.
Это осеннее обострение, в аналы(слово то какое) медицины войдёт как Калманомания - похоже оно заразно 😃
проходит сейчас Artificial Intelligence for Robotics www.udacity.com/course/cs373
Очень доволен
Я в прошлом году или позапрошлом окончил его. Отличный курс, но потом наступает некоторое разочарование, когда начинаешь реально что то делать то оказываться что тебе рассказали лишь введение, и что бы реально применить нужно самостоятельно перелопатить гору информации. может это и к лучшему. Для себя вынес полезного это алгоритм Particle Filter, классная вещь Калман не нужен 😃
- Цикл жесткий - магическое число - регули на 400, значит 2500 (ну можно с вариациями на тему)
- Фильтр даёт ПРЕДСКАЗАНИЕ положения в момент записи в таймеры, а не в момент расчёта
- ПИД обязан вычислить и записать в таймеры нужное ВОВРЕМЯ дабы небыло проскоков периода.
Это так в базефлайте? И вие?
Вообще тут надо отделить мух от котлет:
Расчеты ИНС и ПИДов должны быть в ритме выборки датчиков, а а микшер и управление в ритме формирования ШИМ. Т.е. по готовности датчика выполняются вычисления позиционирования и ошибка от заданного положения, и это происходит ДО следующего отсчета датчиков, а миксер вычисляется от выхода ПИДов по готовности таймера к отработке следующего периода. Итого получается отставание: 1 период отсчета ДУС (F3discovery - 2.63 мсек,F4BY - 4 мсек.) , и до выхода в ШИМ 1 период таймера (т.е. от 2.5 до 20 мсек), т.е. при 400Гц ШИМ = 5(7) мсек, при 50Гц шим 23-24 мсек.
Отсюда вытекают две вещи:
внутренние фильтры датчиков придётся отключать! дабы повысить скорость считывания и не ловить старые данные - они будут не верны
Не понял при чем тут внутренние фильтры? Разница между внутренним и внешним только в месте размещения.
А задержка зависит больше от частоты ШИМ, ну а далее регуль и ВМГ.
Сегодня квадру чуть в космос не отправил 😃, поставил новую батарею 4000мАч на 12х пропах, а настройки оставил старые мингаз 1200, средняя точка 1300. Рванул вверх, а вниз спускаться отказался. Хотел сначала ронять, пока он из радиовидимости не свалил, но потом я его большими кренами назад-вперёд снизил постепенно, а там батарея начала сдавать помалу. Дальше летал практически на нулевом газу.
Не понял при чем тут внутренние фильтры? Разница между внутренним и внешним только в месте размещения.
А задержка зависит больше от частоты ШИМ, ну а далее регуль и ВМГ.
Надо повысить частоту считывания, ибо смысл читать гиры на 800Гц если они выдают показания на 42Гц - и что мы будем читать старые данные? зачем? смысл от 400-490 герцового шима? если изменения вносятся 42 раза в секунду (это я из примера готовности датчиков) скажем для ПИДа хорошо сглаженные двумя фильтрами данные, но для бесплатформенной ИНС помоему не очень?
ДУС (F3discovery - 2.63 мсек,F4BY - 4 мсек.) , и до выхода в ШИМ 1 период таймера (т.е. от 2.5 до 20 мсек), т.е. при 400Гц ШИМ = 5(7) мсек, при 50Гц шим 23-24 мсек.
я вечно путаюсь в микро и мили - мсек-это что?
Надо повысить частоту считывания, ибо смысл читать гиры на 800Гц если они выдают показания на 42Гц - и что мы будем читать старые данные? зачем? смысл от 400-490 герцового шима?
Полностью согласен…
Отсюда же и имею очень скептическое отношение к разного рода “крутым” регулям по I2C, там где надо и не надо… В корень надо смотреть… и иметь минимальный набор “инструментов” для реальной оценки эффективности отдельных узлов аппарата “снизу-вверх”, а не гнаться слепо за “понтами” которые присутствуют зачастую - номинально.
Надо повысить частоту считывания, ибо смысл читать гиры на 800Гц если они выдают показания на 42Гц - и что мы будем читать старые данные? зачем?
Кто сказал, что реальная частота дискретизации 42Гц? Я пока уверен, что СТшные ДУСы вполне адекватно отражают повороты на установленной частоте.Что касаемо МПУ, то там ещё проще, ДУС работает на 1кГц, на это дальше задается делитель отсчетов от 1 до хз (128 макс вроде), он задает частоту отсчетов акселя, ДУС на это же время осредняется. И мы получаем выход с ДУСа и акселя на одной частоте.
я вечно путаюсь в микро и мили - мсек-это что?
м -мили
мк -микро
А смысла ставить ШИМ 490 Гц при обсчете инс 42Гц нет особого, ну кроме того, что при 50Гц ШИМ к 238 мсек от ИНС добавится 200 мсек от регуля. Хотя пробовал летать на 50Гц, для больших винтов и тяжелых аппаратов разницы нет, но там возможно важно иметь обратную связь по оборотам.
Отсюда же и имею очень скептическое отношение к разного рода “крутым” регулям по I2C, там где надо и не надо…
К регулям по и2ц имею резко отрицательное отношение, а вот по КАН положительное. Хотя не то ни другое не использовал, зато хорошо знаком с особенностями первого интерфейса и возможностями второго.
Для себя вынес полезного это алгоритм Particle Filter, классная вещь Калман не нужен
спасибо, почитаю… вот нашел статейку habrahabr.ru/post/152553/
спасибо, почитаю… вот нашел статейку habrahabr.ru/post/152553/
Если честно, когда только начал изучал информацию на русском то это статья меня запутала. Лучше оригинал www.udacity.com/course/viewer#!/c-cs373/l-48704330… там и примеры кода на питоне есть.
Лирическое отступление, немного подвис с разработкой новой платы - пока в ступоре каком-то, а может и лень?
Но дело не в этом, есть незаконченное ПО на скажем так нулевую плату (всё что в гит)
Негоже бросать, думаю. Так вот к чему клоню - кривой USART1 - мысли:
// Transmit DMA
DMA_DeInit(DMA2_Stream7);
while (DMA_GetCmdStatus(DMA2_Stream7) != DISABLE) {
};
DMA_InitStructure.DMA_Channel = DMA_Channel_4;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USART1->DR;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_Init(DMA2_Stream7, &DMA_InitStructure);
DMA_SetCurrDataCounter(DMA2_Stream7, 0); // А не забыл ли я вот эту хрень?
DMA_ITConfig(DMA2_Stream7, DMA_IT_TC, ENABLE);
DMA2_Stream7->NDTR = 0;
USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
USART_Cmd(USART1, ENABLE);
Это базефлайт? Насчет начальной установки счетчика можно не париться, после сброса он в нуле, для спокойствия его надо было через надстроечную структуру прописать DMA_InitStruct.DMA_BufferSize = 0;.
Ну вот, вроде всё работает без костыля github.com/SergDoc/…/drv_uart.c затронут ещё main.c , так что требуется перекомпиляция проекта, надеюсь 100 микросекунд никого не убъёт?
#define UART1_TX_PIN GPIO_Pin_9
#define UART1_RX_PIN GPIO_Pin_10
#define UART1_GPIO GPIOA
#define UART1_TX_PINSOURCE GPIO_PinSource9
#define UART1_RX_PINSOURCE GPIO_PinSource10
#define UART1_BUFFER_SIZE 2048
// Receive buffer, circular DMA
volatile uint8_t rxBuffer[UART1_BUFFER_SIZE];
uint32_t rxDMAPos = 0;
volatile uint8_t txBuffer[UART1_BUFFER_SIZE];
uint16_t txBufferTail = 0;
uint16_t txBufferHead = 0;
///////////////////////////////////////////////////////////////////////////////
// UART1 Transmit via DMA
///////////////////////////////////////////////////////////////////////////////
static void uartTxDMA(void)
{
DMA2_Stream7->M0AR = (uint32_t)&txBuffer[txBufferTail];
if (txBufferHead > txBufferTail)
{
DMA_SetCurrDataCounter(DMA2_Stream7, txBufferHead - txBufferTail);
txBufferTail = txBufferHead;
}
else
{
DMA_SetCurrDataCounter(DMA2_Stream7, UART1_BUFFER_SIZE - txBufferTail);
txBufferTail = 0;
}
DMA_Cmd(DMA2_Stream7, ENABLE);
}
///////////////////////////////////////////////////////////////////////////////
// UART1 TX Complete Interrupt Handler
///////////////////////////////////////////////////////////////////////////////
void DMA2_Stream7_IRQHandler(void)
{
DMA_ClearITPendingBit(DMA2_Stream7, DMA_IT_TCIF7);
DMA_Cmd(DMA2_Stream7, DISABLE);
if (txBufferHead != txBufferTail)
uartTxDMA();
}
///////////////////////////////////////////////////////////////////////////////
// Telemetry Initialization
///////////////////////////////////////////////////////////////////////////////
void uartInit(uint32_t speed)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
USART_StructInit(&USART_InitStructure);
DMA_StructInit(&DMA_InitStructure);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
GPIO_InitStructure.GPIO_Pin = UART1_TX_PIN | UART1_RX_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_PinAFConfig(UART1_GPIO, UART1_TX_PINSOURCE, GPIO_AF_USART1);
GPIO_PinAFConfig(UART1_GPIO, UART1_RX_PINSOURCE, GPIO_AF_USART1);
GPIO_Init(UART1_GPIO, &GPIO_InitStructure);
// DMA TX Interrupt
NVIC_InitStructure.NVIC_IRQChannel = DMA2_Stream7_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
USART_InitStructure.USART_BaudRate = speed;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_Init(USART1, &USART_InitStructure);
// Receive DMA into a circular buffer
DMA_DeInit(DMA2_Stream5);
DMA_InitStructure.DMA_Channel = DMA_Channel_4;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USART1->DR;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)rxBuffer;
//DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
DMA_InitStructure.DMA_BufferSize = UART1_BUFFER_SIZE;
//DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
//DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
//DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
//DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
//DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
//DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
//DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(DMA2_Stream5, &DMA_InitStructure);
DMA_Cmd(DMA2_Stream5, ENABLE);
USART_DMACmd(USART1, USART_DMAReq_Rx, ENABLE);
rxDMAPos = DMA_GetCurrDataCounter(DMA2_Stream5);
// Transmit DMA
DMA_DeInit(DMA2_Stream7);
//DMA_InitStructure.DMA_Channel = DMA_Channel_4;
//DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USART1->DR;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)txBuffer;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
//DMA_InitStructure.DMA_BufferSize = UART_BUFFER_SIZE;
//DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
//DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
//DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
//DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
//DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
//DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
//DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
//DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
//DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(DMA2_Stream7, &DMA_InitStructure);
DMA_SetCurrDataCounter(DMA2_Stream7, 0);
DMA_ITConfig(DMA2_Stream7, DMA_IT_TC, ENABLE);
USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
USART_Cmd(USART1, ENABLE);
}
bool isUartAvailable(void)
{
return (DMA_GetCurrDataCounter(DMA2_Stream5) != rxDMAPos) ? true : false;
}
//bool isUartTransmitDMAEmpty(void)
// {
// return txDMAEmpty;
// }
//bool isUartTransmitEmpty(void)
//{
// return (txBufferTail == txBufferHead);
//}
uint8_t uartRead(void)
{
uint8_t ch;
ch = rxBuffer[UART1_BUFFER_SIZE - rxDMAPos];
// go back around the buffer
if (--rxDMAPos == 0)
rxDMAPos = UART1_BUFFER_SIZE;
return ch;
}
uint8_t uartReadPoll(void)
{
while (!isUartAvailable()); // wait for some bytes
return uartRead();
}
void uartWrite( uint8_t ch)
{
// while (DMA_GetCmdStatus(DMA2_Stream7) != DISABLE) {
// };
txBuffer[txBufferHead] = ch;
txBufferHead = (txBufferHead + 1) % UART1_BUFFER_SIZE;
// if DMA wasn't enabled, fire it up
if (!(DMA2_Stream7->CR & 1))
uartTxDMA();
delayMicroseconds(100);
}
void uartPrint(char *str)
{
while (*str)
uartWrite(*(str++));
}
Кстати а вот и ошибка, почему нифига не работало:
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)txBuffer;
а небыло этой строки вообще 😃
а небыло этой строки вообще
Этож строка только для Ф4. Как она вообще со старой компилилась,? Либо у тебя альтернативная структура для Ф1 гдето в заголовках болтается.
Да нет вот от Ф1 нету ничего, этот проект я полностью пересобирал и цеплял всё сам, компилилась зараза 😦
Не буду утверждать, но когда я делал этот драйвер, то нигде не мог найти подходящей инфы, а тут смотрю аероквады (не путать с автоквад) ковыряют так до боли знакомый кодятник (с тойже бетой) - всё никаких git - надоело 😦
Поковырялся в кодятнике code.google.com/p/aq32plus/ - вот это блин солянка, и вии и baseflight и px4 и даже рево (я учусь makefile делать далеко не удачный пример 😦 )
Случайно увидел в продаже ESC32 контроллеры, которые в теории умеют CAN, по разумной цене: witespyquad.gostorego.com/…/rtf32.html
которые в теории умеют CAN
хех… почему в теории? походу просто ребята сварганили клона создав свою разводку, а прошивка родная от ESC32
а цена действительно шикарная… жалко я сейчас некредитоспособен…
хех… почему в теории?
Ну потому что ESC32 в теории CAN умеет, а по факту, реализации нету в коде (ну или не было раньше, мот уже есть)