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/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..16dcb66e9 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 0 +#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..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 + * @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 USART commands. Inits USARTs, controls USARTs & Interrupt handlers. (STM32 dependent) + * @brief Code to read Futaba S.Bus receiver serial stream * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -34,44 +34,166 @@ #if defined(PIOS_INCLUDE_SBUS) -/* Provide a RCVR driver */ -static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel); +/* 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); -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; +const struct pios_rcvr_driver pios_sbus_rcvr_driver = { + .read = PIOS_SBus_Get, +}; -static void PIOS_SBUS_Supervisor(uint32_t sbus_id); +enum pios_sbus_dev_magic { + PIOS_SBUS_DEV_MAGIC = 0x53427573, +}; -/** - * 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_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; +}; + +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,132 +220,119 @@ 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; + /* should not process any data until new frame is found */ + if (!state->frame_found) + return; - if (frame_found == 0) { - /* no frame found yet, waiting for start byte */ - if (b == SBUS_SOF_BYTE) { - byte_count = 0; - frame_found = 1; - } - } else { - /* do not store start and end of frame bytes */ - if (byte_count < SBUS_FRAME_LENGTH - 2) { - /* store next byte */ - received_data[byte_count++] = b; + 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 { - 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 */ - } - - /* prepare for the next frame */ - frame_found = 0; + /* 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; } } -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 * 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); + 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 = 1; + state->byte_count = 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.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 b7e441c30..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 * @{ * @@ -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 */