Создание собственной системы стабилизации
Натворил чего-то, завтра даст бог, запаяю мпу, буду пробовать читать…
Не буду (пока ) тебе морочить голову в личке, я думаю таких без меня полно
да без проблем! теребят то в основном по фигне всякой, а тут приятный мужской разговор 😃
Вот хочу сравнить ЛСМку с МПУшкой, у тебя ведь последняя? Можешь дать лог сырых данных акселя с висящего коптера, в этом ведь ничего секретного нет, отсчётов 100?
У меня AIOP 1.0 контроллер с mpu6050 на борту… наверное последняя… а их было несколько версий?
с логами сложнее… я в основном в рантайме на графики смотрю в ГУях, но вроде как андроид прилага EZ-GUI позволяет писать… посмотрю…
а их было несколько версий?
Я имел ввиду что у тебя МПУшка, а не другой датчик.
EZ-GUI позволяет писать… посмотрю…
Хорошо, жду… Если можно побольше отсчётов, а то боюсь для статистики не хватит.
Как правильно под spi переделать
ДОстаточно заменить все функции i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, tmp) аналогичной для СПИ.
тебя МПУшка, а не другой датчик
их несколько ревизий было с разными акселями, а единственное, я так понял, что-бы узнать ревизию надо читать регистры MPU_RA_WHO_AM_I и MPU_RA_PRODUCT_ID, а там ревизий 5+6 в зависимости от модификации какой-то…
Ну вот наконец-то нижняя плата в сборе, не утерпел - мелкоплата лишилась барометра - теперь обратной дороги нет, надеюсь за два-то года хоть что-то стоящее получилось 😃
Сергей, когда планируешь прошивку доделать? Разъем ПРОГ это усарт1?
Разъем ПРОГ это усарт1?
да, это первый телеметрия второй, gps - 3-й выведен сверху на верхнюю плату, а с прошивкой чувствую ещё долго телепаться буду 😦 по крайней мере детект мпу не прокатил, буду смотреть отладчиком как время будет… сейчас работают только светодиоды - инициализация, ну и ошибка при детекте - перемигиваемся и всё…
gps - 3-й выведен сверху на верхнюю плату
т.е. на 2м этаже у тебя свой ГПС будет?
ещё пищалка, 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));
------------------------------------
Какая из них являет текущие углы аппарата относительно плоскости Земли ???