[WIP] VTX-MSP support

config/goggles/config.json

@@ -10,5 +10,6 @@
     "fakehd_columns": "S",
     "fakehd_rows": "WWWWWWCCWWWWWWWD",
     "rec_enabled": true,
-    "rec_pb_enabled": true
+    "rec_pb_enabled": true,
+    "vtx_msp": false
 }

config/goggles/schema.json

@@ -60,6 +60,10 @@
 		"hide_diagnostics": {
 			"name": "Hide diagnostic information",
 			"widget": "checkbox"
+		},
+		"vtx_msp": {
+			"name": "Enable VTX MSP",
+			"widget": "checkbox"
 		}
 	},
 	"units": [

config/goggles/schemaV2.json

@@ -58,7 +58,16 @@
                 true,
                 false
             ]
-        }
+        },
+        "vtx_msp": {
+            "type": "boolean",
+            "title": "Enable VTX MSP",
+            "description": "Control video channel from Betaflight or ELRS Lua script VTX admin",
+            "enum": [
+                true,
+                false
+            ]
+		}
     },
     "dependencies": {
         "fakehd_enable": {

config/goggles/uiSchemaV2.json

@@ -31,5 +31,8 @@
     },
     "fakehd_rows": {
         "ui:help": "FakeHD row alignment config, each character configures the alignment for one row. [Read more](https://github.com/fpv-wtf/msp-osd#customising-the-default-fakehd-grid)"
-    }
+    },
+    "vtx_msp": {
+        "ui:help": "Enable VTX MSP protocol to control the video channel from Betaflight or ELRS Lua script VTX admin"
+    },
 }

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/control/control

@@ -1,5 +1,5 @@
 Package: msp-osd
-Version: 0.10.1
+Version: 0.10.2
 Maintainer: bri3d
 Description: MSP OSD service for the DJI HD FPV airunit.
 Architecture: pigeon-airside

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

@@ -1,4 +1,4 @@
 type = bgprocess
 command = /opt/bin/airunit-osd-start.sh
 pid-file = /opt/var/run/airunit-osd-dji.pid
-restart = true
+restart = true

ipk/goggle/control/control

@@ -1,5 +1,5 @@
 Package: msp-osd
-Version: 0.11.1
+Version: 0.11.2
 Maintainer: bri3d
 Description: MSP OSD service for the DJI HD FPV goggles.
 Architecture: pigeon-glasses

jni/Android.mk

@@ -7,7 +7,8 @@ LOCAL_EXPORT_C_INCLUDES := $(LOCAL_PATH)/include
 include $(PREBUILT_SHARED_LIBRARY)
 include $(CLEAR_VARS)
 
-LOCAL_CFLAGS += -fPIC -std=c99 -O3
+LOCAL_CFLAGS += -fPIC -std=c99 -O3 -I.
+LOCAL_CXXFLAGS += -fPIC -std=c99 -O3 -I.
 LOCAL_LDFLAGS += -fPIC
 LOCAL_LDLIBS := -llog
 LOCAL_ARM_NEON := true
@@ -31,11 +32,14 @@ LOCAL_SRC_FILES := \
 	rec/rec_util.c \
 	rec/rec.c \
 	toast/toast.c \
-	util/fs_util.c
+	util/fs_util.c \
+	util/vtx_manager.c
 include $(BUILD_SHARED_LIBRARY)
 
 include $(CLEAR_VARS)
 
+LOCAL_CFLAGS += -fPIC -std=c99 -O3 -I. -DLZ4_STATIC_LINKING_ONLY
+LOCAL_CXXFLAGS += -fPIC -std=c99 -O3 -I.
 LOCAL_SRC_FILES:= \
 	hw/dji_radio_shm.c \
 	json/osd_config.c \

jni/hw/dji_display.c

@@ -1,4 +1,5 @@
 #include <stdlib.h>
+#include <string.h>
 #include "dji_display.h"
 #include "util/debug.h"
 

jni/hw/dji_radio_shm.c

@@ -1,4 +1,5 @@
 #include <assert.h>
+#include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
 #include <sys/mman.h>

jni/lz4/lz4.c

@@ -114,7 +114,6 @@
 #define LZ4_DISABLE_DEPRECATE_WARNINGS /* due to LZ4_decompress_safe_withPrefix64k */
 #endif
 
-#define LZ4_STATIC_LINKING_ONLY  /* LZ4_DISTANCE_MAX */
 #include "lz4.h"
 /* see also "memory routines" below */
 

jni/msp/msp.h

@@ -17,6 +17,12 @@
 #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
+#define MSP_CMD_SET_VTXTABLE_BAND    227    // to FC        set vtxTable band/channel data (one band at a time)
+#define MSP_CMD_SET_VTXTABLE_POWERLEVEL 228 // to FC         set vtxTable powerLevel data (one powerLevel at a time)
+
 typedef enum {
     MSP_ERR_NONE,
     MSP_ERR_HDR,
@@ -58,4 +64,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/msp_vtx.c

@@ -0,0 +1,86 @@
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdint.h>
+#include "msp.h"
+
+static uint8_t message_buffer[256];
+
+uint8_t total_bands = 1;
+uint8_t total_channels = 8;
+uint8_t total_powers = 6;
+
+uint8_t * resetVTXTableMessage() {
+  uint8_t payload_size = 15;
+  uint8_t default_table[15] = {
+    0, // idx LSB
+    0, // idx MSB
+    0, // 25mW Power idx
+    0, // pitmode
+    0, // lowPowerDisarm
+    0, // pitModeFreq LSB
+    0, // pitModeFreq MSB
+    0, // band
+    0, // channel
+    0, // newFreq  LSB
+    0, // newFreq  MSB
+    total_bands, // bandCount,
+    total_channels, // channelCount
+    total_powers, // powerCount
+    1  // vtxtable should be cleared
+  };
+
+  construct_msp_command(message_buffer, MSP_CMD_SET_VTX_CONFIG, default_table, payload_size, MSP_OUTBOUND);
+
+  return message_buffer;
+}
+
+uint8_t * setupVTXPowerMessage(int index, int value, char name[]) {
+  uint8_t name_size = strlen(name);
+  uint8_t payload_size = 4 + name_size;
+  uint8_t vtx_power[4 + 3] = {
+    index, // idx
+    value & 0xFF, // powerValue LSB
+    (value >> 8) & 0xFF, // powerValue MSB
+    strlen(name), // label lenght
+  };
+
+  for(int i = 0; i < strlen(name); i++) {
+    vtx_power[i] = name[i];
+  }
+  /*txPacket[8] = idx;
+    txPacket[9] = saPowerLevelsLut[idx - 1] & 0xFF;         // powerValue LSB
+    txPacket[10] = (saPowerLevelsLut[idx - 1] >> 8) & 0xFF; // powerValue MSB
+    txPacket[11] = POWER_LEVEL_LABEL_LENGTH;
+    txPacket[12] = saPowerLevelsLabel[((idx - 1) * POWER_LEVEL_LABEL_LENGTH) + 0];
+    txPacket[13] = saPowerLevelsLabel[((idx - 1) * POWER_LEVEL_LABEL_LENGTH) + 1];
+    txPacket[14] = saPowerLevelsLabel[((idx - 1) * POWER_LEVEL_LABEL_LENGTH) + 2];*/
+
+  construct_msp_command(message_buffer, MSP_CMD_SET_VTXTABLE_POWERLEVEL, vtx_power, payload_size, MSP_OUTBOUND);
+
+  return message_buffer;
+}
+
+uint8_t * setupVTXBandMessage() {
+  uint8_t payload_size = 8 + total_channels;
+  uint8_t vtx_band[8 + 8] = {
+    0, // band
+    3, // label lenght
+    'D',
+    'J',
+    'I',
+    'D', // band letter
+    0, // is factory band
+    total_channels, // total channel
+  };
+
+  for(int i = 8; i < total_channels; i++) {
+    vtx_band[i] = i & 0xFF;
+    i++;
+    vtx_band[i] = (i >> 8) & 0xFF;
+  }
+
+  construct_msp_command(message_buffer, MSP_CMD_SET_VTXTABLE_POWERLEVEL, vtx_band, payload_size, MSP_OUTBOUND);
+
+  return message_buffer;
+}

jni/msp_displayport_mux.c

@@ -16,6 +16,7 @@
 #include "util/debug.h"
 #include "util/time_util.h"
 #include "util/fs_util.h"
+#include "./msp/msp_vtx.c"
 
 #define CPU_TEMP_PATH "/sys/devices/platform/soc/f0a00000.apb/f0a71000.omc/temp1"
 #define AU_VOLTAGE_PATH "/sys/devices/platform/soc/f0a00000.apb/f0a71000.omc/voltage4"
@@ -74,6 +75,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 +145,32 @@ 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 = resetVTXTableMessage();
+    write(serial_fd, buffer, sizeof(buffer));
+
+    buffer = setupVTXPowerMessage(0, 25, "25 ");
+    write(serial_fd, buffer, sizeof(buffer));
+    buffer = setupVTXPowerMessage(0, 200, "200 ");
+    write(serial_fd, buffer, sizeof(buffer));
+    buffer = setupVTXPowerMessage(0, 500, "500 ");
+    write(serial_fd, buffer, sizeof(buffer));
+    buffer = setupVTXPowerMessage(0, 700, "700 ");
+    write(serial_fd, buffer, sizeof(buffer));
+
+    buffer = setupVTXBandMessage();
+    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,10 +237,19 @@ static void rx_msp_callback(msp_msg_t *msp_message)
             }
             break;
         }
+        case MSP_CMD_VTX_CONFIG: {
+            DEBUG_PRINT("Received VTX CONFIG message...\n");
+            if(sizeof(msp_message->payload) > 2) {
+                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)) {
-                // Either serial passthrough was on, or the cache was enabled but missed (a response was not available). 
+                // Either serial passthrough was on, or the cache was enabled but missed (a response was not available).
                 // Either way, this means we need to send the message through to DJI.
                 write(pty_fd, message_buffer, size);
             }
@@ -249,8 +287,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));
 }
 
@@ -364,8 +403,8 @@ int main(int argc, char *argv[]) {
     printf("Allocated PTY %s\n", pty_name_ptr);
     if ((argc - optind) > 2) {
         unlink(argv[optind + 2]);
-        symlink(pty_name_ptr, argv[optind + 2]);   
-        printf("Relinked %s to %s\n", argv[optind + 2], pty_name_ptr); 
+        symlink(pty_name_ptr, argv[optind + 2]);
+        printf("Relinked %s to %s\n", argv[optind + 2], pty_name_ptr);
     }
     socket_fd = connect_to_server(ip_address, MSP_PORT);
     compressed_fd = connect_to_server(ip_address, COMPRESSED_DATA_PORT);
@@ -390,14 +429,16 @@ 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;
         poll_fds[0].events = POLLIN;
         poll_fds[1].events = POLLIN;
 
         poll(poll_fds, 2, ((MSEC_PER_SEC / update_rate_hz) / 2));
-        
+
         // We got inbound serial data, process it as MSP data.
         if (0 < (serial_data_size = read(serial_fd, serial_data, sizeof(serial_data)))) {
             DEBUG_PRINT("RECEIVED data! length %d\n", serial_data_size);
@@ -431,6 +472,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;