New betaflight hd (#87)

* initial pass at new Betaflight resolution setting support

* switch defines to enum

* move displayport commands to an enum as well

* add compatibility with noncompressed mode

* make Betaflight HD mode disable-able for the discerning user.

* fix config defaults

* bump control

config/airunit/config.json

@@ -2,5 +2,6 @@
     "fast_serial": false,
     "cache_serial": false,
     "osd_update_rate_hz": 10,
-    "compress_osd": false
+    "compress_osd": false,
+    "disable_betaflight_hd": false
 }

config/airunit/schema.json

@@ -18,6 +18,10 @@
         "min": 2,
         "max": 20,
         "step":1
+      },
+      "disable_betaflight_hd": {
+        "name": "Compress OSD",
+        "widget": "checkbox"
       }
     },
     "units": [

config/airunit/schemaV2.json

@@ -40,6 +40,15 @@
             "description": "OSD update rate, used only for compressed OSD.",
             "minimum": 1,
             "maximum": 20
+          },
+          "disable_betaflight_hd": {
+            "name": "Disable Betaflight HD Mode",
+            "type": "boolean",
+            "description": "Disable HD Mode in Betaflight 4.4.",
+            "enum": [
+                true,
+                false
+            ]
           }
     }
 }

ipk/airunit/control/control

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

ipk/goggle/control/control

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

jni/msp/msp.h

@@ -1,5 +1,6 @@
 #include <stdint.h>
 
+#define MSP_CMD_API_VERSION 1
 #define MSP_CMD_FC_VARIANT 2
 #define MSP_CMD_FC_VERSION 3
 #define MSP_CMD_NAME 10
@@ -13,6 +14,7 @@
 #define MSP_CMD_BATTERY_STATE 130
 #define MSP_CMD_STATUS_EX 150
 #define MSP_CMD_DISPLAYPORT 182
+#define MSP_CMD_SET_OSD_CANVAS 188
 
 typedef enum {
     MSP_ERR_NONE,

jni/msp/msp_displayport.c

@@ -5,7 +5,7 @@ static void process_draw_string(displayport_vtable_t *display_driver, uint8_t *p
     if(!display_driver || !display_driver->draw_character) return;
     uint8_t row = payload[0];
     uint8_t col = payload[1];
-    uint8_t attrs = payload[2]; // iNav uses this to specify which font page to draw from
+    uint8_t attrs = payload[2]; // INAV and Betaflight use this to specify a higher page number. 
     uint8_t str_len;
     for(str_len = 1; str_len < 255; str_len++) {
         if(payload[2 + str_len] == '\0') {
@@ -14,9 +14,9 @@ static void process_draw_string(displayport_vtable_t *display_driver, uint8_t *p
     }
     for(uint8_t idx = 0; idx < (str_len - 1); idx++) {
         uint16_t character = payload[3 + idx];
-        if(attrs & 0x1) {
-            // shift over a page if one was specified
-            character |= 0x100;
+        if(attrs & 0x3) {
+            // shift over by the page number if they were specified
+            character |= ((attrs & 0x3) * 0x100);
         }
         display_driver->draw_character(col, row, character);
         col++;
@@ -36,7 +36,7 @@ static void process_draw_complete(displayport_vtable_t *display_driver) {
 static void process_set_options(displayport_vtable_t *display_driver, uint8_t *payload) {
     if(!display_driver || !display_driver->set_options) return;
     uint8_t font = payload[0];
-    uint8_t is_hd = payload[1];
+    msp_hd_options_e is_hd = payload[1];
     display_driver->set_options(font, is_hd);
 }
 
@@ -55,24 +55,24 @@ int displayport_process_message(displayport_vtable_t *display_driver, msp_msg_t
     if (msg->cmd != MSP_CMD_DISPLAYPORT) {
         return 1;
     }
-    uint8_t sub_cmd = msg->payload[0];
+    msp_displayport_cmd_e sub_cmd = msg->payload[0];
     switch(sub_cmd) {
-        case 0: // 0 -> Open/Keep-Alive DisplayPort
+        case MSP_DISPLAYPORT_KEEPALIVE: // 0 -> Open/Keep-Alive DisplayPort
             process_open(display_driver);
             break;
-        case 1: // 1 -> Close DisplayPort
+        case MSP_DISPLAYPORT_CLOSE: // 1 -> Close DisplayPort
             process_close(display_driver);
             break;
-        case 2: // 2 -> Clear Screen
+        case MSP_DISPLAYPORT_CLEAR: // 2 -> Clear Screen
             process_clear_screen(display_driver);
             break;
-        case 3: // 3 -> Draw String
+        case MSP_DISPLAYPORT_DRAW_STRING: // 3 -> Draw String
             process_draw_string(display_driver, &msg->payload[1]);
             break;
-        case 4: // 4 -> Draw Screen
+        case MSP_DISPLAYPORT_DRAW_SCREEN: // 4 -> Draw Screen
             process_draw_complete(display_driver);
             break;
-        case 5: // 5 -> Set Options (HDZero/iNav)
+        case MSP_DISPLAYPORT_SET_OPTIONS: // 5 -> Set Options (HDZero/iNav)
             process_set_options(display_driver, &msg->payload[1]);
             break;
         default:

jni/msp/msp_displayport.h

@@ -1,7 +1,24 @@
 #include <stdint.h>
 
+typedef enum {
+    MSP_DISPLAYPORT_KEEPALIVE,
+    MSP_DISPLAYPORT_CLOSE,
+    MSP_DISPLAYPORT_CLEAR,
+    MSP_DISPLAYPORT_DRAW_STRING,
+    MSP_DISPLAYPORT_DRAW_SCREEN,
+    MSP_DISPLAYPORT_SET_OPTIONS,
+    MSP_DISPLAYPORT_DRAW_SYSTEM
+} msp_displayport_cmd_e;
+
+typedef enum {
+    MSP_SD_OPTION_30_16,
+    MSP_HD_OPTION_50_18,
+    MSP_HD_OPTION_30_16,
+    MSP_HD_OPTION_60_22
+} msp_hd_options_e;
+
 typedef void (*draw_character_func)(uint32_t x, uint32_t y, uint16_t c);
-typedef void (*set_options_func)(uint8_t font, uint8_t is_hd);
+typedef void (*set_options_func)(uint8_t font, msp_hd_options_e is_hd);
 typedef void (*clear_screen_func)();
 typedef void (*draw_complete_func)();
 

jni/msp_displayport_mux.c

@@ -23,6 +23,7 @@
 #define CACHE_SERIAL_KEY "cache_serial"
 #define COMPRESS_KEY "compress_osd"
 #define UPDATE_RATE_KEY "osd_update_rate_hz"
+#define NO_BTFL_HD_KEY "disable_betaflight_hd"
 
 // The MSP_PORT is used to send MSP passthrough messages.
 // The DATA_PORT is used to send arbitrary data - for example, bitrate and temperature data.
@@ -32,8 +33,14 @@
 #define COMPRESSED_DATA_PORT 7656
 
 #define COMPRESSED_DATA_VERSION 1
-#define MAX_DISPLAY_X 60
-#define MAX_DISPLAY_Y 22
+
+enum {
+    MAX_DISPLAY_X = 60,
+    MAX_DISPLAY_Y = 22
+};
+
+// The Betaflight MSP minor version in which MSP DisplayPort sizing is supported.
+#define MSP_DISPLAY_SIZE_VERSION 45
 
 #ifdef DEBUG
 #define DEBUG_PRINT(fmt, args...)    fprintf(stderr, fmt, ## args)
@@ -57,7 +64,7 @@ static char current_fc_identifier[4];
 /* For compressed full-frame transmission */
 static uint16_t msp_character_map_buffer[MAX_DISPLAY_X][MAX_DISPLAY_Y];
 static uint16_t msp_character_map_draw[MAX_DISPLAY_X][MAX_DISPLAY_Y];
-static uint8_t msp_hd_option = 0;
+static msp_hd_options_e msp_hd_option = 0;
 static displayport_vtable_t *display_driver = NULL;
 LZ4_stream_t *lz4_ref_ctx = NULL;
 uint8_t update_rate_hz = 2;
@@ -70,6 +77,7 @@ int compressed_fd;
 static volatile sig_atomic_t quit = 0;
 static uint8_t serial_passthrough = 1;
 static uint8_t compress = 0;
+static uint8_t no_btfl_hd = 0;
 
 static void sig_handler(int _)
 {
@@ -107,7 +115,7 @@ static int16_t msp_msg_from_cache(uint8_t msg_buffer[], uint8_t cmd_id) {
         || cmd_id == MSP_CMD_BATTERY_STATE) {
             struct timespec now;
             clock_gettime(CLOCK_MONOTONIC, &now);
-            if(now.tv_sec > cache_message->time.tv_sec) {
+            if(timespec_subtract_ns(&now, &cache_message->time) > NSEC_PER_SEC) {
                 // message is too old, invalidate cache and force a resend
                 DEBUG_PRINT("MSP cache EXPIRED %d\n", cmd_id);
                 free(cache_message);
@@ -120,53 +128,99 @@ static int16_t msp_msg_from_cache(uint8_t msg_buffer[], uint8_t cmd_id) {
     }
 }
 
+static void send_display_size(int serial_fd) {
+    uint8_t buffer[8];
+    uint8_t payload[2] = {MAX_DISPLAY_X, MAX_DISPLAY_Y};
+    construct_msp_command(buffer, MSP_CMD_SET_OSD_CANVAS, payload, 2, MSP_OUTBOUND);
+    write(serial_fd, &buffer, sizeof(buffer));
+}
+
+static void send_variant_request(int serial_fd) {
+    uint8_t buffer[6];
+    construct_msp_command(buffer, MSP_CMD_FC_VARIANT, NULL, 0, MSP_OUTBOUND);
+    write(serial_fd, &buffer, sizeof(buffer));
+}
+
+static void send_version_request(int serial_fd) {
+    uint8_t buffer[6];
+    construct_msp_command(buffer, MSP_CMD_API_VERSION, 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;
+}
+
 static void rx_msp_callback(msp_msg_t *msp_message)
 {
     // Process a received MSP message from FC and decide whether to send it to the PTY (DJI) or UDP port (MSP-OSD on Goggles)
     DEBUG_PRINT("FC->AU MSP msg %d with data len %d \n", msp_message->cmd, msp_message->size);
-    if(msp_message->cmd == MSP_CMD_DISPLAYPORT) {
-        if (compress) {
-            // This was an MSP DisplayPort message and we're in compressed mode, so pass it off to the DisplayPort handlers.
-            displayport_process_message(display_driver, msp_message);
-        } else {
-            // This was an MSP DisplayPort message and we're in legacy mode, so buffer it until we get a whole frame.
-            if(fb_cursor > sizeof(frame_buffer)) {
-                printf("Exhausted frame buffer! Resetting...\n");
-                fb_cursor = 0;
-                return;
-            }
-            uint16_t size = msp_data_from_msg(message_buffer, msp_message);
-            memcpy(&frame_buffer[fb_cursor], message_buffer, size);
-            fb_cursor += size;
-            if(msp_message->payload[0] == 4) {
-                // Once we have a whole frame of data, send it to the goggles.
-                write(socket_fd, frame_buffer, fb_cursor);
-                DEBUG_PRINT("DRAW! wrote %d bytes\n", fb_cursor);
-                fb_cursor = 0;
+    switch(msp_message->cmd) {
+        case MSP_CMD_DISPLAYPORT: {
+            if (compress) {
+                // This was an MSP DisplayPort message and we're in compressed mode, so pass it off to the DisplayPort handlers.
+                displayport_process_message(display_driver, msp_message);
+            } else {
+                // This was an MSP DisplayPort message and we're in legacy mode, so buffer it until we get a whole frame.
+                if(fb_cursor > sizeof(frame_buffer)) {
+                    printf("Exhausted frame buffer! Resetting...\n");
+                    fb_cursor = 0;
+                    return;
+                }
+                uint16_t size = msp_data_from_msg(message_buffer, msp_message);
+                copy_to_msp_frame_buffer(message_buffer, size);
+                if(msp_message->payload[0] == MSP_DISPLAYPORT_DRAW_SCREEN) {
+                    // Once we have a whole frame of data, send it to the goggles.
+                    write(socket_fd, frame_buffer, fb_cursor);
+                    DEBUG_PRINT("DRAW! wrote %d bytes\n", fb_cursor);
+                    fb_cursor = 0;
+                }
             }
+            break;
         }
-    } else {
-        uint16_t size = msp_data_from_msg(message_buffer, msp_message);
-        // This is an FC Variant response, so we want to use it to set our FC variant.
-        if(msp_message->cmd == MSP_CMD_FC_VARIANT) {
+        case MSP_CMD_FC_VARIANT: {
+            // This is an FC Variant response, so we want to use it to set our FC variant.
             DEBUG_PRINT("Got FC Variant response!\n");
-            memcpy(current_fc_identifier, msp_message->payload, 4);
+            if(strncmp(current_fc_identifier, msp_message->payload, 4) != 0) {
+                // FC variant changed or was updated. Update the current FC identifier and send an MSP version request.
+                memcpy(current_fc_identifier, msp_message->payload, 4);
+                send_version_request(serial_fd);
+            }
+            break;
         }
-        // This isn't an MSP DisplayPort message, so send it to either DJI directly or to the cache.
-        if(serial_passthrough) {
-            write(pty_fd, message_buffer, size);
-        } else {
-            // Serial passthrough is off, so cache the response we got.
-            if(cache_msp_message(msp_message)) {
-                // 1 -> cache miss, so this message expired or hasn't been seen.
-                // this means DJI is waiting for it, so send it over
-                DEBUG_PRINT("DJI was waiting, got msg %d\n", msp_message->cmd);
-                for (int i = 0; i < size; i++) {
-                    DEBUG_PRINT("%02X ", message_buffer[i]);
+        case MSP_CMD_API_VERSION: {
+            // Got an MSP API version response. Compare the version if we have Betaflight in order to see if we should send the new display size message.
+            if(strncmp(current_fc_identifier, "BTFL", 4) == 0) {
+                uint8_t msp_minor_version = msp_message->payload[2];
+                DEBUG_PRINT("Got Betaflight minor MSP version %d\n", msp_minor_version);
+                if(msp_minor_version >= MSP_DISPLAY_SIZE_VERSION) {
+                    if(!no_btfl_hd) {
+                        if(!compress) {
+                            // If compression is disabled, we need to manually inject a canvas-change command into the command stream.
+                            uint8_t displayport_set_size[3] = {MSP_DISPLAYPORT_SET_OPTIONS, 0, MSP_HD_OPTION_60_22};
+                            construct_msp_command(message_buffer, MSP_CMD_DISPLAYPORT, displayport_set_size, 3, MSP_INBOUND);
+                            copy_to_msp_frame_buffer(message_buffer, 9);
+                            DEBUG_PRINT("Sent display size to goggles\n");
+
+                        }
+                        // Betaflight with HD support. Send our display size and set 60x22.
+                        send_display_size(serial_fd);
+                        msp_hd_option = MSP_HD_OPTION_60_22;
+                        DEBUG_PRINT("Sent display size to FC\n");
+                    }
                 }
-                DEBUG_PRINT("\n");
+            }
+            break;
+        }
+        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 way, this means we need to send the message through to DJI.
                 write(pty_fd, message_buffer, size);
             }
+            break;
         }
     }
 }
@@ -194,12 +248,6 @@ static void tx_msp_callback(msp_msg_t *msp_message)
     }
 }
 
-static void send_variant_request(int serial_fd) {
-    uint8_t buffer[6];
-    construct_msp_command(buffer, MSP_CMD_FC_VARIANT, NULL, 0, MSP_OUTBOUND);
-    write(serial_fd, &buffer, 6);
-}
-
 static void send_data_packet(int data_socket_fd, dji_shm_state_t *dji_shm) {
     packet_data_t data;
     memset(&data, 0, sizeof(data));
@@ -214,6 +262,7 @@ static void send_data_packet(int data_socket_fd, dji_shm_state_t *dji_shm) {
 /* MSP DisplayPort handlers for compressed mode */
 
 static void msp_draw_character(uint32_t x, uint32_t y, uint16_t c) {
+    DEBUG_PRINT("drawing char %d at x %d y %d\n", c, x, y);
     msp_character_map_buffer[x][y] = c;
 }
 
@@ -225,20 +274,20 @@ static void msp_draw_complete() {
     memcpy(msp_character_map_draw, msp_character_map_buffer, sizeof(msp_character_map_buffer));
 }
 
-static void msp_set_options(uint8_t font_num, uint8_t is_hd) {
+static void msp_set_options(uint8_t font_num, msp_hd_options_e is_hd) {
+   DEBUG_PRINT("Got options!\n");
    msp_clear_screen();
    msp_hd_option = is_hd;
 }
 
 static void send_compressed_screen(int compressed_fd) {
-
     LZ4_stream_t current_stream_state;
     uint8_t dest_buf[sizeof(compressed_data_header_t) + LZ4_COMPRESSBOUND(sizeof(msp_character_map_draw))];
     void *dest = &dest_buf;
     memcpy(&current_stream_state, lz4_ref_ctx, sizeof(LZ4_stream_t));
     int size = LZ4_compress_fast_extState_fastReset(&current_stream_state, msp_character_map_draw, (dest + sizeof(compressed_data_header_t)), sizeof(msp_character_map_draw), LZ4_compressBound(sizeof(msp_character_map_draw)), 1);
     compressed_data_header_t *dest_header = (compressed_data_header_t *)dest;
-    dest_header->hd_options = msp_hd_option;
+    dest_header->hd_options =(uint16_t)msp_hd_option;
     dest_header->version = COMPRESSED_DATA_VERSION;
     write(compressed_fd, dest, size + sizeof(compressed_data_header_t));
     DEBUG_PRINT("COMPRESSED: Sent %d bytes!\n", size);
@@ -286,6 +335,10 @@ int main(int argc, char *argv[]) {
         compress = 1;
     }
 
+    if(get_boolean_config_value(NO_BTFL_HD_KEY)) {
+        no_btfl_hd = 1;
+    }
+
     if(fast_serial == 1) {
         printf("Configured to use 230400 baud rate. \n");
     }