diff --git a/.gitignore b/.gitignore
index 812f55d..e65298b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,4 +6,7 @@ ipk/airunit/build
 *.ipk
 repo
 obj
-libs
\ No newline at end of file
+libs
+# vim files
+*~
+*.swp
diff --git a/README.md b/README.md
index bdfbba6..2b88508 100644
--- a/README.md
+++ b/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 &
diff --git a/jni/Android.mk b/jni/Android.mk
index 1bb053e..784157d 100644
--- a/jni/Android.mk
+++ b/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)
diff --git a/jni/hw/dji_display.c b/jni/hw/dji_display.c
index 9f40d6c..5bc2668 100644
--- a/jni/hw/dji_display.c
+++ b/jni/hw/dji_display.c
@@ -1,4 +1,5 @@
 #include <stdlib.h>
+#include <string.h>
 #include "dji_display.h"
 #include "util/debug.h"
 
diff --git a/jni/hw/dji_radio_shm.c b/jni/hw/dji_radio_shm.c
index 6afbb1b..92cfe1f 100644
--- a/jni/hw/dji_radio_shm.c
+++ b/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>
 
diff --git a/jni/json/osd_config.h b/jni/json/osd_config.h
index 844be35..e6323fb 100644
--- a/jni/json/osd_config.h
+++ b/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);
\ No newline at end of file
diff --git a/jni/lz4/lz4.c b/jni/lz4/lz4.c
index 8796f78..9f9f41e 100644
--- a/jni/lz4/lz4.c
+++ b/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 */
 
diff --git a/jni/msp/msp.h b/jni/msp/msp.h
index e25b35b..33e008c 100644
--- a/jni/msp/msp.h
+++ b/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);
diff --git a/jni/msp_displayport_mux.c b/jni/msp_displayport_mux.c
index 4a71648..be80dee 100644
--- a/jni/msp_displayport_mux.c
+++ b/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,10 +417,11 @@ 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;
@@ -396,7 +429,7 @@ int main(int argc, char *argv[]) {
         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,10 +465,22 @@ 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);
@@ -443,6 +488,7 @@ int main(int argc, char *argv[]) {
     close(socket_fd);
     close(data_fd);
     close(compressed_fd);
+    close(telemetry_fd);
     if (display_driver != NULL) {
         free(display_driver);
     }
diff --git a/jni/net/network.c b/jni/net/network.c
index 96ac9bd..2a119e6 100644
--- a/jni/net/network.c
+++ b/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;
diff --git a/jni/osd_dji_overlay_udp.c b/jni/osd_dji_overlay_udp.c
index e9ef091..f29f655 100644
--- a/jni/osd_dji_overlay_udp.c
+++ b/jni/osd_dji_overlay_udp.c
@@ -34,9 +34,6 @@
 #include "rec/rec.h"
 #include "rec/rec_pb.h"
 
-#define MSP_PORT 7654
-#define DATA_PORT 7655
-#define COMPRESSED_DATA_PORT 7656
 #define DICTIONARY_VERSION 1
 
 #define WIDTH 1440
@@ -105,6 +102,7 @@ static uint16_t msp_render_character_map[MAX_DISPLAY_X][MAX_DISPLAY_Y];
 static uint16_t overlay_character_map[MAX_DISPLAY_X][MAX_DISPLAY_Y];
 static displayport_vtable_t *display_driver;
 struct timespec last_render;
+static msp_telemetry_t telemetry = {};
 
 static char current_fc_variant[5];
 
@@ -246,7 +244,41 @@ static void msp_clear_screen() {
     memset(msp_render_character_map, 0, sizeof(msp_render_character_map));
 }
 
+// TODO: Change so we change framebuffer and run after draw_screen()
+static void render_telemetry() {
+    char buf[MAX_DISPLAY_X];
+    int sz = snprintf(buf, MAX_DISPLAY_X - 1, "ALT %im", telemetry.altitude / 100);
+    for(int i = 0; i < sz && buf[i]; ++i) {
+        msp_render_character_map[0][i] = buf[i];
+    }
+    sz = snprintf(buf, MAX_DISPLAY_X - 1, "PIT %i", telemetry.pitch / 10);
+    for(int i = 0; i < sz && buf[i]; ++i) {
+        msp_render_character_map[1][i] = buf[i];
+    }
+
+    sz = snprintf(buf, MAX_DISPLAY_X - 1, "ROL %i", telemetry.roll / 10);
+    for(int i = 0; i < sz && buf[i]; ++i) {
+        msp_render_character_map[2][i] = buf[i];
+    }
+
+    sz = snprintf(buf, MAX_DISPLAY_X - 1, "YAW %i", telemetry.yaw);
+    for (int i = 0; i < sz && buf[i]; ++i) {
+        msp_render_character_map[3][i] = buf[i];
+    }
+
+    sz = snprintf(buf, MAX_DISPLAY_X - 1, "SPD %i", telemetry.speed);
+    for (int i = 0; i < sz && buf[i]; ++i) {
+        msp_render_character_map[4][i] = buf[i];
+    }
+    sz = snprintf(buf, MAX_DISPLAY_X - 1, "ALT %i", telemetry.speed);
+    for (int i = 0; i < sz && buf[i]; ++i) {
+        msp_render_character_map[5][i] = buf[i];
+    }
+}
+
 static void render_screen() {
+    // TODO: Move to after draw_screen, once we have graphics
+    render_telemetry();
     draw_screen();
     if (display_mode == DISPLAY_DISABLED) {
         clear_framebuffer();
@@ -264,7 +296,7 @@ static void msp_draw_complete() {
         if (rec_is_osd_recording() == true)
         {
             rec_write_frame(
-                fakehd_is_enabled() ? msp_render_character_map : msp_character_map,
+                fakehd_is_enabled() ? msp_render_character_map[0] : msp_character_map[0],
                 MAX_DISPLAY_X * MAX_DISPLAY_Y);
         }
     }
@@ -509,6 +541,15 @@ static void process_data_packet(uint8_t *buf, int len, dji_shm_state_t *radio_sh
     }
 }
 
+static void process_telemetry_data(void *buf, int len) {
+    if(len == sizeof(telemetry)) {
+        memcpy(&telemetry, buf, len);
+        DEBUG_PRINT("Received valid telemetry data!\n", decompressed_size);
+    } else {
+        DEBUG_PRINT("Received invalid telemetry data! (%sz != %sz)\n", decompressed_size, len, sizeof(telemetry));
+    }
+}
+
 static void process_compressed_data(void *buf, int len, void *dict, int dict_size) {
     compressed_data_header_t *header = (compressed_data_header_t*)buf;
     if (header->version != DICTIONARY_VERSION) {
@@ -533,7 +574,7 @@ static void process_compressed_data(void *buf, int len, void *dict, int dict_siz
             }
             break;
     }
-    int decompressed_size = LZ4_decompress_safe_usingDict((buf + sizeof(compressed_data_header_t)), msp_character_map, len - sizeof(compressed_data_header_t), sizeof(msp_character_map), dict, dict_size);
+    int decompressed_size = LZ4_decompress_safe_usingDict((buf + sizeof(compressed_data_header_t)), (char *)msp_character_map, len - sizeof(compressed_data_header_t), sizeof(msp_character_map), dict, dict_size);
     DEBUG_PRINT("Decompressed %d bytes!\n", decompressed_size);
     msp_draw_complete();
 }
@@ -731,9 +772,10 @@ void osd_directfb(duss_disp_instance_handle_t *disp, duss_hal_obj_handle_t ion_h
     int msp_socket_fd = bind_socket(MSP_PORT);
     int data_socket_fd = bind_socket(DATA_PORT);
     int compressed_socket_fd = bind_socket(COMPRESSED_DATA_PORT);
+    int telemetry_socket_fd = bind_socket(TELEMETRY_DATA_PORT);
     printf("*** MSP-OSD: MSP-OSD started up, listening on port %d\n", MSP_PORT);
 
-    struct pollfd poll_fds[4];
+    struct pollfd poll_fds[5];
     int recv_len = 0;
     uint8_t byte = 0;
     uint8_t buffer[8192];
@@ -763,9 +805,11 @@ void osd_directfb(duss_disp_instance_handle_t *disp, duss_hal_obj_handle_t ion_h
         poll_fds[2].events = POLLIN;
         poll_fds[3].fd = compressed_socket_fd;
         poll_fds[3].events = POLLIN;
+        poll_fds[4].fd = telemetry_socket_fd;
+        poll_fds[4].events = POLLIN;
 
         // spin every 250ms if we didn't get a packet, then check and see if we need to do the toast/overlay logic
-        poll(poll_fds, 4, 250);
+        poll(poll_fds, 5, 250);
 
         clock_gettime(CLOCK_MONOTONIC, &now);
 
@@ -803,6 +847,14 @@ void osd_directfb(duss_disp_instance_handle_t *disp, duss_hal_obj_handle_t ion_h
             }
         }
 
+        if(poll_fds[4].revents) {
+            // Got telemetry data
+            if (0 < (recv_len = recvfrom(compressed_socket_fd,&buffer,sizeof(buffer),0,(struct sockaddr*)&src_addr,&src_addr_len))) {
+                DEBUG_PRINT("got TELEMETRY data packet len %d\n", recv_len);
+                process_telemetry_data(buffer, recv_len);
+            }
+        }
+
         if(poll_fds[3].revents) {
             // Got compressed data
             if (0 < (recv_len = recvfrom(compressed_socket_fd,&buffer,sizeof(buffer),0,(struct sockaddr*)&src_addr,&src_addr_len)))
@@ -836,6 +888,7 @@ void osd_directfb(duss_disp_instance_handle_t *disp, duss_hal_obj_handle_t ion_h
     close(msp_socket_fd);
     close(data_socket_fd);
     close(compressed_socket_fd);
+    close(telemetry_socket_fd);
     close(event_fd);
     return;
 }
diff --git a/jni/util/fs_util.c b/jni/util/fs_util.c
index f1769e0..3570c77 100644
--- a/jni/util/fs_util.c
+++ b/jni/util/fs_util.c
@@ -34,7 +34,7 @@ void *open_dict(int dict_version, int *size) {
     size_t filesize = st.st_size;
     int fd = open(file_path, O_RDONLY, 0);
     if (!fd) {
-        return -1;
+        return (void *)-1;
     }
     void* dict = malloc(filesize);
     void* mmappedData = mmap(NULL, filesize, PROT_READ, MAP_PRIVATE, fd, 0);