mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-19 09:54:15 +01:00
sbus: some code cleanup (no functional changes)
This commit is contained in:
Oleg Semyonov
parent
59da5055cd
commit
6272210df7
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -33,7 +33,6 @@
|
||||
|
||||
#if defined(PIOS_INCLUDE_SPI)
|
||||
|
||||
|
||||
#include <pios_spi_priv.h>
|
||||
|
||||
/* 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);
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user