zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_MSP: run new MSP code through code style script

c415-sdk
Andrew Tridgell 4 years ago
parent
d8ed210b98
commit
f330d5aa77
3 changed files with 207 additions and 207 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      libraries/AP_MSP/AP_MSP.h
  2. 130
      libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
  3. 282
      libraries/AP_MSP/msp.cpp

2
libraries/AP_MSP/AP_MSP.h
Unescape View File

@ -57,7 +57,7 @@ public: @@ -57,7 +57,7 @@ public:
private:
enum msp_option_e : uint8_t{
enum msp_option_e : uint8_t {
OPTION_TELEMETRY_MODE = 1U<<0,
};

130
libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
Unescape View File

@ -102,23 +102,23 @@ void AP_MSP_Telem_Backend::process_outgoing_data() @@ -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) @@ -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 @@ -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 @@ -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;

282
libraries/AP_MSP/msp.cpp
Unescape View File

@ -144,168 +144,168 @@ uint32_t MSP::msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_versi @@ -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;
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:
msp->c_state = MSP_IDLE;
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 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;
}
case 'X':
msp->c_state = MSP_HEADER_X;
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;
}
default:
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;
}
}
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_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->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;
}
} 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
}
break;
}
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_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 {
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;
}
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;
case MSP_PAYLOAD_V2_OVER_V1:
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
msp->checksum1 ^= c;
msp->in_buf[msp->offset++] = c;
// 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;
if (msp->offset == msp->data_size) {
msp->c_state = 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;
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;
if (msp->offset == msp->data_size) {
msp->c_state = MSP_CHECKSUM_V2_OVER_V1;
}
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];
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 {
// 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_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;
}
break;
case MSP_PAYLOAD_V2_NATIVE:
msp->checksum2 = crc8_dvb_s2(msp->checksum2, 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_NATIVE;
}
break;
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;
}

Write Preview
Loading…
Cancel
Save