Add support for VTX MSP

Implement VTX MSP protocol support so the video channel can be changed
from Betaflight, ELRS Lua Script or any other system that uses VTX MPS
protocol.

The plan is be able to change algo power and badnwidth.

Author: Jesus Lopez Garcia

dev_air.sh

@@ -0,0 +1,6 @@
+ndk-build
+adb push libs/armeabi-v7a/msp_displayport_mux /blackbox
+adb shell setprop dji.hdvt_uav_service 0
+adb shell mv /dev/ttyS1 /dev/ttyS1_moved
+adb shell nohup /blackbox/msp_displayport_mux 192.168.41.2 /dev/ttyS1_moved /dev/ttyS1 &
+adb shell setprop dji.hdvt_uav_service 1

dev_goggles.sh

@@ -0,0 +1,4 @@
+ndk-build
+adb shell setprop dji.glasses_wm150_service 0
+adb push libs/armeabi-v7a/libdisplayport_osd_shim.so /tmp
+adb shell LD_PRELOAD=/tmp/libdisplayport_osd_shim.so dji_gls_wm150_original -g

ipk/airunit/data/opt/etc/dinit.d/msp-osd-airside

@@ -2,3 +2,4 @@ type = bgprocess
 command = /opt/bin/airunit-osd-start.sh
 pid-file = /opt/var/run/airunit-osd-dji.pid
 restart = true
+logfile = /blackbox/msp-osd.log

ipk/goggle/data/opt/etc/dinit.d/msp-osd-goggles

@@ -1,3 +1,4 @@
 type = scripted
 command = modmanager enable diy_glasses displayport_osd_shim
-stop-command = modmanager disable diy_glasses displayport_osd_shim
+stop-command = modmanager disable diy_glasses displayport_osd_shim
+logfile = /blackbox/msp-osd.log

jni/msp/msp.h

@@ -17,6 +17,10 @@
 #define MSP_CMD_DISPLAYPORT 182
 #define MSP_CMD_SET_OSD_CANVAS 188
 
+// https://github.com/betaflight/betaflight/blob/master/src/main/msp/msp_protocol.h#L201C1-L202C50
+#define MSP_CMD_VTX_CONFIG 88 // from FC
+#define MSP_CMD_SET_VTX_CONFIG 89 // to FC
+
 typedef enum {
     MSP_ERR_NONE,
     MSP_ERR_HDR,
@@ -58,4 +62,4 @@ typedef struct msp_state_s {
 
 uint16_t msp_data_from_msg(uint8_t message_buffer[], msp_msg_t *msg);
 msp_error_e construct_msp_command(uint8_t message_buffer[], uint8_t command, uint8_t payload[], uint8_t size, msp_direction_e direction);
-msp_error_e msp_process_data(msp_state_t *msp_state, uint8_t dat);
+msp_error_e msp_process_data(msp_state_t *msp_state, uint8_t dat);

jni/msp_displayport_mux.c

@@ -74,6 +74,8 @@ static uint8_t serial_passthrough = 1;
 static uint8_t compress = 0;
 static uint8_t no_btfl_hd = 0;
 
+int8_t fc_vtx_channel = -1;
+
 static void sig_handler(int _)
 {
     quit = 1;
@@ -142,6 +144,20 @@ static void send_version_request(int serial_fd) {
     write(serial_fd, &buffer, sizeof(buffer));
 }
 
+static void send_vtx_config_request(int serial_fd) {
+    DEBUG_PRINT("Sending VTX CONFIG requests message...\n");
+    uint8_t buffer[6];
+    construct_msp_command(buffer, MSP_CMD_VTX_CONFIG, NULL, 0, MSP_OUTBOUND);
+    write(serial_fd, &buffer, sizeof(buffer));
+}
+
+static void send_vtx_set_config_request(int serial_fd) {
+    DEBUG_PRINT("Sending VTX READY message...\n");
+    uint8_t buffer[6];
+    construct_msp_command(buffer, MSP_CMD_SET_VTX_CONFIG, NULL, 0, MSP_OUTBOUND);
+    write(serial_fd, &buffer, sizeof(buffer));
+}
+
 static void copy_to_msp_frame_buffer(void *buffer, uint16_t size) {
     memcpy(&frame_buffer[fb_cursor], buffer, size);
     fb_cursor += size;
@@ -208,6 +224,15 @@ static void rx_msp_callback(msp_msg_t *msp_message)
             }
             break;
         }
+        case MSP_CMD_VTX_CONFIG: {
+            DEBUG_PRINT("Received VTX CONFIG message...\n");
+            printf("data: %d %d %d \n", msp_message->payload[0],  msp_message->payload[1],  msp_message->payload[2]);
+            // TODO: Damage control accessing to index 2 if not exists
+            fc_vtx_channel = msp_message->payload[2];
+        }
+        case MSP_CMD_SET_VTX_CONFIG: {
+            DEBUG_PRINT("Received SET VTX CONFIG message...\n");
+        }
         default: {
             uint16_t size = msp_data_from_msg(message_buffer, msp_message);
             if(serial_passthrough || cache_msp_message(msp_message)) {
@@ -249,8 +274,9 @@ static void send_data_packet(int data_socket_fd, dji_shm_state_t *dji_shm) {
     data.version_specifier = 0xFFFF;
     data.tx_temperature = get_int_from_fs(CPU_TEMP_PATH);
     data.tx_voltage = get_int_from_fs(AU_VOLTAGE_PATH);
+    data.fc_vtx_channel = fc_vtx_channel;
     memcpy(data.fc_variant, current_fc_identifier, sizeof(current_fc_identifier));
-    DEBUG_PRINT("got voltage %f V temp %d C variant %.4s\n", (float)(data.tx_voltage / 64.0f), data.tx_temperature, data.fc_variant);
+    DEBUG_PRINT("got voltage %f V temp %d C variant %.4s and channel %d\n", (float)(data.tx_voltage / 64.0f), data.tx_temperature, data.fc_variant, data.fc_vtx_channel);
     write(data_socket_fd, &data, sizeof(data));
 }
 
@@ -390,6 +416,8 @@ int main(int argc, char *argv[]) {
     clock_gettime(CLOCK_MONOTONIC, &last_data);
     clock_gettime(CLOCK_MONOTONIC, &last_frame);
 
+    send_vtx_set_config_request(serial_fd);
+
     while (!quit) {
         poll_fds[0].fd = serial_fd;
         poll_fds[1].fd = pty_fd;
@@ -431,6 +459,8 @@ int main(int argc, char *argv[]) {
             if(current_fc_identifier[0] == 0) {
                 send_variant_request(serial_fd);
             }
+            //INFO: For testing purpose, request VTX_CONFIG
+            send_vtx_config_request(serial_fd);
         }
         if(compress && (timespec_subtract_ns(&now, &last_frame) > (NSEC_PER_SEC / update_rate_hz))) {
             send_compressed_screen(compressed_fd);

jni/net/data_protocol.h

@@ -4,6 +4,7 @@ typedef struct packet_data_s {
     uint16_t tx_temperature;
     uint16_t version_specifier; // Used to be bitrate! Danger! 0xFFFF means V2 (no bitrate) for now.
     uint16_t tx_voltage;
+    int8_t fc_vtx_channel;
     char fc_variant[4];
 } __attribute__((packed)) packet_data_t;
 

jni/osd_dji_overlay_udp.c

@@ -34,6 +34,8 @@
 #include "rec/rec.h"
 #include "rec/rec_pb.h"
 
+#include "util/vtx_manager.c"
+
 #define MSP_PORT 7654
 #define DATA_PORT 7655
 #define COMPRESSED_DATA_PORT 7656
@@ -485,7 +487,7 @@ static void check_is_au_overlay_enabled()
 
 static void process_data_packet(uint8_t *buf, int len, dji_shm_state_t *radio_shm) {
     packet_data_t *packet = (packet_data_t *)buf;
-    DEBUG_PRINT("got data %04X version spec %d C %f V variant %.4s\n", packet->version_specifier, packet->tx_temperature, packet->tx_voltage / 64.0f, packet->fc_variant);
+    DEBUG_PRINT("got data %04X version spec %d C %f V variant %.4s and vtx channel %d\n", packet->version_specifier, packet->tx_temperature, packet->tx_voltage / 64.0f, packet->fc_variant, packet->fc_vtx_channel);
     char str[8];
     clear_overlay();
     if(au_overlay_enabled) {
@@ -494,6 +496,7 @@ static void process_data_packet(uint8_t *buf, int len, dji_shm_state_t *radio_sh
         snprintf(str, 8, "A %2.1fV", packet->tx_voltage / 64.0f);
         display_print_string(overlay_display_info.char_width - 7, overlay_display_info.char_height - 7, str, 7);
     }
+    changeChannel(packet->fc_vtx_channel);
     if(len > 6) {
         DEBUG_PRINT("Got new packet with variant %.4s\n", packet->fc_variant);
         // should have FC type

jni/util/vtx_manager.c

@@ -0,0 +1,51 @@
+#include <stdlib.h>
+#include <dlfcn.h>
+#include "util/debug.h"
+
+
+
+static void *tp1801_gui_lib = NULL;
+static uint32_t (* setChannelPilotOriginal)(void *this,unsigned short param_1, bool param_2) = 0;
+static uint32_t (* userSettingsGetInstanceOriginal)() = 0;
+static uint32_t userSettingsInstance = 0;
+static int8_t currentChannel = -1;
+
+void changeChannel(int8_t channel) {
+    //Load SetPilotChannel original
+    setChannelPilotOriginal = dlsym (RTLD_NEXT, "_ZN17GlassUserSettings15setPilotChannelEtb");
+    if (setChannelPilotOriginal == NULL) {
+        if(!tp1801_gui_lib){
+            tp1801_gui_lib = dlopen("/system/lib/libtp1801_gui.so", 1);
+        }
+        setChannelPilotOriginal = dlsym (tp1801_gui_lib, "_ZN17GlassUserSettings15setPilotChannelEtb");
+        if (setChannelPilotOriginal == NULL)
+        {
+            printf("dlsym: %s\n", dlerror());
+            return;
+        }
+    }
+
+    //Load UserSettingGetInstance original
+    userSettingsGetInstanceOriginal = dlsym (tp1801_gui_lib, "_ZN17GlassUserSettings11getInstanceEv");
+    if (userSettingsGetInstanceOriginal == NULL)
+    {
+        printf("dlsym: %s\n", dlerror());
+        return;
+    }
+
+    if (channel <= 0 || channel > 8) {
+        printf("VTX_MANAGER Error:, invalid channel index: %d\n", channel);
+        return;
+    }
+
+    if(currentChannel == channel) {
+        return;
+    }
+    currentChannel = channel;
+
+    // if 8, need to set to public or works with the index? 
+    DEBUG_PRINT("VTX_MANAGER: requesting to the goggles to set channel %d\n", channel);
+    
+    int8_t channelIdx = channel - 1;
+    setChannelPilotOriginal(userSettingsGetInstanceOriginal(), channelIdx, true);
+}