mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
sbus: provide a stub based on Spektrum driver (for CC only)
This commit is contained in:
parent
9c6f2e0aea
commit
d8201ec45b
@ -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
|
||||
|
@ -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
|
||||
|
@ -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 <pios_sbus_priv.h>
|
||||
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();
|
||||
|
@ -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]);
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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)
|
||||
{
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
//-------------------------
|
||||
|
@ -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 */
|
||||
|
@ -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 */
|
||||
|
227
flight/PiOS/STM32F10x/pios_sbus.c
Normal file
227
flight/PiOS/STM32F10x/pios_sbus.c
Normal file
@ -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
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -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"
|
||||
|
46
flight/PiOS/inc/pios_sbus.h
Normal file
46
flight/PiOS/inc/pios_sbus.h
Normal file
@ -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 */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
57
flight/PiOS/inc/pios_sbus_priv.h
Normal file
57
flight/PiOS/inc/pios_sbus_priv.h
Normal file
@ -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 <pios.h>
|
||||
#include <pios_stm32.h>
|
||||
#include <pios_usart_priv.h>
|
||||
|
||||
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 */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -79,6 +79,7 @@
|
||||
#include <pios_ppm.h>
|
||||
#include <pios_pwm.h>
|
||||
#include <pios_spektrum.h>
|
||||
#include <pios_sbus.h>
|
||||
#include <pios_usb_hid.h>
|
||||
#include <pios_debug.h>
|
||||
#include <pios_gpio.h>
|
||||
|
Loading…
Reference in New Issue
Block a user