mirror of https://github.com/ArduPilot/ardupilot
AP_MSP: support AP_Periph MSP send
This commit is contained in:
parent
9c7568eb5f
commit
5f5c7735a3
|
@ -310,6 +310,7 @@ void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wi
|
|||
MANU [S]
|
||||
MANU [SS]
|
||||
*/
|
||||
#ifndef HAL_NO_GCS
|
||||
const bool simple_mode = gcs().simple_input_active();
|
||||
const bool supersimple_mode = gcs().supersimple_input_active();
|
||||
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
|
||||
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);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
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;
|
||||
/*
|
||||
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 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];
|
||||
uint32_t hdr_len = 3;
|
||||
uint32_t crc_len = 0;
|
||||
|
|
|
@ -130,7 +130,7 @@ typedef enum : uint8_t {
|
|||
|
||||
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_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);
|
||||
}
|
||||
|
||||
|
|
|
@ -342,3 +342,9 @@
|
|||
#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_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
|
||||
|
|
Loading…
Reference in New Issue