From bbca0bf12eadffaca80558554ae380a27878b5ef Mon Sep 17 00:00:00 2001
From: sumolx <sumolx@gmail.com>
Date: Thu, 30 May 2024 20:27:07 -0400
Subject: [PATCH 1/7] New MSP_VTX_INFO message type added which is broadcasted
 at 1Hz

---
 src/dm6300.c          |  2 +-
 src/hardware.h        |  1 +
 src/msp_displayport.c | 49 +++++++++++++++++++++++++++++++++++++++++++
 src/msp_displayport.h |  2 ++
 4 files changed, 53 insertions(+), 1 deletion(-)

diff --git a/src/dm6300.c b/src/dm6300.c
index d684e76..799aced 100644
--- a/src/dm6300.c
+++ b/src/dm6300.c
@@ -913,7 +913,7 @@ uint8_t DM6300_detect(void) {
         debugf("\r\ndm6300 alive");
     }
 #endif
-    return rdat != 0x18;
+    return (rdat != 0x18) ? 1 : 0;
 }
 
 void DM6300_Init(uint8_t ch, BWType_e bw) {
diff --git a/src/hardware.h b/src/hardware.h
index d1840f1..aa81e69 100644
--- a/src/hardware.h
+++ b/src/hardware.h
@@ -119,6 +119,7 @@ extern uint8_t RF_BW_last;
 extern uint8_t BAUDRATE;
 extern uint8_t SHORTCUT;
 
+extern uint8_t cameraLost;
 extern uint8_t pwr_offset;
 extern uint8_t heat_protect;
 
diff --git a/src/msp_displayport.c b/src/msp_displayport.c
index e27f3fc..4454f7e 100644
--- a/src/msp_displayport.c
+++ b/src/msp_displayport.c
@@ -26,6 +26,7 @@ uint8_t fc_lock = 0;
 uint8_t disp_mode; // DISPLAY_OSD | DISPLAY_CMS;
 uint8_t osd_ready;
 
+uint8_t vtx_variant[4] = {'H', 'D', 'Z', '0'};
 uint8_t fc_variant[4] = {0xff, 0xff, 0xff, 0xff};
 uint8_t fontType = 0x00;
 uint8_t resolution = SD_3016;
@@ -149,6 +150,7 @@ uint8_t hdzero_dynamic_osd_refresh_adapter(uint8_t t1) {
 
 void msp_task() {
     uint8_t len;
+    static uint16_t last_sec = 0;
     static uint8_t t1 = 0;
     static uint8_t vmax = SD_VMAX;
 
@@ -209,6 +211,12 @@ void msp_task() {
         }
     }
 
+    // send vtx info at 1HZ
+    if (last_sec != seconds) {
+        last_sec = seconds;
+        msp_set_vtx_info();
+    }
+
     // set_vtx
     set_vtx_param();
 }
@@ -759,6 +767,47 @@ void msp_eeprom_write() {
     msp_tx(250);
     msp_tx(250);
 }
+
+void msp_set_vtx_info() {
+    uint8_t crc = 0;
+    uint8_t len = 13;
+    uint8_t temp = temperature >> 2;
+    uint8_t faults = cameraLost & 1 | (dm6300_lost << 1) | (heat_protect << 2);
+
+    msp_send_header(0);
+    msp_tx(len);
+    crc ^= len;
+    msp_tx(MSP_CMD_VTX_INFO);
+    crc ^= MSP_CMD_VTX_INFO;
+    msp_tx(vtx_variant[0]);
+    crc ^= vtx_variant[0];
+    msp_tx(vtx_variant[1]);
+    crc ^= vtx_variant[1];
+    msp_tx(vtx_variant[2]);
+    crc ^= vtx_variant[2];
+    msp_tx(vtx_variant[3]);
+    crc ^= vtx_variant[3];
+    msp_tx(VTX_VERSION_MAJOR);
+    crc ^= VTX_VERSION_MAJOR;
+    msp_tx(VTX_VERSION_MINOR);
+    crc ^= VTX_VERSION_MINOR;
+    msp_tx(VTX_VERSION_PATCH_LEVEL);
+    crc ^= VTX_VERSION_PATCH_LEVEL;
+    msp_tx(fc_variant[0]);
+    crc ^= fc_variant[0];
+    msp_tx(fc_variant[1]);
+    crc ^= fc_variant[1];
+    msp_tx(fc_variant[2]);
+    crc ^= fc_variant[2];
+    msp_tx(fc_variant[3]);
+    crc ^= fc_variant[3];
+    msp_tx(temp);
+    crc ^= temp;
+    msp_tx(faults);
+    crc ^= faults;
+    msp_tx(crc);
+}
+
 void msp_set_vtx_config(uint8_t power, uint8_t save) {
     uint8_t crc = 0;
     uint8_t channel = RF_FREQ;
diff --git a/src/msp_displayport.h b/src/msp_displayport.h
index b3cd8e8..632e33a 100644
--- a/src/msp_displayport.h
+++ b/src/msp_displayport.h
@@ -29,6 +29,7 @@
 #define MSP_CMD_DISPLAYPORT_BYTE    0xB6 // 182 // in message
 #define MSP_CMD_SET_OSD_CANVAS_BYTE 0xBC // 188 // in message
 #define MSP_CMD_GET_OSD_CANVAS_BYTE 0xBD // 188 //out message
+#define MSP_CMD_VTX_INFO            0xF8 // 248 //out message
 
 #define DP_HEADER0 0x56
 #define DP_HEADER1 0x80
@@ -159,6 +160,7 @@ uint8_t get_tx_data_osd(uint8_t index);
 void insert_tx_buf(uint8_t len);
 void DP_tx_task();
 void msp_cmd_tx();
+void msp_set_vtx_info();
 void parse_status();
 void parse_rc();
 void parse_variant();

From 1d3043264a45a05885ee14c9d10a1d921c2ac613 Mon Sep 17 00:00:00 2001
From: sumolx <sumolx@gmail.com>
Date: Fri, 31 May 2024 14:05:44 -0400
Subject: [PATCH 2/7] Replaced vtx_variant with VTX_NAME since its already used
 throughout the codebase

---
 src/msp_displayport.c | 25 ++++++++++++-------------
 1 file changed, 12 insertions(+), 13 deletions(-)

diff --git a/src/msp_displayport.c b/src/msp_displayport.c
index 4454f7e..080d086 100644
--- a/src/msp_displayport.c
+++ b/src/msp_displayport.c
@@ -26,7 +26,6 @@ uint8_t fc_lock = 0;
 uint8_t disp_mode; // DISPLAY_OSD | DISPLAY_CMS;
 uint8_t osd_ready;
 
-uint8_t vtx_variant[4] = {'H', 'D', 'Z', '0'};
 uint8_t fc_variant[4] = {0xff, 0xff, 0xff, 0xff};
 uint8_t fontType = 0x00;
 uint8_t resolution = SD_3016;
@@ -414,7 +413,7 @@ uint8_t msp_read_one_frame() {
             state = MSP_HEADER_START;
             break;
         } // switch(state)
-    }     // i
+    } // i
     return ret;
 }
 
@@ -769,24 +768,24 @@ void msp_eeprom_write() {
 }
 
 void msp_set_vtx_info() {
+    uint32_t i = 0;
     uint8_t crc = 0;
-    uint8_t len = 13;
     uint8_t temp = temperature >> 2;
     uint8_t faults = cameraLost & 1 | (dm6300_lost << 1) | (heat_protect << 2);
+    uint8_t vtx_name_len = strlen(VTX_NAME);
+    uint8_t payload_size = 10 + vtx_name_len;
 
     msp_send_header(0);
-    msp_tx(len);
-    crc ^= len;
+    msp_tx(payload_size);
+    crc ^= payload_size;
     msp_tx(MSP_CMD_VTX_INFO);
     crc ^= MSP_CMD_VTX_INFO;
-    msp_tx(vtx_variant[0]);
-    crc ^= vtx_variant[0];
-    msp_tx(vtx_variant[1]);
-    crc ^= vtx_variant[1];
-    msp_tx(vtx_variant[2]);
-    crc ^= vtx_variant[2];
-    msp_tx(vtx_variant[3]);
-    crc ^= vtx_variant[3];
+    msp_tx(vtx_name_len);
+    crc ^= vtx_name_len;
+    for (i = 0; i < vtx_name_len; ++i) {
+        msp_tx(VTX_NAME[i]);
+        crc ^= VTX_NAME[i];
+    }
     msp_tx(VTX_VERSION_MAJOR);
     crc ^= VTX_VERSION_MAJOR;
     msp_tx(VTX_VERSION_MINOR);

From 3038a9e2bd1dfd4055feea4fc780f9897c1ea9dc Mon Sep 17 00:00:00 2001
From: sumolx <sumolx@gmail.com>
Date: Wed, 5 Jun 2024 12:01:37 -0400
Subject: [PATCH 3/7] Refactored to utilize query/response based communications
 over MSP v2 Protocol

---
 .vscode/settings.json |   3 +
 src/msp_displayport.c | 254 +++++++++++++++++++++++++++---------------
 src/msp_displayport.h |  41 ++-----
 src/msp_protocol.h    |  74 ++++++++++++
 4 files changed, 251 insertions(+), 121 deletions(-)
 create mode 100644 src/msp_protocol.h

diff --git a/.vscode/settings.json b/.vscode/settings.json
index ebfde9a..85b5f18 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,3 +1,6 @@
 {
     "editor.formatOnSave": true,
+    "files.associations": {
+        "msp_protocol.h": "c"
+    },
 }
\ No newline at end of file
diff --git a/src/msp_displayport.c b/src/msp_displayport.c
index 080d086..f231f8b 100644
--- a/src/msp_displayport.c
+++ b/src/msp_displayport.c
@@ -13,9 +13,9 @@
 #include "uart.h"
 #include "version.h"
 
-uint8_t osd_buf[HD_VMAX1][HD_HMAX1];
-uint8_t loc_buf[HD_VMAX1][7];
-uint8_t page_extend_buf[HD_VMAX1][7];
+uint8_t osd_buf[OSD_CANVAS_HD_VMAX1][OSD_CANVAS_HD_HMAX1];
+uint8_t loc_buf[OSD_CANVAS_HD_VMAX1][7];
+uint8_t page_extend_buf[OSD_CANVAS_HD_VMAX1][7];
 uint8_t tx_buf[TXBUF_SIZE]; // buffer for sending data to VRX
 uint8_t dptxbuf[256];
 uint8_t dptx_rptr, dptx_wptr;
@@ -116,8 +116,8 @@ uint8_t msp_cmp_fc_variant(const char *variant) {
 }
 
 uint8_t hdzero_dynamic_osd_refresh_adapter(uint8_t t1) {
-    static uint16_t osd_line_crc_lst[HD_VMAX1] = {0};
-    static uint8_t refresh_cnt[HD_VMAX1] = {0};
+    static uint16_t osd_line_crc_lst[OSD_CANVAS_HD_VMAX1] = {0};
+    static uint8_t refresh_cnt[OSD_CANVAS_HD_VMAX1] = {0};
     uint8_t crc_u8[2] = {0, 0};
     uint16_t crc_u16 = 0;
     uint8_t i;
@@ -151,7 +151,7 @@ void msp_task() {
     uint8_t len;
     static uint16_t last_sec = 0;
     static uint8_t t1 = 0;
-    static uint8_t vmax = SD_VMAX;
+    static uint8_t vmax = OSD_CANVAS_SD_VMAX;
 
 #ifdef _DEBUG_DISPLAYPORT
     if (RS0_ERR) {
@@ -164,11 +164,11 @@ void msp_task() {
     // decide by osd_frame size/rate and dptx rate
     if (msp_read_one_frame()) {
         if (resolution == HD_5018) {
-            vmax = HD_VMAX0;
+            vmax = OSD_CANVAS_HD_VMAX0;
         } else if (resolution == HD_5320) {
-            vmax = HD_VMAX1;
+            vmax = OSD_CANVAS_HD_VMAX1;
         } else {
-            vmax = SD_VMAX;
+            vmax = OSD_CANVAS_SD_VMAX;
         }
     }
 
@@ -210,12 +210,6 @@ void msp_task() {
         }
     }
 
-    // send vtx info at 1HZ
-    if (last_sec != seconds) {
-        last_sec = seconds;
-        msp_set_vtx_info();
-    }
-
     // set_vtx
     set_vtx_param();
 }
@@ -243,7 +237,7 @@ uint8_t msp_read_one_frame() {
 
         switch (state) {
         case MSP_HEADER_START:
-            if (rx == MSP_HEADER_START_BYTE) {
+            if (rx == MSP_HEADER_FRAMER) {
                 ptr = 0;
                 state = MSP_HEADER_M;
             }
@@ -254,9 +248,9 @@ uint8_t msp_read_one_frame() {
             break;
 
         case MSP_HEADER_M:
-            if (rx == MSP_HEADER_M_BYTE) {
+            if (rx == MSP_HEADER_V1) {
                 state = MSP_PACKAGE_REPLAY1;
-            } else if (rx == MSP_HEADER_M2_BYTE) {
+            } else if (rx == MSP_HEADER_V2) {
                 state = MSP_PACKAGE_REPLAY2;
             } else {
                 state = MSP_HEADER_START;
@@ -264,7 +258,7 @@ uint8_t msp_read_one_frame() {
             break;
 
         case MSP_PACKAGE_REPLAY1: // 0x3E
-            if (rx == MSP_PACKAGE_REPLAY_BYTE) {
+            if (rx == MSP_HEADER_RESPONSE) {
                 state = MSP_LENGTH;
             } else {
                 state = MSP_HEADER_START;
@@ -280,17 +274,17 @@ uint8_t msp_read_one_frame() {
 
         case MSP_CMD:
             crc ^= rx;
-            if (rx == MSP_CMD_DISPLAYPORT_BYTE) {
+            if (rx == MSP_DISPLAYPORT) {
                 cur_cmd = CUR_DISPLAYPORT;
-            } else if (rx == MSP_CMD_RC_BYTE) {
+            } else if (rx == MSP_RC) {
                 cur_cmd = CUR_RC;
-            } else if (rx == MSP_CMD_STATUS_BYTE) {
+            } else if (rx == MSP_STATUS) {
                 cur_cmd = CUR_STATUS;
-            } else if (rx == MSP_CMD_FC_VARIANT_BYTE) {
+            } else if (rx == MSP_FC_VARIANT) {
                 cur_cmd = CUR_FC_VARIANT;
-            } else if (rx == MSP_CMD_VTX_CONFIG_BYTE) {
+            } else if (rx == MSP_GET_VTX_CONFIG) {
                 cur_cmd = CUR_VTX_CONFIG;
-            } else if (rx == MSP_CMD_GET_OSD_CANVAS_BYTE) {
+            } else if (rx == MSP_GET_OSD_CANVAS) {
                 cur_cmd = CUR_GET_OSD_CANVAS;
             } else
                 cur_cmd = CUR_OTHERS;
@@ -349,7 +343,7 @@ uint8_t msp_read_one_frame() {
             break;
 
         case MSP_PACKAGE_REPLAY2: // 0x3E
-            if (rx == MSP_PACKAGE_REPLAY_BYTE) {
+            if (rx == MSP_HEADER_RESPONSE) {
                 state = MSP_ZERO;
             } else {
                 state = MSP_HEADER_START;
@@ -402,7 +396,26 @@ uint8_t msp_read_one_frame() {
         case MSP_CRC2:
             if (crc == rx) {
                 full_frame = 1;
-                parseMspVtx_V2(cmd_u16);
+                switch (cmd_u16) {
+                case MSP_VTX_GET_MODEL_NAME:
+                    msp_send_vtx_model_name();
+                    break;
+                case MSP_VTX_GET_FC_VARIANT:
+                    msp_send_vtx_fc_variant();
+                    break;
+                case MSP_VTX_GET_FW_VERSION:
+                    msp_send_vtx_fw_version();
+                    break;
+                case MSP_VTX_GET_TEMPERATURE:
+                    msp_send_vtx_temperature();
+                    break;
+                case MSP_VTX_GET_HW_FAULTS:
+                    msp_send_vtx_hw_faults();
+                    break;
+                default:
+                    parseMspVtx_V2(cmd_u16);
+                    break;
+                }
                 msp_lst_rcv_sec = seconds;
                 msp_tx_en = 1;
             }
@@ -425,7 +438,7 @@ void clear_screen() {
 
 void write_string(uint8_t rx, uint8_t row, uint8_t col, uint8_t page_extend) {
     if (disp_mode == DISPLAY_OSD) {
-        if (row < HD_VMAX1 && col < HD_HMAX1) {
+        if (row < OSD_CANVAS_HD_VMAX1 && col < OSD_CANVAS_HD_HMAX1) {
             osd_buf[row][col] = rx;
             if (page_extend)
                 page_extend_buf[row][col >> 3] |= (1 << (col & 0x07));
@@ -575,17 +588,17 @@ uint8_t get_tx_data_osd(uint8_t index) // prepare osd+data to VTX
     uint8_t num = 0;
 
     if (resolution == HD_5018) {
-        hmax = HD_HMAX0;
+        hmax = OSD_CANVAS_HD_HMAX0;
         len_mask = 7;
         ptr = 11;
     } else if (resolution == HD_5320) {
-        hmax = HD_HMAX1;
+        hmax = OSD_CANVAS_HD_HMAX1;
         len_mask = 7;
         ptr = 11;
     } else {
         ptr = 12;
         len_mask = 4;
-        hmax = SD_HMAX;
+        hmax = OSD_CANVAS_SD_HMAX;
     }
 
     // string
@@ -731,13 +744,42 @@ void insert_tx_buf(uint8_t len) {
     insert_tx_byte(crc1);
 }
 
-void msp_send_header(uint8_t dl) {
+void msp_send_command(uint8_t dl, uint8_t version) {
+    if (dl) {
+        WAIT(20);
+    }
+    msp_tx(MSP_HEADER_FRAMER);
+    msp_tx(version);
+    msp_tx(MSP_HEADER_COMMAND);
+}
+
+void msp_send_response(uint8_t dl, uint8_t version) {
     if (dl) {
         WAIT(20);
     }
-    msp_tx(0x24);
-    msp_tx(0x4d);
-    msp_tx(0x3c);
+    msp_tx(MSP_HEADER_FRAMER);
+    msp_tx(version);
+    msp_tx(MSP_HEADER_RESPONSE);
+}
+
+uint8_t msp_send_header_v2(uint16_t len, uint16_t msg) {
+    // Flags
+    msp_tx(0);
+    uint8_t crc = crc8tab[0];
+
+    // Length
+    msp_tx(len >> 8);
+    crc = crc8tab[crc ^ (len >> 8)];
+    msp_tx(len & 0xFF);
+    crc = crc8tab[crc ^ (len & 0xFF)];
+
+    // Message Type
+    msp_tx(msg >> 8);
+    crc = crc8tab[crc ^ (msg >> 8)];
+    msp_tx(msg & 0xFF);
+    crc = crc8tab[crc ^ (msg & 0xFF)];
+
+    return crc;
 }
 
 void msp_cmd_tx() // send 3 commands to FC
@@ -745,15 +787,15 @@ void msp_cmd_tx() // send 3 commands to FC
     uint8_t i;
     uint8_t const count = (fc_lock & FC_VTX_CONFIG_LOCK) ? 4 : 5;
     uint8_t const msp_cmd[5] = {
-        MSP_CMD_FC_VARIANT_BYTE,
-        MSP_CMD_STATUS_BYTE,
-        MSP_CMD_RC_BYTE,
-        MSP_CMD_GET_OSD_CANVAS_BYTE,
-        MSP_CMD_VTX_CONFIG_BYTE,
+        MSP_FC_VARIANT,
+        MSP_STATUS,
+        MSP_RC,
+        MSP_GET_OSD_CANVAS,
+        MSP_GET_VTX_CONFIG,
     };
 
     for (i = 0; i < count; i++) {
-        msp_send_header(0);
+        msp_send_command(0, MSP_HEADER_V1);
         msp_tx(0x00);       // len
         msp_tx(msp_cmd[i]); // function
         msp_tx(msp_cmd[i]); // crc
@@ -761,49 +803,87 @@ void msp_cmd_tx() // send 3 commands to FC
 }
 
 void msp_eeprom_write() {
-    msp_send_header(1);
+    msp_send_command(1, MSP_HEADER_V1);
     msp_tx(0x00);
     msp_tx(250);
     msp_tx(250);
 }
 
-void msp_set_vtx_info() {
+void msp_send_vtx_model_name() {
     uint32_t i = 0;
     uint8_t crc = 0;
-    uint8_t temp = temperature >> 2;
-    uint8_t faults = cameraLost & 1 | (dm6300_lost << 1) | (heat_protect << 2);
-    uint8_t vtx_name_len = strlen(VTX_NAME);
-    uint8_t payload_size = 10 + vtx_name_len;
-
-    msp_send_header(0);
-    msp_tx(payload_size);
-    crc ^= payload_size;
-    msp_tx(MSP_CMD_VTX_INFO);
-    crc ^= MSP_CMD_VTX_INFO;
-    msp_tx(vtx_name_len);
-    crc ^= vtx_name_len;
-    for (i = 0; i < vtx_name_len; ++i) {
+    uint8_t len = strlen(VTX_NAME);
+
+    msp_send_response(0, MSP_HEADER_V2);
+    crc = msp_send_header_v2(len + 1, MSP_VTX_GET_MODEL_NAME);
+
+    // Payload
+    msp_tx(len);
+    crc = crc8tab[crc ^ len];
+    for (i = 0; i < len; ++i) {
         msp_tx(VTX_NAME[i]);
-        crc ^= VTX_NAME[i];
+        crc = crc8tab[crc ^ VTX_NAME[i]];
     }
-    msp_tx(VTX_VERSION_MAJOR);
-    crc ^= VTX_VERSION_MAJOR;
-    msp_tx(VTX_VERSION_MINOR);
-    crc ^= VTX_VERSION_MINOR;
-    msp_tx(VTX_VERSION_PATCH_LEVEL);
-    crc ^= VTX_VERSION_PATCH_LEVEL;
+    msp_tx(crc);
+}
+
+void msp_send_vtx_fc_variant() {
+    uint8_t crc = 0;
+
+    msp_send_response(0, MSP_HEADER_V2);
+    crc = msp_send_header_v2(4, MSP_VTX_GET_FC_VARIANT);
+
+    // Payload
     msp_tx(fc_variant[0]);
-    crc ^= fc_variant[0];
+    crc = crc8tab[crc ^ fc_variant[0]];
     msp_tx(fc_variant[1]);
-    crc ^= fc_variant[1];
+    crc = crc8tab[crc ^ fc_variant[1]];
     msp_tx(fc_variant[2]);
-    crc ^= fc_variant[2];
+    crc = crc8tab[crc ^ fc_variant[2]];
     msp_tx(fc_variant[3]);
-    crc ^= fc_variant[3];
+    crc = crc8tab[crc ^ fc_variant[3]];
+    msp_tx(crc);
+}
+
+void msp_send_vtx_fw_version() {
+    uint8_t crc = 0;
+
+    msp_send_response(0, MSP_HEADER_V2);
+    crc = msp_send_header_v2(3, MSP_VTX_GET_FW_VERSION);
+
+    // Payload
+    msp_tx(VTX_VERSION_MAJOR);
+    crc = crc8tab[crc ^ VTX_VERSION_MAJOR];
+    msp_tx(VTX_VERSION_MINOR);
+    crc = crc8tab[crc ^ VTX_VERSION_MINOR];
+    msp_tx(VTX_VERSION_PATCH_LEVEL);
+    crc = crc8tab[crc ^ VTX_VERSION_PATCH_LEVEL];
+    msp_tx(crc);
+}
+
+void msp_send_vtx_temperature() {
+    uint8_t crc = 0;
+    uint8_t temp = temperature >> 2;
+
+    msp_send_response(0, MSP_HEADER_V2);
+    crc = msp_send_header_v2(1, MSP_VTX_GET_TEMPERATURE);
+
+    // Payload
     msp_tx(temp);
-    crc ^= temp;
+    crc = crc8tab[crc ^ temp];
+    msp_tx(crc);
+}
+
+void msp_send_vtx_hw_faults() {
+    uint8_t crc = 0;
+    uint8_t faults = cameraLost & 1 | (dm6300_lost << 1) | (heat_protect << 2);
+
+    msp_send_response(0, MSP_HEADER_V2);
+    crc = msp_send_header_v2(1, MSP_VTX_GET_HW_FAULTS);
+
+    // Payload
     msp_tx(faults);
-    crc ^= faults;
+    crc = crc8tab[crc ^ faults];
     msp_tx(crc);
 }
 
@@ -837,11 +917,11 @@ void msp_set_vtx_config(uint8_t power, uint8_t save) {
         channel = channel - 11;
     }
 
-    msp_send_header(0);
+    msp_send_command(0, MSP_HEADER_V1);
     msp_tx(0x0f);
     crc ^= 0x0f; // len
-    msp_tx(MSP_CMD_SET_VTX_CONFIG_BYTE);
-    crc ^= MSP_CMD_SET_VTX_CONFIG_BYTE; // cmd
+    msp_tx(MSP_SET_VTX_CONFIG);
+    crc ^= MSP_SET_VTX_CONFIG; // cmd
     msp_tx(0x00);
     crc ^= 0x00; // freq_l
     msp_tx(0x00);
@@ -990,15 +1070,15 @@ void parse_vtx_config() {
 void msp_set_osd_canvas(void) {
     uint8_t crc = 0;
     if (msp_cmp_fc_variant("BTFL")) {
-        msp_send_header(0);
+        msp_send_command(0, MSP_HEADER_V1);
         msp_tx(0x02);
         crc ^= 0x02; // len
-        msp_tx(MSP_CMD_SET_OSD_CANVAS_BYTE);
-        crc ^= MSP_CMD_SET_OSD_CANVAS_BYTE;
-        msp_tx(HD_HMAX0);
-        crc ^= HD_HMAX0;
-        msp_tx(HD_VMAX0);
-        crc ^= HD_VMAX0;
+        msp_tx(MSP_SET_OSD_CANVAS);
+        crc ^= MSP_SET_OSD_CANVAS;
+        msp_tx(OSD_CANVAS_HD_HMAX0);
+        crc ^= OSD_CANVAS_HD_HMAX0;
+        msp_tx(OSD_CANVAS_HD_VMAX0);
+        crc ^= OSD_CANVAS_HD_VMAX0;
         msp_tx(crc);
         // debugf("\r\nmsp_set_osd_canvas");
     }
@@ -1012,10 +1092,10 @@ void msp_set_inav_osd_canvas(void) {
 }
 
 void parse_get_osd_canvas(void) {
-    if (msp_rx_buf[0] == HD_HMAX0 && msp_rx_buf[1] == HD_VMAX0) {
+    if (msp_rx_buf[0] == OSD_CANVAS_HD_HMAX0 && msp_rx_buf[1] == OSD_CANVAS_HD_VMAX0) {
         resolution = HD_5018;
         osd_menu_offset = 8;
-    } else if (msp_rx_buf[0] == SD_HMAX && msp_rx_buf[1] == SD_VMAX) {
+    } else if (msp_rx_buf[0] == OSD_CANVAS_SD_HMAX && msp_rx_buf[1] == OSD_CANVAS_SD_VMAX) {
         resolution = SD_3016;
         osd_menu_offset = 0;
     }
@@ -1032,7 +1112,7 @@ void parseMspVtx_V2(uint16_t const cmd_u16) {
     if (tramp_lock)
         return;
 
-    if (cmd_u16 != MSP_CMD_VTX_CONFIG_BYTE)
+    if (cmd_u16 != MSP_GET_VTX_CONFIG)
         return;
 
     if (!(fc_lock & FC_VTX_CONFIG_LOCK))
@@ -1127,7 +1207,7 @@ void parseMspVtx_V2(uint16_t const cmd_u16) {
             else
 #endif
 #endif
-            if (nxt_pwr == POWER_MAX + 1) {
+                if (nxt_pwr == POWER_MAX + 1) {
                 WriteReg(0, 0x8F, 0x10);
                 dm6300_init_done = 0;
                 cur_pwr = POWER_MAX + 2;
@@ -1663,7 +1743,7 @@ void update_cms_menu(uint16_t roll, uint16_t pitch, uint16_t yaw, uint16_t throt
                     fc_init();
                     break;
                 } // switch
-            }     // if(last_mid)
+            } // if(last_mid)
             // last_mid = mid;
         } else {
             cms_state = CMS_OSD;
@@ -2064,7 +2144,7 @@ void InitVtxTable() {
 
     // set band/channel
     for (i = 0; i < 5; i++) {
-        msp_send_header(1);
+        msp_send_command(1, MSP_HEADER_V1);
         crc = 0;
         for (j = 0; j < 31; j++) {
             msp_tx(bf_vtx_band_table[i][j]);
@@ -2075,7 +2155,7 @@ void InitVtxTable() {
 
     // set low band
     i = lowband_lock ? 6 : 5;
-    msp_send_header(1);
+    msp_send_command(1, MSP_HEADER_V1);
     crc = 0;
     for (j = 0; j < 31; j++) {
         msp_tx(bf_vtx_band_table[i][j]);
@@ -2107,7 +2187,7 @@ void InitVtxTable() {
 
     // send all of them
     for (i = 0; i < 5; i++) {
-        msp_send_header(1);
+        msp_send_command(1, MSP_HEADER_V1);
         crc = 0;
         for (j = 0; j < 9; j++) {
             msp_tx(power_table[i][j]);
diff --git a/src/msp_displayport.h b/src/msp_displayport.h
index 632e33a..1a40bf3 100644
--- a/src/msp_displayport.h
+++ b/src/msp_displayport.h
@@ -2,45 +2,14 @@
 #define __MSP_DISPLAYPORT_H_
 
 #include "common.h"
+#include "msp_protocol.h"
 
 #define IS_HI(x)  ((x) > 1750)
 #define IS_LO(x)  ((x) < 1250)
 #define IS_MID(x) ((!IS_HI(x)) && (!IS_LO(x)))
 
-#define SD_HMAX  30
-#define SD_VMAX  16
-#define HD_HMAX0 50
-#define HD_VMAX0 18
-#define HD_HMAX1 53
-#define HD_VMAX1 20
-
 #define TXBUF_SIZE 74
 
-#define MSP_HEADER_START_BYTE   0x24 // $
-#define MSP_HEADER_M_BYTE       0x4D // M
-#define MSP_HEADER_M2_BYTE      0x58 // X
-#define MSP_PACKAGE_REPLAY_BYTE 0x3E // >
-
-#define MSP_CMD_FC_VARIANT_BYTE     0x02 // 2   //out message
-#define MSP_CMD_VTX_CONFIG_BYTE     0x58 // 88  //out message
-#define MSP_CMD_SET_VTX_CONFIG_BYTE 0x59 // 89  // in message
-#define MSP_CMD_STATUS_BYTE         0x65 // 101 //out message
-#define MSP_CMD_RC_BYTE             0x69 // 105 //out message
-#define MSP_CMD_DISPLAYPORT_BYTE    0xB6 // 182 // in message
-#define MSP_CMD_SET_OSD_CANVAS_BYTE 0xBC // 188 // in message
-#define MSP_CMD_GET_OSD_CANVAS_BYTE 0xBD // 188 //out message
-#define MSP_CMD_VTX_INFO            0xF8 // 248 //out message
-
-#define DP_HEADER0 0x56
-#define DP_HEADER1 0x80
-
-#define FC_OSD_LOCK            0x01
-#define FC_VARIANT_LOCK        0x02
-#define FC_RC_LOCK             0x04
-#define FC_VTX_CONFIG_LOCK     0x08
-#define FC_STATUS_LOCK         0x10
-#define FC_INIT_VTX_TABLE_LOCK 0x80
-
 typedef enum {
     BTN_UP,
     BTN_DOWN,
@@ -160,7 +129,11 @@ uint8_t get_tx_data_osd(uint8_t index);
 void insert_tx_buf(uint8_t len);
 void DP_tx_task();
 void msp_cmd_tx();
-void msp_set_vtx_info();
+void msp_send_vtx_model_name();
+void msp_send_vtx_fc_variant();
+void msp_send_vtx_fw_version();
+void msp_send_vtx_temperature();
+void msp_send_vtx_hw_faults();
 void parse_status();
 void parse_rc();
 void parse_variant();
@@ -178,7 +151,7 @@ uint8_t bfChannel_to_channel(uint8_t const channel);
 #ifdef INIT_VTX_TABLE
 void InitVtxTable();
 #endif
-extern uint8_t osd_buf[HD_VMAX1][HD_HMAX1];
+extern uint8_t osd_buf[OSD_CANVAS_HD_VMAX1][OSD_CANVAS_HD_HMAX1];
 extern uint8_t osd_menu_offset;
 extern uint8_t disp_mode;
 extern uint8_t msp_tx_cnt;
diff --git a/src/msp_protocol.h b/src/msp_protocol.h
new file mode 100644
index 0000000..aa35fc3
--- /dev/null
+++ b/src/msp_protocol.h
@@ -0,0 +1,74 @@
+#ifndef __MSP_PROTO_H_
+#define __MSP_PROTO_H_
+
+/**
+ *  MSP Protocol
+ */
+#define MSP_HEADER_FRAMER   0x24 // $
+#define MSP_HEADER_V1       0x4D // M
+#define MSP_HEADER_V2       0x58 // X
+#define MSP_HEADER_COMMAND  0x3C // <
+#define MSP_HEADER_RESPONSE 0x3E // >
+
+/**
+ *  MSP v1 Protocol Types
+ */
+#define MSP_FC_VARIANT     0x02 // 2   //out message
+#define MSP_GET_VTX_CONFIG 0x58 // 88  //out message
+#define MSP_SET_VTX_CONFIG 0x59 // 89  // in message
+#define MSP_STATUS         0x65 // 101 //out message
+#define MSP_RC             0x69 // 105 //out message
+#define MSP_DISPLAYPORT    0xB6 // 182 // in message
+#define MSP_SET_OSD_CANVAS 0xBC // 188 // in message
+#define MSP_GET_OSD_CANVAS 0xBD // 188 //out message
+
+#define DP_HEADER0 0x56
+#define DP_HEADER1 0x80
+
+#define FC_OSD_LOCK            0x01
+#define FC_VARIANT_LOCK        0x02
+#define FC_RC_LOCK             0x04
+#define FC_VTX_CONFIG_LOCK     0x08
+#define FC_STATUS_LOCK         0x10
+#define FC_INIT_VTX_TABLE_LOCK 0x80
+
+#define OSD_CANVAS_SD_HMAX  30
+#define OSD_CANVAS_SD_VMAX  16
+#define OSD_CANVAS_HD_HMAX0 50
+#define OSD_CANVAS_HD_VMAX0 18
+#define OSD_CANVAS_HD_HMAX1 53
+#define OSD_CANVAS_HD_VMAX1 20
+
+/**
+ *  MSP v2 Protocol Command Types
+ */
+#define MSP_ELRS_BACKPACK_GET_CHANNEL_INDEX   0x0300
+#define MSP_ELRS_BACKPACK_SET_CHANNEL_INDEX   0x0301
+#define MSP_ELRS_BACKPACK_GET_FREQUENCY       0x0302
+#define MSP_ELRS_BACKPACK_SET_FREQUENCY       0x0303
+#define MSP_ELRS_BACKPACK_GET_RECORDING_STATE 0x0304
+#define MSP_ELRS_BACKPACK_SET_RECORDING_STATE 0x0305
+#define MSP_ELRS_BACKPACK_GET_VRX_MODE        0x0306
+#define MSP_ELRS_BACKPACK_SET_VRX_MODE        0x0307
+#define MSP_ELRS_BACKPACK_GET_RSSI            0x0308
+#define MSP_ELRS_BACKPACK_GET_BATTERY_VOLTAGE 0x0309
+#define MSP_ELRS_BACKPACK_GET_FIRMWARE        0x030A
+#define MSP_ELRS_BACKPACK_SET_BUZZER          0x030B
+#define MSP_ELRS_BACKPACK_SET_OSD_ELEMENT     0x030C
+#define MSP_ELRS_BACKPACK_SET_HEAD_TRACKING   0x030D // enable/disable head-tracking forwarding packets to the TX
+#define MSP_ELRS_BACKPACK_SET_RTC             0x030E
+
+// incoming, packets originating from the VRx
+#define MSP_ELRS_BACKPACK_SET_MODE    0x0380 // enable wifi/binding mode
+#define MSP_ELRS_BACKPACK_GET_VERSION 0x0381 // get the bacpack firmware version
+#define MSP_ELRS_BACKPACK_GET_STATUS  0x0382 // get the status of the backpack
+#define MSP_ELRS_BACKPACK_SET_PTR     0x0383 // forwarded back to TX backpack
+
+// incoming uart packets
+#define MSP_VTX_GET_MODEL_NAME  0x0384 // Query VTX for name
+#define MSP_VTX_GET_FC_VARIANT  0x0385 // Query VTX for flight controller variant
+#define MSP_VTX_GET_FW_VERSION  0x0386 // Query VTX for firmware version
+#define MSP_VTX_GET_TEMPERATURE 0x0387 // Query VTX for temperature in celcius
+#define MSP_VTX_GET_HW_FAULTS   0x0388 // Query VTX for hardware errors
+
+#endif /* __MSP_PROTO_H_ */

From 95149f9ead9e0262360271daa6cfe9d2de9402f0 Mon Sep 17 00:00:00 2001
From: sumolx <sumolx@gmail.com>
Date: Thu, 13 Jun 2024 21:18:06 -0400
Subject: [PATCH 4/7] Fixed endianess of v2 header generation and MSPv2 Parser
 of empty length

---
 src/msp_displayport.c | 31 ++++++++++++++++++-------------
 1 file changed, 18 insertions(+), 13 deletions(-)

diff --git a/src/msp_displayport.c b/src/msp_displayport.c
index f231f8b..4e638d4 100644
--- a/src/msp_displayport.c
+++ b/src/msp_displayport.c
@@ -380,8 +380,12 @@ uint8_t msp_read_one_frame() {
         case MSP_LEN_H:
             crc = crc8tab[crc ^ rx];
             len_u16 += ((uint16_t)rx << 8);
-            ptr = 0;
-            state = MSP_RX2;
+            if (len_u16 == 0) {
+                state = MSP_CRC2;
+            } else {
+                ptr = 0;
+                state = MSP_RX2;
+            }
             break;
 
         case MSP_RX2:
@@ -389,8 +393,9 @@ uint8_t msp_read_one_frame() {
             msp_rx_buf[ptr++] = rx;
             ptr &= 63;
             len_u16--;
-            if (len_u16 == 0)
+            if (len_u16 == 0) {
                 state = MSP_CRC2;
+            }
             break;
 
         case MSP_CRC2:
@@ -767,17 +772,17 @@ uint8_t msp_send_header_v2(uint16_t len, uint16_t msg) {
     msp_tx(0);
     uint8_t crc = crc8tab[0];
 
-    // Length
-    msp_tx(len >> 8);
-    crc = crc8tab[crc ^ (len >> 8)];
-    msp_tx(len & 0xFF);
-    crc = crc8tab[crc ^ (len & 0xFF)];
-
     // Message Type
-    msp_tx(msg >> 8);
-    crc = crc8tab[crc ^ (msg >> 8)];
-    msp_tx(msg & 0xFF);
-    crc = crc8tab[crc ^ (msg & 0xFF)];
+    msp_tx(msg & 0x00FF);
+    crc = crc8tab[crc ^ (msg & 0x00FF)];
+    msp_tx((msg & 0xFF00) >> 8);
+    crc = crc8tab[crc ^ ((msg & 0xFF00) >> 8)];
+
+    // Length
+    msp_tx(len & 0x00FF);
+    crc = crc8tab[crc ^ (len & 0x00FF)];
+    msp_tx((len & 0xFF00) >> 8);
+    crc = crc8tab[crc ^ ((len & 0xFF00) >> 8)];
 
     return crc;
 }

From e78e18a660ea7e7509023aa3206a4d1071e47c58 Mon Sep 17 00:00:00 2001
From: sumolx <sumolx@gmail.com>
Date: Fri, 14 Jun 2024 08:49:10 -0400
Subject: [PATCH 5/7] MSP_VTX_GET_MODEL_NAME does not need extra byte for
 string length, use payload length instead.

---
 src/msp_displayport.c | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/msp_displayport.c b/src/msp_displayport.c
index 4e638d4..645af7e 100644
--- a/src/msp_displayport.c
+++ b/src/msp_displayport.c
@@ -820,11 +820,9 @@ void msp_send_vtx_model_name() {
     uint8_t len = strlen(VTX_NAME);
 
     msp_send_response(0, MSP_HEADER_V2);
-    crc = msp_send_header_v2(len + 1, MSP_VTX_GET_MODEL_NAME);
+    crc = msp_send_header_v2(len, MSP_VTX_GET_MODEL_NAME);
 
     // Payload
-    msp_tx(len);
-    crc = crc8tab[crc ^ len];
     for (i = 0; i < len; ++i) {
         msp_tx(VTX_NAME[i]);
         crc = crc8tab[crc ^ VTX_NAME[i]];

From 7aca4a94ebf1a642045d4728d2c988871776529e Mon Sep 17 00:00:00 2001
From: sumolx <sumolx@gmail.com>
Date: Wed, 19 Jun 2024 06:29:45 -0400
Subject: [PATCH 6/7] Restored settings file

---
 .vscode/settings.json | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 85b5f18..44f8082 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,6 +1,3 @@
 {
     "editor.formatOnSave": true,
-    "files.associations": {
-        "msp_protocol.h": "c"
-    },
-}
\ No newline at end of file
+}

From fb0d03204e70c7033ea6f98bb78851c0c8a79e3a Mon Sep 17 00:00:00 2001
From: sumolx <sumolx@gmail.com>
Date: Wed, 19 Jun 2024 06:31:33 -0400
Subject: [PATCH 7/7] Copyied settings.json from main branch in order to
 restore it.

---
 .vscode/settings.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 44f8082..ebfde9a 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,3 +1,3 @@
 {
     "editor.formatOnSave": true,
-}
+}
\ No newline at end of file