mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-26 01:33:56 -04:00
AP_MSP: run new MSP code through code style script
This commit is contained in:
Andrew Tridgell
parent
d8ed210b98
commit
f330d5aa77
@ -57,7 +57,7 @@ public:
|
||||
|
||||
private:
|
||||
|
||||
enum msp_option_e : uint8_t{
|
||||
enum msp_option_e : uint8_t {
|
||||
OPTION_TELEMETRY_MODE = 1U<<0,
|
||||
};
|
||||
|
||||
|
||||
@ -102,23 +102,23 @@ void AP_MSP_Telem_Backend::process_outgoing_data()
|
||||
bool AP_MSP_Telem_Backend::is_packet_ready(uint8_t idx, bool queue_empty)
|
||||
{
|
||||
switch (idx) {
|
||||
case EMPTY_SLOT: // empty slot
|
||||
case NAME: // used for status_text messages
|
||||
case STATUS: // flightmode
|
||||
case CONFIG: // OSD config
|
||||
case RAW_GPS: // lat,lon, speed
|
||||
case COMP_GPS: // home dir,dist
|
||||
case ATTITUDE: // Attitude
|
||||
case ALTITUDE: // Altitude and Vario
|
||||
case ANALOG: // Rssi, Battery, mAh, Current
|
||||
case BATTERY_STATE: // voltage, capacity, current, mAh
|
||||
case EMPTY_SLOT: // empty slot
|
||||
case NAME: // used for status_text messages
|
||||
case STATUS: // flightmode
|
||||
case CONFIG: // OSD config
|
||||
case RAW_GPS: // lat,lon, speed
|
||||
case COMP_GPS: // home dir,dist
|
||||
case ATTITUDE: // Attitude
|
||||
case ALTITUDE: // Altitude and Vario
|
||||
case ANALOG: // Rssi, Battery, mAh, Current
|
||||
case BATTERY_STATE: // voltage, capacity, current, mAh
|
||||
#ifdef HAVE_AP_BLHELI_SUPPORT
|
||||
case ESC_SENSOR_DATA: // esc temp + rpm
|
||||
case ESC_SENSOR_DATA: // esc temp + rpm
|
||||
#endif
|
||||
case RTC_DATETIME: // RTC
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
case RTC_DATETIME: // RTC
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -251,7 +251,7 @@ void AP_MSP_Telem_Backend::update_airspeed(airspeed_state_t &airspeed_state)
|
||||
|
||||
/*
|
||||
MSP OSDs can display up to MSP_TXT_VISIBLE_CHARS chars (UTF8 characters are supported)
|
||||
We display the flight mode string either with or without wind state
|
||||
We display the flight mode string either with or without wind state
|
||||
*/
|
||||
void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wind_enabled)
|
||||
{
|
||||
@ -416,47 +416,47 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_command(msp_packet_t *cmd, ms
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_command(uint16_t cmd_msp, sbuf_t *dst)
|
||||
{
|
||||
switch (cmd_msp) {
|
||||
case MSP_API_VERSION:
|
||||
return msp_process_out_api_version(dst);
|
||||
case MSP_FC_VARIANT:
|
||||
return msp_process_out_fc_variant(dst);
|
||||
case MSP_FC_VERSION:
|
||||
return msp_process_out_fc_version(dst);
|
||||
case MSP_BOARD_INFO:
|
||||
return msp_process_out_board_info(dst);
|
||||
case MSP_BUILD_INFO:
|
||||
return msp_process_out_build_info(dst);
|
||||
case MSP_NAME:
|
||||
return msp_process_out_name(dst);
|
||||
case MSP_OSD_CONFIG:
|
||||
return msp_process_out_osd_config(dst);
|
||||
case MSP_STATUS:
|
||||
case MSP_STATUS_EX:
|
||||
return msp_process_out_status(dst);
|
||||
case MSP_RAW_GPS:
|
||||
return msp_process_out_raw_gps(dst);
|
||||
case MSP_COMP_GPS:
|
||||
return msp_process_out_comp_gps(dst);
|
||||
case MSP_ATTITUDE:
|
||||
return msp_process_out_attitude(dst);
|
||||
case MSP_ALTITUDE:
|
||||
return msp_process_out_altitude(dst);
|
||||
case MSP_ANALOG:
|
||||
return msp_process_out_analog(dst);
|
||||
case MSP_BATTERY_STATE:
|
||||
return msp_process_out_battery_state(dst);
|
||||
case MSP_UID:
|
||||
return msp_process_out_uid(dst);
|
||||
case MSP_API_VERSION:
|
||||
return msp_process_out_api_version(dst);
|
||||
case MSP_FC_VARIANT:
|
||||
return msp_process_out_fc_variant(dst);
|
||||
case MSP_FC_VERSION:
|
||||
return msp_process_out_fc_version(dst);
|
||||
case MSP_BOARD_INFO:
|
||||
return msp_process_out_board_info(dst);
|
||||
case MSP_BUILD_INFO:
|
||||
return msp_process_out_build_info(dst);
|
||||
case MSP_NAME:
|
||||
return msp_process_out_name(dst);
|
||||
case MSP_OSD_CONFIG:
|
||||
return msp_process_out_osd_config(dst);
|
||||
case MSP_STATUS:
|
||||
case MSP_STATUS_EX:
|
||||
return msp_process_out_status(dst);
|
||||
case MSP_RAW_GPS:
|
||||
return msp_process_out_raw_gps(dst);
|
||||
case MSP_COMP_GPS:
|
||||
return msp_process_out_comp_gps(dst);
|
||||
case MSP_ATTITUDE:
|
||||
return msp_process_out_attitude(dst);
|
||||
case MSP_ALTITUDE:
|
||||
return msp_process_out_altitude(dst);
|
||||
case MSP_ANALOG:
|
||||
return msp_process_out_analog(dst);
|
||||
case MSP_BATTERY_STATE:
|
||||
return msp_process_out_battery_state(dst);
|
||||
case MSP_UID:
|
||||
return msp_process_out_uid(dst);
|
||||
#ifdef HAVE_AP_BLHELI_SUPPORT
|
||||
case MSP_ESC_SENSOR_DATA:
|
||||
return msp_process_out_esc_sensor_data(dst);
|
||||
case MSP_ESC_SENSOR_DATA:
|
||||
return msp_process_out_esc_sensor_data(dst);
|
||||
#endif
|
||||
case MSP_RTC:
|
||||
return msp_process_out_rtc(dst);
|
||||
case MSP_RC:
|
||||
return msp_process_out_rc(dst);
|
||||
default:
|
||||
return MSP_RESULT_ERROR;
|
||||
case MSP_RTC:
|
||||
return msp_process_out_rtc(dst);
|
||||
case MSP_RC:
|
||||
return msp_process_out_rc(dst);
|
||||
default:
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
@ -465,16 +465,16 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_sensor_command(uint16_t cmd_m
|
||||
MSP_UNUSED(src);
|
||||
|
||||
switch (cmd_msp) {
|
||||
case MSP2_SENSOR_RANGEFINDER: {
|
||||
const MSP::msp_rangefinder_sensor_t pkt = *(const MSP::msp_rangefinder_sensor_t *)src->ptr;
|
||||
msp_handle_rangefinder(pkt);
|
||||
}
|
||||
break;
|
||||
case MSP2_SENSOR_OPTIC_FLOW: {
|
||||
const MSP::msp_opflow_sensor_t pkt = *(const MSP::msp_opflow_sensor_t *)src->ptr;
|
||||
msp_handle_opflow(pkt);
|
||||
}
|
||||
break;
|
||||
case MSP2_SENSOR_RANGEFINDER: {
|
||||
const MSP::msp_rangefinder_sensor_t pkt = *(const MSP::msp_rangefinder_sensor_t *)src->ptr;
|
||||
msp_handle_rangefinder(pkt);
|
||||
}
|
||||
break;
|
||||
case MSP2_SENSOR_OPTIC_FLOW: {
|
||||
const MSP::msp_opflow_sensor_t pkt = *(const MSP::msp_opflow_sensor_t *)src->ptr;
|
||||
msp_handle_opflow(pkt);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return MSP_RESULT_NO_REPLY;
|
||||
|
||||
@ -144,168 +144,168 @@ uint32_t MSP::msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_versi
|
||||
bool MSP::msp_parse_received_data(msp_port_t *msp, uint8_t c)
|
||||
{
|
||||
switch (msp->c_state) {
|
||||
default:
|
||||
case MSP_IDLE: // Waiting for '$' character
|
||||
if (c == '$') {
|
||||
msp->msp_version = MSP_V1;
|
||||
msp->c_state = MSP_HEADER_START;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_START: // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
|
||||
switch (c) {
|
||||
case 'M':
|
||||
msp->c_state = MSP_HEADER_M;
|
||||
break;
|
||||
case 'X':
|
||||
msp->c_state = MSP_HEADER_X;
|
||||
break;
|
||||
default:
|
||||
case MSP_IDLE: // Waiting for '$' character
|
||||
if (c == '$') {
|
||||
msp->msp_version = MSP_V1;
|
||||
msp->c_state = MSP_HEADER_START;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
msp->c_state = MSP_IDLE;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_START: // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
|
||||
switch (c) {
|
||||
case 'M':
|
||||
msp->c_state = MSP_HEADER_M;
|
||||
break;
|
||||
case 'X':
|
||||
msp->c_state = MSP_HEADER_X;
|
||||
break;
|
||||
default:
|
||||
msp->c_state = MSP_IDLE;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case MSP_HEADER_M: // Waiting for '<'
|
||||
if (c == '<') {
|
||||
msp->offset = 0;
|
||||
msp->checksum1 = 0;
|
||||
msp->checksum2 = 0;
|
||||
msp->c_state = MSP_HEADER_V1;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_M: // Waiting for '<'
|
||||
if (c == '<') {
|
||||
msp->offset = 0;
|
||||
msp->checksum1 = 0;
|
||||
msp->checksum2 = 0;
|
||||
msp->c_state = MSP_HEADER_V1;
|
||||
} else {
|
||||
case MSP_HEADER_X:
|
||||
if (c == '<') {
|
||||
msp->offset = 0;
|
||||
msp->checksum2 = 0;
|
||||
msp->msp_version = MSP_V2_NATIVE;
|
||||
msp->c_state = MSP_HEADER_V2_NATIVE;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_V1: // Now receive v1 header (size/cmd), this is already checksummable
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum1 ^= c;
|
||||
if (msp->offset == sizeof(msp_header_v1_t)) {
|
||||
msp_header_v1_t * hdr = (msp_header_v1_t *)&msp->in_buf[0];
|
||||
// Check incoming buffer size limit
|
||||
if (hdr->size > MSP_PORT_INBUF_SIZE) {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_X:
|
||||
if (c == '<') {
|
||||
msp->offset = 0;
|
||||
msp->checksum2 = 0;
|
||||
msp->msp_version = MSP_V2_NATIVE;
|
||||
msp->c_state = MSP_HEADER_V2_NATIVE;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_V1: // Now receive v1 header (size/cmd), this is already checksummable
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum1 ^= c;
|
||||
if (msp->offset == sizeof(msp_header_v1_t)) {
|
||||
msp_header_v1_t * hdr = (msp_header_v1_t *)&msp->in_buf[0];
|
||||
// Check incoming buffer size limit
|
||||
if (hdr->size > MSP_PORT_INBUF_SIZE) {
|
||||
msp->c_state = MSP_IDLE;
|
||||
} else if (hdr->cmd == MSP_V2_FRAME_ID) {
|
||||
// MSPv1 payload must be big enough to hold V2 header + extra checksum
|
||||
if (hdr->size >= sizeof(msp_header_v2_t) + 1) {
|
||||
msp->msp_version = MSP_V2_OVER_V1;
|
||||
msp->c_state = MSP_HEADER_V2_OVER_V1;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
} else if (hdr->cmd == MSP_V2_FRAME_ID) {
|
||||
// MSPv1 payload must be big enough to hold V2 header + extra checksum
|
||||
if (hdr->size >= sizeof(msp_header_v2_t) + 1) {
|
||||
msp->msp_version = MSP_V2_OVER_V1;
|
||||
msp->c_state = MSP_HEADER_V2_OVER_V1;
|
||||
} else {
|
||||
msp->data_size = hdr->size;
|
||||
msp->cmd_msp = hdr->cmd;
|
||||
msp->cmd_flags = 0;
|
||||
msp->offset = 0; // re-use buffer
|
||||
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1; // If no payload - jump to checksum byte
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_PAYLOAD_V1:
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum1 ^= c;
|
||||
if (msp->offset == msp->data_size) {
|
||||
msp->c_state = MSP_CHECKSUM_V1;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CHECKSUM_V1:
|
||||
if (msp->checksum1 == c) {
|
||||
msp->c_state = MSP_COMMAND_RECEIVED;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
msp->data_size = hdr->size;
|
||||
msp->cmd_msp = hdr->cmd;
|
||||
msp->cmd_flags = 0;
|
||||
msp->offset = 0; // re-use buffer
|
||||
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1; // If no payload - jump to checksum byte
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_V2_OVER_V1: // V2 header is part of V1 payload - we need to calculate both checksums now
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum1 ^= c;
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
if (msp->offset == (sizeof(msp_header_v2_t) + sizeof(msp_header_v1_t))) {
|
||||
msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[sizeof(msp_header_v1_t)];
|
||||
case MSP_PAYLOAD_V1:
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum1 ^= c;
|
||||
if (msp->offset == msp->data_size) {
|
||||
msp->c_state = MSP_CHECKSUM_V1;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CHECKSUM_V1:
|
||||
if (msp->checksum1 == c) {
|
||||
msp->c_state = MSP_COMMAND_RECEIVED;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_V2_OVER_V1: // V2 header is part of V1 payload - we need to calculate both checksums now
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum1 ^= c;
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
if (msp->offset == (sizeof(msp_header_v2_t) + sizeof(msp_header_v1_t))) {
|
||||
msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[sizeof(msp_header_v1_t)];
|
||||
msp->data_size = hdrv2->size;
|
||||
|
||||
// Check for potential buffer overflow
|
||||
if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
|
||||
msp->c_state = MSP_IDLE;
|
||||
} else {
|
||||
msp->cmd_msp = hdrv2->cmd;
|
||||
msp->cmd_flags = hdrv2->flags;
|
||||
msp->offset = 0; // re-use buffer
|
||||
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_PAYLOAD_V2_OVER_V1:
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
msp->checksum1 ^= c;
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
|
||||
if (msp->offset == msp->data_size) {
|
||||
msp->c_state = MSP_CHECKSUM_V2_OVER_V1;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CHECKSUM_V2_OVER_V1:
|
||||
msp->checksum1 ^= c;
|
||||
if (msp->checksum2 == c) {
|
||||
msp->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_V2_NATIVE:
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
if (msp->offset == sizeof(msp_header_v2_t)) {
|
||||
msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[0];
|
||||
|
||||
// Check for potential buffer overflow
|
||||
if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
|
||||
msp->c_state = MSP_IDLE;
|
||||
} else {
|
||||
msp->data_size = hdrv2->size;
|
||||
|
||||
// Check for potential buffer overflow
|
||||
if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
|
||||
msp->c_state = MSP_IDLE;
|
||||
} else {
|
||||
msp->cmd_msp = hdrv2->cmd;
|
||||
msp->cmd_flags = hdrv2->flags;
|
||||
msp->offset = 0; // re-use buffer
|
||||
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
|
||||
}
|
||||
msp->cmd_msp = hdrv2->cmd;
|
||||
msp->cmd_flags = hdrv2->flags;
|
||||
msp->offset = 0; // re-use buffer
|
||||
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_PAYLOAD_V2_OVER_V1:
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
msp->checksum1 ^= c;
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
case MSP_PAYLOAD_V2_NATIVE:
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
|
||||
if (msp->offset == msp->data_size) {
|
||||
msp->c_state = MSP_CHECKSUM_V2_OVER_V1;
|
||||
}
|
||||
break;
|
||||
if (msp->offset == msp->data_size) {
|
||||
msp->c_state = MSP_CHECKSUM_V2_NATIVE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CHECKSUM_V2_OVER_V1:
|
||||
msp->checksum1 ^= c;
|
||||
if (msp->checksum2 == c) {
|
||||
msp->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_V2_NATIVE:
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
if (msp->offset == sizeof(msp_header_v2_t)) {
|
||||
msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[0];
|
||||
|
||||
// Check for potential buffer overflow
|
||||
if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
|
||||
msp->c_state = MSP_IDLE;
|
||||
} else {
|
||||
msp->data_size = hdrv2->size;
|
||||
msp->cmd_msp = hdrv2->cmd;
|
||||
msp->cmd_flags = hdrv2->flags;
|
||||
msp->offset = 0; // re-use buffer
|
||||
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_PAYLOAD_V2_NATIVE:
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
|
||||
if (msp->offset == msp->data_size) {
|
||||
msp->c_state = MSP_CHECKSUM_V2_NATIVE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CHECKSUM_V2_NATIVE:
|
||||
if (msp->checksum2 == c) {
|
||||
msp->c_state = MSP_COMMAND_RECEIVED;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
case MSP_CHECKSUM_V2_NATIVE:
|
||||
if (msp->checksum2 == c) {
|
||||
msp->c_state = MSP_COMMAND_RECEIVED;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user