From 77386e6391e0b7c5ca9d665ce87d8bf238ea551c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C5=A0t=C4=9Bp=C3=A1n=20Daleck=C3=BD?= Date: Wed, 8 Dec 2021 18:44:46 +0100 Subject: [PATCH] IRC Ghost 12 bit support --- radio/src/datastructs_private.h | 4 ++ radio/src/gui/128x64/model_setup.cpp | 4 ++ radio/src/gui/212x64/model_setup.cpp | 4 ++ radio/src/gui/colorlcd/model_setup.cpp | 5 ++ radio/src/gui/gui_common.h | 2 + radio/src/pulses/ghost.cpp | 98 ++++++++++++++------------ radio/src/telemetry/ghost.cpp | 8 +++ radio/src/telemetry/ghost.h | 21 +++--- 8 files changed, 94 insertions(+), 52 deletions(-) diff --git a/radio/src/datastructs_private.h b/radio/src/datastructs_private.h index f2cd1fc32..8ddda6324 100644 --- a/radio/src/datastructs_private.h +++ b/radio/src/datastructs_private.h @@ -488,6 +488,10 @@ PACK(struct ModuleData { uint8_t spare2:1 SKIP; int8_t refreshRate; // definition as framelength for ppm (* 5 + 225 = time in 1/10 ms) } sbus); + NOBACKUP(struct { + uint8_t raw12bits:1; + uint8_t spare1:7; + } ghost); NOBACKUP(struct { uint8_t receivers:7; // 4 bits spare uint8_t racingMode:1; diff --git a/radio/src/gui/128x64/model_setup.cpp b/radio/src/gui/128x64/model_setup.cpp index ceceb045f..57a286db4 100644 --- a/radio/src/gui/128x64/model_setup.cpp +++ b/radio/src/gui/128x64/model_setup.cpp @@ -1651,6 +1651,10 @@ void menuModelSetup(event_t event) lcdDrawTextAlignedLeft(y, STR_WARN_BATTVOLTAGE); putsVolts(lcdLastRightPos, y, getBatteryVoltage(), attr | PREC2 | LEFT); } + else if (isModuleGhost(moduleIdx)) { + auto & module = g_model.moduleData[moduleIdx]; + module.ghost.raw12bits = editCheckBox(module.ghost.raw12bits , MODEL_SETUP_2ND_COLUMN, y, INDENT "Raw 12 bits", attr, event); + } break; } diff --git a/radio/src/gui/212x64/model_setup.cpp b/radio/src/gui/212x64/model_setup.cpp index 74efb6451..7bc135b4d 100644 --- a/radio/src/gui/212x64/model_setup.cpp +++ b/radio/src/gui/212x64/model_setup.cpp @@ -1431,6 +1431,10 @@ void menuModelSetup(event_t event) lcdDrawTextAlignedLeft(y, STR_WARN_BATTVOLTAGE); putsVolts(lcdLastRightPos, y, getBatteryVoltage(), attr | PREC2 | LEFT); } + else if (isModuleGhost(moduleIdx)) { + auto & module = g_model.moduleData[moduleIdx]; + module.ghost.raw12bits = editCheckBox(module.ghost.raw12bits , MODEL_SETUP_2ND_COLUMN, y, INDENT "Raw 12 bits", attr, event); + } } break; diff --git a/radio/src/gui/colorlcd/model_setup.cpp b/radio/src/gui/colorlcd/model_setup.cpp index 25d062e4f..85132098f 100644 --- a/radio/src/gui/colorlcd/model_setup.cpp +++ b/radio/src/gui/colorlcd/model_setup.cpp @@ -1255,6 +1255,11 @@ class ModuleWindow : public FormGroup { grid.nextLine(); } + if (isModuleGhost(moduleIdx)) { + new StaticText(this, grid.getLabelSlot(true), "Raw 12 bits", 0, COLOR_THEME_PRIMARY1); + new CheckBox(this, grid.getFieldSlot(), GET_SET_DEFAULT(g_model.moduleData[moduleIdx].ghost.raw12bits)); + } + auto par = getParent(); par->moveWindowsTop(top() + 1, adjustHeight()); par->adjustInnerHeight(); diff --git a/radio/src/gui/gui_common.h b/radio/src/gui/gui_common.h index d5606cf89..e8128d576 100644 --- a/radio/src/gui/gui_common.h +++ b/radio/src/gui/gui_common.h @@ -361,6 +361,8 @@ inline uint8_t MODULE_OPTION_ROW(uint8_t moduleIdx) { return TITLE_ROW; if(isModuleAFHDS3(moduleIdx)) return HIDDEN_ROW; + if(isModuleGhost(moduleIdx)) + return 0; return MULTIMODULE_OPTIONS_ROW(moduleIdx); } diff --git a/radio/src/pulses/ghost.cpp b/radio/src/pulses/ghost.cpp index 6ec31caab..ebb58e2f6 100755 --- a/radio/src/pulses/ghost.cpp +++ b/radio/src/pulses/ghost.cpp @@ -24,59 +24,73 @@ uint8_t createGhostMenuControlFrame(uint8_t * frame, int16_t * pulses) { uint8_t * buf = frame; -#if SPORT_MAX_BAUDRATE < 400000 - *buf++ = g_eeGeneral.telemetryBaudrate == GHST_TELEMETRY_RATE_400K ? GHST_ADDR_MODULE_SYM : GHST_ADDR_MODULE_ASYM; -#else - *buf++ = GHST_ADDR_MODULE_SYM; -#endif - *buf++ = GHST_UL_RC_CHANS_SIZE; - uint8_t * crc_start = buf; - *buf++ = GHST_UL_MENU_CTRL; + *buf++ = getGhostModuleAddr(); // addr + *buf++ = GHST_UL_RC_CHANS_SIZE; // length + const uint8_t * crc_start = buf; + *buf++ = GHST_UL_MENU_CTRL; // type + + // payload *buf++ = reusableBuffer.ghostMenu.buttonAction; // Joystick states, Up, Down, Left, Right, Press *buf++ = reusableBuffer.ghostMenu.menuAction; // menu control, open, close, etc. - - for (uint8_t i = 0; i < 8; i++) + for (uint8_t i = 0; i < 8; i++) { *buf++ = 0; // padding to make this the same size as the pulses packet + } + // crc *buf++ = crc8(crc_start, GHST_UL_RC_CHANS_SIZE - 1); return buf - frame; } // Range for pulses (channels output) is [-1024:+1024] -uint8_t createGhostChannelsFrame(uint8_t * frame, int16_t * pulses) +uint8_t createGhostChannelsFrame(uint8_t * frame, int16_t * pulses, bool raw12bits) { - static uint8_t lastGhostFrameId = GHST_UL_RC_CHANS_HS4_5TO8; + static uint8_t lastGhostFrameId = 0; uint8_t ghostUpper4Offset = 0; switch (lastGhostFrameId) { case GHST_UL_RC_CHANS_HS4_5TO8: - ghostUpper4Offset = 0; - break; - case GHST_UL_RC_CHANS_HS4_9TO12: + case GHST_UL_RC_CHANS_HS4_12_5TO8: + lastGhostFrameId = raw12bits ? GHST_UL_RC_CHANS_HS4_12_9TO12 : GHST_UL_RC_CHANS_HS4_9TO12; ghostUpper4Offset = 4; - break; - case GHST_UL_RC_CHANS_HS4_13TO16: + break; + + case GHST_UL_RC_CHANS_HS4_9TO12: + case GHST_UL_RC_CHANS_HS4_12_9TO12: + lastGhostFrameId = raw12bits ? GHST_UL_RC_CHANS_HS4_12_13TO16 : GHST_UL_RC_CHANS_HS4_13TO16; ghostUpper4Offset = 8; break; + + case GHST_UL_RC_CHANS_HS4_13TO16: + case GHST_UL_RC_CHANS_HS4_12_13TO16: + lastGhostFrameId = raw12bits ? GHST_UL_RC_CHANS_HS4_12_5TO8 : GHST_UL_RC_CHANS_HS4_5TO8; + ghostUpper4Offset = 0; + break; + + default: // We don't have known previous state so init + lastGhostFrameId = raw12bits ? GHST_UL_RC_CHANS_HS4_12_5TO8 : GHST_UL_RC_CHANS_HS4_5TO8; + ghostUpper4Offset = 0; + break; } uint8_t * buf = frame; -#if SPORT_MAX_BAUDRATE < 400000 - *buf++ = g_eeGeneral.telemetryBaudrate == GHST_TELEMETRY_RATE_400K ? GHST_ADDR_MODULE_SYM : GHST_ADDR_MODULE_ASYM; -#else - *buf++ = GHST_ADDR_MODULE_SYM; -#endif - *buf++ = GHST_UL_RC_CHANS_SIZE; + *buf++ = getGhostModuleAddr(); // addr + *buf++ = GHST_UL_RC_CHANS_SIZE; // len uint8_t * crc_start = buf; - *buf++ = lastGhostFrameId; + *buf++ = lastGhostFrameId; // type + // payload // first 4 high speed, 12 bit channels (11 relevant bits with openTx) uint32_t bits = 0; uint8_t bitsavailable = 0; for (int i = 0; i < 4; i++) { - uint32_t value = limit(0, GHST_RC_CTR_VAL_12BIT + (((pulses[i] + 2 * PPM_CH_CENTER(i) - 2 * PPM_CENTER) << 3) / 5), 2 * GHST_RC_CTR_VAL_12BIT); + uint32_t value; + if(raw12bits) { + value = limit(0, (1024 + (pulses[i] + 2 * PPM_CH_CENTER(i) - 2 * PPM_CENTER)) << 1, 0xFFF); + } else { + value = limit(0, GHST_RC_CTR_VAL_12BIT + (((pulses[i] + 2 * PPM_CH_CENTER(i) - 2 * PPM_CENTER) << 3) / 5), 2 * GHST_RC_CTR_VAL_12BIT); + } bits |= value << bitsavailable; bitsavailable += GHST_CH_BITS_12; while (bitsavailable >= 8) { @@ -89,23 +103,18 @@ uint8_t createGhostChannelsFrame(uint8_t * frame, int16_t * pulses) // second 4 lower speed, 8 bit channels for (int i = 4; i < 8; ++i) { uint8_t channelIndex = i + ghostUpper4Offset; - *buf++ = limit(0, GHST_RC_CTR_VAL_8BIT + (((pulses[channelIndex] + 2 * PPM_CH_CENTER(channelIndex) - 2 * PPM_CENTER) >> 1) / 5), 2 * GHST_RC_CTR_VAL_8BIT); + uint8_t value; + if(raw12bits) { + value = limit(0, 128 + ((pulses[channelIndex] + 2 * PPM_CH_CENTER(channelIndex) - 2 * PPM_CENTER) >> 3), 0xFF); + } else { + value = limit(0, GHST_RC_CTR_VAL_8BIT + (((pulses[channelIndex] + 2 * PPM_CH_CENTER(channelIndex) - 2 * PPM_CENTER) >> 1) / 5), 2 * GHST_RC_CTR_VAL_8BIT); + } + *buf++ = value; } + // crc *buf++ = crc8(crc_start, GHST_UL_RC_CHANS_SIZE - 1); - switch (lastGhostFrameId) { - case GHST_UL_RC_CHANS_HS4_5TO8: - lastGhostFrameId = GHST_UL_RC_CHANS_HS4_9TO12; - break; - case GHST_UL_RC_CHANS_HS4_9TO12: - lastGhostFrameId = GHST_UL_RC_CHANS_HS4_13TO16; - break; - case GHST_UL_RC_CHANS_HS4_13TO16: - lastGhostFrameId = GHST_UL_RC_CHANS_HS4_5TO8; - break; - } - return buf - frame; } @@ -113,11 +122,12 @@ void setupPulsesGhost() { if (telemetryProtocol == PROTOCOL_TELEMETRY_GHOST) { uint8_t * pulses = extmodulePulsesData.ghost.pulses; - if (moduleState[EXTERNAL_MODULE].counter == GHST_MENU_CONTROL) - extmodulePulsesData.ghost.length = createGhostMenuControlFrame(pulses, &channelOutputs[g_model.moduleData[EXTERNAL_MODULE].channelsStart]); - else - extmodulePulsesData.ghost.length = createGhostChannelsFrame(pulses, &channelOutputs[g_model.moduleData[EXTERNAL_MODULE].channelsStart]); - + auto & module = g_model.moduleData[EXTERNAL_MODULE]; + if (moduleState[EXTERNAL_MODULE].counter == GHST_MENU_CONTROL) { + extmodulePulsesData.ghost.length = createGhostMenuControlFrame(pulses, &channelOutputs[module.channelsStart]); + } else { + extmodulePulsesData.ghost.length = createGhostChannelsFrame(pulses, &channelOutputs[module.channelsStart], module.ghost.raw12bits); + } moduleState[EXTERNAL_MODULE].counter = GHST_FRAME_CHANNEL; } -} \ No newline at end of file +} diff --git a/radio/src/telemetry/ghost.cpp b/radio/src/telemetry/ghost.cpp index 835316207..4734c2758 100755 --- a/radio/src/telemetry/ghost.cpp +++ b/radio/src/telemetry/ghost.cpp @@ -88,6 +88,14 @@ const GhostSensor ghostSensors[] = { {0x00, NULL, UNIT_RAW, 0}, }; +uint8_t getGhostModuleAddr() { +#if SPORT_MAX_BAUDRATE < 400000 + return g_eeGeneral.telemetryBaudrate == GHST_TELEMETRY_RATE_400K ? GHST_ADDR_MODULE_SYM : GHST_ADDR_MODULE_ASYM; +#else + return GHST_ADDR_MODULE_SYM; +#endif +} + const GhostSensor *getGhostSensor(uint8_t id) { for (const GhostSensor * sensor = ghostSensors; sensor->id; sensor++) { diff --git a/radio/src/telemetry/ghost.h b/radio/src/telemetry/ghost.h index 8c5f52f2d..46fb1f16c 100755 --- a/radio/src/telemetry/ghost.h +++ b/radio/src/telemetry/ghost.h @@ -30,17 +30,21 @@ #define GHST_ADDR_MODULE_ASYM 0x88 // asymmetrical, 400k pulses, 115k telemetry #define GHST_ADDR_FC 0x82 #define GHST_ADDR_GOGGLES 0x83 // phase 2 -#define GHST_ADDR_5G_TXCTRL 0x84 // phase 3 +#define GHST_ADDR_5G_TXCTRL 0x84 // phase 3 #define GHST_ADDR_5G_TWRSCAN 0x85 #define GHST_ADDR_5G_RLY 0x86 -#define GHST_UL_RC_CHANS_HS4_5TO8 0x10 // High Speed 4 channel (12 bits), plus CH5-8 (8 bits) -#define GHST_UL_RC_CHANS_HS4_9TO12 0x11 // High Speed 4 channel (12 bits), plus CH9-12 (8 bits) -#define GHST_UL_RC_CHANS_HS4_13TO16 0x12 // High Speed 4 channel (12 bits), plus CH13-16 (8 bits) -#define GHST_UL_RC_CHANS_SIZE 12 // 1 (type) + 10 (data) + 1 (crc) +#define GHST_UL_RC_CHANS_HS4_5TO8 0x10 // High Speed 4 channel (12 bit legacy), plus CH5-8 (8 bits) +#define GHST_UL_RC_CHANS_HS4_9TO12 0x11 // High Speed 4 channel (12 bit legacy), plus CH9-12 (8 bits) +#define GHST_UL_RC_CHANS_HS4_13TO16 0x12 // High Speed 4 channel (12 bit legacy), plus CH13-16 (8 bits) +#define GHST_UL_RC_CHANS_SIZE 12 // 1 (type) + 10 (data) + 1 (crc) #define GHST_UL_MENU_CTRL 0x13 -#define GHST_DL_OPENTX_SYNC 0x20 +#define GHST_UL_RC_CHANS_HS4_12_5TO8 0x30 // High Speed 4 channel (12 bit raw), plus CH5-8 (8 bit raw) +#define GHST_UL_RC_CHANS_HS4_12_9TO12 0x31 // High Speed 4 channel (12 bit raw), plus CH9-12 (8 bit raw) +#define GHST_UL_RC_CHANS_HS4_12_13TO16 0x32 // High Speed 4 channel (12 bit raw), plus CH13-16 (8 bit raw) + +#define GHST_DL_OPENTX_SYNC 0x20 #define GHST_DL_LINK_STAT 0x21 #define GHST_DL_VTX_STAT 0x22 #define GHST_DL_PACK_STAT 0x23 @@ -48,8 +52,8 @@ #define GHST_DL_GPS_PRIMARY 0x25 #define GHST_DL_GPS_SECONDARY 0x26 -#define GHST_RC_CTR_VAL_12BIT 0x7C0 // 0x3e0 << 1 -#define GHST_RC_CTR_VAL_8BIT 0x7C +#define GHST_RC_CTR_VAL_12BIT 0x7C0 // 0x3e0 << 1 +#define GHST_RC_CTR_VAL_8BIT 0x7C #define GHST_CH_BITS_12 12 #define GHST_CH_BITS_8 8 @@ -101,6 +105,7 @@ enum GhstVtxBand void processGhostTelemetryData(uint8_t data); void ghostSetDefault(int index, uint8_t id, uint8_t subId); +uint8_t getGhostModuleAddr(); #if SPORT_MAX_BAUDRATE < 400000 // For radios which can't support telemetry at high rates, offer baud rate choices