rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

sbus: better frame syncronization, some cosmetic changes

Browse Source 
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.
This commit is contained in:
Oleg Semyonov 2011-10-22 00:44:09 +03:00
parent 92b81e3f88
commit 58d0812309
6 changed files with 74 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/CopterControl/System/pios_board.c
View File

@ -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 <pios_sbus_priv.h>
@ -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);
}

2
flight/OpenPilot/System/openpilot.c
View File

@ -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));

2
flight/PiOS/Boards/STM3210E_OP.h
View File

@ -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)
//-------------------------

125
flight/PiOS/STM32F10x/pios_sbus.c
View File

@ -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;
}
}

2
flight/PiOS/inc/pios_sbus.h
View File

@ -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

4
flight/PiOS/inc/pios_sbus_priv.h
View File

@ -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);