Add msp-v2 support

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>
 
@@ -28,4 +29,4 @@ uint16_t dji_radio_latency_ms(dji_shm_state_t *shm) {
 
 uint16_t dji_radio_mbits(dji_shm_state_t *shm) {
     return shm->modem_info->channel_status;
-}
+}

jni/msp/msp.c

@@ -3,16 +3,38 @@
 #include <stdio.h>
 #include "msp.h"
 
+uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a)
+{
+    crc ^= a;
+    for (int ii = 0; ii < 8; ++ii) {
+        if (crc & 0x80) {
+            crc = (crc << 1) ^ 0xD5;
+        } else {
+            crc = crc << 1;
+        }
+    }
+    return crc;
+}
+
 uint16_t msp_data_from_msg(uint8_t message_buffer[], msp_msg_t *msg) {
     // return size
     construct_msp_command(message_buffer, msg->cmd, msg->payload, msg->size, msg->direction);
     return msg->size + 6;
 }
 
-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 construct_msp_command(uint8_t message_buffer[], uint16_t command, uint8_t payload[], uint16_t size, msp_direction_e direction) {
+    if(command < 0xF && size < 0xf)
+    {
+        return construct_msp_command_v1(message_buffer, (uint8_t)command, payload, (uint8_t)size, direction);
+    }
+
+    return construct_msp_command_v2(message_buffer, command, payload, size, direction);
+}
+
+msp_error_e construct_msp_command_v1(uint8_t message_buffer[], uint8_t command, uint8_t payload[], uint8_t size, msp_direction_e direction) {
     uint8_t checksum;
     message_buffer[0] = '$'; // Header
-    message_buffer[1] = 'M'; // MSP V1
+    message_buffer[1] = MSP_V1; // MSP V1
     if (direction == MSP_OUTBOUND) {
         message_buffer[2] = '<';
     } else {
@@ -30,6 +52,37 @@ msp_error_e construct_msp_command(uint8_t message_buffer[], uint8_t command, uin
     return 0;
 }
 
+msp_error_e construct_msp_command_v2(uint8_t message_buffer[], uint16_t command, uint8_t payload[], uint16_t size, msp_direction_e direction) {
+    uint8_t checksum;
+    int off = 0;
+    message_buffer[off++] = '$'; // 0: Header0
+    message_buffer[off++] = MSP_V2; // 1: MSP V2
+    if (direction == MSP_OUTBOUND) {
+        message_buffer[off++] = '<'; // 2: direction
+    } else {
+        message_buffer[off++] = '>'; // 2: direction
+    }
+    message_buffer[off++] = 0; // 3: flag
+    checksum = crc8_dvb_s2(0, message_buffer[off]);
+
+    message_buffer[off++] = (0xf & command); // 4: cmd low
+    checksum = crc8_dvb_s2(0, message_buffer[off]);
+    message_buffer[off++] = (0xf & (command>>8)); // 5: cmd high
+    checksum = crc8_dvb_s2(0, message_buffer[off]);
+
+    message_buffer[off++] = (0xf & size); // 6: size low
+    checksum = crc8_dvb_s2(0, message_buffer[off]);
+    message_buffer[off++] = (0xf & (size>>8)); // 7: size high
+    checksum = crc8_dvb_s2(0, message_buffer[off]);
+
+    for(uint16_t i = 0; i < size; i++) {
+        message_buffer[off+ i] = payload[i];
+        checksum = crc8_dvb_s2(0, message_buffer[off + i]);
+    }
+    message_buffer[off + size] = checksum;
+    return 0;
+}
+
 msp_error_e msp_process_data(msp_state_t *msp_state, uint8_t dat)
 {
     switch (msp_state->state)
@@ -46,18 +99,24 @@ msp_error_e msp_process_data(msp_state_t *msp_state, uint8_t dat)
             }
             break;
         case MSP_VERSION: // Look for 'M' (MSP V1, we do not support V2 at this time)
-            if (dat == 'M')
+            switch(dat)
             {
+            case MSP_V1:
                 msp_state->state = MSP_DIRECTION;
-            }
-            else
-            { // Got garbage instead, try again
+                msp_state->version = dat;
+                break;
+            case MSP_V2:
+                msp_state->state = MSP_DIRECTION;
+                msp_state->version = dat;
+                break;
+            default:
+                // Got garbage instead, try again
                 msp_state->state = MSP_IDLE;
                 return MSP_ERR_HDR;
             }
             break;
         case MSP_DIRECTION: // < for command, > for reply
-            msp_state->state = MSP_SIZE;
+            msp_state->state = msp_state->version == MSP_V1 ? MSP_SIZE : MSP_FLAG_V2; // this is where v1 diverts from v2
             switch (dat)
             {
             case '<':
@@ -123,6 +182,50 @@ msp_error_e msp_process_data(msp_state_t *msp_state, uint8_t dat)
                 return MSP_ERR_CKS;
             }
             break;
+
+            // MSP_V2
+            case MSP_FLAG_V2: // CRC
+                if(dat != 0) {
+                    msp_state->state = MSP_IDLE;
+                    return MSP_ERR_HDR;
+                }
+                msp_state->message.checksum = crc8_dvb_s2(0, dat);
+                msp_state->state = MSP_CMD_L_V2;
+                break;
+            case MSP_CMD_L_V2: // CRC
+                msp_state->message.cmd = dat;
+                msp_state->message.checksum = crc8_dvb_s2(msp_state->message.checksum, dat);
+                msp_state->state = MSP_CMD_H_V2;
+                break;
+            case MSP_CMD_H_V2: // CRC
+                msp_state->message.cmd |= (dat << 8);
+                msp_state->message.checksum = crc8_dvb_s2(msp_state->message.checksum, dat);
+                msp_state->state = MSP_SIZE_L_V2;
+                break;
+            case MSP_SIZE_L_V2: // CRC
+                msp_state->message.size = dat;
+                msp_state->message.checksum = crc8_dvb_s2(msp_state->message.checksum, dat);
+                msp_state->state = MSP_SIZE_H_V2;
+                break;
+            case MSP_SIZE_H_V2: // CRC
+                msp_state->message.size |= (dat << 8);
+                if(msp_state->message.size > MSP_V2_MAX_PAYLOAD) {
+                    msp_state->state = MSP_IDLE;
+                    return MSP_ERR_HDR;
+                }
+                msp_state->message.checksum = crc8_dvb_s2(msp_state->message.checksum, dat);
+                msp_state->state = msp_state->message.size > 0 ? MSP_PAYLOAD_V2 : MSP_CHECKSUM;
+                break;
+         case MSP_PAYLOAD_V2: // if we had a payload, keep going
+            msp_state->message.payload[msp_state->buf_ptr] = dat;
+            msp_state->message.checksum = crc8_dvb_s2(msp_state->message.checksum, dat);
+            msp_state->buf_ptr++;
+            if (msp_state->buf_ptr == msp_state->message.size)
+            {
+                msp_state->buf_ptr = 0;
+                msp_state->state = MSP_CHECKSUM;
+            }
+            break;
     }
     return MSP_ERR_NONE;
 }

jni/msp/msp.h

@@ -9,6 +9,7 @@
 #define MSP_CMD_PID_ADVANCED 94
 #define MSP_CMD_STATUS 101
 #define MSP_CMD_RC 105
+#define MSP_CMD_ATTITUDE 108
 #define MSP_CMD_ANALOG 110
 #define MSP_CMD_RC_TUNING 111
 #define MSP_CMD_PID 112
@@ -17,6 +18,9 @@
 #define MSP_CMD_DISPLAYPORT 182
 #define MSP_CMD_SET_OSD_CANVAS 188
 
+//#define MSP_V2_MAX_PAYLOAD  65535
+#define MSP_V2_MAX_PAYLOAD  8192
+
 typedef enum {
     MSP_ERR_NONE,
     MSP_ERR_HDR,
@@ -32,30 +36,44 @@ typedef enum {
     MSP_CMD,
     MSP_PAYLOAD,
     MSP_CHECKSUM,
+    MSP_FLAG_V2,
+    MSP_CMD_L_V2,
+    MSP_CMD_H_V2,
+    MSP_SIZE_L_V2,
+    MSP_SIZE_H_V2,
+    MSP_PAYLOAD_V2,
 } msp_state_machine_e;
 
 typedef enum {
     MSP_INBOUND,
     MSP_OUTBOUND
 } msp_direction_e;
 
+typedef enum {
+    MSP_V1 = 'M',
+    MSP_V2 = 'X'
+} msp_version_e;
+
 typedef struct msp_msg_s {
     uint8_t checksum;
-    uint8_t cmd;
-    uint8_t size;
+    uint16_t cmd;
+    uint16_t size;
     msp_direction_e direction;
-    uint8_t payload[256];
+    uint8_t payload[MSP_V2_MAX_PAYLOAD];
 } msp_msg_t;
 
 typedef void (*msp_msg_callback)(msp_msg_t *);
 
 typedef struct msp_state_s {
     msp_msg_callback cb;
     msp_state_machine_e state;
-    uint8_t buf_ptr;
+    uint16_t buf_ptr;
     msp_msg_t message;
+    msp_version_e version;
 } msp_state_t;
 
 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 construct_msp_command(uint8_t message_buffer[], uint16_t command, uint8_t payload[], uint16_t size, msp_direction_e direction);
+msp_error_e construct_msp_command_v1(uint8_t message_buffer[], uint8_t command, uint8_t payload[], uint8_t size, msp_direction_e direction);
+msp_error_e construct_msp_command_v2(uint8_t message_buffer[], uint16_t command, uint8_t payload[], uint16_t size, msp_direction_e direction);
 msp_error_e msp_process_data(msp_state_t *msp_state, uint8_t dat);

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);