From 3dfb27f22067d3821aa9b0ab9208361a683a78ab Mon Sep 17 00:00:00 2001 From: 3djc Date: Fri, 28 Aug 2020 17:01:13 +0200 Subject: [PATCH] Add rx num handling to CRSF (#7881) --- companion/src/firmwares/moduledata.cpp | 1 + companion/src/modeledit/setup.cpp | 1 + radio/src/crc.cpp | 45 ++++++++++++++++++++++++++ radio/src/crc.h | 1 + radio/src/gui/128x64/model_setup.cpp | 12 ++++--- radio/src/gui/212x64/model_setup.cpp | 12 ++++--- radio/src/gui/480x272/model_setup.cpp | 13 +++++--- radio/src/gui/gui_common.h | 11 ++++--- radio/src/pulses/crossfire.cpp | 25 +++++++++++++- radio/src/pulses/modules_helpers.h | 3 ++ radio/src/telemetry/crossfire.cpp | 4 +++ radio/src/telemetry/crossfire.h | 12 +++++++ radio/src/telemetry/telemetry.cpp | 5 +++ 13 files changed, 125 insertions(+), 20 deletions(-) diff --git a/companion/src/firmwares/moduledata.cpp b/companion/src/firmwares/moduledata.cpp index 3ce9836fb..b36d88e04 100644 --- a/companion/src/firmwares/moduledata.cpp +++ b/companion/src/firmwares/moduledata.cpp @@ -66,6 +66,7 @@ bool ModuleData::supportRxNum() const case PULSES_PXX_R9M_LITE: case PULSES_PXX_R9M_LITE_PRO: case PULSES_MULTIMODULE: + case PULSES_CROSSFIRE: case PULSES_ACCESS_ISRM: case PULSES_ACCST_ISRM_D16: case PULSES_ACCESS_R9M: diff --git a/companion/src/modeledit/setup.cpp b/companion/src/modeledit/setup.cpp index df6792ff8..2bd8cc055 100644 --- a/companion/src/modeledit/setup.cpp +++ b/companion/src/modeledit/setup.cpp @@ -424,6 +424,7 @@ void ModulePanel::update() max_rx_num = 20; break; case PULSES_CROSSFIRE: + mask |= MASK_CHANNELS_RANGE | MASK_RX_NUMBER; case PULSES_GHOST: mask |= MASK_CHANNELS_RANGE; module.channelsCount = 16; diff --git a/radio/src/crc.cpp b/radio/src/crc.cpp index 26a1af500..c0be4d7fa 100644 --- a/radio/src/crc.cpp +++ b/radio/src/crc.cpp @@ -79,3 +79,48 @@ uint8_t crc8(const uint8_t * ptr, uint32_t len) } return crc; } + +// CRC8 implementation with polynom = 0xBA +const unsigned char crc8tab_BA[256] = { + 0x00, 0xBA, 0xCE, 0x74, 0x26, 0x9C, 0xE8, 0x52, + 0x4C, 0xF6, 0x82, 0x38, 0x6A, 0xD0, 0xA4, 0x1E, + 0x98, 0x22, 0x56, 0xEC, 0xBE, 0x04, 0x70, 0xCA, + 0xD4, 0x6E, 0x1A, 0xA0, 0xF2, 0x48, 0x3C, 0x86, + 0x8A, 0x30, 0x44, 0xFE, 0xAC, 0x16, 0x62, 0xD8, + 0xC6, 0x7C, 0x08, 0xB2, 0xE0, 0x5A, 0x2E, 0x94, + 0x12, 0xA8, 0xDC, 0x66, 0x34, 0x8E, 0xFA, 0x40, + 0x5E, 0xE4, 0x90, 0x2A, 0x78, 0xC2, 0xB6, 0x0C, + 0xAE, 0x14, 0x60, 0xDA, 0x88, 0x32, 0x46, 0xFC, + 0xE2, 0x58, 0x2C, 0x96, 0xC4, 0x7E, 0x0A, 0xB0, + 0x36, 0x8C, 0xF8, 0x42, 0x10, 0xAA, 0xDE, 0x64, + 0x7A, 0xC0, 0xB4, 0x0E, 0x5C, 0xE6, 0x92, 0x28, + 0x24, 0x9E, 0xEA, 0x50, 0x02, 0xB8, 0xCC, 0x76, + 0x68, 0xD2, 0xA6, 0x1C, 0x4E, 0xF4, 0x80, 0x3A, + 0xBC, 0x06, 0x72, 0xC8, 0x9A, 0x20, 0x54, 0xEE, + 0xF0, 0x4A, 0x3E, 0x84, 0xD6, 0x6C, 0x18, 0xA2, + 0xE6, 0x5C, 0x28, 0x92, 0xC0, 0x7A, 0x0E, 0xB4, + 0xAA, 0x10, 0x64, 0xDE, 0x8C, 0x36, 0x42, 0xF8, + 0x7E, 0xC4, 0xB0, 0x0A, 0x58, 0xE2, 0x96, 0x2C, + 0x32, 0x88, 0xFC, 0x46, 0x14, 0xAE, 0xDA, 0x60, + 0x6C, 0xD6, 0xA2, 0x18, 0x4A, 0xF0, 0x84, 0x3E, + 0x20, 0x9A, 0xEE, 0x54, 0x06, 0xBC, 0xC8, 0x72, + 0xF4, 0x4E, 0x3A, 0x80, 0xD2, 0x68, 0x1C, 0xA6, + 0xB8, 0x02, 0x76, 0xCC, 0x9E, 0x24, 0x50, 0xEA, + 0x48, 0xF2, 0x86, 0x3C, 0x6E, 0xD4, 0xA0, 0x1A, + 0x04, 0xBE, 0xCA, 0x70, 0x22, 0x98, 0xEC, 0x56, + 0xD0, 0x6A, 0x1E, 0xA4, 0xF6, 0x4C, 0x38, 0x82, + 0x9C, 0x26, 0x52, 0xE8, 0xBA, 0x00, 0x74, 0xCE, + 0xC2, 0x78, 0x0C, 0xB6, 0xE4, 0x5E, 0x2A, 0x90, + 0x8E, 0x34, 0x40, 0xFA, 0xA8, 0x12, 0x66, 0xDC, + 0x5A, 0xE0, 0x94, 0x2E, 0x7C, 0xC6, 0xB2, 0x08, + 0x16, 0xAC, 0xD8, 0x62, 0x30, 0x8A, 0xFE, 0x44 +}; + +uint8_t crc8_BA(const uint8_t * ptr, uint32_t len) +{ + uint8_t crc = 0; + for (uint32_t i=0; i 0) { CHECK_INCDEC_MODELVAR_ZERO(event, g_model.header.modelId[moduleIdx], getMaxRxNum(moduleIdx)); if (checkIncDec_Ret) { + if (isModuleCrossfire(moduleIdx)) + moduleState[EXTERNAL_MODULE].counter = CRSF_FRAME_MODELID; modelHeaders[g_eeGeneral.currModel].modelId[moduleIdx] = g_model.header.modelId[moduleIdx]; } else if (event == EVT_KEY_LONG(KEY_ENTER)) { @@ -1366,6 +1363,11 @@ void menuModelSetup(event_t event) } } } + } + else if (attr) { + l_posHorz += 1; + } + if (isModuleBindRangeAvailable(moduleIdx)) { lcdDrawText(bindButtonPos, y, STR_MODULE_BIND, l_posHorz == 1 ? attr : 0); if (isModuleRangeAvailable(moduleIdx)) { lcdDrawText(lcdNextPos + 2, y, STR_MODULE_RANGE, l_posHorz == 2 ? attr : 0); diff --git a/radio/src/gui/212x64/model_setup.cpp b/radio/src/gui/212x64/model_setup.cpp index 36c7783bc..7503ae92a 100644 --- a/radio/src/gui/212x64/model_setup.cpp +++ b/radio/src/gui/212x64/model_setup.cpp @@ -1160,15 +1160,12 @@ void menuModelSetup(event_t event) if (isModuleRxNumAvailable(moduleIdx)) { lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, g_model.header.modelId[moduleIdx], (l_posHorz == 0 ? attr : 0) | LEADING0 | LEFT, 2); bindButtonPos = lcdNextPos + FW; - } - else if (attr) { - l_posHorz += 1; - } - if (isModuleBindRangeAvailable(moduleIdx)) { if (attr && l_posHorz == 0) { if (s_editMode > 0) { CHECK_INCDEC_MODELVAR_ZERO(event, g_model.header.modelId[moduleIdx], getMaxRxNum(moduleIdx)); if (checkIncDec_Ret) { + if (isModuleCrossfire(moduleIdx)) + moduleState[EXTERNAL_MODULE].counter = CRSF_FRAME_MODELID; modelHeaders[g_eeGeneral.currModel].modelId[moduleIdx] = g_model.header.modelId[moduleIdx]; } else if (event == EVT_KEY_LONG(KEY_ENTER)) { @@ -1181,6 +1178,11 @@ void menuModelSetup(event_t event) } } } + } + else if (attr) { + l_posHorz += 1; + } + if (isModuleBindRangeAvailable(moduleIdx)) { lcdDrawText(bindButtonPos, y, STR_MODULE_BIND, l_posHorz == 1 ? attr : 0); if (isModuleRangeAvailable(moduleIdx)) { lcdDrawText(lcdNextPos + FW, y, STR_MODULE_RANGE, l_posHorz == 2 ? attr : 0); diff --git a/radio/src/gui/480x272/model_setup.cpp b/radio/src/gui/480x272/model_setup.cpp index 556b1f2bf..315d5a23e 100644 --- a/radio/src/gui/480x272/model_setup.cpp +++ b/radio/src/gui/480x272/model_setup.cpp @@ -1543,14 +1543,12 @@ bool menuModelSetup(event_t event) if (isModuleRxNumAvailable(moduleIdx)) { lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, g_model.header.modelId[moduleIdx], (l_posHorz==0 ? attr : 0) | LEADING0 | LEFT, 2); bindButtonPos += 40; - } - else if (attr) { - l_posHorz += 1; - } - if (isModuleBindRangeAvailable(moduleIdx)) { if (attr && l_posHorz == 0) { if (s_editMode > 0) { CHECK_INCDEC_MODELVAR_ZERO(event, g_model.header.modelId[moduleIdx], getMaxRxNum(moduleIdx)); + if (checkIncDec_Ret && isModuleCrossfire(moduleIdx)) { + moduleState[EXTERNAL_MODULE].counter = CRSF_FRAME_MODELID; + } if (event == EVT_KEY_LONG(KEY_ENTER)) { killEvents(event); uint8_t newVal = modelslist.findNextUnusedModelId(moduleIdx); @@ -1561,6 +1559,11 @@ bool menuModelSetup(event_t event) } } } + } + else if (attr) { + l_posHorz += 1; + } + if (isModuleBindRangeAvailable(moduleIdx)) { drawButton(bindButtonPos, y, STR_MODULE_BIND, (moduleState[moduleIdx].mode == MODULE_MODE_BIND ? BUTTON_ON : BUTTON_OFF) | (l_posHorz==1 ? attr : 0)); if (isModuleRangeAvailable(moduleIdx)) { drawButton(bindButtonPos + 80, y, STR_MODULE_RANGE, (moduleState[moduleIdx].mode == MODULE_MODE_RANGECHECK ? BUTTON_ON : BUTTON_OFF) | (l_posHorz==2 ? attr : 0)); diff --git a/radio/src/gui/gui_common.h b/radio/src/gui/gui_common.h index 2c89a24ed..ab08dad08 100644 --- a/radio/src/gui/gui_common.h +++ b/radio/src/gui/gui_common.h @@ -145,11 +145,14 @@ void runFatalErrorScreen(const char * message); inline uint8_t MODULE_BIND_ROWS(int moduleIdx) { + if (isModuleCrossfire(moduleIdx)) + return 0; + if (isModuleMultimodule(moduleIdx)) { - if (IS_RX_MULTI(moduleIdx)) - return 1; - else - return 2; + if (IS_RX_MULTI(moduleIdx)) + return 1; + else + return 2; } else if (isModuleXJTD8(moduleIdx) || isModuleSBUS(moduleIdx) || isModuleAFHDS3(moduleIdx)) { return 1; diff --git a/radio/src/pulses/crossfire.cpp b/radio/src/pulses/crossfire.cpp index 2f5427902..cbe4373e4 100644 --- a/radio/src/pulses/crossfire.cpp +++ b/radio/src/pulses/crossfire.cpp @@ -28,6 +28,23 @@ #define CROSSFIRE_CENTER_CH_OFFSET(ch) (0) #endif + +uint8_t createCrossfireModelIDFrame(uint8_t * frame) +{ + uint8_t * buf = frame; + *buf++ = UART_SYNC; /* device address */ + *buf++ = 8; /* frame length */ + *buf++ = COMMAND_ID; /* cmd type */ + *buf++ = MODULE_ADDRESS; /* Destination Address */ + *buf++ = RADIO_ADDRESS; /* Origin Address */ + *buf++ = SUBCOMMAND_CRSF; /* sub command */ + *buf++ = COMMAND_MODEL_SELECT_ID; /* command of set model/receiver id */ + *buf++ = g_model.header.modelId[EXTERNAL_MODULE]; /* model ID */ + *buf++ = crc8_BA(frame + 2, 6); + *buf++ = crc8(frame + 2, 7); + return buf - frame; +} + // Range for pulses (channels output) is [-1024:+1024] uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses) { @@ -65,7 +82,13 @@ void setupPulsesCrossfire() else #endif { - extmodulePulsesData.crossfire.length = createCrossfireChannelsFrame(pulses, &channelOutputs[g_model.moduleData[EXTERNAL_MODULE].channelsStart]); + if (moduleState[EXTERNAL_MODULE].counter == CRSF_FRAME_MODELID) { + extmodulePulsesData.crossfire.length = createCrossfireModelIDFrame(pulses); + moduleState[EXTERNAL_MODULE].counter = CRSF_FRAME_MODELID_SENT; + } + else { + extmodulePulsesData.crossfire.length = createCrossfireChannelsFrame(pulses, &channelOutputs[g_model.moduleData[EXTERNAL_MODULE].channelsStart]); + } } } } diff --git a/radio/src/pulses/modules_helpers.h b/radio/src/pulses/modules_helpers.h index 2d56bf564..24e45e717 100644 --- a/radio/src/pulses/modules_helpers.h +++ b/radio/src/pulses/modules_helpers.h @@ -429,6 +429,9 @@ inline bool isModuleRxNumAvailable(uint8_t moduleIdx) if (isModuleMultimodule(moduleIdx)) return true; + if (isModuleCrossfire(moduleIdx)) + return true; + return false; } diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index c71f6ce54..6c876b7b7 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/radio/src/telemetry/crossfire.cpp @@ -106,6 +106,10 @@ void processCrossfireTelemetryFrame() return; } + if (telemetryState == TELEMETRY_INIT && moduleState[EXTERNAL_MODULE].counter != CRSF_FRAME_MODELID_SENT) { + moduleState[EXTERNAL_MODULE].counter = CRSF_FRAME_MODELID; + } + uint8_t id = telemetryRxBuffer[2]; int32_t value; switch(id) { diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h index 79cc5f025..9855b78f4 100644 --- a/radio/src/telemetry/crossfire.h +++ b/radio/src/telemetry/crossfire.h @@ -40,7 +40,12 @@ #define PING_DEVICES_ID 0x28 #define DEVICE_INFO_ID 0x29 #define REQUEST_SETTINGS_ID 0x2A +#define COMMAND_ID 0x32 +#define RADIO_ID 0x3A +#define UART_SYNC 0xC8 +#define SUBCOMMAND_CRSF 0x10 +#define COMMAND_MODEL_SELECT_ID 0x05 struct CrossfireSensor { const uint8_t id; @@ -79,8 +84,15 @@ enum CrossfireSensorIndexes { UNKNOWN_INDEX, }; +enum CrossfireFrames{ + CRSF_FRAME_CHANNEL, + CRSF_FRAME_MODELID, + CRSF_FRAME_MODELID_SENT +}; + void processCrossfireTelemetryData(uint8_t data); void crossfireSetDefault(int index, uint8_t id, uint8_t subId); +uint8_t createCrossfireModelIDFrame(uint8_t * frame); const uint32_t CROSSFIRE_BAUDRATES[] = { 400000, diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 5c5b3f514..39e2f10e0 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -216,6 +216,11 @@ void telemetryWakeup() if (TELEMETRY_STREAMING()) { if (telemetryState == TELEMETRY_KO) { AUDIO_TELEMETRY_BACK(); +#if defined(CROSSFIRE) + if (isModuleCrossfire(EXTERNAL_MODULE)) { + moduleState[EXTERNAL_MODULE].counter = CRSF_FRAME_MODELID; + } +#endif } telemetryState = TELEMETRY_OK; }