rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-03 11:24:10 +01:00
Code Issues Releases Activity

spektrum: alternative DSMx (DSM2/DSMJ/DSMX) driver

Browse Source 
- does not glitch when used in 2-frame mode (DM9, 9503, etc)
- does NOT provides yet DSMX stream decoding - do NOT merge
- uses a bit more time in the interrupt, but frees 16 bytes of RAM.
  This is done to help decoding the weird DSMX stream which does not
  contain explicit resolution/frame/lost frames info and needs special
  processing (to be done yet).
This commit is contained in:
Oleg Semyonov 2011-10-21 23:35:17 +03:00
parent 92b81e3f88
commit 62bca651d5
5 changed files with 384 additions and 273 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -619,7 +619,6 @@ static const struct pios_spektrum_cfg pios_spektrum_main_cfg = {
.GPIO_Mode = GPIO_Mode_Out_PP, .GPIO_Mode = GPIO_Mode_Out_PP,
}, },
}, },
.remap = 0,
}; };
static const struct pios_usart_cfg pios_usart_spektrum_flexi_cfg = { static const struct pios_usart_cfg pios_usart_spektrum_flexi_cfg = {
@ -667,7 +666,6 @@ static const struct pios_spektrum_cfg pios_spektrum_flexi_cfg = {
.GPIO_Mode = GPIO_Mode_Out_PP, .GPIO_Mode = GPIO_Mode_Out_PP,
}, },
}, },
.remap = 0,
}; };
#endif /* PIOS_INCLUDE_SPEKTRUM */ #endif /* PIOS_INCLUDE_SPEKTRUM */
@ -1066,7 +1064,7 @@ void PIOS_Board_Init(void) {
} }
uint32_t pios_spektrum_id; uint32_t pios_spektrum_id;
if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, 0)) { if (PIOS_Spektrum_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, 0)) {
PIOS_Assert(0); PIOS_Assert(0);
} }
@ -1139,7 +1137,7 @@ void PIOS_Board_Init(void) {
} }
uint32_t pios_spektrum_id; uint32_t pios_spektrum_id;
if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, hwsettings_DSMxBind)) { if (PIOS_Spektrum_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, hwsettings_DSMxBind)) {
PIOS_Assert(0); PIOS_Assert(0);
} }

3
flight/OpenPilot/System/pios_board.c
View File

@ -589,7 +589,6 @@ static const struct pios_spektrum_cfg pios_spektrum_cfg = {
.GPIO_Mode = GPIO_Mode_Out_PP, .GPIO_Mode = GPIO_Mode_Out_PP,
}, },
}, },
.remap = 0,
}; };
#endif /* PIOS_COM_SPEKTRUM */ #endif /* PIOS_COM_SPEKTRUM */
@ -1213,7 +1212,7 @@ void PIOS_Board_Init(void) {
} }
uint32_t pios_spektrum_id; uint32_t pios_spektrum_id;
if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { if (PIOS_Spektrum_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
PIOS_Assert(0); PIOS_Assert(0);
} }

545
flight/PiOS/STM32F10x/pios_spektrum.c
View File

@ -2,13 +2,13 @@
****************************************************************************** ******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer * @addtogroup PIOS PIOS Core hardware abstraction layer
* @{ * @{
* @addtogroup PIOS_SPEKTRUM Spektrum receiver functions * @addtogroup PIOS_Spektrum Spektrum/JR DSMx satellite receiver functions
* @brief Code to read Spektrum input * @brief Code to bind and read Spektrum/JR DSMx satellite receiver serial stream
* @{ * @{
* *
* @file pios_spektrum.c * @file pios_spektrum.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief USART commands. Inits USARTs, controls USARTs & Interrupt handlers. (STM32 dependent) * @brief Code bind and read Spektrum/JR DSMx satellite receiver serial stream
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
@ -30,252 +30,87 @@
/* Project Includes */ /* Project Includes */
#include "pios.h" #include "pios.h"
#include "pios_spektrum_priv.h"
#if defined(PIOS_INCLUDE_SPEKTRUM) #if defined(PIOS_INCLUDE_SPEKTRUM)
#include "pios_spektrum_priv.h" /* Forward Declarations */
static int32_t PIOS_Spektrum_Get(uint32_t rcvr_id, uint8_t channel);
/** static uint16_t PIOS_Spektrum_RxInCallback(uint32_t context,
* @Note Framesyncing: uint8_t *buf,
* The code resets the watchdog timer whenever a single byte is received, so what watchdog code uint16_t buf_len,
* is never called if regularly getting bytes. uint16_t *headroom,
* RTC timer is running @625Hz, supervisor timer has divider 5 so frame sync comes every 1/125Hz=8ms. bool *need_yield);
* Good for both 11ms and 22ms framecycles static void PIOS_Spektrum_Supervisor(uint32_t spektrum_id);
*/
/* Global Variables */
/* Provide a RCVR driver */
static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel);
/* Local Variables */
const struct pios_rcvr_driver pios_spektrum_rcvr_driver = { const struct pios_rcvr_driver pios_spektrum_rcvr_driver = {
.read = PIOS_SPEKTRUM_Get, .read = PIOS_Spektrum_Get,
}; };
enum pios_spektrum_dev_magic { enum pios_spektrum_dev_magic {
PIOS_SPEKTRUM_DEV_MAGIC = 0xa9b9c9d9, PIOS_SPEKTRUM_DEV_MAGIC = 0x44534d78,
}; };
struct pios_spektrum_fsm { struct pios_spektrum_state {
uint16_t channel; uint16_t channel_data[PIOS_SPEKTRUM_NUM_INPUTS];
uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS]; uint8_t received_data[SPEKTRUM_FRAME_LENGTH];
uint16_t CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS]; uint8_t receive_timer;
uint8_t prev_byte; uint8_t failsafe_timer;
uint8_t sync; uint8_t frame_found;
uint8_t bytecount; uint8_t byte_count;
uint8_t datalength; uint8_t resolution;
uint8_t frame_error; uint8_t frame_mode;
uint8_t sync_of; #ifdef SPEKTRUM_LOST_FRAME_COUNTER
uint8_t frames_lost_last;
uint16_t frames_lost;
#endif
}; };
struct pios_spektrum_dev { struct pios_spektrum_dev {
enum pios_spektrum_dev_magic magic; enum pios_spektrum_dev_magic magic;
const struct pios_spektrum_cfg * cfg; const struct pios_spektrum_cfg *cfg;
struct pios_spektrum_state state;
struct pios_spektrum_fsm fsm;
uint16_t supv_timer;
}; };
static bool PIOS_SPEKTRUM_validate(struct pios_spektrum_dev * spektrum_dev) /* Allocate Spektrum device descriptor */
{
return (spektrum_dev->magic == PIOS_SPEKTRUM_DEV_MAGIC);
}
#if defined(PIOS_INCLUDE_FREERTOS) #if defined(PIOS_INCLUDE_FREERTOS)
static struct pios_spektrum_dev * PIOS_SPEKTRUM_alloc(void) static struct pios_spektrum_dev *PIOS_Spektrum_Alloc(void)
{ {
struct pios_spektrum_dev * spektrum_dev; struct pios_spektrum_dev *spektrum_dev;
spektrum_dev = (struct pios_spektrum_dev *)pvPortMalloc(sizeof(*spektrum_dev)); spektrum_dev = (struct pios_spektrum_dev *)pvPortMalloc(sizeof(*spektrum_dev));
if (!spektrum_dev) return(NULL); if (!spektrum_dev)
return NULL;
spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC; spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC;
return(spektrum_dev); return spektrum_dev;
} }
#else #else
static struct pios_spektrum_dev pios_spektrum_devs[PIOS_SPEKTRUM_MAX_DEVS]; static struct pios_spektrum_dev pios_spektrum_devs[PIOS_SPEKTRUM_MAX_DEVS];
static uint8_t pios_spektrum_num_devs; static uint8_t pios_spektrum_num_devs;
static struct pios_spektrum_dev * PIOS_SPEKTRUM_alloc(void) static struct pios_spektrum_dev *PIOS_Spektrum_Alloc(void)
{ {
struct pios_spektrum_dev * spektrum_dev; struct pios_spektrum_dev *spektrum_dev;
if (pios_spektrum_num_devs >= PIOS_SPEKTRUM_MAX_DEVS) { if (pios_spektrum_num_devs >= PIOS_SPEKTRUM_MAX_DEVS)
return (NULL); return NULL;
}
spektrum_dev = &pios_spektrum_devs[pios_spektrum_num_devs++]; spektrum_dev = &pios_spektrum_devs[pios_spektrum_num_devs++];
spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC; spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC;
return (spektrum_dev); return spektrum_dev;
} }
#endif #endif
static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id); /* Validate Spektrum device descriptor */
static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bind); static bool PIOS_Spektrum_Validate(struct pios_spektrum_dev *spektrum_dev)
static int32_t PIOS_SPEKTRUM_UpdateFSM(struct pios_spektrum_fsm * fsm, uint8_t b);
static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{ {
struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)context; return (spektrum_dev->magic == PIOS_SPEKTRUM_DEV_MAGIC);
bool valid = PIOS_SPEKTRUM_validate(spektrum_dev);
PIOS_Assert(valid);
/* process byte(s) and clear receive timer */
for (uint8_t i = 0; i < buf_len; i++) {
PIOS_SPEKTRUM_UpdateFSM(&(spektrum_dev->fsm), buf[i]);
spektrum_dev->supv_timer = 0;
}
/* Always signal that we can accept another byte */
if (headroom) {
*headroom = 1;
}
/* We never need a yield */
*need_yield = false;
/* Always indicate that all bytes were consumed */
return (buf_len);
} }
static void PIOS_SPEKTRUM_ResetFSM(struct pios_spektrum_fsm * fsm) /* Try to bind DSMx satellite using specified number of pulses */
{ static void PIOS_Spektrum_Bind(const struct pios_spektrum_cfg *cfg, uint8_t bind)
fsm->channel = 0;
fsm->prev_byte = 0xFF;
fsm->sync = 0;
fsm->bytecount = 0;
fsm->datalength = 0;
fsm->frame_error = 0;
fsm->sync_of = 0;
}
/**
* Decodes a byte
* \param[in] b byte which should be spektrum decoded
* \return 0 if no error
* \return -1 if USART not available
* \return -2 if buffer full (retry)
*/
static int32_t PIOS_SPEKTRUM_UpdateFSM(struct pios_spektrum_fsm * fsm, uint8_t b)
{
fsm->bytecount++;
if (fsm->sync == 0) {
/* Known sync bytes, 0x01, 0x02, 0x12, 0xb2 */
/* 0xb2 DX8 3bind pulses only */
if (fsm->bytecount == 2) {
if ((b == 0x01) || (b == 0x02) || (b == 0xb2)) {
fsm->datalength=0; // 10bit
fsm->sync = 1;
fsm->bytecount = 2;
}
else if(b == 0x12) {
fsm->datalength=1; // 11bit
fsm->sync = 1;
fsm->bytecount = 2;
}
else
{
fsm->bytecount = 0;
}
}
} else {
if ((fsm->bytecount % 2) == 0) {
uint16_t data;
uint8_t channeln;
fsm->channel = (fsm->prev_byte << 8) + b;
channeln = (fsm->channel >> (10+fsm->datalength)) & 0x0F;
data = fsm->channel & (0x03FF+(0x0400*fsm->datalength));
if(channeln==0 && data<10) // discard frame if throttle misbehaves
{
fsm->frame_error=1;
}
if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !fsm->frame_error)
fsm->CaptureValueTemp[channeln] = data;
}
}
if (fsm->bytecount == 16) {
fsm->bytecount = 0;
fsm->sync = 0;
fsm->sync_of = 0;
if (!fsm->frame_error)
{
for(int i=0;i<PIOS_SPEKTRUM_NUM_INPUTS;i++)
{
fsm->CaptureValue[i] = fsm->CaptureValueTemp[i];
}
}
fsm->frame_error=0;
}
fsm->prev_byte = b;
return 0;
}
/**
* Bind and Initialise Spektrum satellite receiver
*/
int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t bind)
{
PIOS_DEBUG_Assert(spektrum_id);
PIOS_DEBUG_Assert(cfg);
PIOS_DEBUG_Assert(driver);
struct pios_spektrum_dev * spektrum_dev;
spektrum_dev = (struct pios_spektrum_dev *) PIOS_SPEKTRUM_alloc();
if (!spektrum_dev) goto out_fail;
/* Bind the configuration to the device instance */
spektrum_dev->cfg = cfg;
if (bind) {
PIOS_SPEKTRUM_Bind(cfg,bind);
}
PIOS_SPEKTRUM_ResetFSM(&(spektrum_dev->fsm));
*spektrum_id = (uint32_t)spektrum_dev;
(driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, *spektrum_id);
if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, *spektrum_id)) {
PIOS_DEBUG_Assert(0);
}
return (0);
out_fail:
return(-1);
}
/**
* Get the value of an input channel
* \param[in] Channel Number of the channel desired
* \output -1 Channel not available
* \output >0 Channel value
*/
static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel)
{
struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)rcvr_id;
if(!PIOS_SPEKTRUM_validate(spektrum_dev))
return PIOS_RCVR_INVALID;
/* Return error if channel not available */
if (channel >= PIOS_SPEKTRUM_NUM_INPUTS) {
return PIOS_RCVR_INVALID;
}
return spektrum_dev->fsm.CaptureValue[channel];
}
/**
* Spektrum bind function
* \output true Successful bind
* \output false Bind failed
*/
static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bind)
{ {
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin; GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin;
@ -283,13 +118,15 @@ static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bin
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
/* just to limit bind pulses */ /* just to limit bind pulses */
bind=(bind<=10)?bind:10; if (bind > 10)
bind = 10;
GPIO_Init(cfg->bind.gpio, &cfg->bind.init); GPIO_Init(cfg->bind.gpio, &cfg->bind.init);
/* RX line, set high */ /* RX line, set high */
GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
/* on CC works upto 140ms, I guess bind window is around 20-140ms after powerup */ /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */
PIOS_DELAY_WaitmS(60); PIOS_DELAY_WaitmS(60);
for (int i = 0; i < bind ; i++) { for (int i = 0; i < bind ; i++) {
@ -300,51 +137,253 @@ static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bin
GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(120); PIOS_DELAY_WaituS(120);
} }
/* RX line, set input and wait for data, PIOS_SPEKTRUM_Init */ /* RX line, set input and wait for data */
GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure);
return true;
} }
/** /* Reset channels in case of lost signal or explicit failsafe receiver flag */
*@brief This function is called between frames and when a spektrum word hasnt been decoded for too long static void PIOS_Spektrum_ResetChannels(struct pios_spektrum_state *state)
*@brief clears the channel values
*/
static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id)
{ {
struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)spektrum_id; for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) {
state->channel_data[i] = PIOS_RCVR_TIMEOUT;
bool valid = PIOS_SPEKTRUM_validate(spektrum_dev);
PIOS_Assert(valid);
/* 625hz */
spektrum_dev->supv_timer++;
if(spektrum_dev->supv_timer > 4) {
/* sync between frames */
struct pios_spektrum_fsm * fsm = &(spektrum_dev->fsm);
fsm->sync = 0;
fsm->bytecount = 0;
fsm->prev_byte = 0xFF;
fsm->frame_error = 0;
fsm->sync_of++;
/* watchdog activated after 200ms silence */
if (fsm->sync_of > 30) {
/* signal lost */
fsm->sync_of = 0;
for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) {
fsm->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
fsm->CaptureValueTemp[i] = PIOS_RCVR_TIMEOUT;
}
}
spektrum_dev->supv_timer = 0;
} }
} }
/**
* 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_Spektrum_Get(uint32_t rcvr_id, uint8_t channel)
{
struct pios_spektrum_dev *spektrum_dev = (struct pios_spektrum_dev *)rcvr_id;
if (!PIOS_Spektrum_Validate(spektrum_dev))
return PIOS_RCVR_INVALID;
/* return error if channel is not available */
if (channel >= PIOS_SPEKTRUM_NUM_INPUTS)
return PIOS_RCVR_INVALID;
/* may also be PIOS_RCVR_TIMEOUT set by other function */
return spektrum_dev->state.channel_data[channel];
}
/* Reset Spektrum receiver state */
static void PIOS_Spektrum_ResetState(struct pios_spektrum_state *state)
{
state->receive_timer = 0;
state->failsafe_timer = 0;
state->frame_found = 0;
#ifdef SPEKTRUM_LOST_FRAME_COUNTER
state->frames_lost_last = 0;
state->frames_lost = 0;
#endif #endif
PIOS_Spektrum_ResetChannels(state);
}
/** /**
* @} * Check and unroll complete frame data.
* @} * \output 0 frame data accepted
*/ * \output -1 frame error found
*/
static int PIOS_Spektrum_UnrollChannels(struct pios_spektrum_state *state)
{
#ifdef SPEKTRUM_LOST_FRAME_COUNTER
/* increment the lost frame counter */
uint8_t frames_lost = state->received_data[0];
state->frames_lost += (frames_lost - state->frames_lost_last);
state->frames_lost_last = frames_lost;
#endif
/* check the frame type assuming master satellite stream */
uint8_t type = state->received_data[1];
switch (type) {
case 0x01:
case 0x02:
case 0x12:
state->resolution = (type & SPEKTRUM_DSM2_RES_MASK) ? 11 : 10;
state->frame_mode = (type & SPEKTRUM_DSM2_FNUM_MASK);
break;
case 0xB2:
/* FIXME: we should guess the resolution using frame mode found */
goto fail;
default:
goto fail;
}
/* unroll channels */
uint8_t *s = &(state->received_data[2]);
uint16_t mask = (state->resolution == 10) ? 0x03ff : 0x07ff;
for (int i = 0; i < SPEKTRUM_CHANNELS_PER_FRAME; i++) {
uint16_t word = ((uint16_t)s[0] << 8) | s[1];
s += 2;
/* check for frame mode or missing channel data */
if (word & SPEKTRUM_2ND_FRAME_MASK) {
/* this is ok for the 1st word only */
if (i == 0) {
/* means the 2-frame mode */
state->frame_mode = 2;
} else {
if (word == 0xffff)
/* skip the missing channel */
continue;
else
/* wrong frame received */
goto fail;
}
}
/* extract and save the channel value */
uint8_t channel_num = (word >> state->resolution) & 0x0f;
if (channel_num < PIOS_SPEKTRUM_NUM_INPUTS)
state->channel_data[channel_num] = (word & mask);
}
#ifdef SPEKTRUM_LOST_FRAME_COUNTER
/* put lost frames counter into the last channel for debugging */
state->channel_data[PIOS_SPEKTRUM_NUM_INPUTS-1] = state->frames_lost;
#endif
/* all channels processed, missing channels skipped */
return 0;
fail:
return -1;
}
/* Update decoder state processing input byte from the DSMx stream */
static void PIOS_Spektrum_UpdateState(struct pios_spektrum_state *state, uint8_t b)
{
if (state->frame_found) {
/* receiving the data frame */
if (state->byte_count < SPEKTRUM_FRAME_LENGTH) {
/* store next byte */
state->received_data[state->byte_count++] = b;
if (state->byte_count == SPEKTRUM_FRAME_LENGTH) {
/* full frame received - process and wait for new one */
if (!PIOS_Spektrum_UnrollChannels(state))
/* data looking good */
state->failsafe_timer = 0;
/* prepare for the next frame */
state->frame_found = 0;
}
}
}
}
/* Initialise Spektrum receiver interface */
int32_t PIOS_Spektrum_Init(uint32_t *spektrum_id,
const struct pios_spektrum_cfg *cfg,
const struct pios_com_driver *driver,
uint32_t lower_id,
uint8_t bind)
{
PIOS_DEBUG_Assert(spektrum_id);
PIOS_DEBUG_Assert(cfg);
PIOS_DEBUG_Assert(driver);
struct pios_spektrum_dev *spektrum_dev;
spektrum_dev = (struct pios_spektrum_dev *)PIOS_Spektrum_Alloc();
if (!spektrum_dev)
return -1;
/* Bind the configuration to the device instance */
spektrum_dev->cfg = cfg;
/* Bind the receiver if requested */
if (bind)
PIOS_Spektrum_Bind(cfg, bind);
PIOS_Spektrum_ResetState(&(spektrum_dev->state));
*spektrum_id = (uint32_t)spektrum_dev;
/* Set comm driver callback */
(driver->bind_rx_cb)(lower_id, PIOS_Spektrum_RxInCallback, *spektrum_id);
if (!PIOS_RTC_RegisterTickCallback(PIOS_Spektrum_Supervisor, *spektrum_id)) {
PIOS_DEBUG_Assert(0);
}
return 0;
}
/* Comm byte received callback */
static uint16_t PIOS_Spektrum_RxInCallback(uint32_t context,
uint8_t *buf,
uint16_t buf_len,
uint16_t *headroom,
bool *need_yield)
{
struct pios_spektrum_dev *spektrum_dev = (struct pios_spektrum_dev *)context;
bool valid = PIOS_Spektrum_Validate(spektrum_dev);
PIOS_Assert(valid);
struct pios_spektrum_state *state = &(spektrum_dev->state);
/* process byte(s) and clear receive timer */
for (uint8_t i = 0; i < buf_len; i++) {
PIOS_Spektrum_UpdateState(state, buf[i]);
state->receive_timer = 0;
}
/* Always signal that we can accept another byte */
if (headroom)
*headroom = SPEKTRUM_FRAME_LENGTH;
/* We never need a yield */
*need_yield = false;
/* Always indicate that all bytes were consumed */
return buf_len;
}
/**
* Input data supervisor is called periodically and provides
* two functions: frame syncing and failsafe triggering.
*
* Spektrum frames come at 11ms or 22ms rate at 115200bps.
* RTC timer is running at 625Hz (1.6ms). So with divider 5 it gives
* 8ms pause between frames which is good for both Spektrum 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_Spektrum_Supervisor(uint32_t spektrum_id)
{
struct pios_spektrum_dev *spektrum_dev = (struct pios_spektrum_dev *)spektrum_id;
bool valid = PIOS_Spektrum_Validate(spektrum_dev);
PIOS_Assert(valid);
struct pios_spektrum_state *state = &(spektrum_dev->state);
/* waiting for new frame if no bytes were received in 8ms */
if (++state->receive_timer > 4) {
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_Spektrum_ResetChannels(state);
state->failsafe_timer = 0;
}
}
#endif /* PIOS_INCLUDE_SPEKTRUM */
/**
* @}
* @}
*/

13
flight/PiOS/inc/pios_spektrum.h
View File

@ -2,13 +2,12 @@
****************************************************************************** ******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer * @addtogroup PIOS PIOS Core hardware abstraction layer
* @{ * @{
* @addtogroup PIOS_SPEKTRUM Spektrum receiver functions * @addtogroup PIOS_Spektrum Spektrum/JR DSMx satellite receiver functions
* @{ * @{
* *
* @file pios_spektrum.h * @file pios_spektrum.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* Parts by Thorsten Klose (tk@midibox.org) * @brief Spektrum/JR DSMx satellite receiver functions header.
* @brief Spektrum satellite functions header.
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
@ -38,6 +37,6 @@
#endif /* PIOS_SPEKTRUM_H */ #endif /* PIOS_SPEKTRUM_H */
/** /**
* @} * @}
* @} * @}
*/ */

88
flight/PiOS/inc/pios_spektrum_priv.h
View File

@ -2,13 +2,13 @@
****************************************************************************** ******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer * @addtogroup PIOS PIOS Core hardware abstraction layer
* @{ * @{
* @addtogroup PIOS_SPEKTRUM SPEKTRUM Functions * @addtogroup PIOS_Spektrum Spektrum/JR DSMx satellite receiver functions
* @brief PIOS interface to read and write from spektrum port * @brief PIOS interface to bind and read Spektrum/JR DSMx satellite receiver
* @{ * @{
* *
* @file pios_spektrum_priv.h * @file pios_spektrum_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief Servo private structures. * @brief Spektrum/JR DSMx satellite receiver private structures.
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
@ -35,14 +35,90 @@
#include <pios_stm32.h> #include <pios_stm32.h>
#include <pios_usart_priv.h> #include <pios_usart_priv.h>
/*
* Currently known DSMx (DSM2, DSMJ, DSMX) satellite serial port settings:
* 115200bps serial stream, 8 bits, no parity, 1 stop bit
* size of each frame: 16 bytes
* data resolution: 10 or 11 bits
* number of frames: 1 or 2
* frame period: 11ms or 22ms
*
* Currently known DSMx frame structure:
* 2 bytes - depend on protocol version:
* for DSM2/DSMJ:
* 1 byte - lost frame counter (8 bit)
* 1 byte - data format (for master receiver bound with 3 or 5 pulses),
* or unknown (for slave receiver bound with 4 or 6 pulses,
* some sources call it also the lost frame counter)
* for DSMX:
* 1 byte - unknown data (does not look like lost frame counter)
* 1 byte - unknown data, has been seen only 0xB2 so far
* 14 bytes - up to 7 channels (16 bit word per channel) with encoded channel
* number, channel value, the "2nd frame in a sequence" flag.
* Unused channels have FF FF instead of data bytes.
*
* Data format identification:
* - for DSM2/DSMJ: [0 0 0 R 0 0 N1 N0]
* where
* R is data resolution (0 - 10 bits, 1 - 11 bits),
* N1..N0 is the number of frames required to receive all channel
* data (01 or 10 are known to the moment, which means 1 or 2 frames).
* Three values for the transmitter information byte have been seen
* thus far: 0x01, 0x02, 0x12.
* - for DSMX this byte contains just 0xB2 value for any resolution.
* Hopefully it always uses 11 bit resolution for 2-frame mode and
* 10 bit otherwise. Also some weird throttle channel (0) behavior
* was found in some streams (all zeroes). Thus DSMX needs special
* processing.
*
* Channel data are:
* - for 10 bit: [F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0]
* - for 11 bit: [F C3 C2 C1 C0 D10 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0]
* where
* F is normally 0 but set to 1 for the first channel of the 2nd frame,
* C3 to C0 is the channel number, 4 bit, zero-based, in any order,
* Dx..D0 - channel data (10 or 11 bits)
*
* DSM2 protocol bug: in some cases in 2-frame format some bytes of the
* frame can contain invalid values from the previous frame. They usually
* are the last 5 bytes and can be equal to FF or other data from last
* frame. There is no explicit workaround currently known.
*
* Binding: the number of pulses within bind window after power up defines
* if this receiver is a master (provides receiver capabilities info to
* the transmitter to choose data format) or slave (does not respond to
* the transmitter which falls back to the old DSM mode in that case).
* Currently known are 3(4) pulses for low resolution (10 bit) mode, and
* 5(6) pulses for high resolution (11 bit) mode. Thus only 3 or 5 pulses
* should be used for stand-alone satellite receiver to be bound correctly
* as the master. 5 pulses (high resolution) mode simulates high-end
* receivers which should work in all cases except user explicitly wants
* to bind in low resolution mode.
*/
#define SPEKTRUM_CHANNELS_PER_FRAME 7
#define SPEKTRUM_FRAME_LENGTH (1+1+SPEKTRUM_CHANNELS_PER_FRAME*2)
#define SPEKTRUM_DSM2_RES_MASK 0x0010
#define SPEKTRUM_DSM2_FNUM_MASK 0x0003
#define SPEKTRUM_2ND_FRAME_MASK 0x8000
#undef SPEKTRUM_LOST_FRAME_COUNTER /* include lost frame counter, not used by OP */
/*
* Spektrum receiver instance configuration
*/
struct pios_spektrum_cfg { struct pios_spektrum_cfg {
struct stm32_gpio bind; struct stm32_gpio bind;
uint32_t remap; /* GPIO_Remap_* */
}; };
extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver; extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver;
extern int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t bind); extern int32_t PIOS_Spektrum_Init(uint32_t *spektrum_id,
const struct pios_spektrum_cfg *cfg,
const struct pios_com_driver *driver,
uint32_t lower_id,
uint8_t bind);
#endif /* PIOS_SPEKTRUM_PRIV_H */ #endif /* PIOS_SPEKTRUM_PRIV_H */