New MSP_VTX_INFO message type added which is broadcasted at 1Hz

SumolX · May 31, 2024 · bbca0bf · bbca0bf
1 parent 0fdfb9e
commit bbca0bf
Show file tree

Hide file tree

Showing 4 changed files with 53 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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
@@ -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();