Si además quieres enviarnos un Artículo para el Blog y redes sociales, pulsa el siguiente botón:
Hola a todos,
Estoy realizando un controlador de posición para un cuadróptero (es mi PFC), quiero que sea Open-Hardware así que iré publicando por aquí y en el blog los avances que vaya haciendo. Se que ya existen alternativas comerciales y abiertas (el openPilot, el KK o el multiWii por ejemplo), pero mi idea es que al final sea totalmente autónomo.
Por otra parte quiero hacer el sistema lo más general posible para que pueda aplicarse a todo tipo de drones (áereos, terrestre, acuaticos, submarinos o espaciales).
He partido el proyecto en varios trozos:
- Piloto --> controlador de posición (mi PFC) evita que el cuadróptero se vaya al traste, controlará los sensores y enviará las señales a los drivers para controlar la estabilidad del conjunto.
- Navegador --> Es el que toma las decisiones para que sea autónomo. Por falta de tiempo no voy a realizarlo dentro del PFC, pero iré avanzando en él cuando tenga la parte del piloto hecha.
- Maquinista --> Mi idea es desarrollar unos drivers para motores brushless (los famosos ESC) que se controlen por bus SPI que den una mayor velocidad de respuesta al sistema (los 20ms del PWM me parecen mucho en una situación crítica).
- Controlador --> Servicio de telemetría en tierra para la recepción de la telemetría y el control (indicaciones) del equipo.
Las sugerencias serán bienvenidas.
S2
Ranganok Schahzaman
Aunque las librerías difieres mucho entre fabricantes, te dejo un ejemplo sencillo que he hecho para STM32-H103 (olimex).
/**
*****************************************************************************
* @title Simple
* @author Luisón
* @date 26 Sep 2012 - 26 Sep 2012
* @email theinit@yahoo.es
* @brief Programa sencillo que responde con un eco por el puerto serie y
* enciende o apaga un led cada vez que se pulsa un botón. Está todo
* en base a interrupciones. Programado para la placa STM32-H103
*
*
*****************************************************************************
*/
#include "stm32f10x.h"
#include "stm32f10x_gpio.h"
#include "stm32f10x_rcc.h"
#include "stm32f10x_exti.h"
#include "stm32f10x_usart.h"
#include "misc.h"
#define LED_ST_PORT GPIOC
#define LED_ST_PIN GPIO_Pin_12
#define BOTON1_PORT GPIOA
#define BOTON1_PIN GPIO_Pin_0
#define SERIAL_PORT GPIOA
#define RX_PIN GPIO_Pin_10
#define TX_PIN GPIO_Pin_9
//************************************************************************
//
// Variables globales
//
// ********************************************************************
GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
//*********************************************************************
//
// Declaro las funciones
//
// ***********************************************************************
void FLIP_PIN(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
void RCC_Configuration(void);
void GPIO_Configuration(void);
void NVIC_Configuration(void);
void SetupEXTI(void);
void SetupUSART(void);
void USARTxSend(USART_TypeDef* USARTx, const unsigned char *pucBuffer, unsigned long ulCount);
void EXTI0_IRQHandler(void);
void USART1_IRQHandler(void);
int main(void)
{
//
// Configuro los relojes del sistema
//
RCC_Configuration();
//
// Configuro el vector NVIC
//
NVIC_Configuration();
//
// Configuro los puertos GPIO
//
GPIO_Configuration();
//
// Configurar el puerto serie 1 (USART1)
//
SetupUSART();
//
// Configurar la interrupción externa (EXTI0)
//
SetupEXTI();
while (1)
{
}
}
//************************************************************************
//
//Configura los diferentes relojes del sistema.
//
// ********************************************************************
void RCC_Configuration(void)
{
// 1. Clocking the controller from internal HSI RC (8 MHz)
RCC_HSICmd(ENABLE);
// wait until the HSI is ready
while(RCC_GetFlagStatus(RCC_FLAG_HSIRDY) == RESET);
RCC_SYSCLKConfig(RCC_SYSCLKSource_HSI);
// 2. Enable ext. high frequency OSC
RCC_HSEConfig(RCC_HSE_ON);
// wait until the HSE is ready
while(RCC_GetFlagStatus(RCC_FLAG_HSERDY) == RESET);
// 3. Init PLL
RCC_PLLConfig(RCC_PLLSource_HSE_Div1,RCC_PLLMul_9); // 72MHz en mi caso
RCC_PLLCmd(ENABLE);
// wait until the PLL is ready
while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET);
// 4. Set system clock dividers
RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5);
RCC_ADCCLKConfig(RCC_PCLK2_Div8);
RCC_PCLK2Config(RCC_HCLK_Div1);
RCC_PCLK1Config(RCC_HCLK_Div2);
RCC_HCLKConfig(RCC_SYSCLK_Div1);
// 5. Clock system from PLL
RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
//
// Habilito el reloj en los puertos que utilizo y el AFIO clock
//
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_USART1 | RCC_APB2Periph_AFIO, ENABLE);
}
//*********************************************************************
//
//Configura los diferentes puertos GPIO.
//
//************************************************************************
void GPIO_Configuration(void)
{
/* Configuro PA0 como puerto de entrada */
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_Init(GPIOA, &GPIO_InitStructure);
/* ConfiguroUSART1 Tx (PA.09)*/
GPIO_InitStructure.GPIO_Pin = TX_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
/* Configuro USART1 Rx (PA.10) */
GPIO_InitStructure.GPIO_Pin = RX_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
//************************************************************************
//
//Configura "the nested vectored interrupt controller"
//
//************************************************************************
void NVIC_Configuration(void)
{
NVIC_InitTypeDef NVIC_InitStructure;
//
// Set the Vector Table base location at 0x08000000
//
NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);
//
// Configure one bit for preemption priority
//
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
//
// Habilito la interrupción EXTI0 (con menos prioridad que el puerto serie)
//
NVIC_InitStructure.NVIC_IRQChannel = EXTI0_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
//
// Habilito la interrupción del puerto Serie
//
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
void SetupEXTI(void)
{
//
// Conecto EXTI Line0 to PA0
//
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource0);
//
// Configuro EXTI Line0 para generar una interrupción cada vez que se pulsa
//
EXTI_InitStructure.EXTI_Line = EXTI_Line0;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
}
void SetupUSART(void)
{
USART_InitTypeDef USART_InitStructure;
USART_InitStructure.USART_BaudRate = 460800;
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 Peripheral Enable */
USART_Cmd(USART1, ENABLE);
/* Apply USART configuration */
USART_Init(USART1, &USART_InitStructure);
/* Enable RX interrupt */
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
}
void USARTxSend(USART_TypeDef* USARTx, const unsigned char *pucBuffer, unsigned long ulCount)
{
//
// Mientras haya algo que enviar...
//
while(ulCount--)
{
while(USART_GetFlagStatus(USARTx, USART_FLAG_TC) == RESET); //Espera a que el USARTx esté libre, si se pone después del
//envío (como en ejemplos) se pierde el primer carácter enviado
USART_SendData(USARTx, (uint16_t) *pucBuffer++);
}
}
//***********************************************************************
//
//Función que cambia el estado de un pin.
//
//***********************************************************************
void FLIP_PIN(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
{
GPIOx->ODR ^= GPIO_Pin;
}
//***********************************************************************
//
//Manejo de interrupciones.
//
//***********************************************************************
void EXTI0_IRQHandler(void)
{
FLIP_PIN(LED_ST_PORT,LED_ST_PIN);
EXTI_ClearITPendingBit(EXTI_Line0);
}
void USART1_IRQHandler(void)
{
if ((USART1->SR & USART_FLAG_RXNE) != (u16)RESET)
{
unsigned char caracter[] = " ";
caracter[0] = USART_ReceiveData(USART1);
USARTxSend(USART1, caracter, 1);
}
}
Gracias, lo apunto como plantilla para los ST.
S2
Ranganok Schahzaman
¿Por casualidad te has pasado por CooCox.org? Allí tienen muchas cosas, como librerías, ejemplos, IDE gratuito, RTOS, incluso proyectos compartidos. Como muchos, perderás algo de tu valioso tiempo, aunque no creo que sea un desperdicio.
Estaba buscando un cubo o unos ejes (gráficos en 3D) para C#, tipo lo que me ha pasado sistemasorp en http://foro.webdearde.com/viewtopic.php?t=3323&p=39316#p39316 " onclick="window.open(this.href);return false; . Lo que quiero hacer es pasarle las medidas de los sensores vía serie o RF y probar distintos algoritmos en el PC:
- Directo.
- Filtros complementarios.
- Kalman.
- DCM (creo que este es sólo para corregir no para estimar el ángulo, pero me lo tengo que estudiar a fondo todavía).
- AHRS
De esta forma se podrá ver la mejor opción (coste/beneficio) para implementarla en el procesador.
S2
Ranganok Schahzaman
PD: He realizado un pequeño post para ver el funcionamiento de los filtros complementarios (por si alguien quiere hecharle un vistazo): http://skiras.blogspot.com.es/2012/10/4copter-fusion-de-sensores-i.html " onclick="window.open(this.href);return false;
Si estás intentando calcular la posición exacta de tu placa en 3D, no sólo en un eje, ya puedes ir descartando :
- Directo
- Filtros complementarios
Ya que esos NO sirven. En cuanto tienes un movimiento en 3D, el ángulo de un eje depende del giro de otro eje y del estado anterior, con lo que el cambio de ángulo que te da el giro es absolutamente inservible, lo mismo para los demás, los cuales ni se enterarán de ciertos giros.
Sobre Kalman, ahora mismo no me acuerdo, pero creo recordar que era sólo un Filtro, no estoy 100% seguro, desde luego no es el que se usa. Y si se usa era para filtrar la señal y meterla en el DCM más limpia (y no hace falta).
Lo que debes usar es DCM, el cual, a pesar de lo que comentas, sí estima el ángulo, es el único que lo hace, de hecho.
Hace un año o más, puse por aquí un vídeo mio de lo que quieres hacer pero hecho en Java para poder ser exportado a todas las plataformas, lo digo porque seguramente me preguntes si hablo sólo desde la teoría o si he hecho el proyecto 😉
El DCM es un proyecto bien bonito, ¡ánimo!!
