setting vtx wip

Author: jesuslopezxing

jni/msp/msp.h

@@ -20,6 +20,8 @@
 // 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,

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 = sizeof(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
+    sizeof(name), // label lenght
+  };
+
+  for(int i = 0; i < sizeof(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"
@@ -153,9 +154,21 @@ static void send_vtx_config_request(int serial_fd) {
 
 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));
+
+    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) {
@@ -236,7 +249,7 @@ static void rx_msp_callback(msp_msg_t *msp_message)
         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);
             }
@@ -390,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);
@@ -425,7 +438,7 @@ int main(int argc, char *argv[]) {
         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);