diff --git a/radio/src/pulses/multi.cpp b/radio/src/pulses/multi.cpp index 42d57b231..ec901abf1 100644 --- a/radio/src/pulses/multi.cpp +++ b/radio/src/pulses/multi.cpp @@ -42,6 +42,7 @@ static void sendD16BindOption(uint8_t moduleIdx); #if defined(LUA) static void sendSport(uint8_t moduleIdx); static void sendHott(uint8_t moduleIdx); +static void sendDSM(uint8_t moduleIdx); #endif void multiPatchCustom(uint8_t moduleIdx) @@ -194,6 +195,9 @@ void setupPulsesMulti(uint8_t moduleIdx) else if (IS_HOTT_MULTI(moduleIdx)) { sendHott(moduleIdx); //1 byte of additional data } + else if (IS_DSM_MULTI(moduleIdx)) { + sendDSM(moduleIdx); //7 bytes of additional data + } #endif } } @@ -401,4 +405,18 @@ void sendHott(uint8_t moduleIdx) sendMulti(moduleIdx, Multi_Buffer[5]); } } + +void sendDSM(uint8_t moduleIdx) +{ + // Multi_Buffer[0..2]=="DSM" -> Lua script is running + // Multi_Buffer[3]==0x70 + len -> TX to RX data ready to be sent + // Multi_Buffer[4..9]=6 bytes of TX to RX data + // Multi_Buffer[10..25]=16 bytes of RX to TX data + if (Multi_Buffer && memcmp(Multi_Buffer, "DSM", 3) == 0 && (Multi_Buffer[3] & 0xF8) == 0x70) { + for(uint8_t i = 0; i < 7; i++) { + sendMulti(moduleIdx, Multi_Buffer[3+i]); + } + Multi_Buffer[3] = 0x00; // Data sent + } +} #endif diff --git a/radio/src/telemetry/spektrum.cpp b/radio/src/telemetry/spektrum.cpp index aab694db0..1bd1829d5 100644 --- a/radio/src/telemetry/spektrum.cpp +++ b/radio/src/telemetry/spektrum.cpp @@ -47,6 +47,7 @@ #define DSM_BIND_PACKET_LENGTH 12 #define I2C_HIGH_CURRENT 0x03 +#define I2C_FWD_PGM 0x09 #define I2C_TEXTGEN 0x0c #define I2C_GPS 0x17 #define I2C_GPS2 0x17 @@ -349,6 +350,20 @@ void processSpektrumPacket(const uint8_t *packet) // highest bit indicates that TM1100 is in use, ignore it uint8_t i2cAddress = (packet[2] & 0x7f); + if (i2cAddress == I2C_FWD_PGM) { +#if defined(LUA) + // Forward Programming + if (Multi_Buffer && memcmp(Multi_Buffer, "DSM", 3) == 0) { + // Multi_Buffer[0..2]=="DSM" -> Lua script is running + // Multi_Buffer[3]==0x70 -> TX to RX data ready to be sent + // Multi_Buffer[4..9]=6 bytes of TX to RX data + // Multi_Buffer[10..25]=16 bytes of RX to TX data + Multi_Buffer[10] = i2cAddress; + memcpy(&Multi_Buffer[11], &packet[3], 15); // Store the received RX answer in the buffer + } +#endif + return; // Not a sensor + } //SmartBat Hack if (i2cAddress == I2C_SMART_BAT_BASE_ADDRESS) { i2cAddress = i2cAddress + (packet[4] >> 4); // use type to create virtual I2CAddresses diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index b03412355..5680a837f 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -115,6 +115,7 @@ extern uint8_t telemetryProtocol; || (g_model.moduleData[module].getMultiProtocol() == MODULE_SUBTYPE_MULTI_FRSKYX2)) #define IS_R9_MULTI(module) (g_model.moduleData[module].getMultiProtocol() == MODULE_SUBTYPE_MULTI_FRSKY_R9) #define IS_HOTT_MULTI(module) (g_model.moduleData[module].getMultiProtocol() == MODULE_SUBTYPE_MULTI_HOTT) + #define IS_DSM_MULTI(module) (g_model.moduleData[module].getMultiProtocol() == MODULE_SUBTYPE_MULTI_DSM2) #define IS_RX_MULTI(module) ((g_model.moduleData[module].getMultiProtocol() == MODULE_SUBTYPE_MULTI_AFHDS2A_RX) || (g_model.moduleData[module].getMultiProtocol() == MODULE_SUBTYPE_MULTI_FRSKYX_RX) \ || (g_model.moduleData[module].getMultiProtocol() == MODULE_SUBTYPE_MULTI_BAYANG_RX) || (g_model.moduleData[module].getMultiProtocol() == MODULE_SUBTYPE_MULTI_DSM_RX)) #if defined(HARDWARE_INTERNAL_MODULE) @@ -126,6 +127,7 @@ extern uint8_t telemetryProtocol; #define IS_D16_MULTI(module) false #define IS_R9_MULTI(module) false #define IS_HOTT_MULTI(module) false + #define IS_DSM_MULTI(module) false #define IS_FRSKY_SPORT_PROTOCOL() (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_SPORT) #define IS_RX_MULTI(module) false #endif