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

OP-1625: DSM2/DSMX autodetection of channel resolution.

Browse Source
This commit is contained in:
Mathieu Rondonneau 2014-11-18 19:35:57 -08:00
parent 141cf194f1
commit b0cc9ce021
2 changed files with 24 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
flight/pios/stm32f10x/pios_dsm.c
View File

@ -180,7 +180,9 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev)
static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
{ {
struct pios_dsm_state *state = &(dsm_dev->state); struct pios_dsm_state *state = &(dsm_dev->state);
uint8_t resolution; /* Fix resolution for detection. */
static uint8_t resolution = 11;
uint32_t channel_log = 0;
#ifdef DSM_LOST_FRAME_COUNTER #ifdef DSM_LOST_FRAME_COUNTER
/* increment the lost frame counter */ /* increment the lost frame counter */
@ -189,37 +191,6 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
state->frames_lost_last = frames_lost; state->frames_lost_last = frames_lost;
#endif #endif
/* check the frame type assuming master satellite stream */
uint8_t type = state->received_data[1];
switch (type) {
case 0x01:
case 0x02:
case 0x12:
/* DSM2, DSMJ stream */
if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) {
/* DSM2/DSMJ resolution is known from the header */
resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10;
} else {
/* DSMX resolution should explicitly be selected */
goto stream_error;
}
break;
case 0xA2:
case 0xB2:
/* DSMX stream */
if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) {
resolution = 10;
} else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) {
resolution = 11;
} else {
/* DSMX resolution should explicitly be selected */
goto stream_error;
}
break;
default:
/* unknown yet data stream */
goto stream_error;
}
/* unroll channels */ /* unroll channels */
uint8_t *s = &(state->received_data[2]); uint8_t *s = &(state->received_data[2]);
@ -243,7 +214,16 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
/* extract and save the channel value */ /* extract and save the channel value */
uint8_t channel_num = (word >> resolution) & 0x0f; uint8_t channel_num = (word >> resolution) & 0x0f;
if (channel_num < PIOS_DSM_NUM_INPUTS) { if (channel_num < PIOS_DSM_NUM_INPUTS) {
if (channel_log & (1 << channel_num))
{
/* Found duplicate! */
/* Update resolution and restart processing the current frame. */
resolution = 10;
return PIOS_DSM_UnrollChannels(dsm_dev);
}
state->channel_data[channel_num] = (word & mask); state->channel_data[channel_num] = (word & mask);
/* keep track of this channel */
channel_log |= (1 << channel_num);
} }
} }

45
flight/pios/stm32f4xx/pios_dsm.c
View File

@ -181,7 +181,9 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev)
static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
{ {
struct pios_dsm_state *state = &(dsm_dev->state); struct pios_dsm_state *state = &(dsm_dev->state);
uint8_t resolution; /* Fix resolution for detection. */
static uint8_t resolution = 11;
uint32_t channel_log = 0;
#ifdef DSM_LOST_FRAME_COUNTER #ifdef DSM_LOST_FRAME_COUNTER
/* increment the lost frame counter */ /* increment the lost frame counter */
@ -190,38 +192,6 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
state->frames_lost_last = frames_lost; state->frames_lost_last = frames_lost;
#endif #endif
/* check the frame type assuming master satellite stream */
uint8_t type = state->received_data[1];
switch (type) {
case 0x01:
case 0x02:
case 0x12:
/* DSM2, DSMJ stream */
if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) {
/* DSM2/DSMJ resolution is known from the header */
resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10;
} else {
/* DSMX resolution should explicitly be selected */
goto stream_error;
}
break;
case 0xA2:
case 0xB2:
/* DSMX stream */
if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) {
resolution = 10;
} else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) {
resolution = 11;
} else {
/* DSMX resolution should explicitly be selected */
goto stream_error;
}
break;
default:
/* unknown yet data stream */
goto stream_error;
}
/* unroll channels */ /* unroll channels */
uint8_t *s = &(state->received_data[2]); uint8_t *s = &(state->received_data[2]);
uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff; uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff;
@ -244,7 +214,16 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
/* extract and save the channel value */ /* extract and save the channel value */
uint8_t channel_num = (word >> resolution) & 0x0f; uint8_t channel_num = (word >> resolution) & 0x0f;
if (channel_num < PIOS_DSM_NUM_INPUTS) { if (channel_num < PIOS_DSM_NUM_INPUTS) {
if (channel_log & (1 << channel_num))
{
/* Found duplicate! */
/* Update resolution and restart processing the current frame. */
resolution = 10;
return PIOS_DSM_UnrollChannels(dsm_dev);
}
state->channel_data[channel_num] = (word & mask); state->channel_data[channel_num] = (word & mask);
/* keep track of this channel */
channel_log |= (1 << channel_num);
} }
} }