Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_MSP: support AP_Periph MSP send

This commit is contained in:
Andrew Tridgell 2020-09-03 21:48:21 +10:00
parent 9c7568eb5f
commit 5f5c7735a3
4 changed files with 21 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
View File

@ -310,6 +310,7 @@ void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wi
MANU [S] MANU [S]
MANU [SS] MANU [SS]
*/ */
#ifndef HAL_NO_GCS
const bool simple_mode = gcs().simple_input_active(); const bool simple_mode = gcs().simple_input_active();
const bool supersimple_mode = gcs().supersimple_input_active(); const bool supersimple_mode = gcs().supersimple_input_active();
const char* simple_mode_str = simple_mode ? " [S]" : (supersimple_mode ? " [SS]" : ""); const char* simple_mode_str = simple_mode ? " [S]" : (supersimple_mode ? " [SS]" : "");
@ -320,6 +321,7 @@ void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wi
// left pad // left pad
uint8_t left_padded_len = MSP_TXT_VISIBLE_CHARS - (MSP_TXT_VISIBLE_CHARS - used)/2; uint8_t left_padded_len = MSP_TXT_VISIBLE_CHARS - (MSP_TXT_VISIBLE_CHARS - used)/2;
snprintf(flight_mode_str, MSP_TXT_BUFFER_SIZE, "%*s", left_padded_len, buffer); snprintf(flight_mode_str, MSP_TXT_BUFFER_SIZE, "%*s", left_padded_len, buffer);
#endif
} }
} }

12
libraries/AP_MSP/msp.cpp
View File

@ -48,14 +48,22 @@ uint32_t MSP::msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, uint32
/* /*
ported from betaflight/src/main/msp/msp_serial.c ported from betaflight/src/main/msp/msp_serial.c
*/ */
uint32_t MSP::msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version) uint32_t MSP::msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version, bool is_request)
{ {
static const uint8_t msp_magic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER; static const uint8_t msp_magic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER;
/* /*
Note: after calling sbuf_switch_to_reader() sbuf_bytes_remaining() returns the size of the packet Note: after calling sbuf_switch_to_reader() sbuf_bytes_remaining() returns the size of the packet
*/ */
const uint16_t data_len = sbuf_bytes_remaining(&packet->buf); const uint16_t data_len = sbuf_bytes_remaining(&packet->buf);
const uint8_t hdr_buf[16] = { '$', msp_magic[msp_version], static_cast<uint8_t>(packet->result == MSP_RESULT_ERROR ? '!' : '>')}; uint8_t code;
if (is_request) {
code = '<';
} else if (packet->result == MSP_RESULT_ERROR) {
code = '!';
} else {
code = '>';
}
const uint8_t hdr_buf[16] = { '$', msp_magic[msp_version], code };
uint8_t crc_buf[2]; uint8_t crc_buf[2];
uint32_t hdr_len = 3; uint32_t hdr_len = 3;
uint32_t crc_len = 0; uint32_t crc_len = 0;

2
libraries/AP_MSP/msp.h
View File

@ -130,7 +130,7 @@ typedef enum : uint8_t {
uint8_t msp_serial_checksum_buf(uint8_t checksum, const uint8_t *data, uint32_t len); uint8_t msp_serial_checksum_buf(uint8_t checksum, const uint8_t *data, uint32_t len);
uint32_t msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, uint32_t hdr_len, const uint8_t * data, uint32_t data_len, const uint8_t * crc, uint32_t crc_len); uint32_t msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, uint32_t hdr_len, const uint8_t * data, uint32_t data_len, const uint8_t * crc, uint32_t crc_len);
uint32_t msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version); uint32_t msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version, bool is_request=false);
bool msp_parse_received_data(msp_port_t *msp, uint8_t c); bool msp_parse_received_data(msp_port_t *msp, uint8_t c);
} }

6
libraries/AP_MSP/msp_protocol.h
View File

@ -342,3 +342,9 @@
#define MSP_RTC 247 //out message Gets the RTC clock #define MSP_RTC 247 //out message Gets the RTC clock
#define MSP_SET_BOARD_INFO 248 //in message Sets the board information for this board #define MSP_SET_BOARD_INFO 248 //in message Sets the board information for this board
#define MSP_SET_SIGNATURE 249 //in message Sets the signature of the board and serial number #define MSP_SET_SIGNATURE 249 //in message Sets the signature of the board and serial number
// V2 commands
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
#define MSP2_SENSOR_GPS 0x1F03
#define MSP2_SENSOR_COMPASS 0x1F04
#define MSP2_SENSOR_BAROMETER 0x1F05