diff --git a/flight/pios/stm32f10x/pios_dsm.c b/flight/pios/stm32f10x/pios_dsm.c index a0ff382d9..2a5ee90d9 100644 --- a/flight/pios/stm32f10x/pios_dsm.c +++ b/flight/pios/stm32f10x/pios_dsm.c @@ -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) { 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 /* 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; #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 */ 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 */ uint8_t channel_num = (word >> resolution) & 0x0f; 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); + /* keep track of this channel */ + channel_log |= (1 << channel_num); } } diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c index b6d8f0d85..6b54b9cc8 100644 --- a/flight/pios/stm32f4xx/pios_dsm.c +++ b/flight/pios/stm32f4xx/pios_dsm.c @@ -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) { 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 /* 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; #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 */ uint8_t *s = &(state->received_data[2]); 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 */ uint8_t channel_num = (word >> resolution) & 0x0f; 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); + /* keep track of this channel */ + channel_log |= (1 << channel_num); } }