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 +#include #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..03ef2dd 100644 --- a/jni/hw/dji_radio_shm.c +++ b/jni/hw/dji_radio_shm.c @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -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; -} \ No newline at end of file +} diff --git a/jni/msp/msp.c b/jni/msp/msp.c index 7b4ccc9..c427ded 100644 --- a/jni/msp/msp.c +++ b/jni/msp/msp.c @@ -3,16 +3,38 @@ #include #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; } \ No newline at end of file diff --git a/jni/msp/msp.h b/jni/msp/msp.h index e25b35b..81b45ee 100644 --- a/jni/msp/msp.h +++ b/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,6 +36,12 @@ 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 { @@ -39,12 +49,17 @@ typedef enum { 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 *); @@ -52,10 +67,13 @@ 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); \ No newline at end of file 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);