Создание собственной системы стабилизации
ещё пищалка, MicroSD, батарейка…
Ну вот наконец-то нижняя плата в сборе
Я сейчас изучаю углы, рассчитанные IMU со своей самоделки, и вдруг задумался (!): а правильно ли я разместил относительно друг друга гиру и аксель+маг на плате ? Одноименные оси (по даташиту) получились совмещены в одном направлении… правильно?
Есть ли правило или методика их установки, а то похоже что у мня присутствует взаимное влияние X на Y…
вообще-то без разницы, потом просто оси переназначить программно и всё
типа:
static void mpu6050AccAlign(int16_t *accData)
{
int16_t temp[2];
temp[0] = accData[0];
temp[1] = accData[1];
// official direction is RPY
accData[0] = temp[1];
accData[1] = -temp[0];
accData[2] = accData[2];
}
просто оси переназначить
Так вот и вопрос, то КАК,… по идее ось “X” гиро. и ось “Х” акселя должны быть в одном направлении, а с другой стороны вращение (и угловая скорость) у гиры не вдоль оси “Х” а перпедикулярно… что то я совсем запутался.
ну да, всё правильно показания по оси x гироскопа показывают скорость угловую - убегания оси y от чего-то там что мы считаем горизонтом, у меня аксель программно развёрнут на 90градусов относительно гиры…
а правильно ли я разместил относительно друг друга гиру и аксель+маг на плате
Я так проверял: сначала отключаешь коррекцию по аксу и магу, смотришь как ведет себя интегрированный гироскоп, если не так исправляешь знаки, меняешь местами координаты. Потом вколючаем коррекцию по акселю, крутим в руках, проблема проявляется как медленный завал в сторону, либо при резком движении положение меняется правильно, потом медленно переползает в противоположную сторону,если неправильный знак у оси акселя. Аналогично маг.
я что-то с spi начудил, в отладчике вижу, что порты настроены на af5- т.е. всё правильно - spi, где глядеть cs-переключается или нет? не прочитал я регистр (значение0х00 осталось), вот автодетект и стопорнул всё…
да cs криво настроил, теперь получил - ну не то что хотел, ковыряю дальше…
нет всё нормально с spi - завелось, считывание правильное…
буду ковырять дальше, видимо ещё с уартом намудрил…
На плате с барометром накосячил, когда пробовал по spi развести - вторую лапу зацепил на корпус, а когда переделывал на i2c забыл про неё, а надо было зацепить на питание, барометр остался в режиме spi переделать надо будет, у себя переделаю без проблем, главное на будущее учесть, а да следующая будет по spi 😃 плохо то что барометр выпаять надо сейчас 😦
Переделал и перезалил в git так же продублирую сюда
Так, у меня несколько новостей, больше наверно плохих, uart с dma я победил, у меня работает cli и gui коннектится, но не работает i2c (возможно из-за барометра) и нет показаний с гироакселя, хотя плата на наклоны реагирует (подмигивает)… оторвал проводок от jtag в потёмках явно не запаяю, отложу пока, ибо без отладчика делать нечего дальше…
уарт - пробы и ошибки не удалял просто коментировал, может кому пригодится:
#include "board.h"
/*
DMA UART routines idea lifted from AutoQuad
Copyright © 2011 Bill Nesbitt
*/
#define UART_BUFFER_SIZE 256
// Receive buffer, circular DMA
volatile uint8_t rxBuffer[UART_BUFFER_SIZE];
uint32_t rxDMAPos = 0;
volatile uint8_t txBuffer[UART_BUFFER_SIZE];
uint32_t txBufferTail = 0;
uint32_t txBufferHead = 0;
static void uartTxDMA(void)
{
DMA2_Stream7->M0AR = (uint32_t)&txBuffer[txBufferTail];
if (txBufferHead > txBufferTail) {
DMA2_Stream7->NDTR = txBufferHead - txBufferTail;
txBufferTail = txBufferHead;
} else {
DMA2_Stream7->NDTR = UART_BUFFER_SIZE - txBufferTail;
txBufferTail = 0;
}
DMA_Cmd(DMA2_Stream7, ENABLE);
}
void DMA2_Stream7_IRQHandler(void)
{
DMA_ClearITPendingBit(DMA2_Stream7, DMA_IT_TCIF7);
DMA_Cmd(DMA2_Stream7, DISABLE);
if (txBufferHead != txBufferTail)
uartTxDMA();
}
void uartInit(uint32_t speed)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
// USART1_TX PA9
// USART1_RX PA10
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStructure);
/*GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_USART1);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource10,GPIO_AF_USART1);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);*/
/* Connect USART pins to AF */
GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_USART1);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource10, GPIO_AF_USART1);
// DMA TX Interrupt
NVIC_InitStructure.NVIC_IRQChannel = DMA2_Stream7_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
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_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
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_Priority = DMA_Priority_Medium;
DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
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_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_BufferSize = UART_BUFFER_SIZE;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
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);
/* Enable the USART Rx DMA request */
USART_DMACmd(USART1, USART_DMAReq_Rx, ENABLE);
/* Enable DMA Stream Half Transfer and Transfer Complete interrupt */
// DMA_ITConfig(DMA2_Stream5, DMA_IT_TC, ENABLE);
// DMA_ITConfig(DMA2_Stream5, DMA_IT_HT, ENABLE);
/* Enable the DMA RX Stream */
//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_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_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_Init(DMA2_Stream7, &DMA_InitStructure);
DMA_ITConfig(DMA2_Stream7, DMA_IT_TC, ENABLE);
//DMA_ITConfig(DMA2_Stream7, DMA_IT_HT, ENABLE);
DMA2_Stream7->NDTR = 0;
USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);//DMA_Cmd(DMA2_Stream7, ENABLE);
USART_Cmd(USART1, ENABLE);
}
uint16_t uartAvailable(void)
{
return (DMA_GetCurrDataCounter(DMA2_Stream5) != rxDMAPos) ? true : false;
}
bool uartTransmitEmpty(void)
{
return (txBufferTail == txBufferHead);
}
uint8_t uartRead(void)
{
uint8_t ch;
ch = rxBuffer[UART_BUFFER_SIZE - rxDMAPos];
// go back around the buffer
if (--rxDMAPos == 0)
rxDMAPos = UART_BUFFER_SIZE;
return ch;
}
uint8_t uartReadPoll(void)
{
while (!uartAvailable()); // wait for some bytes
return uartRead();
}
void uartWrite(uint8_t ch)
{
txBuffer[txBufferHead] = ch;
txBufferHead = (txBufferHead + 1) % UART_BUFFER_SIZE;
// if DMA wasn't enabled, fire it up
if (!(DMA2_Stream7->CR & 1))
uartTxDMA();
}
void uartPrint(char *str)
{
while (*str)
uartWrite(*(str++));
}
/* -------------------------- UART2 ----------------------------- */
uartReceiveCallbackPtr uart2Callback = NULL;
#define UART2_BUFFER_SIZE 128
volatile uint8_t tx2Buffer[UART2_BUFFER_SIZE];
uint32_t tx2BufferTail = 0;
uint32_t tx2BufferHead = 0;
bool uart2RxOnly = false;
static void uart2Open(uint32_t speed)
{
USART_InitTypeDef USART_InitStructure;
USART_StructInit(&USART_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 | (uart2RxOnly ? 0 : USART_Mode_Tx);
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_Init(USART2, &USART_InitStructure);
USART_Cmd(USART2, ENABLE);
}
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly)
{
NVIC_InitTypeDef NVIC_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
uart2RxOnly = rxOnly;
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// USART2_TX PD5
// USART2_RX PD6
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
if (!rxOnly)
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_PinAFConfig(GPIOD, GPIO_PinSource5, GPIO_AF_USART2);
GPIO_PinAFConfig(GPIOD, GPIO_PinSource6,GPIO_AF_USART2);
uart2Open(speed);
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
if (!rxOnly)
USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
USART_Cmd(USART2, ENABLE);
uart2Callback = func;
}
void uart2ChangeBaud(uint32_t speed)
{
uart2Open(speed);
}
void uart2Write(uint8_t ch)
{
if (uart2RxOnly)
return;
tx2Buffer[tx2BufferHead] = ch;
tx2BufferHead = (tx2BufferHead + 1) % UART2_BUFFER_SIZE;
USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
}
bool uart2TransmitEmpty(void)
{
return tx2BufferTail == tx2BufferHead;
}
void USART2_IRQHandler(void)
{
uint16_t SR = USART2->SR;
if (SR & USART_IT_RXNE) {
if (uart2Callback)
uart2Callback(USART_ReceiveData(USART2));
}
if (SR & USART_FLAG_TXE) {
if (tx2BufferTail != tx2BufferHead) {
USART2->DR = tx2Buffer[tx2BufferTail];
tx2BufferTail = (tx2BufferTail + 1) % UART2_BUFFER_SIZE;
} else {
USART_ITConfig(USART2, USART_IT_TXE, DISABLE);
}
}
}
/* -------------------------- UART3 ----------------------------- */
uartReceiveCallbackPtr uart3Callback = NULL;
#define UART3_BUFFER_SIZE 128
volatile uint8_t tx3Buffer[UART3_BUFFER_SIZE];
uint32_t tx3BufferTail = 0;
uint32_t tx3BufferHead = 0;
bool uart3RxOnly = false;
static void uart3Open(uint32_t speed)
{
USART_InitTypeDef USART_InitStructure;
USART_StructInit(&USART_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 | (uart3RxOnly ? 0 : USART_Mode_Tx);
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_Init(USART3, &USART_InitStructure);
USART_Cmd(USART3, ENABLE);
}
void uart3Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly)
{
NVIC_InitTypeDef NVIC_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
uart3RxOnly = rxOnly;
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// USART2_TX PD8
// USART2_RX PD9
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
if (!rxOnly)
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_PinAFConfig(GPIOD, GPIO_PinSource8, GPIO_AF_USART2);
GPIO_PinAFConfig(GPIOD, GPIO_PinSource9,GPIO_AF_USART2);
uart3Open(speed);
USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
if (!rxOnly)
USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
USART_Cmd(USART3, ENABLE);
uart3Callback = func;
}
void uart3ChangeBaud(uint32_t speed)
{
uart3Open(speed);
}
void uart3Write(uint8_t ch)
{
if (uart3RxOnly)
return;
tx3Buffer[tx3BufferHead] = ch;
tx3BufferHead = (tx3BufferHead + 1) % UART3_BUFFER_SIZE;
USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
}
bool uart3TransmitEmpty(void)
{
return tx3BufferTail == tx3BufferHead;
}
void USART3_IRQHandler(void)
{
uint16_t SR = USART3->SR;
if (SR & USART_IT_RXNE) {
if (uart3Callback)
uart3Callback(USART_ReceiveData(USART3));
}
if (SR & USART_FLAG_TXE) {
if (tx3BufferTail != tx3BufferHead) {
USART3->DR = tx3Buffer[tx3BufferTail];
tx3BufferTail = (tx3BufferTail + 1) % UART3_BUFFER_SIZE;
} else {
USART_ITConfig(USART3, USART_IT_TXE, DISABLE);
}
}
}
а может ещё где ошибка есть…
Не наверно ещё что-то с портом не то, пару секунд данные идут потом затыкаются… зафиксирован цикл - 793 мкс , для сравнения на мелкоплате с f103 ~2600 мкс…
Привет. Тема о создании собственной системы стабилизации, а по сути - адаптация чужого кода на свои, порождаемые непонятной логикой железки. Где свое?
Где свое?
“Свое” у каждого будет непременно, а уж изобрести что то круче чем кватернионный фильтр, это уж перебор…(для меня например)
Кстати вопрос: в реализации FREE IMU присутствуют две разные функции получения углов из кватернионов:
-------------------------------------------------------------------------------------------
angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
--------------------------------------------------------
и вторая:
---------------------------------------------
gx = 2 * (q[1]*q[3] - q[0]*q[2]);
gy = 2 * (q[0]*q[1] + q[2]*q[3]);
gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
------------------------------------
Какая из них являет текущие углы аппарата относительно плоскости Земли ???
Тема о создании собственной системы стабилизации, а по сути - адаптация чужого кода на свои, порождаемые непонятной логикой железки. Где свое?
а мне всё равно, что вы думаете… плата была разработана с учётом пожеланий(не я один приложил к этому руку), и по возможности как можно дешевле в исполнении… а до ПО, предлагайте, если есть что, адаптация - это первый этап - посмотреть как железка себя поведёт, что лучше изменить в будущем, а дальше увидим…
p.s. проекты на f4 - растут как грибы после дождя, и железка поддерживает их все (ну может с небольшей корректировкой кода), неплохой AIO-F4 получается 😃
неплохой AIO-F4 получается…
Это вам так кажется.
если оно не стыкуется из коробки с библиотеками Maple, это ещё ни о чём не говорит, я сознательно отвернулся от них… хотя при желании можно (у меня есть AeroQad и Multipilot32 оба живут под эклипсом)…
а по сути - адаптация чужого кода на свои, порождаемые непонятной логикой железки. Где свое?
Андрей, ну что ты заладил “своё - своё”. Я вот в этом форуме ни одной буквы своей не написал, всё либо кирилица, либо латиница😁
Железка порождена вполне понятной логикой- много мощи и ног, жизненно важные датчики через SPI (в следующей итерации возможен уход с и2ц - транзит компаса через МПУ6000), ШИМ разведён под аппаратные выводы, имеем свободные порты уарт и СПИ, портируется любой проект под СТМ (с АВРа тупой порт не имеет смысла). Покажи аналогичную по цене до 100 баксов?
Это вам так кажется.
Вообще каждый человек воспринимает всё субъективно, т.е. всё что он видит и чувствует ему только кажется, в т.ч. этот форум и тов.Дринкер. Это я к тому, что какая галюцинация наиболее реальна, каждый решает для себя сам.
Это вам так кажется.
с аргументами плз. 😉
Я что-то с уартом и дма напорол, и не знаю что, вроде всё правильно, но затыкается зараза и передаёт абы что, надо наверно удалить драйвер покалеченый и всё заново переписать 😦 знаю точно что i2c запустилась - баро правильную высоту выдаёт (единственное, что нормально передаётся) ну иногда гироаксель появляется, а магнитометр ну ооочень редко, но тоже присутствует…
Разобрался с FreeIMU x-io.co.uk/open-source-imu-and-ahrs-algorithms/ , - хорошая штука (всего две функции), позволяет “засадить” обсчет положения прямо в прерывание опроса датчиков, типа прочитал->получил углы, считает вроде хорошо и быстро (пока только на столе), теперь надо ПИД и можно в реале пробовать.
хорошая штука (всего две функции),
Компас подключен? Если да, то засеките положение, переверните и подождите 10 мин. Потом сравните.