diff --git a/libraries/AP_MSP/AP_MSP.h b/libraries/AP_MSP/AP_MSP.h index 85422ccaf0..7f39b91963 100644 --- a/libraries/AP_MSP/AP_MSP.h +++ b/libraries/AP_MSP/AP_MSP.h @@ -57,7 +57,7 @@ public: private: - enum msp_option_e : uint8_t{ + enum msp_option_e : uint8_t { OPTION_TELEMETRY_MODE = 1U<<0, }; diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index 2e5a3915cb..b0edc2ad58 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -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; diff --git a/libraries/AP_MSP/msp.cpp b/libraries/AP_MSP/msp.cpp index 800fda9838..a5c231d692 100644 --- a/libraries/AP_MSP/msp.cpp +++ b/libraries/AP_MSP/msp.cpp @@ -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; }