From d8201ec45bd1dba1d4110ddde84a6060ebbdb858 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Mon, 13 Jun 2011 03:04:49 +0300 Subject: [PATCH] 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