[WIP] MSP Telemetry based widgets

.gitignore

@@ -6,4 +6,7 @@ ipk/airunit/build
 *.ipk
 repo
 obj
-libs
+libs
+# vim files
+*~
+*.swp

README.md

@@ -386,7 +386,7 @@ adb shell LD_PRELOAD=/tmp/libdisplayport_osd_shim.so dji_gls_wm150_original -g
 
 ```
 ndk-build
-adb push msp_displayport_mux /blackbox
+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 &

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
@@ -36,6 +37,8 @@ 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 \
@@ -48,5 +51,6 @@ LOCAL_SRC_FILES:= \
 	util/fs_util.c \
 	lz4/lz4.c
 LOCAL_MODULE := msp_displayport_mux
+
 include $(BUILD_EXECUTABLE)
 include $(CLEAR_VARS)

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,5 +1,6 @@
 #include <assert.h>
 #include <stdio.h>
+#include <unistd.h>
 #include <fcntl.h>
 #include <sys/mman.h>
 

jni/json/osd_config.h

@@ -1,3 +1,5 @@
+#pragma once
+
 int get_boolean_config_value(const char* key);
 const char *get_string_config_value(const char *key);
 int get_integer_config_value(const char *key);

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,20 @@
 #define MSP_CMD_DISPLAYPORT 182
 #define MSP_CMD_SET_OSD_CANVAS 188
 
+// Telemetry commands
+#define MSP_CMD_RAW_GPS  106
+#define MSP_CMD_ATTITUDE 108
+#define MSP_CMD_ALTITUDE 109
+
+// The MSP_PORT is used to send MSP passthrough messages.
+#define MSP_PORT 7654
+// The DATA_PORT is used to send arbitrary data - for example, bitrate and temperature data.
+#define DATA_PORT 7655
+// COMPRESSED_DATA_PORT is used to send MSP DisplayPort character framebuffer
+#define COMPRESSED_DATA_PORT 7656
+// The TELEMETRY_DATA_PORT is used to send telemetry state
+#define TELEMETRY_DATA_PORT 7657
+
 typedef enum {
     MSP_ERR_NONE,
     MSP_ERR_HDR,
@@ -49,11 +63,20 @@ typedef struct msp_msg_s {
 
 typedef void (*msp_msg_callback)(msp_msg_t *);
 
+typedef struct msp_telemetry_s {
+    uint32_t altitude; // cm
+    uint16_t speed; // 
+    uint16_t roll; // 0.1 degrees
+    uint16_t pitch; // 0.1 degrees
+    uint16_t yaw; // degrees
+} msp_telemetry_t;
+
 typedef struct msp_state_s {
     msp_msg_callback cb;
     msp_state_machine_e state;
     uint8_t buf_ptr;
     msp_msg_t message;
+    msp_telemetry_t telemetry;
 } msp_state_t;
 
 uint16_t msp_data_from_msg(uint8_t message_buffer[], msp_msg_t *msg);

jni/msp_displayport_mux.c

@@ -26,13 +26,6 @@
 #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.
-
-#define MSP_PORT 7654
-#define DATA_PORT 7655
-#define COMPRESSED_DATA_PORT 7656
-
 #define COMPRESSED_DATA_VERSION 1
 
 enum {
@@ -55,6 +48,10 @@ static uint32_t fb_cursor = 0;
 
 static uint8_t message_buffer[256]; // only needs to be the maximum size of an MSP packet, we only care to fwd MSP
 static char current_fc_identifier[4];
+/* For telemetry handling */
+
+static msp_state_t *rx_msp_state = NULL;
+static msp_state_t *tx_msp_state = NULL;
 
 /* For compressed full-frame transmission */
 static uint16_t msp_character_map_buffer[MAX_DISPLAY_X][MAX_DISPLAY_Y];
@@ -68,6 +65,8 @@ int pty_fd;
 int serial_fd;
 int socket_fd;
 int compressed_fd;
+int telemetry_fd;
+int data_fd;
 
 static volatile sig_atomic_t quit = 0;
 static uint8_t serial_passthrough = 1;
@@ -177,7 +176,7 @@ static void rx_msp_callback(msp_msg_t *msp_message)
         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");
-            if(strncmp(current_fc_identifier, msp_message->payload, 4) != 0) {
+            if(strncmp(current_fc_identifier, (const char *)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);
@@ -208,12 +207,36 @@ static void rx_msp_callback(msp_msg_t *msp_message)
             }
             break;
         }
+        case MSP_CMD_ALTITUDE:
+            {
+                uint32_t *msp_altitude = (uint32_t *)msp_message->payload;
+                rx_msp_state->telemetry.altitude =  *msp_altitude;
+            }
+            goto passthrough;
+        case MSP_CMD_ATTITUDE:
+            {
+                uint16_t *angles = (uint16_t *)msp_message->payload;
+                rx_msp_state->telemetry.roll = angles[0];
+                rx_msp_state->telemetry.pitch = angles[1];
+                rx_msp_state->telemetry.yaw = angles[2];
+            }
+            goto passthrough;
+        case MSP_CMD_RAW_GPS:
+            {
+                uint16_t *speed = (uint16_t *)(msp_message->payload + 12);
+                rx_msp_state->telemetry.speed =  *speed;
+            }
+            goto passthrough;
         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);
+            passthrough:
+            {
+                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;
         }
@@ -275,12 +298,17 @@ static void msp_set_options(uint8_t font_num, msp_hd_options_e is_hd) {
    msp_hd_option = is_hd;
 }
 
+static void send_telemetry_data(int telmetry_fd) {
+    int size = write(telmetry_fd, &rx_msp_state->telemetry, sizeof(msp_telemetry_t));
+    DEBUG_PRINT("TELEMETRY: Sent %d bytes!\n", size);
+}
+
 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);
+    int size = LZ4_compress_fast_extState_fastReset(&current_stream_state, (const char *)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 =(uint16_t)msp_hd_option;
     dest_header->version = COMPRESSED_DATA_VERSION;
@@ -293,6 +321,9 @@ int main(int argc, char *argv[]) {
     memset(msp_character_map_buffer, 0, sizeof(msp_character_map_buffer));
     memset(msp_character_map_draw, 0, sizeof(msp_character_map_draw));
 
+    uint16_t telemetry_commands[] = { MSP_CMD_ALTITUDE, MSP_CMD_ATTITUDE, MSP_CMD_RAW_GPS};
+    uint8_t current_telemetry_item = 0;
+
     int compression_dict_size = 0;
     void *compression_dict = open_dict(COMPRESSED_DATA_VERSION, &compression_dict_size);
 
@@ -351,8 +382,8 @@ int main(int argc, char *argv[]) {
     signal(SIGINT, sig_handler);
     struct pollfd poll_fds[2];
     const char *pty_name_ptr;
-    msp_state_t *rx_msp_state = calloc(1, sizeof(msp_state_t));
-    msp_state_t *tx_msp_state = calloc(1, sizeof(msp_state_t));
+    rx_msp_state = calloc(1, sizeof(msp_state_t));
+    tx_msp_state = calloc(1, sizeof(msp_state_t));
     rx_msp_state->cb = &rx_msp_callback;
     tx_msp_state->cb = &tx_msp_callback;
     serial_fd = open_serial_port(serial_port, fast_serial ? B230400 : B115200);
@@ -369,7 +400,8 @@ int main(int argc, char *argv[]) {
     }
     socket_fd = connect_to_server(ip_address, MSP_PORT);
     compressed_fd = connect_to_server(ip_address, COMPRESSED_DATA_PORT);
-    int data_fd = connect_to_server(ip_address, DATA_PORT);
+    telemetry_fd = connect_to_server(ip_address, TELEMETRY_DATA_PORT);
+    data_fd = connect_to_server(ip_address, DATA_PORT);
 
     if (compress) {
         update_rate_hz = get_integer_config_value(UPDATE_RATE_KEY);
@@ -385,18 +417,19 @@ int main(int argc, char *argv[]) {
 
     uint8_t serial_data[256];
     ssize_t serial_data_size;
-    struct timespec now, last_data, last_frame;
+    struct timespec now, last_data, last_frame, last_telemetry;;
     clock_gettime(CLOCK_MONOTONIC, &now);
     clock_gettime(CLOCK_MONOTONIC, &last_data);
     clock_gettime(CLOCK_MONOTONIC, &last_frame);
+    clock_gettime(CLOCK_MONOTONIC, &last_telemetry);
 
     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));
+        poll(poll_fds, 2, ((MSEC_PER_SEC / (update_rate_hz * 3))));
 
         // We got inbound serial data, process it as MSP data.
         if (0 < (serial_data_size = read(serial_fd, serial_data, sizeof(serial_data)))) {
@@ -432,17 +465,30 @@ int main(int argc, char *argv[]) {
                 send_variant_request(serial_fd);
             }
         }
-        if(compress && (timespec_subtract_ns(&now, &last_frame) > (NSEC_PER_SEC / update_rate_hz))) {
-            send_compressed_screen(compressed_fd);
+        if((timespec_subtract_ns(&now, &last_frame) > (NSEC_PER_SEC / update_rate_hz))) {
+            send_telemetry_data(telemetry_fd);
+            if(compress) {
+                send_compressed_screen(compressed_fd);
+            }
             clock_gettime(CLOCK_MONOTONIC, &last_frame);
         }
+
+        if(timespec_subtract_ns(&now, &last_telemetry) > NSEC_PER_SEC / (update_rate_hz * 3))
+        {
+            uint8_t buffer[6] = "";
+            clock_gettime(CLOCK_MONOTONIC, &last_data);
+            construct_msp_command(buffer, telemetry_commands[current_telemetry_item++], NULL, 0, MSP_OUTBOUND);
+            write(serial_fd, buffer, 6);
+            current_telemetry_item = current_telemetry_item % 3;
+        }
     }
     close_dji_radio_shm(&dji_radio);
     close(serial_fd);
     close(pty_fd);
     close(socket_fd);
     close(data_fd);
     close(compressed_fd);
+    close(telemetry_fd);
     if (display_driver != NULL) {
         free(display_driver);
     }

jni/net/network.c

@@ -27,7 +27,7 @@ int connect_to_server(char *address, int port)
     servaddr.sin_addr.s_addr = inet_addr(address);
     servaddr.sin_port = htons(port);
 
-    if (connect(sockfd, &servaddr, sizeof(servaddr)) != 0)
+    if (connect(sockfd, (const struct sockaddr *)&servaddr, sizeof(servaddr)) != 0)
     {
         printf("connection failed!\n");
         return -1;