diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 024af1045..de343cb83 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -1057,7 +1057,7 @@ void PIOS_I2C_main_adapter_er_irq_handler(void) #include "pios_rcvr_priv.h" /* One slot per selectable receiver group. - * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS + * eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS * NOTE: No slot in this map for NONE. */ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; @@ -1197,17 +1197,35 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_GPS */ break; - case HWSETTINGS_CC_MAINPORT_SPEKTRUM1: - case HWSETTINGS_CC_MAINPORT_SPEKTRUM2: + case HWSETTINGS_CC_MAINPORT_DSM2: + case HWSETTINGS_CC_MAINPORT_DSMX10BIT: + case HWSETTINGS_CC_MAINPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_SPEKTRUM) { + enum pios_dsm_proto proto; + switch (hwsettings_cc_mainport) { + case HWSETTINGS_CC_MAINPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_CC_MAINPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + proto = PIOS_DSM_PROTO_DSM2; + break; + } + uint32_t pios_usart_spektrum_id; if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_main_cfg)) { PIOS_Assert(0); } 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, + proto, 0)) { PIOS_Assert(0); } @@ -1215,11 +1233,7 @@ void PIOS_Board_Init(void) { if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) { PIOS_Assert(0); } - if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM1) { - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id; - } else { - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2] = pios_spektrum_rcvr_id; - } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_spektrum_rcvr_id; } #endif /* PIOS_INCLUDE_SPEKTRUM */ break; @@ -1270,17 +1284,35 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_GPS */ break; - case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM1: - case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM2: + case HWSETTINGS_CC_FLEXIPORT_DSM2: + case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: + case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_SPEKTRUM) { + enum pios_dsm_proto proto; + switch (hwsettings_cc_flexiport) { + case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: + proto = PIOS_DSM_PROTO_DSMX10BIT; + break; + case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: + proto = PIOS_DSM_PROTO_DSMX11BIT; + break; + default: + proto = PIOS_DSM_PROTO_DSM2; + break; + } + uint32_t pios_usart_spektrum_id; if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_flexi_cfg)) { PIOS_Assert(0); } 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, + proto, hwsettings_DSMxBind)) { PIOS_Assert(0); } @@ -1288,11 +1320,7 @@ void PIOS_Board_Init(void) { if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) { PIOS_Assert(0); } - if (hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM1) { - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id; - } else { - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2] = pios_spektrum_rcvr_id; - } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_spektrum_rcvr_id; } #endif /* PIOS_INCLUDE_SPEKTRUM */ break; diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 6186a6af9..db98283df 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -439,11 +439,11 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1: - group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM1; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2: - group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM2; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; break; case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 272961e09..80eb232ce 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -59,9 +59,7 @@ struct pios_spektrum_state { uint8_t failsafe_timer; uint8_t frame_found; uint8_t byte_count; - uint8_t resolution; - uint8_t frame_mode; -#ifdef SPEKTRUM_LOST_FRAME_COUNTER +#if SPEKTRUM_LOST_FRAME_COUNTER uint8_t frames_lost_last; uint16_t frames_lost; #endif @@ -70,6 +68,7 @@ struct pios_spektrum_state { struct pios_spektrum_dev { enum pios_spektrum_dev_magic magic; const struct pios_spektrum_cfg *cfg; + enum pios_dsm_proto proto; struct pios_spektrum_state state; }; @@ -110,8 +109,10 @@ static bool PIOS_Spektrum_Validate(struct pios_spektrum_dev *spektrum_dev) } /* Try to bind DSMx satellite using specified number of pulses */ -static void PIOS_Spektrum_Bind(const struct pios_spektrum_cfg *cfg, uint8_t bind) +static void PIOS_Spektrum_Bind(struct pios_spektrum_dev *spektrum_dev, uint8_t bind) { + const struct pios_spektrum_cfg *cfg = spektrum_dev->cfg; + GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin; GPIO_InitStructure.GPIO_Speed = cfg->bind.init.GPIO_Speed; @@ -142,46 +143,26 @@ static void PIOS_Spektrum_Bind(const struct pios_spektrum_cfg *cfg, uint8_t bind } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ -static void PIOS_Spektrum_ResetChannels(struct pios_spektrum_state *state) +static void PIOS_Spektrum_ResetChannels(struct pios_spektrum_dev *spektrum_dev) { + struct pios_spektrum_state *state = &(spektrum_dev->state); for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) { state->channel_data[i] = PIOS_RCVR_TIMEOUT; } } -/** - * 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) +static void PIOS_Spektrum_ResetState(struct pios_spektrum_dev *spektrum_dev) { + struct pios_spektrum_state *state = &(spektrum_dev->state); state->receive_timer = 0; state->failsafe_timer = 0; state->frame_found = 0; -#ifdef SPEKTRUM_LOST_FRAME_COUNTER +#if SPEKTRUM_LOST_FRAME_COUNTER state->frames_lost_last = 0; state->frames_lost = 0; #endif - PIOS_Spektrum_ResetChannels(state); + PIOS_Spektrum_ResetChannels(spektrum_dev); } /** @@ -189,9 +170,12 @@ static void PIOS_Spektrum_ResetState(struct pios_spektrum_state *state) * \output 0 frame data accepted * \output -1 frame error found */ -static int PIOS_Spektrum_UnrollChannels(struct pios_spektrum_state *state) +static int PIOS_Spektrum_UnrollChannels(struct pios_spektrum_dev *spektrum_dev) { -#ifdef SPEKTRUM_LOST_FRAME_COUNTER + struct pios_spektrum_state *state = &(spektrum_dev->state); + uint8_t resolution; + +#if 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); @@ -204,69 +188,81 @@ static int PIOS_Spektrum_UnrollChannels(struct pios_spektrum_state *state) case 0x01: case 0x02: case 0x12: - state->resolution = (type & SPEKTRUM_DSM2_RES_MASK) ? 11 : 10; - state->frame_mode = (type & SPEKTRUM_DSM2_FNUM_MASK); + /* DSM2, DSMJ stream */ + if (spektrum_dev->proto == PIOS_DSM_PROTO_DSM2) { + /* DSM2/DSMJ resolution is known from the header */ + resolution = (type & SPEKTRUM_DSM2_RES_MASK) ? 11 : 10; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } break; + case 0xA2: case 0xB2: - /* FIXME: we should guess the resolution using frame mode found */ - goto fail; + /* DSMX stream */ + if (spektrum_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { + resolution = 10; + } else if (spektrum_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { + resolution = 11; + } else { + /* DSMX resolution should explicitly be selected */ + goto stream_error; + } + break; default: - goto fail; + /* unknown yet data stream */ + goto stream_error; } /* unroll channels */ uint8_t *s = &(state->received_data[2]); - uint16_t mask = (state->resolution == 10) ? 0x03ff : 0x07ff; + uint16_t mask = (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; - } + /* skip empty channel slot */ + if (word == 0xffff) + continue; + + /* minimal data validation */ + if ((i > 0) && (word & SPEKTRUM_2ND_FRAME_MASK)) { + /* invalid frame data, ignore rest of the frame */ + goto stream_error; } /* extract and save the channel value */ - uint8_t channel_num = (word >> state->resolution) & 0x0f; + uint8_t channel_num = (word >> resolution) & 0x0f; if (channel_num < PIOS_SPEKTRUM_NUM_INPUTS) state->channel_data[channel_num] = (word & mask); } -#ifdef SPEKTRUM_LOST_FRAME_COUNTER +#if 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 */ + /* all channels processed */ return 0; -fail: +stream_error: + /* either DSM2 selected with DSMX stream found, or vice-versa */ 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) +static void PIOS_Spektrum_UpdateState(struct pios_spektrum_dev *spektrum_dev, uint8_t byte) { + struct pios_spektrum_state *state = &(spektrum_dev->state); 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; + state->received_data[state->byte_count++] = byte; if (state->byte_count == SPEKTRUM_FRAME_LENGTH) { /* full frame received - process and wait for new one */ - if (!PIOS_Spektrum_UnrollChannels(state)) + if (!PIOS_Spektrum_UnrollChannels(spektrum_dev)) /* data looking good */ state->failsafe_timer = 0; @@ -282,6 +278,7 @@ 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, + enum pios_dsm_proto proto, uint8_t bind) { PIOS_DEBUG_Assert(spektrum_id); @@ -296,12 +293,13 @@ int32_t PIOS_Spektrum_Init(uint32_t *spektrum_id, /* Bind the configuration to the device instance */ spektrum_dev->cfg = cfg; + spektrum_dev->proto = proto; /* Bind the receiver if requested */ if (bind) - PIOS_Spektrum_Bind(cfg, bind); + PIOS_Spektrum_Bind(spektrum_dev, bind); - PIOS_Spektrum_ResetState(&(spektrum_dev->state)); + PIOS_Spektrum_ResetState(spektrum_dev); *spektrum_id = (uint32_t)spektrum_dev; @@ -327,12 +325,10 @@ static uint16_t PIOS_Spektrum_RxInCallback(uint32_t 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; + PIOS_Spektrum_UpdateState(spektrum_dev, buf[i]); + spektrum_dev->state.receive_timer = 0; } /* Always signal that we can accept another byte */ @@ -346,6 +342,28 @@ static uint16_t PIOS_Spektrum_RxInCallback(uint32_t context, return buf_len; } +/** + * 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]; +} + /** * Input data supervisor is called periodically and provides * two functions: frame syncing and failsafe triggering. @@ -376,7 +394,7 @@ static void PIOS_Spektrum_Supervisor(uint32_t spektrum_id) /* activate failsafe if no frames have arrived in 102.4ms */ if (++state->failsafe_timer > 64) { - PIOS_Spektrum_ResetChannels(state); + PIOS_Spektrum_ResetChannels(spektrum_dev); state->failsafe_timer = 0; } } diff --git a/flight/PiOS/inc/pios_spektrum_priv.h b/flight/PiOS/inc/pios_spektrum_priv.h index 46dfcedc2..2c7f4520d 100644 --- a/flight/PiOS/inc/pios_spektrum_priv.h +++ b/flight/PiOS/inc/pios_spektrum_priv.h @@ -66,11 +66,13 @@ * 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. + * - for DSMX this byte contains just 0xB2 or 0xA2 value for any resolution. + * It is not known at the moment how to find the exact resolution from the + * DSMX data stream. The frame number (1 or 2) and 10/11 bit resolution were + * found in different data streams. So it is safer at the moment to ask user + * explicitly choose the resolution. + * Also some weird throttle channel (0) behavior was found in some streams + * from DX8 transmitter (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] @@ -100,14 +102,22 @@ #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 + * Include lost frame counter and provide it as a last channel value + * for debugging. Currently is not used by the receiver layer. */ +#define SPEKTRUM_LOST_FRAME_COUNTER 0 + +/* Spektrum protocol variations */ +enum pios_dsm_proto { + PIOS_DSM_PROTO_DSM2, + PIOS_DSM_PROTO_DSMX10BIT, + PIOS_DSM_PROTO_DSMX11BIT, +}; + +/* Spektrum receiver instance configuration */ struct pios_spektrum_cfg { struct stm32_gpio bind; }; @@ -118,6 +128,7 @@ 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, + enum pios_dsm_proto proto, uint8_t bind); #endif /* PIOS_SPEKTRUM_PRIV_H */ diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp index 70223d57f..1eca667a8 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp @@ -85,8 +85,8 @@ void inputChannelForm::groupUpdated() count = 8; // Need to make this 6 for CC break; case ManualControlSettings::CHANNELGROUPS_PPM: - case ManualControlSettings::CHANNELGROUPS_SPEKTRUM1: - case ManualControlSettings::CHANNELGROUPS_SPEKTRUM2: + case ManualControlSettings::CHANNELGROUPS_DSMMAINPORT: + case ManualControlSettings::CHANNELGROUPS_DSMFLEXIPORT: count = 12; break; case ManualControlSettings::CHANNELGROUPS_SBUS: diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 714cc9d4f..e5126fbfa 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,13 +1,13 @@ Selection of optional hardware configurations. - - + + - + - + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index ae7c9b24c..78f38011f 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -3,7 +3,7 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. + options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,GCS,None" defaultvalue="None"/> Monitors which receiver channels have been active within the last second.