From 3573a4bcb21784348b1d902817f0bd1f5d8da9c8 Mon Sep 17 00:00:00 2001 From: Wez <59873510+wvarty@users.noreply.github.com> Date: Mon, 30 Sep 2024 07:22:41 +1000 Subject: [PATCH] Arm DJI O3 directly from an ELRS receiver (#2926) * Initial commit - WIP - more to do * Allow on serial1 * Cleanup * Remove unneeded msp msgs * Use device duration now that it works --------- Co-authored-by: Paul Kendall --- src/html/index.html | 2 ++ src/include/common.h | 4 ++- src/lib/LUA/rx_devLUA.cpp | 4 +-- src/src/rx-serial/SerialDisplayport.cpp | 60 +++++++++++++++++++++++++++++++++ src/src/rx-serial/SerialDisplayport.h | 55 ++++++++++++++++++++++++++++++ src/src/rx_main.cpp | 31 +++++++++++------ 6 files changed, 143 insertions(+), 13 deletions(-) create mode 100644 src/src/rx-serial/SerialDisplayport.cpp create mode 100644 src/src/rx-serial/SerialDisplayport.h diff --git a/src/html/index.html b/src/html/index.html index e72b3bce..e8a134e8 100644 --- a/src/html/index.html +++ b/src/html/index.html @@ -258,6 +258,7 @@ + @@ -275,6 +276,7 @@ + diff --git a/src/include/common.h b/src/include/common.h index 5ad76e87..4313d710 100644 --- a/src/include/common.h +++ b/src/include/common.h @@ -222,7 +222,8 @@ enum eSerialProtocol : uint8_t PROTOCOL_SUMD, PROTOCOL_DJI_RS_PRO, PROTOCOL_HOTT_TLM, - PROTOCOL_MAVLINK + PROTOCOL_MAVLINK, + PROTOCOL_MSP_DISPLAYPORT, }; #if defined(PLATFORM_ESP32) @@ -238,6 +239,7 @@ enum eSerial1Protocol : uint8_t PROTOCOL_SERIAL1_HOTT_TLM, PROTOCOL_SERIAL1_TRAMP, PROTOCOL_SERIAL1_SMARTAUDIO, + PROTOCOL_SERIAL1_MSP_DISPLAYPORT, }; #endif diff --git a/src/lib/LUA/rx_devLUA.cpp b/src/lib/LUA/rx_devLUA.cpp index 945ef2b2..43d1fcec 100644 --- a/src/lib/LUA/rx_devLUA.cpp +++ b/src/lib/LUA/rx_devLUA.cpp @@ -19,7 +19,7 @@ static char pwmModes[] = "50Hz;60Hz;100Hz;160Hz;333Hz;400Hz;10kHzDuty;On/Off;DSh static struct luaItem_selection luaSerialProtocol = { {"Protocol", CRSF_TEXT_SELECTION}, 0, // value - "CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;MAVLINK", + "CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;MAVLINK;DisplayPort", STR_EMPTYSPACE }; @@ -27,7 +27,7 @@ static struct luaItem_selection luaSerialProtocol = { static struct luaItem_selection luaSerial1Protocol = { {"Protocol2", CRSF_TEXT_SELECTION}, 0, // value - "Off;CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;Tramp;SmartAudio", + "Off;CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;Tramp;SmartAudio;DisplayPort", STR_EMPTYSPACE }; #endif diff --git a/src/src/rx-serial/SerialDisplayport.cpp b/src/src/rx-serial/SerialDisplayport.cpp new file mode 100644 index 00000000..0e724350 --- /dev/null +++ b/src/src/rx-serial/SerialDisplayport.cpp @@ -0,0 +1,60 @@ +/************************************************************************************ +Credits: + This software is based on and uses software published by Richard Amiss 2023, + QLiteOSD, which is based on work by Paul Kurucz (pkuruz):opentelem_to_bst_bridge + as well as software from d3ngit : djihdfpv_mavlink_to_msp_V2 and + crashsalot : VOT_to_DJIFPV + +THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES, WHETHER EXPRESS, +IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE +COMPANY SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER. +************************************************************************************/ + +#include "SerialDisplayport.h" +#include "crsf_protocol.h" + +void SerialDisplayport::send(uint8_t messageID, void * payload, uint8_t size, Stream * _stream) +{ + _stream->write('$'); + _stream->write('M'); + _stream->write('<'); + _stream->write(size); + _stream->write(messageID); + uint8_t checksum = size ^ messageID; + uint8_t * payloadPtr = (uint8_t*)payload; + for (uint8_t i = 0; i < size; ++i) + { + uint8_t b = *(payloadPtr++); + checksum ^= b; + } + _stream->write((uint8_t*)payload, size); + _stream->write(checksum); +} + +uint32_t SerialDisplayport::sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) +{ + // Use ch5 to check armed state + bool armed = channelData[4] > CRSF_CHANNEL_VALUE_MID; + + // Send extended status MSP + msp_status_DJI_t status_DJI; + status_DJI.cycleTime = 0x0080; + status_DJI.i2cErrorCounter = 0; + status_DJI.sensor = 0x23; + status_DJI.flightModeFlags = armed ? 0x3 : 0x2; + status_DJI.configProfileIndex = 0; + status_DJI.averageSystemLoadPercent = 7; + status_DJI.accCalibrationAxisFlags = 0; + status_DJI.DJI_ARMING_DISABLE_FLAGS_COUNT = 20; + status_DJI.djiPackArmingDisabledFlags = (1 << 24); + status_DJI.armingFlags = 0x0303; + send(MSP_STATUS_EX, &status_DJI, sizeof(status_DJI), _outputPort); + + // Send status MSP + status_DJI.armingFlags = 0x0000; + send(MSP_STATUS, &status_DJI, sizeof(status_DJI), _outputPort); + + return MSP_MSG_PERIOD_MS; // Send MSP msgs to DJI at 10Hz +} \ No newline at end of file diff --git a/src/src/rx-serial/SerialDisplayport.h b/src/src/rx-serial/SerialDisplayport.h new file mode 100644 index 00000000..8dce00c9 --- /dev/null +++ b/src/src/rx-serial/SerialDisplayport.h @@ -0,0 +1,55 @@ +/************************************************************************************ +Credits: + This software is based on and uses software published by Richard Amiss 2023, + QLiteOSD, which is based on work by Paul Kurucz (pkuruz):opentelem_to_bst_bridge + as well as software from d3ngit : djihdfpv_mavlink_to_msp_V2 and + crashsalot : VOT_to_DJIFPV + +THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES, WHETHER EXPRESS, +IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE +COMPANY SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER. +************************************************************************************/ + +#pragma once +#include "SerialIO.h" + +#define MSP_STATUS 101 +#define MSP_STATUS_EX 150 +#define MSP_MSG_PERIOD_MS 100 + +// MSP_STATUS_DJI +struct msp_status_DJI_t +{ + uint16_t cycleTime; + uint16_t i2cErrorCounter; + uint16_t sensor; // MSP_STATUS_SENSOR_... + uint32_t flightModeFlags; // see getActiveModes() + uint8_t configProfileIndex; + uint16_t averageSystemLoadPercent; // 0...100 + uint16_t armingFlags; //0x0103 or 0x0301 + uint8_t accCalibrationAxisFlags; //0 + uint8_t DJI_ARMING_DISABLE_FLAGS_COUNT; //25 + uint32_t djiPackArmingDisabledFlags; //(1 << 24) +} __attribute__ ((packed)); + +//////////////////////////// + +class SerialDisplayport : public SerialIO +{ +public: + explicit SerialDisplayport(Stream &out, Stream &in) : SerialIO(&out, &in), m_lastSentMSP(0) {} + virtual ~SerialDisplayport() {} + + void queueLinkStatisticsPacket() override {} + void queueMSPFrameTransmission(uint8_t* data) override {}; + void sendQueuedData(uint32_t maxBytesToSend) override {}; + uint32_t sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) override; + +private: + void processBytes(uint8_t *bytes, uint16_t size) override {}; + void send(uint8_t messageID, void * payload, uint8_t size, Stream * _stream); + + uint32_t m_lastSentMSP; +}; diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp index b89314f1..94afb638 100644 --- a/src/src/rx_main.cpp +++ b/src/src/rx_main.cpp @@ -26,6 +26,7 @@ #include "rx-serial/SerialMavlink.h" #include "rx-serial/SerialTramp.h" #include "rx-serial/SerialSmartAudio.h" +#include "rx-serial/SerialDisplayport.h" #include "rx-serial/devSerialIO.h" #include "devLED.h" @@ -1338,6 +1339,10 @@ static void setupSerial() mavlinkSerialOutput = true; serialBaud = 460800; } + else if (config.GetSerialProtocol() == PROTOCOL_MSP_DISPLAYPORT) + { + serialBaud = 115200; + } #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32) else if (config.GetSerialProtocol() == PROTOCOL_HOTT_TLM) { @@ -1348,29 +1353,29 @@ static void setupSerial() bool invert = config.GetSerialProtocol() == PROTOCOL_SBUS || config.GetSerialProtocol() == PROTOCOL_INVERTED_CRSF || config.GetSerialProtocol() == PROTOCOL_DJI_RS_PRO; #if defined(PLATFORM_ESP8266) - SerialConfig config = SERIAL_8N1; + SerialConfig serialConfig = SERIAL_8N1; if(sbusSerialOutput) { - config = SERIAL_8E2; + serialConfig = SERIAL_8E2; } else if(hottTlmSerial) { - config = SERIAL_8N2; + serialConfig = SERIAL_8N2; } SerialMode mode = (sbusSerialOutput || sumdSerialOutput) ? SERIAL_TX_ONLY : SERIAL_FULL; - Serial.begin(serialBaud, config, mode, -1, invert); + Serial.begin(serialBaud, serialConfig, mode, -1, invert); #elif defined(PLATFORM_ESP32) - uint32_t config = SERIAL_8N1; + uint32_t serialConfig = SERIAL_8N1; if(sbusSerialOutput) { - config = SERIAL_8E2; + serialConfig = SERIAL_8E2; } else if(hottTlmSerial) { - config = SERIAL_8N2; + serialConfig = SERIAL_8N2; } // ARDUINO_CORE_INVERT_FIX PT2 @@ -1382,7 +1387,7 @@ static void setupSerial() #endif // ARDUINO_CORE_INVERT_FIX PT2 end - Serial.begin(serialBaud, config, GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX, invert); + Serial.begin(serialBaud, serialConfig, GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX, invert); #endif if (firmwareOptions.is_airport) @@ -1401,12 +1406,14 @@ static void setupSerial() { serialIO = new SerialMavlink(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX); } -#if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32) + else if (config.GetSerialProtocol() == PROTOCOL_MSP_DISPLAYPORT) + { + serialIO = new SerialDisplayport(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX); + } else if (hottTlmSerial) { serialIO = new SerialHoTT_TLM(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX); } -#endif else { serialIO = new SerialCRSF(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX); @@ -1499,6 +1506,10 @@ static void setupSerial1() Serial1.begin(4800, SERIAL_8N2, UNDEF_PIN, serial1TXpin, false); serial1IO = new SerialSmartAudio(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX, serial1TXpin); break; + case PROTOCOL_SERIAL1_MSP_DISPLAYPORT: + Serial1.begin(115200, SERIAL_8N1, UNDEF_PIN, serial1TXpin, false); + serial1IO = new SerialDisplayport(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX); + break; } } -- 2.11.4.GIT