From d8201ec45bd1dba1d4110ddde84a6060ebbdb858 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Mon, 13 Jun 2011 03:04:49 +0300 Subject: [PATCH 1/8] sbus: provide a stub based on Spektrum driver (for CC only) --- flight/CopterControl/Makefile | 5 + flight/CopterControl/System/inc/pios_config.h | 2 + flight/CopterControl/System/pios_board.c | 103 ++++++++ flight/Modules/ManualControl/manualcontrol.c | 2 + flight/OpenPilot/Makefile | 5 + flight/OpenPilot/System/inc/pios_config.h | 2 + flight/OpenPilot/System/openpilot.c | 3 + flight/OpenPilot/System/pios_board.c | 4 + flight/PiOS/Boards/STM32103CB_CC_Rev1.h | 6 + flight/PiOS/Boards/STM3210E_OP.h | 6 + flight/PiOS/Common/pios_hcsr04.c | 4 +- flight/PiOS/STM32F10x/pios_rtc.c | 2 +- flight/PiOS/STM32F10x/pios_sbus.c | 227 ++++++++++++++++++ flight/PiOS/STM32F10x/pios_spektrum.c | 4 +- flight/PiOS/inc/pios_sbus.h | 46 ++++ flight/PiOS/inc/pios_sbus_priv.h | 57 +++++ flight/PiOS/pios.h | 1 + 17 files changed, 474 insertions(+), 5 deletions(-) create mode 100644 flight/PiOS/STM32F10x/pios_sbus.c create mode 100644 flight/PiOS/inc/pios_sbus.h create mode 100644 flight/PiOS/inc/pios_sbus_priv.h diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 21fa59f97..96d3aefc4 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -46,6 +46,7 @@ ENABLE_DEBUG_PINS ?= NO ENABLE_AUX_UART ?= NO USE_SPEKTRUM ?= NO +USE_SBUS ?= NO USE_I2C ?= NO @@ -184,6 +185,7 @@ SRC += $(PIOSSTM32F10X)/pios_spi.c SRC += $(PIOSSTM32F10X)/pios_ppm.c SRC += $(PIOSSTM32F10X)/pios_pwm.c SRC += $(PIOSSTM32F10X)/pios_spektrum.c +SRC += $(PIOSSTM32F10X)/pios_sbus.c SRC += $(PIOSSTM32F10X)/pios_debug.c SRC += $(PIOSSTM32F10X)/pios_gpio.c SRC += $(PIOSSTM32F10X)/pios_exti.c @@ -381,6 +383,9 @@ endif ifeq ($(USE_SPEKTRUM), YES) CDEFS += -DUSE_SPEKTRUM endif +ifeq ($(USE_SBUS), YES) +CDEFS += -DUSE_SBUS +endif ifeq ($(USE_I2C), YES) CDEFS += -DUSE_I2C diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 98c900387..ab9251b12 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -47,6 +47,8 @@ #if defined(USE_SPEKTRUM) #define PIOS_INCLUDE_SPEKTRUM +#elif defined(USE_SBUS) +#define PIOS_INCLUDE_SBUS #else #define PIOS_INCLUDE_GPS //#define PIOS_INCLUDE_PPM diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 53dd5b505..ab84ba5c2 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -383,6 +383,95 @@ void PIOS_SUPV_irq_handler() { } #endif /* PIOS_INCLUDE_SPEKTRUM */ +#if defined(PIOS_INCLUDE_SBUS) +/* + * SBUS USART + */ +void PIOS_USART_sbus_irq_handler(void); +void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_sbus_irq_handler"))); +const struct pios_usart_cfg pios_usart_sbus_cfg = { + .regs = USART3, + .init = { + #if defined (PIOS_COM_SBUS_BAUDRATE) + .USART_BaudRate = PIOS_COM_SBUS_BAUDRATE, + #else + .USART_BaudRate = 115200, + #endif + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .handler = PIOS_USART_sbus_irq_handler, + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +static uint32_t pios_usart_sbus_id; +void PIOS_USART_sbus_irq_handler(void) +{ + SBUS_IRQHandler(pios_usart_sbus_id); +} + +#include +void RTC_IRQHandler(); +void RTC_IRQHandler() __attribute__ ((alias ("PIOS_SUPV_irq_handler"))); +const struct pios_sbus_cfg pios_sbus_cfg = { + .pios_usart_sbus_cfg = &pios_usart_sbus_cfg, + .gpio_init = { //used for bind feature + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + .remap = 0, + .irq = { + .handler = RTC_IRQHandler, + .init = { + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .port = GPIOB, + .pin = GPIO_Pin_11, +}; + +void PIOS_SUPV_irq_handler() { + if (RTC_GetITStatus(RTC_IT_SEC)) + { + /* Call the right handler */ + PIOS_SBUS_irq_handler(pios_usart_sbus_id); + + /* Wait until last write operation on RTC registers has finished */ + RTC_WaitForLastTask(); + /* Clear the RTC Second interrupt */ + RTC_ClearITPendingBit(RTC_IT_SEC); + } +} +#endif /* PIOS_INCLUDE_SBUS */ + static uint32_t pios_usart_telem_rf_id; void PIOS_USART_telem_irq_handler(void) { @@ -659,6 +748,7 @@ uint32_t pios_com_telem_rf_id; uint32_t pios_com_telem_usb_id; uint32_t pios_com_gps_id; uint32_t pios_com_spektrum_id; +uint32_t pios_com_sbus_id; /** * PIOS_Board_Init() @@ -691,6 +781,19 @@ void PIOS_Board_Init(void) { PIOS_DEBUG_Assert(0); } #endif + +#if defined(PIOS_INCLUDE_SBUS) + /* SBUS init must come before comms */ + PIOS_SBUS_Init(); + + if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_cfg)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_COM_Init(&pios_com_sbus_id, &pios_usart_com_driver, pios_usart_sbus_id)) { + PIOS_DEBUG_Assert(0); + } +#endif + /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 63543c67a..098c7a491 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -213,6 +213,8 @@ static void manualControlTask(void *parameters) cmd.Channel[n] = PIOS_PPM_Get(n); #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); +#elif defined(PIOS_INCLUDE_SBUS) + cmd.Channel[n] = PIOS_SBUS_Get(n); #endif scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index a316f4cc3..68ad6f561 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -44,6 +44,7 @@ ENABLE_DEBUG_PINS ?= NO ENABLE_AUX_UART ?= NO USE_SPEKTRUM ?= NO +USE_SBUS ?= NO # Set to YES when using Code Sourcery toolchain @@ -170,6 +171,7 @@ SRC += $(PIOSSTM32F10X)/pios_spi.c SRC += $(PIOSSTM32F10X)/pios_ppm.c SRC += $(PIOSSTM32F10X)/pios_pwm.c SRC += $(PIOSSTM32F10X)/pios_spektrum.c +SRC += $(PIOSSTM32F10X)/pios_sbus.c SRC += $(PIOSSTM32F10X)/pios_debug.c SRC += $(PIOSSTM32F10X)/pios_gpio.c SRC += $(PIOSSTM32F10X)/pios_exti.c @@ -366,6 +368,9 @@ endif ifeq ($(USE_SPEKTRUM), YES) CDEFS += -DUSE_SPEKTRUM endif +ifeq ($(USE_SBUS), YES) +CDEFS += -DUSE_SBUS +endif # Place project-specific -D and/or -U options for diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index f3fe8671b..939ee73b6 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -44,6 +44,8 @@ #if defined(USE_SPEKTRUM) #define PIOS_INCLUDE_SPEKTRUM +#elif defined(USE_SBUS) +#define PIOS_INCLUDE_SBUS #else //#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PWM diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c index be50653f7..04ee39696 100644 --- a/flight/OpenPilot/System/openpilot.c +++ b/flight/OpenPilot/System/openpilot.c @@ -153,6 +153,9 @@ static void TaskTesting(void *pvParameters) #if defined(PIOS_INCLUDE_SPEKTRUM) PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SPEKTRUM_Get(0), PIOS_SPEKTRUM_Get(1), PIOS_SPEKTRUM_Get(2), PIOS_SPEKTRUM_Get(3), PIOS_SPEKTRUM_Get(4), PIOS_SPEKTRUM_Get(5), PIOS_SPEKTRUM_Get(6), PIOS_SPEKTRUM_Get(7)); #endif +#if defined(PIOS_INCLUDE_SBUS) + PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SBUS_Get(0), PIOS_SBUS_Get(1), PIOS_SBUS_Get(2), PIOS_SBUS_Get(3), PIOS_SBUS_Get(4), PIOS_SBUS_Get(5), PIOS_SBUS_Get(6), PIOS_SBUS_Get(7)); +#endif #if defined(PIOS_INCLUDE_PWM) PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PWM_Get(0), PIOS_PWM_Get(1), PIOS_PWM_Get(2), PIOS_PWM_Get(3), PIOS_PWM_Get(4), PIOS_PWM_Get(5), PIOS_PWM_Get(6), PIOS_PWM_Get(7)); #endif diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 14be197e8..52a059777 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -539,6 +539,10 @@ void PIOS_SUPV_irq_handler() { } #endif /* PIOS_COM_SPEKTRUM */ +#if defined(PIOS_INCLUDE_SBUS) +#error PIOS_INCLUDE_SBUS not implemented +#endif /* PIOS_INCLUDE_SBUS */ + static uint32_t pios_usart_telem_rf_id; void PIOS_USART_telem_irq_handler(void) { diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 16e58b5c2..e80cb960c 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -161,6 +161,12 @@ extern uint32_t pios_com_spektrum_id; #define PIOS_COM_SPEKTRUM (pios_com_spektrum_id) #endif +#ifdef PIOS_INCLUDE_SBUS +#define PIOS_COM_SBUS_BAUDRATE 115200 +extern uint32_t pios_com_sbus_id; +#define PIOS_COM_SBUS (pios_com_sbus_id) +#endif + //------------------------- // ADC // PIOS_ADC_PinGet(0) = Gyro Z diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h index c042184ea..e82858fb1 100644 --- a/flight/PiOS/Boards/STM3210E_OP.h +++ b/flight/PiOS/Boards/STM3210E_OP.h @@ -173,6 +173,12 @@ extern uint32_t pios_com_spektrum_id; #define PIOS_COM_SPEKTRUM (pios_com_spektrum_id) #endif +#ifdef PIOS_INCLUDE_SBUS +#define PIOS_COM_SBUS_BAUDRATE 100000 +extern uint32_t pios_com_sbus_id; +#define PIOS_COM_SBUS (pios_com_sbus_id) +#endif + //------------------------- // Delay Timer //------------------------- diff --git a/flight/PiOS/Common/pios_hcsr04.c b/flight/PiOS/Common/pios_hcsr04.c index aa6dece83..6db1bc401 100644 --- a/flight/PiOS/Common/pios_hcsr04.c +++ b/flight/PiOS/Common/pios_hcsr04.c @@ -32,8 +32,8 @@ #include "pios.h" #if defined(PIOS_INCLUDE_HCSR04) -#if !defined(PIOS_INCLUDE_SPEKTRUM) -#error Only supported with spektrum interface! +#if !(defined(PIOS_INCLUDE_SPEKTRUM) || defined(PIOS_INCLUDE_SBUS)) +#error Only supported with Spektrum or S.Bus interface! #endif /* Local Variables */ diff --git a/flight/PiOS/STM32F10x/pios_rtc.c b/flight/PiOS/STM32F10x/pios_rtc.c index 98d3160c1..2a0d55915 100644 --- a/flight/PiOS/STM32F10x/pios_rtc.c +++ b/flight/PiOS/STM32F10x/pios_rtc.c @@ -49,7 +49,7 @@ void PIOS_RTC_Init() RTC_WaitForSynchro(); RTC_WaitForLastTask(); -#if defined(PIOS_INCLUDE_SPEKTRUM) +#if defined(PIOS_INCLUDE_SPEKTRUM) || defined(PIOS_INCLUDE_SBUS) /* Enable the RTC Second interrupt */ RTC_ITConfig( RTC_IT_SEC, ENABLE ); /* Wait until last write operation on RTC registers has finished */ diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c new file mode 100644 index 000000000..5862c527a --- /dev/null +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -0,0 +1,227 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SBUS Futaba S.Bus receiver functions + * @brief Code to read Futaba S.Bus input + * @{ + * + * @file pios_sbus.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief USART commands. Inits USARTs, controls USARTs & Interrupt handlers. (STM32 dependent) + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/* Project Includes */ +#include "pios.h" +#include "pios_sbus_priv.h" + +#if defined(PIOS_INCLUDE_SBUS) +#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_SPEKTRUM) +#error "Both SBUS and either of PWM or SPEKTRUM inputs defined, choose only one" +#endif +#if defined(PIOS_COM_AUX) +#error "AUX com cannot be used with SBUS" +#endif + +/** + * @Note Framesyncing: + * The code resets the watchdog timer whenever a single byte is received, so what watchdog code + * is never called if regularly getting bytes. + * RTC timer is running @625Hz, supervisor timer has divider 5 so frame sync comes every 1/125Hz=8ms. + * Good for both 11ms and 22ms framecycles + */ + +/* Global Variables */ + +/* Local Variables */ +static uint16_t CaptureValue[12],CaptureValueTemp[12]; +static uint8_t prev_byte = 0xFF, sync = 0, bytecount = 0, datalength=0, frame_error=0, byte_array[20] = { 0 }; +uint8_t sync_of = 0; +uint16_t supv_timer=0; + +/** +* Bind and Initialise S.Bus receiver +*/ +void PIOS_SBUS_Init(void) +{ + /* Init RTC supervisor timer interrupt */ + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannel = RTC_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + /* Init RTC clock */ + PIOS_RTC_Init(); +} + +/** +* Get the value of an input channel +* \param[in] Channel Number of the channel desired +* \output -1 Channel not available +* \output >0 Channel value +*/ +int16_t PIOS_SBUS_Get(int8_t Channel) +{ + /* Return error if channel not available */ + if (Channel >= 12) { + return -1; + } + return CaptureValue[Channel]; +} + +/** +* Decodes a byte +* \param[in] b byte which should be sbus decoded +* \return 0 if no error +* \return -1 if USART not available +* \return -2 if buffer full (retry) +* \note Applications shouldn't call these functions directly +*/ +int32_t PIOS_SBUS_Decode(uint8_t b) +{ + static uint16_t channel = 0; /*, sync_word = 0;*/ + uint8_t channeln = 0, frame = 0; + uint16_t data = 0; + byte_array[bytecount] = b; + bytecount++; + if (sync == 0) { + //sync_word = (prev_byte << 8) + b; +#if 0 + /* maybe create object to show this data */ + if(bytecount==1) + { + /* record losscounter into channel8 */ + CaptureValueTemp[7]=b; + /* instant write */ + CaptureValue[7]=b; + } +#endif + /* Known sync bytes, 0x01, 0x02, 0x12 */ + if (bytecount == 2) { + if (b == 0x01) { + datalength=0; // 10bit + //frames=1; + sync = 1; + bytecount = 2; + } + else if(b == 0x02) { + datalength=0; // 10bit + //frames=2; + sync = 1; + bytecount = 2; + } + else if(b == 0x12) { + datalength=1; // 11bit + //frames=2; + sync = 1; + bytecount = 2; + } + else + { + bytecount = 0; + } + } + } else { + if ((bytecount % 2) == 0) { + channel = (prev_byte << 8) + b; + frame = channel >> 15; + channeln = (channel >> (10+datalength)) & 0x0F; + data = channel & (0x03FF+(0x0400*datalength)); + if(channeln==0 && data<10) // discard frame if throttle misbehaves + { + frame_error=1; + } + if (channeln < 12 && !frame_error) + CaptureValueTemp[channeln] = data; + } + } + if (bytecount == 16) { + //PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF,byte_array,16); //00 2c 58 84 b0 dc ff + bytecount = 0; + sync = 0; + sync_of = 0; + if (!frame_error) + { + for(int i=0;i<12;i++) + { + CaptureValue[i] = CaptureValueTemp[i]; + } + } + frame_error=0; + } + prev_byte = b; + return 0; +} + +/* Interrupt handler for USART */ +void SBUS_IRQHandler(uint32_t usart_id) { + /* by always reading DR after SR make sure to clear any error interrupts */ + volatile uint16_t sr = pios_sbus_cfg.pios_usart_sbus_cfg->regs->SR; + volatile uint8_t b = pios_sbus_cfg.pios_usart_sbus_cfg->regs->DR; + + /* check if RXNE flag is set */ + if (sr & USART_SR_RXNE) { + if (PIOS_SBUS_Decode(b) < 0) { + /* Here we could add some error handling */ + } + } + + if (sr & USART_SR_TXE) { // check if TXE flag is set + /* Disable TXE interrupt (TXEIE=0) */ + USART_ITConfig(pios_sbus_cfg.pios_usart_sbus_cfg->regs, USART_IT_TXE, DISABLE); + } + /* byte arrived so clear "watchdog" timer */ + supv_timer=0; +} + +/** + *@brief This function is called between frames and when a sbus word hasnt been decoded for too long + *@brief clears the channel values + */ +void PIOS_SBUS_irq_handler() { + /* 125hz */ + supv_timer++; + if(supv_timer > 5) { + /* sync between frames */ + sync = 0; + bytecount = 0; + prev_byte = 0xFF; + frame_error = 0; + sync_of++; + /* watchdog activated after 100ms silence */ + if (sync_of > 12) { + /* signal lost */ + sync_of = 0; + for (int i = 0; i < 12; i++) { + CaptureValue[i] = 0; + CaptureValueTemp[i] = 0; + } + } + supv_timer = 0; + } +} + +#endif + +/** + * @} + * @} + */ diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 0eabe36a2..8b00a32a3 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -34,8 +34,8 @@ #include "pios_spektrum_priv.h" #if defined(PIOS_INCLUDE_SPEKTRUM) -#if defined(PIOS_INCLUDE_PWM) -#error "Both PWM and SPEKTRUM input defined, choose only one" +#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_SBUS) +#error "Both SPEKTRUM and either of PWM or SBUS inputs defined, choose only one" #endif #if defined(PIOS_COM_AUX) #error "AUX com cannot be used with SPEKTRUM" diff --git a/flight/PiOS/inc/pios_sbus.h b/flight/PiOS/inc/pios_sbus.h new file mode 100644 index 000000000..eeb3d2bdd --- /dev/null +++ b/flight/PiOS/inc/pios_sbus.h @@ -0,0 +1,46 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SBUS Futaba S.Bus receiver functions + * @{ + * + * @file pios_sbus.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Futaba S.Bus functions header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_SBUS_H +#define PIOS_SBUS_H + +/* Global Types */ + +/* Public Functions */ +extern void PIOS_SBUS_Init(void); +extern int32_t PIOS_SBUS_Decode(uint8_t b); +extern int16_t PIOS_SBUS_Get(int8_t Channel); +extern void SBUS_IRQHandler(uint32_t usart_id); + +#endif /* PIOS_SBUS_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_sbus_priv.h b/flight/PiOS/inc/pios_sbus_priv.h new file mode 100644 index 000000000..0dfd16b8a --- /dev/null +++ b/flight/PiOS/inc/pios_sbus_priv.h @@ -0,0 +1,57 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SBUS S.Bus Functions + * @brief PIOS interface to read and write from Futaba S.Bus port + * @{ + * + * @file pios_sbus_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Futaba S.Bus Private structures. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_SBUS_PRIV_H +#define PIOS_SBUS_PRIV_H + +#include +#include +#include + +struct pios_sbus_cfg { + const struct pios_usart_cfg *pios_usart_sbus_cfg; + GPIO_InitTypeDef gpio_init; + uint32_t remap; /* GPIO_Remap_* */ + struct stm32_irq irq; + GPIO_TypeDef *port; + uint16_t pin; +}; + +extern void PIOS_SBUS_irq_handler(); + +extern uint8_t pios_sbus_num_channels; +extern const struct pios_sbus_cfg pios_sbus_cfg; + +#endif /* PIOS_SBUS_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index ff7cd1af5..39802a2a9 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -79,6 +79,7 @@ #include #include #include +#include #include #include #include From 50e819192b58c73a447b1c934d747d4383117d44 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Wed, 15 Jun 2011 22:37:44 +0300 Subject: [PATCH 2/8] usart: make CC USART ports compile-time configurable TODO: This should be dynamic in the future. But for now define any compatile combination of: USE_I2C (shared with USART3) USE_TELEMETRY USE_GPS USE_SPEKTRUM USE_SBUS (USART1 only, it needs an invertor) and optionally define PIOS_PORT_* to USART port numbers Defaults are: #define PIOS_PORT_TELEMETRY 1 #define PIOS_PORT_GPS 3 #define PIOS_PORT_SPEKTRUM 3 #define PIOS_PORT_SBUS 1 #define USE_TELEMETRY #define USE_GPS Telemetry, GPS and PWM input are enabled by default. --- flight/CopterControl/System/inc/pios_config.h | 141 +++++++++++++++++- flight/CopterControl/System/pios_board.c | 64 ++++---- 2 files changed, 166 insertions(+), 39 deletions(-) diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index ab9251b12..2b3a8eb7a 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -30,11 +30,9 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - #ifndef PIOS_CONFIG_H #define PIOS_CONFIG_H - /* Enable/Disable PiOS Modules */ #define PIOS_INCLUDE_ADC #define PIOS_INCLUDE_DELAY @@ -45,17 +43,146 @@ #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED -#if defined(USE_SPEKTRUM) -#define PIOS_INCLUDE_SPEKTRUM -#elif defined(USE_SBUS) -#define PIOS_INCLUDE_SBUS +/* + * Serial port configuration. + * TODO: This should be dynamic in the future. + * But for now define any compatile combination of: + * USE_I2C (shared with USART3) + * USE_TELEMETRY + * USE_GPS + * USE_SPEKTRUM + * USE_SBUS (USART1 only, it needs an invertor) + * and optionally define PIOS_PORT_* to USART port numbers + */ + +/* Current defaults */ +#define USE_TELEMETRY +#define USE_GPS + +/* Serial telemetry: USART1 or USART3 */ +#if !defined(PIOS_PORT_TELEMETRY) +#define PIOS_PORT_TELEMETRY 1 +#endif + +/* GPS receiver: USART1 or USART3 */ +#if !defined(PIOS_PORT_GPS) +#define PIOS_PORT_GPS 3 +#endif + +/* Spektrum satellite receiver: USART1 or USART3 */ +#if !defined(PIOS_PORT_SPEKTRUM) +#define PIOS_PORT_SPEKTRUM 3 +#endif + +/* Futaba S.Bus receiver: USART1 only (needs invertor) */ +#if !defined(PIOS_PORT_SBUS) +#define PIOS_PORT_SBUS 1 +#endif + +/* + * Define USART ports and check for conflicts. + * Make sure it does not conflict with each other and with I2C. + */ +#define USART_GPIO(port) (((port) == 1) ? GPIOA : GPIOB) +#define USART_RXIO(port) (((port) == 1) ? GPIO_Pin_10 : GPIO_Pin_11) +#define USART_TXIO(port) (((port) == 1) ? GPIO_Pin_9 : GPIO_Pin_10) + +#if defined(USE_TELEMETRY) +#if defined(USE_I2C) && (PIOS_PORT_TELEMETRY == 3) +#error defined(USE_I2C) && (PIOS_PORT_TELEMETRY == 3) +#endif +#if (PIOS_PORT_TELEMETRY == 1) +#define PIOS_USART_TELEMETRY USART1 +#define PIOS_IRQH_TELEMETRY USART1_IRQHandler +#define PIOS_IRQC_TELEMETRY USART1_IRQn #else +#define PIOS_USART_TELEMETRY USART3 +#define PIOS_IRQH_TELEMETRY USART3_IRQHandler +#define PIOS_IRQC_TELEMETRY USART3_IRQn +#endif +#define PIOS_GPIO_TELEMETRY USART_GPIO(PIOS_PORT_TELEMETRY) +#define PIOS_RXIO_TELEMETRY USART_RXIO(PIOS_PORT_TELEMETRY) +#define PIOS_TXIO_TELEMETRY USART_TXIO(PIOS_PORT_TELEMETRY) +#endif + +#if defined(USE_GPS) +#if defined(USE_I2C) && (PIOS_PORT_GPS == 3) +#error defined(USE_I2C) && (PIOS_PORT_GPS == 3) +#endif +#if defined(USE_TELEMETRY) && (PIOS_PORT_TELEMETRY == PIOS_PORT_GPS) +#error defined(USE_TELEMETRY) && (PIOS_PORT_TELEMETRY == PIOS_PORT_GPS) +#endif +#if (PIOS_PORT_GPS == 1) +#define PIOS_USART_GPS USART1 +#define PIOS_IRQH_GPS USART1_IRQHandler +#define PIOS_IRQC_GPS USART1_IRQn +#else +#define PIOS_USART_GPS USART3 +#define PIOS_IRQH_GPS USART3_IRQHandler +#define PIOS_IRQC_GPS USART3_IRQn +#endif +#define PIOS_GPIO_GPS USART_GPIO(PIOS_PORT_GPS) +#define PIOS_RXIO_GPS USART_RXIO(PIOS_PORT_GPS) +#define PIOS_TXIO_GPS USART_TXIO(PIOS_PORT_GPS) #define PIOS_INCLUDE_GPS +#endif + +#if defined(USE_SPEKTRUM) +#if defined(USE_I2C) && (PIOS_PORT_SPEKTRUM == 3) +#error defined(USE_I2C) && (PIOS_PORT_SPEKTRUM == 3) +#endif +#if defined(USE_TELEMETRY) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_TELEMETRY) +#error defined(USE_TELEMETRY) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_TELEMETRY) +#endif +#if defined(USE_GPS) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_GPS) +#error defined(USE_GPS) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_GPS) +#endif +#if defined(USE_SBUS) +#error defined(USE_SPEKTRUM) && defined(USE_SBUS) +#endif +#if (PIOS_PORT_SPEKTRUM == 1) +#define PIOS_USART_SPEKTRUM USART1 +#define PIOS_IRQH_SPEKTRUM USART1_IRQHandler +#define PIOS_IRQC_SPEKTRUM USART1_IRQn +#else +#define PIOS_USART_SPEKTRUM USART3 +#define PIOS_IRQH_SPEKTRUM USART3_IRQHandler +#define PIOS_IRQC_SPEKTRUM USART3_IRQn +#endif +#define PIOS_GPIO_SPEKTRUM USART_GPIO(PIOS_PORT_SPEKTRUM) +#define PIOS_RXIO_SPEKTRUM USART_RXIO(PIOS_PORT_SPEKTRUM) +#define PIOS_TXIO_SPEKTRUM USART_TXIO(PIOS_PORT_SPEKTRUM) +#define PIOS_INCLUDE_SPEKTRUM +#endif + +#if defined(USE_SBUS) +#if (PIOS_PORT_SBUS != 1) +#error (PIOS_PORT_SBUS != 1) +#endif +#if defined(USE_TELEMETRY) && (PIOS_PORT_SBUS == PIOS_PORT_TELEMETRY) +#error defined(USE_TELEMETRY) && (PIOS_PORT_SBUS == PIOS_PORT_TELEMETRY) +#endif +#if defined(USE_GPS) && (PIOS_PORT_SBUS == PIOS_PORT_GPS) +#error defined(USE_GPS) && (PIOS_PORT_SBUS == PIOS_PORT_GPS) +#endif +#if defined(USE_SPEKTRUM) +#error defined(USE_SPEKTRUM) && defined(USE_SBUS) +#endif +#define PIOS_USART_SBUS USART1 +#define PIOS_IRQH_SBUS USART1_IRQHandler +#define PIOS_IRQC_SBUS USART1_IRQn +#define PIOS_GPIO_SBUS USART_GPIO(PIOS_PORT_SBUS) +#define PIOS_RXIO_SBUS USART_RXIO(PIOS_PORT_SBUS) +#define PIOS_TXIO_SBUS USART_TXIO(PIOS_PORT_SBUS) +#define PIOS_INCLUDE_SBUS +#endif + +/* Receiver interfaces - only one allowed */ +#if !defined(USE_SPEKTRUM) && !defined(USE_SBUS) //#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PWM #endif - #define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SYS diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index ab84ba5c2..b5aabc8b1 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -204,9 +204,9 @@ void PIOS_ADC_handler() { * Telemetry USART */ void PIOS_USART_telem_irq_handler(void); -void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_telem_irq_handler"))); +void PIOS_IRQH_TELEMETRY() __attribute__ ((alias ("PIOS_USART_telem_irq_handler"))); const struct pios_usart_cfg pios_usart_telem_cfg = { - .regs = USART1, + .regs = PIOS_USART_TELEMETRY, .init = { #if defined (PIOS_COM_TELEM_BAUDRATE) .USART_BaudRate = PIOS_COM_TELEM_BAUDRATE, @@ -222,24 +222,24 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { .irq = { .handler = PIOS_USART_telem_irq_handler, .init = { - .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannel = PIOS_IRQC_TELEMETRY, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelCmd = ENABLE, }, }, .rx = { - .gpio = GPIOA, + .gpio = PIOS_GPIO_TELEMETRY, .init = { - .GPIO_Pin = GPIO_Pin_10, + .GPIO_Pin = PIOS_RXIO_TELEMETRY, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_IPU, }, }, .tx = { - .gpio = GPIOA, + .gpio = PIOS_GPIO_TELEMETRY, .init = { - .GPIO_Pin = GPIO_Pin_9, + .GPIO_Pin = PIOS_TXIO_TELEMETRY, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_AF_PP, }, @@ -251,9 +251,9 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { * GPS USART */ void PIOS_USART_gps_irq_handler(void); -void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_gps_irq_handler"))); +void PIOS_IRQH_GPS() __attribute__ ((alias ("PIOS_USART_gps_irq_handler"))); const struct pios_usart_cfg pios_usart_gps_cfg = { - .regs = USART3, + .regs = PIOS_USART_GPS, .init = { #if defined (PIOS_COM_GPS_BAUDRATE) .USART_BaudRate = PIOS_COM_GPS_BAUDRATE, @@ -269,24 +269,24 @@ const struct pios_usart_cfg pios_usart_gps_cfg = { .irq = { .handler = PIOS_USART_gps_irq_handler, .init = { - .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannel = PIOS_IRQC_GPS, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelCmd = ENABLE, }, }, .rx = { - .gpio = GPIOB, + .gpio = PIOS_GPIO_GPS, .init = { - .GPIO_Pin = GPIO_Pin_11, + .GPIO_Pin = PIOS_RXIO_GPS, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_IPU, }, }, .tx = { - .gpio = GPIOB, + .gpio = PIOS_GPIO_GPS, .init = { - .GPIO_Pin = GPIO_Pin_10, + .GPIO_Pin = PIOS_TXIO_GPS, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_AF_PP, }, @@ -299,9 +299,9 @@ const struct pios_usart_cfg pios_usart_gps_cfg = { * SPEKTRUM USART */ void PIOS_USART_spektrum_irq_handler(void); -void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_spektrum_irq_handler"))); +void PIOS_IRQH_SPEKTRUM() __attribute__ ((alias ("PIOS_USART_spektrum_irq_handler"))); const struct pios_usart_cfg pios_usart_spektrum_cfg = { - .regs = USART3, + .regs = PIOS_USART_SPEKTRUM, .init = { #if defined (PIOS_COM_SPEKTRUM_BAUDRATE) .USART_BaudRate = PIOS_COM_SPEKTRUM_BAUDRATE, @@ -317,24 +317,24 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = { .irq = { .handler = PIOS_USART_spektrum_irq_handler, .init = { - .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannel = PIOS_IRQC_SPEKTRUM, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, .NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelCmd = ENABLE, }, }, .rx = { - .gpio = GPIOB, + .gpio = PIOS_GPIO_SPEKTRUM, .init = { - .GPIO_Pin = GPIO_Pin_11, + .GPIO_Pin = PIOS_RXIO_SPEKTRUM, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_IPU, }, }, .tx = { - .gpio = GPIOB, + .gpio = PIOS_GPIO_SPEKTRUM, .init = { - .GPIO_Pin = GPIO_Pin_10, + .GPIO_Pin = PIOS_TXIO_SPEKTRUM, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_IN_FLOATING, }, @@ -365,8 +365,8 @@ const struct pios_spektrum_cfg pios_spektrum_cfg = { .NVIC_IRQChannelCmd = ENABLE, }, }, - .port = GPIOB, - .pin = GPIO_Pin_11, + .port = PIOS_GPIO_SPEKTRUM, + .pin = PIOS_RXIO_SPEKTRUM, }; void PIOS_SUPV_irq_handler() { @@ -388,9 +388,9 @@ void PIOS_SUPV_irq_handler() { * SBUS USART */ void PIOS_USART_sbus_irq_handler(void); -void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_sbus_irq_handler"))); +void PIOS_IRQH_SBUS() __attribute__ ((alias ("PIOS_USART_sbus_irq_handler"))); const struct pios_usart_cfg pios_usart_sbus_cfg = { - .regs = USART3, + .regs = PIOS_USART_SBUS, .init = { #if defined (PIOS_COM_SBUS_BAUDRATE) .USART_BaudRate = PIOS_COM_SBUS_BAUDRATE, @@ -406,24 +406,24 @@ const struct pios_usart_cfg pios_usart_sbus_cfg = { .irq = { .handler = PIOS_USART_sbus_irq_handler, .init = { - .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannel = PIOS_IRQC_SBUS, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, .NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelCmd = ENABLE, }, }, .rx = { - .gpio = GPIOB, + .gpio = PIOS_GPIO_SBUS, .init = { - .GPIO_Pin = GPIO_Pin_11, + .GPIO_Pin = PIOS_RXIO_SBUS, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_IPU, }, }, .tx = { - .gpio = GPIOB, + .gpio = PIOS_GPIO_SBUS, .init = { - .GPIO_Pin = GPIO_Pin_10, + .GPIO_Pin = PIOS_TXIO_SBUS, .GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Mode = GPIO_Mode_IN_FLOATING, }, @@ -454,8 +454,8 @@ const struct pios_sbus_cfg pios_sbus_cfg = { .NVIC_IRQChannelCmd = ENABLE, }, }, - .port = GPIOB, - .pin = GPIO_Pin_11, + .port = PIOS_GPIO_SBUS, + .pin = PIOS_RXIO_SBUS, }; void PIOS_SUPV_irq_handler() { From aeda61d252269a2009d34323821b82bd9f850ffb Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Thu, 16 Jun 2011 15:06:01 +0300 Subject: [PATCH 3/8] usart: serial telemetry can be disabled to free USART It was tested being merged with OP-472_CorvusCorax_CopterControl-Guidance_v3 branch, Spektrum on USART3 and GPS on USART1 and seems to work. Currently defaults mimic original behavior, that is, if USE_SPEKTRUM is not defined - define USE_PWM and USE_GPS. Thsi should be refactored later to make it configurable from the Makefile. Also it was not ported to the OP MB: it currently does not support the S.Bus hardware and still has original behavior with the patch. But this is one more step to dynamic configuration of ports. --- flight/CopterControl/System/inc/pios_config.h | 5 ++++- flight/CopterControl/System/pios_board.c | 6 ++++++ flight/PiOS/Common/pios_com.c | 6 ++++-- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 2b3a8eb7a..fddef9c72 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -55,9 +55,11 @@ * and optionally define PIOS_PORT_* to USART port numbers */ -/* Current defaults */ +/* Current defaults - mimic original behavior */ #define USE_TELEMETRY +#if !defined(USE_SPEKTRUM) #define USE_GPS +#endif /* Serial telemetry: USART1 or USART3 */ #if !defined(PIOS_PORT_TELEMETRY) @@ -103,6 +105,7 @@ #define PIOS_GPIO_TELEMETRY USART_GPIO(PIOS_PORT_TELEMETRY) #define PIOS_RXIO_TELEMETRY USART_RXIO(PIOS_PORT_TELEMETRY) #define PIOS_TXIO_TELEMETRY USART_TXIO(PIOS_PORT_TELEMETRY) +#define PIOS_INCLUDE_TELEMETRY_RF #endif #if defined(USE_GPS) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index b5aabc8b1..142bb2ba3 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -200,6 +200,7 @@ void PIOS_ADC_handler() { #include "pios_usart_priv.h" +#if defined(PIOS_INCLUDE_TELEMETRY_RF) /* * Telemetry USART */ @@ -245,6 +246,7 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { }, }, }; +#endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_GPS) /* @@ -472,11 +474,13 @@ void PIOS_SUPV_irq_handler() { } #endif /* PIOS_INCLUDE_SBUS */ +#if defined(PIOS_INCLUDE_TELEMETRY_RF) static uint32_t pios_usart_telem_rf_id; void PIOS_USART_telem_irq_handler(void) { PIOS_USART_IRQ_Handler(pios_usart_telem_rf_id); } +#endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_GPS) static uint32_t pios_usart_gps_id; @@ -807,12 +811,14 @@ void PIOS_Board_Init(void) { /* Initialize the PiOS library */ #if defined(PIOS_INCLUDE_COM) +#if defined(PIOS_INCLUDE_TELEMETRY_RF) if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) { PIOS_DEBUG_Assert(0); } +#endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_GPS) if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { PIOS_DEBUG_Assert(0); diff --git a/flight/PiOS/Common/pios_com.c b/flight/PiOS/Common/pios_com.c index 0d2c21d30..4bd00c22b 100644 --- a/flight/PiOS/Common/pios_com.c +++ b/flight/PiOS/Common/pios_com.c @@ -38,7 +38,7 @@ static bool PIOS_COM_validate(struct pios_com_dev * com_dev) { - return (com_dev->magic == PIOS_COM_DEV_MAGIC); + return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); } #if defined(PIOS_INCLUDE_FREERTOS) && 0 @@ -298,7 +298,9 @@ int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id) if (!PIOS_COM_validate(com_dev)) { /* Undefined COM port for this board (see pios_board.c) */ - PIOS_DEBUG_Assert(0); + /* This is commented out so com_id=NULL can be used to disable telemetry */ + //PIOS_DEBUG_Assert(0); + return 0; } if (!com_dev->driver->rx_avail) { From 311902f1f234a676eb3451c166b2bd391f40eedc Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sat, 18 Jun 2011 23:19:57 +0300 Subject: [PATCH 4/8] sbus: implemented S.Bus stream decoding --- flight/CopterControl/Makefile | 10 +- flight/CopterControl/System/inc/pios_config.h | 9 +- flight/CopterControl/System/pios_board.c | 6 +- flight/PiOS/Boards/STM32103CB_CC_Rev1.h | 2 +- flight/PiOS/STM32F10x/pios_sbus.c | 284 +++++++++--------- flight/PiOS/inc/pios_sbus.h | 8 +- flight/PiOS/inc/pios_sbus_priv.h | 34 ++- 7 files changed, 191 insertions(+), 162 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 96d3aefc4..294e80e5f 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -45,6 +45,8 @@ ENABLE_DEBUG_PINS ?= NO # Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs ENABLE_AUX_UART ?= NO +USE_TELEMETRY ?= YES +USE_GPS ?= NO USE_SPEKTRUM ?= NO USE_SBUS ?= NO @@ -380,18 +382,22 @@ endif ifeq ($(ERASE_FLASH), YES) CDEFS += -DERASE_FLASH endif +ifeq ($(USE_TELEMETRY), YES) +CDEFS += -DUSE_TELEMETRY +endif +ifeq ($(USE_GPS), YES) +CDEFS += -DUSE_GPS +endif ifeq ($(USE_SPEKTRUM), YES) CDEFS += -DUSE_SPEKTRUM endif ifeq ($(USE_SBUS), YES) CDEFS += -DUSE_SBUS endif - ifeq ($(USE_I2C), YES) CDEFS += -DUSE_I2C endif - # Place project-specific -D and/or -U options for # Assembler with preprocessor here. #ADEFS = -DUSE_IRQ_ASM_WRAPPER diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index fddef9c72..430b0e3ee 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -55,12 +55,6 @@ * and optionally define PIOS_PORT_* to USART port numbers */ -/* Current defaults - mimic original behavior */ -#define USE_TELEMETRY -#if !defined(USE_SPEKTRUM) -#define USE_GPS -#endif - /* Serial telemetry: USART1 or USART3 */ #if !defined(PIOS_PORT_TELEMETRY) #define PIOS_PORT_TELEMETRY 1 @@ -177,6 +171,9 @@ #define PIOS_GPIO_SBUS USART_GPIO(PIOS_PORT_SBUS) #define PIOS_RXIO_SBUS USART_RXIO(PIOS_PORT_SBUS) #define PIOS_TXIO_SBUS USART_TXIO(PIOS_PORT_SBUS) +#define PIOS_GPIO_INV_PORT GPIOB +#define PIOS_GPIO_INV_PIN GPIO_Pin_2 +#define PIOS_GPIO_INV_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE) #define PIOS_INCLUDE_SBUS #endif diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 142bb2ba3..72107bdee 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -397,11 +397,11 @@ const struct pios_usart_cfg pios_usart_sbus_cfg = { #if defined (PIOS_COM_SBUS_BAUDRATE) .USART_BaudRate = PIOS_COM_SBUS_BAUDRATE, #else - .USART_BaudRate = 115200, + .USART_BaudRate = 100000, #endif .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, + .USART_Parity = USART_Parity_Even, + .USART_StopBits = USART_StopBits_2, .USART_HardwareFlowControl = USART_HardwareFlowControl_None, .USART_Mode = USART_Mode_Rx, }, diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index e80cb960c..b5b570e53 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -162,7 +162,7 @@ extern uint32_t pios_com_spektrum_id; #endif #ifdef PIOS_INCLUDE_SBUS -#define PIOS_COM_SBUS_BAUDRATE 115200 +#define PIOS_COM_SBUS_BAUDRATE 100000 extern uint32_t pios_com_sbus_id; #define PIOS_COM_SBUS (pios_com_sbus_id) #endif diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index 5862c527a..120cd2371 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -33,195 +33,193 @@ #include "pios_sbus_priv.h" #if defined(PIOS_INCLUDE_SBUS) -#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_SPEKTRUM) -#error "Both SBUS and either of PWM or SPEKTRUM inputs defined, choose only one" -#endif -#if defined(PIOS_COM_AUX) -#error "AUX com cannot be used with SBUS" -#endif - -/** - * @Note Framesyncing: - * The code resets the watchdog timer whenever a single byte is received, so what watchdog code - * is never called if regularly getting bytes. - * RTC timer is running @625Hz, supervisor timer has divider 5 so frame sync comes every 1/125Hz=8ms. - * Good for both 11ms and 22ms framecycles - */ /* Global Variables */ /* Local Variables */ -static uint16_t CaptureValue[12],CaptureValueTemp[12]; -static uint8_t prev_byte = 0xFF, sync = 0, bytecount = 0, datalength=0, frame_error=0, byte_array[20] = { 0 }; -uint8_t sync_of = 0; -uint16_t supv_timer=0; +static uint16_t channel_data[SBUS_NUMBER_OF_CHANNELS]; +static uint8_t received_data[SBUS_FRAME_LENGTH - 2]; +static uint8_t receive_timer; +static uint8_t failsafe_timer; +static uint8_t frame_found; /** -* Bind and Initialise S.Bus receiver -*/ + * reset_channels function clears all channel data in case of + * lost signal or explicit failsafe flag from the S.Bus data stream + */ +static void reset_channels(void) +{ + for (int i = 0; i < SBUS_NUMBER_OF_CHANNELS; i++) { + channel_data[i] = 0; + } +} + +/** + * unroll_channels function computes channel_data[] from received_data[] + * For efficiency it unrolls first 8 channel without loops + */ +static void unroll_channels(void) +{ + uint8_t *s = received_data; + uint16_t *d = channel_data; + +#if (SBUS_NUMBER_OF_CHANNELS != 8) +#error Current S.Bus code unrolls only first 8 channels +#endif + +#define F(v,s) ((v) >> s) & 0x7ff + *d++ = F(s[0] | s[1] << 8, 0); + *d++ = F(s[1] | s[2] << 8, 3); + *d++ = F(s[2] | s[3] << 8 | s[4] << 16, 6); + *d++ = F(s[4] | s[5] << 8, 1); + *d++ = F(s[5] | s[6] << 8, 4); + *d++ = F(s[6] | s[7] << 8 | s[8] << 16, 7); + *d++ = F(s[8] | s[9] << 8, 2); + *d = F(s[9] | s[10] << 8, 5); +} + +/** + * Initialise S.Bus receiver interface + */ void PIOS_SBUS_Init(void) { + /* Enable USART input invertor clock */ + PIOS_GPIO_INV_FUNCTION; + + /* Set invertor pin mode */ + static const GPIO_InitTypeDef GPIO_InitStructure = { + .GPIO_Pin = PIOS_GPIO_INV_PIN, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }; + GPIO_Init(PIOS_GPIO_INV_PORT, &GPIO_InitStructure); + + /* Enable invertor */ + GPIO_WriteBit(PIOS_GPIO_INV_PORT, PIOS_GPIO_INV_PIN, Bit_SET); + /* Init RTC supervisor timer interrupt */ - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = RTC_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + static const NVIC_InitTypeDef NVIC_InitStructure = { + .NVIC_IRQChannel = RTC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }; NVIC_Init(&NVIC_InitStructure); + /* Init RTC clock */ PIOS_RTC_Init(); } /** -* Get the value of an input channel -* \param[in] Channel Number of the channel desired -* \output -1 Channel not available -* \output >0 Channel value -*/ -int16_t PIOS_SBUS_Get(int8_t Channel) + * Get the value of an input channel + * \param[in] channel Number of the channel desired (zero based) + * \output -1 channel not available + * \output >0 channel value + */ +int16_t PIOS_SBUS_Get(int8_t channel) { - /* Return error if channel not available */ - if (Channel >= 12) { + /* return error if channel is not available */ + if (channel >= SBUS_NUMBER_OF_CHANNELS) { return -1; } - return CaptureValue[Channel]; + return channel_data[channel]; } /** -* Decodes a byte -* \param[in] b byte which should be sbus decoded -* \return 0 if no error -* \return -1 if USART not available -* \return -2 if buffer full (retry) -* \note Applications shouldn't call these functions directly -*/ -int32_t PIOS_SBUS_Decode(uint8_t b) + * Decodes a byte + * \param[in] b byte which should be decoded + */ +void PIOS_SBUS_Decode(uint8_t b) { - static uint16_t channel = 0; /*, sync_word = 0;*/ - uint8_t channeln = 0, frame = 0; - uint16_t data = 0; - byte_array[bytecount] = b; - bytecount++; - if (sync == 0) { - //sync_word = (prev_byte << 8) + b; -#if 0 - /* maybe create object to show this data */ - if(bytecount==1) - { - /* record losscounter into channel8 */ - CaptureValueTemp[7]=b; - /* instant write */ - CaptureValue[7]=b; - } -#endif - /* Known sync bytes, 0x01, 0x02, 0x12 */ - if (bytecount == 2) { - if (b == 0x01) { - datalength=0; // 10bit - //frames=1; - sync = 1; - bytecount = 2; - } - else if(b == 0x02) { - datalength=0; // 10bit - //frames=2; - sync = 1; - bytecount = 2; - } - else if(b == 0x12) { - datalength=1; // 11bit - //frames=2; - sync = 1; - bytecount = 2; - } - else - { - bytecount = 0; - } + static uint8_t byte_count; + + if (frame_found == 0) { + /* no frame found yet, waiting for start byte */ + if (b == SBUS_SOF_BYTE) { + byte_count = 0; + frame_found = 1; } } else { - if ((bytecount % 2) == 0) { - channel = (prev_byte << 8) + b; - frame = channel >> 15; - channeln = (channel >> (10+datalength)) & 0x0F; - data = channel & (0x03FF+(0x0400*datalength)); - if(channeln==0 && data<10) // discard frame if throttle misbehaves - { - frame_error=1; + /* do not store start and end of frame bytes */ + if (byte_count < SBUS_FRAME_LENGTH - 2) { + /* store next byte */ + received_data[byte_count++] = b; + } else { + if (b == SBUS_EOF_BYTE) { + /* full frame received */ + uint8_t flags = received_data[SBUS_FRAME_LENGTH - 3]; + if (flags & SBUS_FLAG_FL) { + /* frame lost, do not update */ + } else if (flags & SBUS_FLAG_FS) { + /* failsafe flag active */ + reset_channels(); + } else { + /* data looking good */ + unroll_channels(); + failsafe_timer = 0; + } + } else { + /* discard whole frame */ } - if (channeln < 12 && !frame_error) - CaptureValueTemp[channeln] = data; + + /* prepare for the next frame */ + frame_found = 0; } } - if (bytecount == 16) { - //PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF,byte_array,16); //00 2c 58 84 b0 dc ff - bytecount = 0; - sync = 0; - sync_of = 0; - if (!frame_error) - { - for(int i=0;i<12;i++) - { - CaptureValue[i] = CaptureValueTemp[i]; - } - } - frame_error=0; - } - prev_byte = b; - return 0; } /* Interrupt handler for USART */ -void SBUS_IRQHandler(uint32_t usart_id) { +void SBUS_IRQHandler(uint32_t usart_id) +{ /* by always reading DR after SR make sure to clear any error interrupts */ volatile uint16_t sr = pios_sbus_cfg.pios_usart_sbus_cfg->regs->SR; volatile uint8_t b = pios_sbus_cfg.pios_usart_sbus_cfg->regs->DR; - - /* check if RXNE flag is set */ + + /* process received byte if new one has arrived */ if (sr & USART_SR_RXNE) { - if (PIOS_SBUS_Decode(b) < 0) { - /* Here we could add some error handling */ - } + PIOS_SBUS_Decode(b); + + /* byte has arrived, clear receive timer */ + receive_timer = 0; } - if (sr & USART_SR_TXE) { // check if TXE flag is set + /* ignore TXE interrupts */ + if (sr & USART_SR_TXE) { /* Disable TXE interrupt (TXEIE=0) */ USART_ITConfig(pios_sbus_cfg.pios_usart_sbus_cfg->regs, USART_IT_TXE, DISABLE); } - /* byte arrived so clear "watchdog" timer */ - supv_timer=0; } /** - *@brief This function is called between frames and when a sbus word hasnt been decoded for too long - *@brief clears the channel values + * Input data superviser is called periodically and provides + * two functions: frame syncing and failsafe triggering. + * + * S.Bus frames come at 7ms (HS) or 14ms (FS) rate at 100000bps. RTC + * timer is running at 625Hz (1.6ms). So with divider 2 it gives + * 3.2ms pause between frames which is good for both S.Bus data rates. + * + * Data receive function must clear the receive_timer to confirm new + * data reception. If no new data received in 100ms, we must call the + * failsafe function which clears all channels. */ -void PIOS_SBUS_irq_handler() { - /* 125hz */ - supv_timer++; - if(supv_timer > 5) { - /* sync between frames */ - sync = 0; - bytecount = 0; - prev_byte = 0xFF; - frame_error = 0; - sync_of++; - /* watchdog activated after 100ms silence */ - if (sync_of > 12) { - /* signal lost */ - sync_of = 0; - for (int i = 0; i < 12; i++) { - CaptureValue[i] = 0; - CaptureValueTemp[i] = 0; - } - } - supv_timer = 0; +void PIOS_SBUS_irq_handler() +{ + /* waiting for new frame if no bytes were received in 3.2ms */ + if (++receive_timer > 2) { + receive_timer = 0; + frame_found = 0; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++failsafe_timer > 64) { + failsafe_timer = 0; + reset_channels(); } } #endif /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_sbus.h b/flight/PiOS/inc/pios_sbus.h index eeb3d2bdd..3e99f6f6c 100644 --- a/flight/PiOS/inc/pios_sbus.h +++ b/flight/PiOS/inc/pios_sbus.h @@ -34,13 +34,13 @@ /* Public Functions */ extern void PIOS_SBUS_Init(void); -extern int32_t PIOS_SBUS_Decode(uint8_t b); +extern void PIOS_SBUS_Decode(uint8_t b); extern int16_t PIOS_SBUS_Get(int8_t Channel); extern void SBUS_IRQHandler(uint32_t usart_id); #endif /* PIOS_SBUS_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_sbus_priv.h b/flight/PiOS/inc/pios_sbus_priv.h index 0dfd16b8a..ff774d75b 100644 --- a/flight/PiOS/inc/pios_sbus_priv.h +++ b/flight/PiOS/inc/pios_sbus_priv.h @@ -35,6 +35,36 @@ #include #include +/* + * S.Bus serial port settings: + * 100000bps inverted serial stream, 8 bits, even parity, 2 stop bits + * frame period is 7ms (HS) or 14ms (FS) + * + * Frame structure: + * 1 byte - 0x0f (start of frame byte) + * 22 bytes - channel data (11 bit/channel, 16 channels, LSB first) + * 1 byte - bit flags: + * 0x01 - digital channel 1, + * 0x02 - digital channel 2, + * 0x04 - lost frame flag, + * 0x08 - failsafe flag, + * 0xf0 - reserved + * 1 byte - 0x00 (end of frame byte) + */ +#define SBUS_FRAME_LENGTH (1+22+1+1) +#define SBUS_SOF_BYTE 0x0f +#define SBUS_EOF_BYTE 0x00 +#define SBUS_FLAG_DG1 0x01 +#define SBUS_FLAG_DG2 0x02 +#define SBUS_FLAG_FL 0x04 +#define SBUS_FLAG_FS 0x08 + +/* + * S.Bus protocol provides up to 16 analog and 2 digital channels. + * Only 8 channels are currently supported by the OpenPilot. + */ +#define SBUS_NUMBER_OF_CHANNELS 8 + struct pios_sbus_cfg { const struct pios_usart_cfg *pios_usart_sbus_cfg; GPIO_InitTypeDef gpio_init; @@ -43,12 +73,10 @@ struct pios_sbus_cfg { GPIO_TypeDef *port; uint16_t pin; }; +extern const struct pios_sbus_cfg pios_sbus_cfg; extern void PIOS_SBUS_irq_handler(); -extern uint8_t pios_sbus_num_channels; -extern const struct pios_sbus_cfg pios_sbus_cfg; - #endif /* PIOS_SBUS_PRIV_H */ /** From 59da5055cd8e22ca9a74fe81450d31b5e4b685dc Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sat, 18 Jun 2011 23:54:32 +0300 Subject: [PATCH 5/8] usart: make USART ports configurable from make command line For CopterControl the following make options are available: USE_TELEMETRY=[YES|NO|1|3] (default is YES, USART1) USE_GPS=[YES|NO|1|3] (default is NO, USART3) USE_SPEKTRUM=[YES|NO|1|3] (default is NO, USART3) USE_SBUS=[YES|NO|1] (default is NO, USART1 only) --- flight/CopterControl/Makefile | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 294e80e5f..c0bfd28ae 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -382,18 +382,39 @@ endif ifeq ($(ERASE_FLASH), YES) CDEFS += -DERASE_FLASH endif + +ifneq ($(USE_TELEMETRY), NO) ifeq ($(USE_TELEMETRY), YES) CDEFS += -DUSE_TELEMETRY +else +CDEFS += -DUSE_TELEMETRY -DPIOS_PORT_TELEMETRY=$(USE_TELEMETRY) endif +endif + +ifneq ($(USE_GPS), NO) ifeq ($(USE_GPS), YES) CDEFS += -DUSE_GPS +else +CDEFS += -DUSE_GPS -DPIOS_PORT_GPS=$(USE_GPS) endif +endif + +ifneq ($(USE_SPEKTRUM), NO) ifeq ($(USE_SPEKTRUM), YES) CDEFS += -DUSE_SPEKTRUM +else +CDEFS += -DUSE_SPEKTRUM -DPIOS_PORT_SPEKTRUM=$(USE_SPEKTRUM) endif +endif + +ifneq ($(USE_SBUS), NO) ifeq ($(USE_SBUS), YES) CDEFS += -DUSE_SBUS +else +CDEFS += -DUSE_SBUS -DPIOS_PORT_SBUS=$(USE_SBUS) endif +endif + ifeq ($(USE_I2C), YES) CDEFS += -DUSE_I2C endif From 6272210df753fe021a960751f6b4805976c4cc8c Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sun, 19 Jun 2011 14:30:13 +0300 Subject: [PATCH 6/8] sbus: some code cleanup (no functional changes) --- flight/CopterControl/Makefile | 23 ++- flight/CopterControl/System/inc/pios_config.h | 116 ++------------ flight/CopterControl/System/pios_board.c | 141 ++++++++++++++---- flight/PiOS/STM32F10x/pios_sbus.c | 120 +++++++-------- flight/PiOS/inc/pios_sbus.h | 1 - flight/PiOS/inc/pios_sbus_priv.h | 13 +- 6 files changed, 206 insertions(+), 208 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index c0bfd28ae..25b2a5675 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -383,35 +383,34 @@ ifeq ($(ERASE_FLASH), YES) CDEFS += -DERASE_FLASH endif +# USART port functions. NO means do not use this function. +# YES means use with default USART port number (hardware-dependent). +# Number means use with specified USART port number (this value). ifneq ($(USE_TELEMETRY), NO) -ifeq ($(USE_TELEMETRY), YES) CDEFS += -DUSE_TELEMETRY -else -CDEFS += -DUSE_TELEMETRY -DPIOS_PORT_TELEMETRY=$(USE_TELEMETRY) +ifneq ($(USE_TELEMETRY), YES) +CDEFS += -DPIOS_PORT_TELEMETRY=$(USE_TELEMETRY) endif endif ifneq ($(USE_GPS), NO) -ifeq ($(USE_GPS), YES) CDEFS += -DUSE_GPS -else -CDEFS += -DUSE_GPS -DPIOS_PORT_GPS=$(USE_GPS) +ifneq ($(USE_GPS), YES) +CDEFS += -DPIOS_PORT_GPS=$(USE_GPS) endif endif ifneq ($(USE_SPEKTRUM), NO) -ifeq ($(USE_SPEKTRUM), YES) CDEFS += -DUSE_SPEKTRUM -else -CDEFS += -DUSE_SPEKTRUM -DPIOS_PORT_SPEKTRUM=$(USE_SPEKTRUM) +ifneq ($(USE_SPEKTRUM), YES) +CDEFS += -DPIOS_PORT_SPEKTRUM=$(USE_SPEKTRUM) endif endif ifneq ($(USE_SBUS), NO) -ifeq ($(USE_SBUS), YES) CDEFS += -DUSE_SBUS -else -CDEFS += -DUSE_SBUS -DPIOS_PORT_SBUS=$(USE_SBUS) +ifneq ($(USE_SBUS), YES) +CDEFS += -DPIOS_PORT_SBUS=$(USE_SBUS) endif endif diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 430b0e3ee..0e9baf9d1 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -75,114 +75,26 @@ #define PIOS_PORT_SBUS 1 #endif -/* - * Define USART ports and check for conflicts. - * Make sure it does not conflict with each other and with I2C. - */ -#define USART_GPIO(port) (((port) == 1) ? GPIOA : GPIOB) -#define USART_RXIO(port) (((port) == 1) ? GPIO_Pin_10 : GPIO_Pin_11) -#define USART_TXIO(port) (((port) == 1) ? GPIO_Pin_9 : GPIO_Pin_10) - -#if defined(USE_TELEMETRY) -#if defined(USE_I2C) && (PIOS_PORT_TELEMETRY == 3) -#error defined(USE_I2C) && (PIOS_PORT_TELEMETRY == 3) -#endif -#if (PIOS_PORT_TELEMETRY == 1) -#define PIOS_USART_TELEMETRY USART1 -#define PIOS_IRQH_TELEMETRY USART1_IRQHandler -#define PIOS_IRQC_TELEMETRY USART1_IRQn -#else -#define PIOS_USART_TELEMETRY USART3 -#define PIOS_IRQH_TELEMETRY USART3_IRQHandler -#define PIOS_IRQC_TELEMETRY USART3_IRQn -#endif -#define PIOS_GPIO_TELEMETRY USART_GPIO(PIOS_PORT_TELEMETRY) -#define PIOS_RXIO_TELEMETRY USART_RXIO(PIOS_PORT_TELEMETRY) -#define PIOS_TXIO_TELEMETRY USART_TXIO(PIOS_PORT_TELEMETRY) -#define PIOS_INCLUDE_TELEMETRY_RF -#endif - -#if defined(USE_GPS) -#if defined(USE_I2C) && (PIOS_PORT_GPS == 3) -#error defined(USE_I2C) && (PIOS_PORT_GPS == 3) -#endif -#if defined(USE_TELEMETRY) && (PIOS_PORT_TELEMETRY == PIOS_PORT_GPS) -#error defined(USE_TELEMETRY) && (PIOS_PORT_TELEMETRY == PIOS_PORT_GPS) -#endif -#if (PIOS_PORT_GPS == 1) -#define PIOS_USART_GPS USART1 -#define PIOS_IRQH_GPS USART1_IRQHandler -#define PIOS_IRQC_GPS USART1_IRQn -#else -#define PIOS_USART_GPS USART3 -#define PIOS_IRQH_GPS USART3_IRQHandler -#define PIOS_IRQC_GPS USART3_IRQn -#endif -#define PIOS_GPIO_GPS USART_GPIO(PIOS_PORT_GPS) -#define PIOS_RXIO_GPS USART_RXIO(PIOS_PORT_GPS) -#define PIOS_TXIO_GPS USART_TXIO(PIOS_PORT_GPS) -#define PIOS_INCLUDE_GPS -#endif - -#if defined(USE_SPEKTRUM) -#if defined(USE_I2C) && (PIOS_PORT_SPEKTRUM == 3) -#error defined(USE_I2C) && (PIOS_PORT_SPEKTRUM == 3) -#endif -#if defined(USE_TELEMETRY) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_TELEMETRY) -#error defined(USE_TELEMETRY) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_TELEMETRY) -#endif -#if defined(USE_GPS) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_GPS) -#error defined(USE_GPS) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_GPS) -#endif -#if defined(USE_SBUS) -#error defined(USE_SPEKTRUM) && defined(USE_SBUS) -#endif -#if (PIOS_PORT_SPEKTRUM == 1) -#define PIOS_USART_SPEKTRUM USART1 -#define PIOS_IRQH_SPEKTRUM USART1_IRQHandler -#define PIOS_IRQC_SPEKTRUM USART1_IRQn -#else -#define PIOS_USART_SPEKTRUM USART3 -#define PIOS_IRQH_SPEKTRUM USART3_IRQHandler -#define PIOS_IRQC_SPEKTRUM USART3_IRQn -#endif -#define PIOS_GPIO_SPEKTRUM USART_GPIO(PIOS_PORT_SPEKTRUM) -#define PIOS_RXIO_SPEKTRUM USART_RXIO(PIOS_PORT_SPEKTRUM) -#define PIOS_TXIO_SPEKTRUM USART_TXIO(PIOS_PORT_SPEKTRUM) -#define PIOS_INCLUDE_SPEKTRUM -#endif - -#if defined(USE_SBUS) -#if (PIOS_PORT_SBUS != 1) -#error (PIOS_PORT_SBUS != 1) -#endif -#if defined(USE_TELEMETRY) && (PIOS_PORT_SBUS == PIOS_PORT_TELEMETRY) -#error defined(USE_TELEMETRY) && (PIOS_PORT_SBUS == PIOS_PORT_TELEMETRY) -#endif -#if defined(USE_GPS) && (PIOS_PORT_SBUS == PIOS_PORT_GPS) -#error defined(USE_GPS) && (PIOS_PORT_SBUS == PIOS_PORT_GPS) -#endif -#if defined(USE_SPEKTRUM) -#error defined(USE_SPEKTRUM) && defined(USE_SBUS) -#endif -#define PIOS_USART_SBUS USART1 -#define PIOS_IRQH_SBUS USART1_IRQHandler -#define PIOS_IRQC_SBUS USART1_IRQn -#define PIOS_GPIO_SBUS USART_GPIO(PIOS_PORT_SBUS) -#define PIOS_RXIO_SBUS USART_RXIO(PIOS_PORT_SBUS) -#define PIOS_TXIO_SBUS USART_TXIO(PIOS_PORT_SBUS) -#define PIOS_GPIO_INV_PORT GPIOB -#define PIOS_GPIO_INV_PIN GPIO_Pin_2 -#define PIOS_GPIO_INV_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE) -#define PIOS_INCLUDE_SBUS -#endif - /* Receiver interfaces - only one allowed */ #if !defined(USE_SPEKTRUM) && !defined(USE_SBUS) //#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PWM #endif +/* USART-based PIOS modules */ +#if defined(USE_TELEMETRY) +#define PIOS_INCLUDE_TELEMETRY_RF +#endif +#if defined(USE_GPS) +#define PIOS_INCLUDE_GPS +#endif +#if defined(USE_SPEKTRUM) +#define PIOS_INCLUDE_SPEKTRUM +#endif +#if defined(USE_SBUS) +#define PIOS_INCLUDE_SBUS +#endif + #define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SYS diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 72107bdee..0a9d32740 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -33,7 +33,6 @@ #if defined(PIOS_INCLUDE_SPI) - #include /* Flash/Accel Interface @@ -200,6 +199,101 @@ void PIOS_ADC_handler() { #include "pios_usart_priv.h" +/* + * Define USART port configurations and check for conflicts + * making sure they do not conflict with each other and with I2C. + */ +#define USART_GPIO(port) (((port) == 1) ? GPIOA : GPIOB) +#define USART_RXIO(port) (((port) == 1) ? GPIO_Pin_10 : GPIO_Pin_11) +#define USART_TXIO(port) (((port) == 1) ? GPIO_Pin_9 : GPIO_Pin_10) + +#if defined(USE_TELEMETRY) +#if defined(USE_I2C) && (PIOS_PORT_TELEMETRY == 3) +#error defined(USE_I2C) && (PIOS_PORT_TELEMETRY == 3) +#endif +#if (PIOS_PORT_TELEMETRY == 1) +#define PIOS_USART_TELEMETRY USART1 +#define PIOS_IRQH_TELEMETRY USART1_IRQHandler +#define PIOS_IRQC_TELEMETRY USART1_IRQn +#else +#define PIOS_USART_TELEMETRY USART3 +#define PIOS_IRQH_TELEMETRY USART3_IRQHandler +#define PIOS_IRQC_TELEMETRY USART3_IRQn +#endif +#define PIOS_GPIO_TELEMETRY USART_GPIO(PIOS_PORT_TELEMETRY) +#define PIOS_RXIO_TELEMETRY USART_RXIO(PIOS_PORT_TELEMETRY) +#define PIOS_TXIO_TELEMETRY USART_TXIO(PIOS_PORT_TELEMETRY) +#endif + +#if defined(USE_GPS) +#if defined(USE_I2C) && (PIOS_PORT_GPS == 3) +#error defined(USE_I2C) && (PIOS_PORT_GPS == 3) +#endif +#if defined(USE_TELEMETRY) && (PIOS_PORT_TELEMETRY == PIOS_PORT_GPS) +#error defined(USE_TELEMETRY) && (PIOS_PORT_TELEMETRY == PIOS_PORT_GPS) +#endif +#if (PIOS_PORT_GPS == 1) +#define PIOS_USART_GPS USART1 +#define PIOS_IRQH_GPS USART1_IRQHandler +#define PIOS_IRQC_GPS USART1_IRQn +#else +#define PIOS_USART_GPS USART3 +#define PIOS_IRQH_GPS USART3_IRQHandler +#define PIOS_IRQC_GPS USART3_IRQn +#endif +#define PIOS_GPIO_GPS USART_GPIO(PIOS_PORT_GPS) +#define PIOS_RXIO_GPS USART_RXIO(PIOS_PORT_GPS) +#define PIOS_TXIO_GPS USART_TXIO(PIOS_PORT_GPS) +#endif + +#if defined(USE_SPEKTRUM) +#if defined(USE_I2C) && (PIOS_PORT_SPEKTRUM == 3) +#error defined(USE_I2C) && (PIOS_PORT_SPEKTRUM == 3) +#endif +#if defined(USE_TELEMETRY) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_TELEMETRY) +#error defined(USE_TELEMETRY) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_TELEMETRY) +#endif +#if defined(USE_GPS) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_GPS) +#error defined(USE_GPS) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_GPS) +#endif +#if defined(USE_SBUS) +#error defined(USE_SPEKTRUM) && defined(USE_SBUS) +#endif +#if (PIOS_PORT_SPEKTRUM == 1) +#define PIOS_USART_SPEKTRUM USART1 +#define PIOS_IRQH_SPEKTRUM USART1_IRQHandler +#define PIOS_IRQC_SPEKTRUM USART1_IRQn +#else +#define PIOS_USART_SPEKTRUM USART3 +#define PIOS_IRQH_SPEKTRUM USART3_IRQHandler +#define PIOS_IRQC_SPEKTRUM USART3_IRQn +#endif +#define PIOS_GPIO_SPEKTRUM USART_GPIO(PIOS_PORT_SPEKTRUM) +#define PIOS_RXIO_SPEKTRUM USART_RXIO(PIOS_PORT_SPEKTRUM) +#define PIOS_TXIO_SPEKTRUM USART_TXIO(PIOS_PORT_SPEKTRUM) +#endif + +#if defined(USE_SBUS) +#if (PIOS_PORT_SBUS != 1) +#error (PIOS_PORT_SBUS != 1) +#endif +#if defined(USE_TELEMETRY) && (PIOS_PORT_SBUS == PIOS_PORT_TELEMETRY) +#error defined(USE_TELEMETRY) && (PIOS_PORT_SBUS == PIOS_PORT_TELEMETRY) +#endif +#if defined(USE_GPS) && (PIOS_PORT_SBUS == PIOS_PORT_GPS) +#error defined(USE_GPS) && (PIOS_PORT_SBUS == PIOS_PORT_GPS) +#endif +#if defined(USE_SPEKTRUM) +#error defined(USE_SPEKTRUM) && defined(USE_SBUS) +#endif +#define PIOS_USART_SBUS USART1 +#define PIOS_IRQH_SBUS USART1_IRQHandler +#define PIOS_IRQC_SBUS USART1_IRQn +#define PIOS_GPIO_SBUS USART_GPIO(PIOS_PORT_SBUS) +#define PIOS_RXIO_SBUS USART_RXIO(PIOS_PORT_SBUS) +#define PIOS_TXIO_SBUS USART_TXIO(PIOS_PORT_SBUS) +#endif + #if defined(PIOS_INCLUDE_TELEMETRY_RF) /* * Telemetry USART @@ -442,22 +536,19 @@ void PIOS_USART_sbus_irq_handler(void) void RTC_IRQHandler(); void RTC_IRQHandler() __attribute__ ((alias ("PIOS_SUPV_irq_handler"))); const struct pios_sbus_cfg pios_sbus_cfg = { + /* USART configuration structure */ .pios_usart_sbus_cfg = &pios_usart_sbus_cfg, - .gpio_init = { //used for bind feature + + /* Invertor configuration */ + .gpio_clk_func = RCC_APB2PeriphClockCmd, + .gpio_clk_periph = RCC_APB2Periph_GPIOB, + .gpio_inv_port = GPIOB, + .gpio_inv_init = { + .GPIO_Pin = GPIO_Pin_2, .GPIO_Mode = GPIO_Mode_Out_PP, .GPIO_Speed = GPIO_Speed_2MHz, }, - .remap = 0, - .irq = { - .handler = RTC_IRQHandler, - .init = { - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .port = PIOS_GPIO_SBUS, - .pin = PIOS_RXIO_SBUS, + .gpio_inv_enable = Bit_SET, }; void PIOS_SUPV_irq_handler() { @@ -786,18 +877,6 @@ void PIOS_Board_Init(void) { } #endif -#if defined(PIOS_INCLUDE_SBUS) - /* SBUS init must come before comms */ - PIOS_SBUS_Init(); - - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_cfg)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_COM_Init(&pios_com_sbus_id, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_DEBUG_Assert(0); - } -#endif - /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -827,7 +906,17 @@ void PIOS_Board_Init(void) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_GPS */ -#endif /* PIOS_INCLUDE_COM */ +#if defined(PIOS_INCLUDE_SBUS) + PIOS_SBUS_Init(); + + if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_cfg)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_COM_Init(&pios_com_sbus_id, &pios_usart_com_driver, pios_usart_sbus_id)) { + PIOS_DEBUG_Assert(0); + } +#endif /* PIOS_INCLUDE_SBUS */ +#endif /* PIOS_INCLUDE_COM */ /* Remap AFIO pin */ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index 120cd2371..c742397a5 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -44,7 +44,7 @@ static uint8_t failsafe_timer; static uint8_t frame_found; /** - * reset_channels function clears all channel data in case of + * reset_channels() function clears all channel data in case of * lost signal or explicit failsafe flag from the S.Bus data stream */ static void reset_channels(void) @@ -55,8 +55,11 @@ static void reset_channels(void) } /** - * unroll_channels function computes channel_data[] from received_data[] - * For efficiency it unrolls first 8 channel without loops + * unroll_channels() function computes channel_data[] from received_data[] + * For efficiency it unrolls first 8 channels without loops. If other + * 8 channels are needed they can be unrolled using the same code + * starting from s[11] instead of s[0]. Two extra digital channels are + * accessible using (s[22] & SBUS_FLAG_DGx) logical expressions. */ static void unroll_channels(void) { @@ -75,61 +78,13 @@ static void unroll_channels(void) *d++ = F(s[5] | s[6] << 8, 4); *d++ = F(s[6] | s[7] << 8 | s[8] << 16, 7); *d++ = F(s[8] | s[9] << 8, 2); - *d = F(s[9] | s[10] << 8, 5); + *d++ = F(s[9] | s[10] << 8, 5); } /** - * Initialise S.Bus receiver interface + * process_byte() function processes incoming byte from S.Bus stream */ -void PIOS_SBUS_Init(void) -{ - /* Enable USART input invertor clock */ - PIOS_GPIO_INV_FUNCTION; - - /* Set invertor pin mode */ - static const GPIO_InitTypeDef GPIO_InitStructure = { - .GPIO_Pin = PIOS_GPIO_INV_PIN, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }; - GPIO_Init(PIOS_GPIO_INV_PORT, &GPIO_InitStructure); - - /* Enable invertor */ - GPIO_WriteBit(PIOS_GPIO_INV_PORT, PIOS_GPIO_INV_PIN, Bit_SET); - - /* Init RTC supervisor timer interrupt */ - static const NVIC_InitTypeDef NVIC_InitStructure = { - .NVIC_IRQChannel = RTC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }; - NVIC_Init(&NVIC_InitStructure); - - /* Init RTC clock */ - PIOS_RTC_Init(); -} - -/** - * Get the value of an input channel - * \param[in] channel Number of the channel desired (zero based) - * \output -1 channel not available - * \output >0 channel value - */ -int16_t PIOS_SBUS_Get(int8_t channel) -{ - /* return error if channel is not available */ - if (channel >= SBUS_NUMBER_OF_CHANNELS) { - return -1; - } - return channel_data[channel]; -} - -/** - * Decodes a byte - * \param[in] b byte which should be decoded - */ -void PIOS_SBUS_Decode(uint8_t b) +static void process_byte(uint8_t b) { static uint8_t byte_count; @@ -168,30 +123,71 @@ void PIOS_SBUS_Decode(uint8_t b) } } -/* Interrupt handler for USART */ +/** + * Initialise S.Bus receiver interface + */ +void PIOS_SBUS_Init(void) +{ + /* Enable USART input invertor clock and enable the invertor */ + (*pios_sbus_cfg.gpio_clk_func)(pios_sbus_cfg.gpio_clk_periph, ENABLE); + GPIO_Init(pios_sbus_cfg.gpio_inv_port, &pios_sbus_cfg.gpio_inv_init); + GPIO_WriteBit(pios_sbus_cfg.gpio_inv_port, + pios_sbus_cfg.gpio_inv_init.GPIO_Pin, + pios_sbus_cfg.gpio_inv_enable); + + /* Init RTC supervisor timer interrupt */ + static const NVIC_InitTypeDef NVIC_InitStructure = { + .NVIC_IRQChannel = RTC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }; + NVIC_Init(&NVIC_InitStructure); + + /* Init RTC clock */ + PIOS_RTC_Init(); +} + +/** + * Get the value of an input channel + * \param[in] channel Number of the channel desired (zero based) + * \output -1 channel not available + * \output >0 channel value + */ +int16_t PIOS_SBUS_Get(int8_t channel) +{ + /* return error if channel is not available */ + if (channel >= SBUS_NUMBER_OF_CHANNELS) { + return -1; + } + return channel_data[channel]; +} + +/** + * Interrupt handler for USART + */ void SBUS_IRQHandler(uint32_t usart_id) { /* by always reading DR after SR make sure to clear any error interrupts */ volatile uint16_t sr = pios_sbus_cfg.pios_usart_sbus_cfg->regs->SR; volatile uint8_t b = pios_sbus_cfg.pios_usart_sbus_cfg->regs->DR; - /* process received byte if new one has arrived */ + /* process received byte if one has arrived */ if (sr & USART_SR_RXNE) { - PIOS_SBUS_Decode(b); - - /* byte has arrived, clear receive timer */ + /* process byte and clear receive timer */ + process_byte(b); receive_timer = 0; } /* ignore TXE interrupts */ if (sr & USART_SR_TXE) { - /* Disable TXE interrupt (TXEIE=0) */ + /* disable TXE interrupt (TXEIE=0) */ USART_ITConfig(pios_sbus_cfg.pios_usart_sbus_cfg->regs, USART_IT_TXE, DISABLE); } } /** - * Input data superviser is called periodically and provides + * Input data supervisor is called periodically and provides * two functions: frame syncing and failsafe triggering. * * S.Bus frames come at 7ms (HS) or 14ms (FS) rate at 100000bps. RTC @@ -212,8 +208,8 @@ void PIOS_SBUS_irq_handler() /* activate failsafe if no frames have arrived in 102.4ms */ if (++failsafe_timer > 64) { - failsafe_timer = 0; reset_channels(); + failsafe_timer = 0; } } diff --git a/flight/PiOS/inc/pios_sbus.h b/flight/PiOS/inc/pios_sbus.h index 3e99f6f6c..d30b1432d 100644 --- a/flight/PiOS/inc/pios_sbus.h +++ b/flight/PiOS/inc/pios_sbus.h @@ -34,7 +34,6 @@ /* Public Functions */ extern void PIOS_SBUS_Init(void); -extern void PIOS_SBUS_Decode(uint8_t b); extern int16_t PIOS_SBUS_Get(int8_t Channel); extern void SBUS_IRQHandler(uint32_t usart_id); diff --git a/flight/PiOS/inc/pios_sbus_priv.h b/flight/PiOS/inc/pios_sbus_priv.h index ff774d75b..aa626dbdd 100644 --- a/flight/PiOS/inc/pios_sbus_priv.h +++ b/flight/PiOS/inc/pios_sbus_priv.h @@ -65,13 +65,16 @@ */ #define SBUS_NUMBER_OF_CHANNELS 8 +/* + * S.Bus configuration includes USART and programmable invertor + */ struct pios_sbus_cfg { const struct pios_usart_cfg *pios_usart_sbus_cfg; - GPIO_InitTypeDef gpio_init; - uint32_t remap; /* GPIO_Remap_* */ - struct stm32_irq irq; - GPIO_TypeDef *port; - uint16_t pin; + void (*gpio_clk_func)(uint32_t periph, FunctionalState state); + uint32_t gpio_clk_periph; + GPIO_TypeDef* gpio_inv_port; + GPIO_InitTypeDef gpio_inv_init; + BitAction gpio_inv_enable; }; extern const struct pios_sbus_cfg pios_sbus_cfg; From d372c615ed7068508fecb4d72168639868cb3863 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sun, 19 Jun 2011 16:26:14 +0300 Subject: [PATCH 7/8] sbus: added packaging target for CopterControl S.Bus firmware Serial telemetry relocated to other port (USART3) for this firmware. PPM OP MB firmware seems to be broken (the same as PWM one). Fix needed. --- package/Makefile | 38 ++++++++++++++++++++------------------ 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/package/Makefile b/package/Makefile index 1b27755e8..9b495d00e 100644 --- a/package/Makefile +++ b/package/Makefile @@ -33,16 +33,15 @@ CLEAN_GROUND := YES CLEAN_FLIGHT := YES endif -# Set up targets -FW_STEMS_COMMON := ahrs pipxtreme -FW_STEMS_INPUT := coptercontrol openpilot -FW_STEMS_TOOLS := coptercontrol -FW_STEMS_ALL := $(FW_STEMS_COMMON) $(FW_STEMS_INPUT) -FW_TARGETS_COMMON := $(addprefix fw_, $(FW_STEMS_COMMON)) -FW_TARGETS_INPUT := $(addprefix fw_, $(FW_STEMS_INPUT)) -FW_TARGETS_TOOLS := $(addprefix fw_, $(FW_STEMS_TOOLS)) -BL_TARGETS := $(addprefix bl_, $(FW_STEMS_ALL)) -BU_TARGETS := $(addprefix bu_, $(FW_STEMS_ALL)) +# Set up targets (PPM target seems to be broken at the moment) +FW_TARGETS_COMMON := $(addprefix fw_, ahrs pipxtreme) +FW_TARGETS_PWM := $(addprefix fw_, coptercontrol openpilot) +FW_TARGETS_DSM2 := $(addprefix fw_, coptercontrol openpilot) +FW_TARGETS_SBUS := $(addprefix fw_, coptercontrol) +FW_TARGETS_PPM := $(addprefix fw_, openpilot) +FW_TARGETS_TOOLS := $(addprefix fw_, coptercontrol) +BL_TARGETS := $(addprefix bl_, coptercontrol openpilot ahrs pipxtreme) +BU_TARGETS := $(addprefix bu_, coptercontrol openpilot ahrs pipxtreme) help: @echo @@ -102,9 +101,10 @@ endef # Firmware for different input drivers $(eval $(call INSTALL_TEMPLATE,fw_common,uavobjects,$(FW_DIR),,-$(PACKAGE_LBL),,,$(FW_TARGETS_COMMON),install)) -$(eval $(call INSTALL_TEMPLATE,fw_pwm,uavobjects,$(FW_DIR),,-pwm-$(PACKAGE_LBL),,clean,$(FW_TARGETS_INPUT),install)) -$(eval $(call INSTALL_TEMPLATE,fw_spektrum,uavobjects,$(FW_DIR),,-dsm2sat-$(PACKAGE_LBL),USE_SPEKTRUM=YES,clean,$(FW_TARGETS_INPUT),install)) -$(eval $(call INSTALL_TEMPLATE,fw_ppm,uavobjects,$(FW_DIR),,-ppm-$(PACKAGE_LBL),USE_PPM=YES,clean,$(FW_TARGETS_INPUT),install)) +$(eval $(call INSTALL_TEMPLATE,fw_pwm,uavobjects,$(FW_DIR),,-pwm-$(PACKAGE_LBL),,clean,$(FW_TARGETS_PWM),install)) +$(eval $(call INSTALL_TEMPLATE,fw_dsm2,uavobjects,$(FW_DIR),,-dsm2sat-$(PACKAGE_LBL),USE_SPEKTRUM=YES,clean,$(FW_TARGETS_DSM2),install)) +$(eval $(call INSTALL_TEMPLATE,fw_sbus,uavobjects,$(FW_DIR),,-sbus-$(PACKAGE_LBL),USE_SBUS=YES USE_TELEMETRY=3,clean,$(FW_TARGETS_SBUS),install)) +$(eval $(call INSTALL_TEMPLATE,fw_ppm,uavobjects,$(FW_DIR),,-ppm-$(PACKAGE_LBL),USE_PPM=YES,clean,$(FW_TARGETS_PPM),install)) # Bootloaders (change 'install' to 'bin' if you don't want to install bootloaders) $(eval $(call INSTALL_TEMPLATE,all_bl,uavobjects,$(BL_DIR),,-$(PACKAGE_LBL),,,$(BL_TARGETS),install)) @@ -119,15 +119,17 @@ $(eval $(call INSTALL_TEMPLATE,fw_tools,uavobjects,$(FE_DIR),,-flash-erase-$(PAC # They are bit complicated to support parallel (-j) builds and to create # the pwm/ppm/spektrum and CC flash eraser targets in some fixed order -fw_pwm: | # default dependencies, will be built first +fw_pwm: | # default dependencies, will be built first -fw_spektrum: | fw_pwm # ordered build +fw_dsm2: | fw_pwm # ordered build -fw_ppm: | fw_spektrum # ordered build +fw_sbus: | fw_dsm2 # ordered build -fw_tools: | fw_spektrum # ordered build, replace fw_spektrum by fw_ppm if uncommented below +fw_ppm: | fw_sbus # ordered build -package_fw: | fw_common fw_pwm fw_spektrum # fw_ppm +fw_tools: | fw_ppm # ordered build + +package_fw: | fw_common fw_pwm fw_dsm2 fw_sbus fw_ppm package_bu: | all_bu From fba201c8b320a696b0d4bacc344087f94b4daf8c Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Wed, 22 Jun 2011 01:32:18 +0300 Subject: [PATCH 8/8] usart: move default port numbers from pios_config.h to pios_board.c --- flight/CopterControl/System/inc/pios_config.h | 32 ------------------- flight/CopterControl/System/pios_board.c | 32 +++++++++++++++++++ 2 files changed, 32 insertions(+), 32 deletions(-) diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 0e9baf9d1..67b7e7836 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -43,38 +43,6 @@ #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED -/* - * Serial port configuration. - * TODO: This should be dynamic in the future. - * But for now define any compatile combination of: - * USE_I2C (shared with USART3) - * USE_TELEMETRY - * USE_GPS - * USE_SPEKTRUM - * USE_SBUS (USART1 only, it needs an invertor) - * and optionally define PIOS_PORT_* to USART port numbers - */ - -/* Serial telemetry: USART1 or USART3 */ -#if !defined(PIOS_PORT_TELEMETRY) -#define PIOS_PORT_TELEMETRY 1 -#endif - -/* GPS receiver: USART1 or USART3 */ -#if !defined(PIOS_PORT_GPS) -#define PIOS_PORT_GPS 3 -#endif - -/* Spektrum satellite receiver: USART1 or USART3 */ -#if !defined(PIOS_PORT_SPEKTRUM) -#define PIOS_PORT_SPEKTRUM 3 -#endif - -/* Futaba S.Bus receiver: USART1 only (needs invertor) */ -#if !defined(PIOS_PORT_SBUS) -#define PIOS_PORT_SBUS 1 -#endif - /* Receiver interfaces - only one allowed */ #if !defined(USE_SPEKTRUM) && !defined(USE_SBUS) //#define PIOS_INCLUDE_PPM diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 0a9d32740..cd1e7952a 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -199,6 +199,38 @@ void PIOS_ADC_handler() { #include "pios_usart_priv.h" +/* + * Serial port configuration. + * TODO: This should be dynamic in the future. + * But for now define any compatile combination of: + * USE_I2C (shared with USART3) + * USE_TELEMETRY + * USE_GPS + * USE_SPEKTRUM + * USE_SBUS (USART1 only, it needs an invertor) + * and optionally define PIOS_PORT_* to USART port numbers + */ + +/* Serial telemetry: USART1 or USART3 */ +#if !defined(PIOS_PORT_TELEMETRY) +#define PIOS_PORT_TELEMETRY 1 +#endif + +/* GPS receiver: USART1 or USART3 */ +#if !defined(PIOS_PORT_GPS) +#define PIOS_PORT_GPS 3 +#endif + +/* Spektrum satellite receiver: USART1 or USART3 */ +#if !defined(PIOS_PORT_SPEKTRUM) +#define PIOS_PORT_SPEKTRUM 3 +#endif + +/* Futaba S.Bus receiver: USART1 only (needs invertor) */ +#if !defined(PIOS_PORT_SBUS) +#define PIOS_PORT_SBUS 1 +#endif + /* * Define USART port configurations and check for conflicts * making sure they do not conflict with each other and with I2C.