From 92b81e3f886b785319a2f986b726b2ac29df362a Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Thu, 20 Oct 2011 18:23:23 +0300 Subject: [PATCH 1/2] sbus: refactor the code using unified PIOS RCVR driver structure - allow more than one S.Bus receiver (needs hardware support) - use dynamic memory allocation (frees around 72 bytes of RAM when unused) --- flight/PiOS/Boards/STM32103CB_CC_Rev1.h | 6 + flight/PiOS/Boards/STM3210E_OP.h | 6 + flight/PiOS/STM32F10x/pios_sbus.c | 280 ++++++++++++++++-------- flight/PiOS/inc/pios_sbus_priv.h | 9 +- 4 files changed, 209 insertions(+), 92 deletions(-) diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 42b3a5e4d..0815d350a 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -227,6 +227,12 @@ extern uint32_t pios_com_telem_usb_id; #define PIOS_SPEKTRUM_MAX_DEVS 2 #define PIOS_SPEKTRUM_NUM_INPUTS 12 +//------------------------- +// Receiver S.Bus input +//------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16+2) + //------------------------- // Servo outputs //------------------------- diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h index ea925a271..8d232a119 100644 --- a/flight/PiOS/Boards/STM3210E_OP.h +++ b/flight/PiOS/Boards/STM3210E_OP.h @@ -200,6 +200,12 @@ extern uint32_t pios_com_aux_id; #define PIOS_SPEKTRUM_MAX_DEVS 1 #define PIOS_SPEKTRUM_NUM_INPUTS 12 +//------------------------- +// Receiver S.Bus input +//------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16+2) + //------------------------- // Servo outputs //------------------------- diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index 319142dc6..da80e531f 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -3,12 +3,12 @@ * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ * @addtogroup PIOS_SBUS Futaba S.Bus receiver functions - * @brief Code to read Futaba S.Bus input + * @brief Code to read Futaba S.Bus input serial stream * @{ * * @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) + * @brief Code to read Futaba S.Bus input serial stream * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -34,44 +34,166 @@ #if defined(PIOS_INCLUDE_SBUS) -/* Provide a RCVR driver */ +/* Forward Declarations */ static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel); +static uint16_t PIOS_SBUS_RxInCallback(uint32_t context, + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); +static void PIOS_SBUS_Supervisor(uint32_t sbus_id); + +/* Local Variables */ const struct pios_rcvr_driver pios_sbus_rcvr_driver = { .read = PIOS_SBUS_Get, }; -/* Local Variables */ -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; +enum pios_sbus_dev_magic { + PIOS_SBUS_DEV_MAGIC = 0x53427573, +}; -static void PIOS_SBUS_Supervisor(uint32_t sbus_id); +struct pios_sbus_state { + uint16_t channel_data[PIOS_SBUS_NUM_INPUTS]; + uint8_t received_data[SBUS_FRAME_LENGTH - 2]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t frame_found; + uint8_t byte_count; +}; -/** - * 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) +struct pios_sbus_dev { + enum pios_sbus_dev_magic magic; + const struct pios_sbus_cfg *cfg; + struct pios_sbus_state state; +}; + +/* Allocate S.Bus device descriptor */ +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_sbus_dev *PIOS_SBUS_Alloc(void) { - for (int i = 0; i < SBUS_NUMBER_OF_CHANNELS; i++) { - channel_data[i] = PIOS_RCVR_TIMEOUT; + struct pios_sbus_dev *sbus_dev; + + sbus_dev = (struct pios_sbus_dev *)pvPortMalloc(sizeof(*sbus_dev)); + if (!sbus_dev) return(NULL); + + sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; + return(sbus_dev); +} +#else +static struct pios_sbus_dev pios_sbus_devs[PIOS_SBUS_MAX_DEVS]; +static uint8_t pios_sbus_num_devs; +static struct pios_sbus_dev *PIOS_SBUS_Alloc(void) +{ + struct pios_sbus_dev *sbus_dev; + + if (pios_sbus_num_devs >= PIOS_SBUS_MAX_DEVS) { + return (NULL); + } + + sbus_dev = &pios_sbus_devs[pios_sbus_num_devs++]; + sbus_dev->magic = PIOS_SBUS_DEV_MAGIC; + + return (sbus_dev); +} +#endif + +/* Validate S.Bus device descriptor */ +static bool PIOS_SBUS_Validate(struct pios_sbus_dev *sbus_dev) +{ + return (sbus_dev->magic == PIOS_SBUS_DEV_MAGIC); +} + +/* Reset channels in case of lost signal or explicit failsafe receiver flag */ +static void PIOS_SBUS_ResetChannels(struct pios_sbus_state *state) +{ + for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; } } +/* Reset S.Bus receiver state */ +static void PIOS_SBUS_ResetState(struct pios_sbus_state *state) +{ + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = 0; + PIOS_SBUS_ResetChannels(state); +} + +/* Initialise S.Bus receiver interface */ +int32_t PIOS_SBUS_Init(uint32_t *sbus_id, + const struct pios_sbus_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id) +{ + PIOS_DEBUG_Assert(sbus_id); + PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(driver); + + struct pios_sbus_dev *sbus_dev; + + sbus_dev = (struct pios_sbus_dev *)PIOS_SBUS_Alloc(); + if (!sbus_dev) goto out_fail; + + /* Bind the configuration to the device instance */ + sbus_dev->cfg = cfg; + + PIOS_SBUS_ResetState(&(sbus_dev->state)); + + *sbus_id = (uint32_t)sbus_dev; + + /* Enable inverter clock and enable the inverter */ + (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE); + GPIO_Init(cfg->inv.gpio, &cfg->inv.init); + GPIO_WriteBit(cfg->inv.gpio, cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable); + + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_SBUS_RxInCallback, *sbus_id); + + if (!PIOS_RTC_RegisterTickCallback(PIOS_SBUS_Supervisor, *sbus_id)) { + PIOS_DEBUG_Assert(0); + } + + return 0; + +out_fail: + return -1; +} + /** - * unroll_channels() function computes channel_data[] from received_data[] + * Get the value of an input channel + * \param[in] channel Number of the channel desired (zero based) + * \output PIOS_RCVR_INVALID channel not available + * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver + * \output >0 channel value + */ +static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)rcvr_id; + + if (!PIOS_SBUS_Validate(sbus_dev)) + return PIOS_RCVR_INVALID; + + /* return error if channel is not available */ + if (channel >= PIOS_SBUS_NUM_INPUTS) { + return PIOS_RCVR_INVALID; + } + + return sbus_dev->state.channel_data[channel]; +} + +/** + * Compute channel_data[] from received_data[]. * For efficiency it unrolls first 8 channels without loops and does the * same for other 8 channels. Also 2 discrete channels will be set. */ -static void unroll_channels(void) +static void PIOS_SBUS_UnrollChannels(struct pios_sbus_state *state) { - uint8_t *s = received_data; - uint16_t *d = channel_data; + uint8_t *s = state->received_data; + uint16_t *d = state->channel_data; -#define F(v,s) ((v) >> s) & 0x7ff +#define F(v,s) (((v) >> (s)) & 0x7ff) /* unroll channels 1-8 */ *d++ = F(s[0] | s[1] << 8, 0); @@ -98,111 +220,82 @@ static void unroll_channels(void) *d++ = (s[22] & SBUS_FLAG_DC2) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; } -/** - * process_byte() function processes incoming byte from S.Bus stream - */ -static void process_byte(uint8_t b) +/* Update decoder state processing input byte from the S.Bus stream */ +static void PIOS_SBUS_UpdateState(struct pios_sbus_state *state, uint8_t b) { - static uint8_t byte_count; - - if (frame_found == 0) { + if (state->frame_found == 0) { /* no frame found yet, waiting for start byte */ if (b == SBUS_SOF_BYTE) { - byte_count = 0; - frame_found = 1; + state->byte_count = 0; + state->frame_found = 1; } } else { /* do not store start and end of frame bytes */ - if (byte_count < SBUS_FRAME_LENGTH - 2) { + if (state->byte_count < SBUS_FRAME_LENGTH - 2) { /* store next byte */ - received_data[byte_count++] = b; + state->received_data[state->byte_count++] = b; } else { if (b == SBUS_EOF_BYTE) { /* full frame received */ - uint8_t flags = received_data[SBUS_FRAME_LENGTH - 3]; + uint8_t flags = state->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(); + PIOS_SBUS_ResetChannels(state); } else { /* data looking good */ - unroll_channels(); - failsafe_timer = 0; + PIOS_SBUS_UnrollChannels(state); + state->failsafe_timer = 0; } } else { /* discard whole frame */ } /* prepare for the next frame */ - frame_found = 0; + state->frame_found = 0; } } } -static uint16_t PIOS_SBUS_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +/* Comm byte received callback */ +static uint16_t PIOS_SBUS_RxInCallback(uint32_t context, + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) { + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)context; + + bool valid = PIOS_SBUS_Validate(sbus_dev); + PIOS_Assert(valid); + + struct pios_sbus_state *state = &(sbus_dev->state); + /* process byte(s) and clear receive timer */ for (uint8_t i = 0; i < buf_len; i++) { - process_byte(buf[i]); - receive_timer = 0; + PIOS_SBUS_UpdateState(state, buf[i]); + state->receive_timer = 0; } /* Always signal that we can accept another byte */ - if (headroom) { + if (headroom) *headroom = SBUS_FRAME_LENGTH; - } /* We never need a yield */ *need_yield = false; /* Always indicate that all bytes were consumed */ - return (buf_len); -} - -/** - * Initialise S.Bus receiver interface - */ -int32_t PIOS_SBUS_Init(uint32_t * sbus_id, const struct pios_sbus_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id) -{ - /* Enable inverter clock and enable the inverter */ - (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE); - GPIO_Init(cfg->inv.gpio, &cfg->inv.init); - GPIO_WriteBit(cfg->inv.gpio, - cfg->inv.init.GPIO_Pin, - cfg->gpio_inv_enable); - - (driver->bind_rx_cb)(lower_id, PIOS_SBUS_RxInCallback, 0); - - if (!PIOS_RTC_RegisterTickCallback(PIOS_SBUS_Supervisor, 0)) { - PIOS_Assert(0); - } - - return (0); -} - -/** - * 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 - */ -static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel) -{ - /* return error if channel is not available */ - if (channel >= SBUS_NUMBER_OF_CHANNELS) { - return PIOS_RCVR_INVALID; - } - return channel_data[channel]; + return buf_len; } /** * 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 - * 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. + * 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 frame 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 @@ -210,20 +303,27 @@ static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel) */ static void PIOS_SBUS_Supervisor(uint32_t sbus_id) { + struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id; + + bool valid = PIOS_SBUS_Validate(sbus_dev); + PIOS_Assert(valid); + + struct pios_sbus_state *state = &(sbus_dev->state); + /* waiting for new frame if no bytes were received in 3.2ms */ - if (++receive_timer > 2) { - receive_timer = 0; - frame_found = 0; + if (++state->receive_timer > 2) { + state->frame_found = 0; + state->receive_timer = 0; } /* activate failsafe if no frames have arrived in 102.4ms */ - if (++failsafe_timer > 64) { - reset_channels(); - failsafe_timer = 0; + if (++state->failsafe_timer > 64) { + PIOS_SBUS_ResetChannels(state); + state->failsafe_timer = 0; } } -#endif +#endif /* PIOS_INCLUDE_SBUS */ /** * @} diff --git a/flight/PiOS/inc/pios_sbus_priv.h b/flight/PiOS/inc/pios_sbus_priv.h index b7e441c30..579ad65e9 100644 --- a/flight/PiOS/inc/pios_sbus_priv.h +++ b/flight/PiOS/inc/pios_sbus_priv.h @@ -63,7 +63,9 @@ * S.Bus protocol provides 16 proportional and 2 discrete channels. * Do not change unless driver code is updated accordingly. */ -#define SBUS_NUMBER_OF_CHANNELS (16 + 2) +#if (PIOS_SBUS_NUM_INPUTS != (16+2)) +#error "S.Bus protocol provides 16 proportional and 2 discrete channels" +#endif /* Discrete channels represented as bits, provide values for them */ #define SBUS_VALUE_MIN 352 @@ -81,7 +83,10 @@ struct pios_sbus_cfg { extern const struct pios_rcvr_driver pios_sbus_rcvr_driver; -extern int32_t PIOS_SBUS_Init(uint32_t * sbus_id, const struct pios_sbus_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id); +extern int32_t PIOS_SBUS_Init(uint32_t *sbus_id, + const struct pios_sbus_cfg *cfg, + const struct pios_com_driver *driver, + uint32_t lower_id); #endif /* PIOS_SBUS_PRIV_H */ From 58d081230982874fb3678472a00526557465b293 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sat, 22 Oct 2011 00:44:09 +0300 Subject: [PATCH 2/2] sbus: better frame syncronization, some cosmetic changes In the previous version the decoder could in rare cases get synced from the middle of data stream in case of data byte equal to the S.Bus start of frame (SOF) byte (wrong data will be rejected but it was not perfect). Now it waits for the real start of frame and then checks the SOF byte. --- flight/CopterControl/System/pios_board.c | 4 +- flight/OpenPilot/System/openpilot.c | 2 +- flight/PiOS/Boards/STM3210E_OP.h | 2 +- flight/PiOS/STM32F10x/pios_sbus.c | 125 ++++++++++++----------- flight/PiOS/inc/pios_sbus.h | 2 +- flight/PiOS/inc/pios_sbus_priv.h | 4 +- 6 files changed, 74 insertions(+), 65 deletions(-) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 1cf41e5bc..a3e47d45a 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -674,7 +674,7 @@ static const struct pios_spektrum_cfg pios_spektrum_flexi_cfg = { #if defined(PIOS_INCLUDE_SBUS) /* - * SBUS USART + * S.Bus USART */ #include @@ -1025,7 +1025,7 @@ void PIOS_Board_Init(void) { } uint32_t pios_sbus_id; - if (PIOS_SBUS_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { + if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c index 81a8f17b1..0e6bcf671 100644 --- a/flight/OpenPilot/System/openpilot.c +++ b/flight/OpenPilot/System/openpilot.c @@ -147,7 +147,7 @@ static void TaskTesting(void *pvParameters) 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)); + 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)); diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h index 8d232a119..16dcb66e9 100644 --- a/flight/PiOS/Boards/STM3210E_OP.h +++ b/flight/PiOS/Boards/STM3210E_OP.h @@ -203,7 +203,7 @@ extern uint32_t pios_com_aux_id; //------------------------- // Receiver S.Bus input //------------------------- -#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_MAX_DEVS 0 #define PIOS_SBUS_NUM_INPUTS (16+2) //------------------------- diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index da80e531f..53730b16b 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -2,13 +2,13 @@ ****************************************************************************** * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ - * @addtogroup PIOS_SBUS Futaba S.Bus receiver functions - * @brief Code to read Futaba S.Bus input serial stream + * @addtogroup PIOS_SBus Futaba S.Bus receiver functions + * @brief Code to read Futaba S.Bus receiver serial stream * @{ * * @file pios_sbus.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @brief Code to read Futaba S.Bus input serial stream + * @brief Code to read Futaba S.Bus receiver serial stream * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -35,18 +35,18 @@ #if defined(PIOS_INCLUDE_SBUS) /* Forward Declarations */ -static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel); -static uint16_t PIOS_SBUS_RxInCallback(uint32_t context, +static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel); +static uint16_t PIOS_SBus_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield); -static void PIOS_SBUS_Supervisor(uint32_t sbus_id); +static void PIOS_SBus_Supervisor(uint32_t sbus_id); /* Local Variables */ const struct pios_rcvr_driver pios_sbus_rcvr_driver = { - .read = PIOS_SBUS_Get, + .read = PIOS_SBus_Get, }; enum pios_sbus_dev_magic { @@ -70,7 +70,7 @@ struct pios_sbus_dev { /* Allocate S.Bus device descriptor */ #if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_sbus_dev *PIOS_SBUS_Alloc(void) +static struct pios_sbus_dev *PIOS_SBus_Alloc(void) { struct pios_sbus_dev *sbus_dev; @@ -83,7 +83,7 @@ static struct pios_sbus_dev *PIOS_SBUS_Alloc(void) #else static struct pios_sbus_dev pios_sbus_devs[PIOS_SBUS_MAX_DEVS]; static uint8_t pios_sbus_num_devs; -static struct pios_sbus_dev *PIOS_SBUS_Alloc(void) +static struct pios_sbus_dev *PIOS_SBus_Alloc(void) { struct pios_sbus_dev *sbus_dev; @@ -99,13 +99,13 @@ static struct pios_sbus_dev *PIOS_SBUS_Alloc(void) #endif /* Validate S.Bus device descriptor */ -static bool PIOS_SBUS_Validate(struct pios_sbus_dev *sbus_dev) +static bool PIOS_SBus_Validate(struct pios_sbus_dev *sbus_dev) { return (sbus_dev->magic == PIOS_SBUS_DEV_MAGIC); } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ -static void PIOS_SBUS_ResetChannels(struct pios_sbus_state *state) +static void PIOS_SBus_ResetChannels(struct pios_sbus_state *state) { for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) { state->channel_data[i] = PIOS_RCVR_TIMEOUT; @@ -113,16 +113,16 @@ static void PIOS_SBUS_ResetChannels(struct pios_sbus_state *state) } /* Reset S.Bus receiver state */ -static void PIOS_SBUS_ResetState(struct pios_sbus_state *state) +static void PIOS_SBus_ResetState(struct pios_sbus_state *state) { state->receive_timer = 0; state->failsafe_timer = 0; state->frame_found = 0; - PIOS_SBUS_ResetChannels(state); + PIOS_SBus_ResetChannels(state); } /* Initialise S.Bus receiver interface */ -int32_t PIOS_SBUS_Init(uint32_t *sbus_id, +int32_t PIOS_SBus_Init(uint32_t *sbus_id, const struct pios_sbus_cfg *cfg, const struct pios_com_driver *driver, uint32_t lower_id) @@ -133,13 +133,13 @@ int32_t PIOS_SBUS_Init(uint32_t *sbus_id, struct pios_sbus_dev *sbus_dev; - sbus_dev = (struct pios_sbus_dev *)PIOS_SBUS_Alloc(); + sbus_dev = (struct pios_sbus_dev *)PIOS_SBus_Alloc(); if (!sbus_dev) goto out_fail; /* Bind the configuration to the device instance */ sbus_dev->cfg = cfg; - PIOS_SBUS_ResetState(&(sbus_dev->state)); + PIOS_SBus_ResetState(&(sbus_dev->state)); *sbus_id = (uint32_t)sbus_dev; @@ -149,9 +149,9 @@ int32_t PIOS_SBUS_Init(uint32_t *sbus_id, GPIO_WriteBit(cfg->inv.gpio, cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable); /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_SBUS_RxInCallback, *sbus_id); + (driver->bind_rx_cb)(lower_id, PIOS_SBus_RxInCallback, *sbus_id); - if (!PIOS_RTC_RegisterTickCallback(PIOS_SBUS_Supervisor, *sbus_id)) { + if (!PIOS_RTC_RegisterTickCallback(PIOS_SBus_Supervisor, *sbus_id)) { PIOS_DEBUG_Assert(0); } @@ -168,11 +168,11 @@ out_fail: * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver * \output >0 channel value */ -static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel) +static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel) { struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)rcvr_id; - if (!PIOS_SBUS_Validate(sbus_dev)) + if (!PIOS_SBus_Validate(sbus_dev)) return PIOS_RCVR_INVALID; /* return error if channel is not available */ @@ -188,7 +188,7 @@ static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel) * For efficiency it unrolls first 8 channels without loops and does the * same for other 8 channels. Also 2 discrete channels will be set. */ -static void PIOS_SBUS_UnrollChannels(struct pios_sbus_state *state) +static void PIOS_SBus_UnrollChannels(struct pios_sbus_state *state) { uint8_t *s = state->received_data; uint16_t *d = state->channel_data; @@ -221,45 +221,53 @@ static void PIOS_SBUS_UnrollChannels(struct pios_sbus_state *state) } /* Update decoder state processing input byte from the S.Bus stream */ -static void PIOS_SBUS_UpdateState(struct pios_sbus_state *state, uint8_t b) +static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b) { - if (state->frame_found == 0) { - /* no frame found yet, waiting for start byte */ - if (b == SBUS_SOF_BYTE) { - state->byte_count = 0; - state->frame_found = 1; - } - } else { - /* do not store start and end of frame bytes */ - if (state->byte_count < SBUS_FRAME_LENGTH - 2) { - /* store next byte */ - state->received_data[state->byte_count++] = b; - } else { - if (b == SBUS_EOF_BYTE) { - /* full frame received */ - uint8_t flags = state->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 */ - PIOS_SBUS_ResetChannels(state); - } else { - /* data looking good */ - PIOS_SBUS_UnrollChannels(state); - state->failsafe_timer = 0; - } - } else { - /* discard whole frame */ - } + /* should not process any data until new frame is found */ + if (!state->frame_found) + return; - /* prepare for the next frame */ + if (state->byte_count == 0) { + if (b != SBUS_SOF_BYTE) { + /* discard the whole frame if the 1st byte is not correct */ state->frame_found = 0; + } else { + /* do not store the SOF byte */ + state->byte_count++; } + return; + } + + /* do not store last frame byte as well */ + if (state->byte_count < SBUS_FRAME_LENGTH - 1) { + /* store next byte */ + state->received_data[state->byte_count - 1] = b; + state->byte_count++; + } else { + if (b == SBUS_EOF_BYTE) { + /* full frame received */ + uint8_t flags = state->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 */ + PIOS_SBus_ResetChannels(state); + } else { + /* data looking good */ + PIOS_SBus_UnrollChannels(state); + state->failsafe_timer = 0; + } + } else { + /* discard whole frame */ + } + + /* prepare for the next frame */ + state->frame_found = 0; } } /* Comm byte received callback */ -static uint16_t PIOS_SBUS_RxInCallback(uint32_t context, +static uint16_t PIOS_SBus_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, @@ -267,14 +275,14 @@ static uint16_t PIOS_SBUS_RxInCallback(uint32_t context, { struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)context; - bool valid = PIOS_SBUS_Validate(sbus_dev); + bool valid = PIOS_SBus_Validate(sbus_dev); PIOS_Assert(valid); struct pios_sbus_state *state = &(sbus_dev->state); /* process byte(s) and clear receive timer */ for (uint8_t i = 0; i < buf_len; i++) { - PIOS_SBUS_UpdateState(state, buf[i]); + PIOS_SBus_UpdateState(state, buf[i]); state->receive_timer = 0; } @@ -301,24 +309,25 @@ static uint16_t PIOS_SBUS_RxInCallback(uint32_t context, * data reception. If no new data received in 100ms, we must call the * failsafe function which clears all channels. */ -static void PIOS_SBUS_Supervisor(uint32_t sbus_id) +static void PIOS_SBus_Supervisor(uint32_t sbus_id) { struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id; - bool valid = PIOS_SBUS_Validate(sbus_dev); + bool valid = PIOS_SBus_Validate(sbus_dev); PIOS_Assert(valid); struct pios_sbus_state *state = &(sbus_dev->state); /* waiting for new frame if no bytes were received in 3.2ms */ if (++state->receive_timer > 2) { - state->frame_found = 0; + state->frame_found = 1; + state->byte_count = 0; state->receive_timer = 0; } /* activate failsafe if no frames have arrived in 102.4ms */ if (++state->failsafe_timer > 64) { - PIOS_SBUS_ResetChannels(state); + PIOS_SBus_ResetChannels(state); state->failsafe_timer = 0; } } diff --git a/flight/PiOS/inc/pios_sbus.h b/flight/PiOS/inc/pios_sbus.h index 868fedd70..fd8580f2e 100644 --- a/flight/PiOS/inc/pios_sbus.h +++ b/flight/PiOS/inc/pios_sbus.h @@ -2,7 +2,7 @@ ****************************************************************************** * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ - * @addtogroup PIOS_SBUS Futaba S.Bus receiver functions + * @addtogroup PIOS_SBus Futaba S.Bus receiver functions * @{ * * @file pios_sbus.h diff --git a/flight/PiOS/inc/pios_sbus_priv.h b/flight/PiOS/inc/pios_sbus_priv.h index 579ad65e9..127db9a46 100644 --- a/flight/PiOS/inc/pios_sbus_priv.h +++ b/flight/PiOS/inc/pios_sbus_priv.h @@ -2,7 +2,7 @@ ****************************************************************************** * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ - * @addtogroup PIOS_SBUS S.Bus Functions + * @addtogroup PIOS_SBus S.Bus Functions * @brief PIOS interface to read and write from Futaba S.Bus port * @{ * @@ -83,7 +83,7 @@ struct pios_sbus_cfg { extern const struct pios_rcvr_driver pios_sbus_rcvr_driver; -extern int32_t PIOS_SBUS_Init(uint32_t *sbus_id, +extern int32_t PIOS_SBus_Init(uint32_t *sbus_id, const struct pios_sbus_cfg *cfg, const struct pios_com_driver *driver, uint32_t lower_id);