From ceba245d4659fbd5a63791ef4a05d023b271522f Mon Sep 17 00:00:00 2001 From: tk Date: Mon, 19 Aug 2024 14:34:12 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E7=B1=BB=20=20=E5=9E=8B=E3=80=91?= =?UTF-8?q?=EF=BC=9Astyle=20=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91?= =?UTF-8?q?=EF=BC=9A=20=E3=80=90=E8=BF=87=20=20=E7=A8=8B=E3=80=91=EF=BC=9A?= =?UTF-8?q?=20=E3=80=90=E5=BD=B1=20=20=E5=93=8D=E3=80=91=EF=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lib/mavlink/mavlink_helpers.h | 128 ++++++++++++++++++---------------- src/commser.cpp | 4 +- 2 files changed, 70 insertions(+), 62 deletions(-) diff --git a/lib/mavlink/mavlink_helpers.h b/lib/mavlink/mavlink_helpers.h index fba7e70..1fce8a9 100644 --- a/lib/mavlink/mavlink_helpers.h +++ b/lib/mavlink/mavlink_helpers.h @@ -1,5 +1,5 @@ -#ifndef _MAVLINK_HELPERS_H_ -#define _MAVLINK_HELPERS_H_ +#ifndef _MAVLINK_HELPERS_H_ +#define _MAVLINK_HELPERS_H_ #include "string.h" #include "checksum.h" @@ -12,7 +12,7 @@ /* internal function to give access to the channel status for each channel */ -MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +MAVLINK_HELPER mavlink_status_t *mavlink_get_channel_status(uint8_t chan) { static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; return &m_mavlink_status[chan]; @@ -21,9 +21,9 @@ MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) /* internal function to give access to the channel buffer for each channel */ -MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) +MAVLINK_HELPER mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan) { - + #if MAVLINK_EXTERNAL_RX_BUFFER // No m_mavlink_message array defined in function, // has to be defined externally @@ -49,11 +49,11 @@ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) * @param length Message length */ #if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length, uint8_t crc_extra) +MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t *msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint8_t length, uint8_t crc_extra) #else -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length) +MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t *msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint8_t length) #endif { // This code part is the same for all messages; @@ -64,8 +64,8 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui msg->compid = component_id; // One sequence number per component msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq + 1; + checksum = crc_calculate((uint8_t *)&msg->len, length + MAVLINK_CORE_HEADER_LEN); #if MAVLINK_CRC_EXTRA crc_accumulate(crc_extra, &checksum); #endif @@ -75,19 +75,18 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; } - /** * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel */ #if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length, uint8_t crc_extra) +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t *msg, uint8_t system_id, uint8_t component_id, + uint8_t length, uint8_t crc_extra) { return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); } #else -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length) +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t *msg, uint8_t system_id, uint8_t component_id, + uint8_t length) { return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); } @@ -100,8 +99,8 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, * @brief Finalize a MAVLink message with channel assignment and send */ #if MAVLINK_CRC_EXTRA -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, - uint8_t length, uint8_t crc_extra) +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, + uint8_t length, uint8_t crc_extra) #else MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) #endif @@ -118,7 +117,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint buf[5] = msgid; status->current_tx_seq++; - checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate((uint8_t *)&buf[1], MAVLINK_CORE_HEADER_LEN); crc_accumulate_buffer(&checksum, packet, length); #if MAVLINK_CRC_EXTRA crc_accumulate(crc_extra, &checksum); @@ -161,7 +160,8 @@ MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlin return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; } -union __mavlink_bitfield { +union __mavlink_bitfield +{ uint8_t uint8; int8_t int8; uint16_t uint16; @@ -170,13 +170,12 @@ union __mavlink_bitfield { int32_t int32; }; - -MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) +MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t *msg) { crc_init(&msg->checksum); } -MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t *msg, uint8_t c) { crc_accumulate(c, &msg->checksum); } @@ -217,12 +216,12 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) * * @endcode */ -MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t *r_message, mavlink_status_t *r_mavlink_status) { - /* - default message crc function. You can override this per-system to - put this data in a different memory segment - */ +/* + default message crc function. You can override this per-system to + put this data in a different memory segment +*/ #if MAVLINK_CRC_EXTRA #ifndef MAVLINK_MESSAGE_CRC static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; @@ -230,8 +229,8 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa #endif #endif - mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + mavlink_message_t *rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message + mavlink_status_t *status = mavlink_get_channel_status(chan); ///< The current decode status int bufferIndex = 0; status->msg_received = 0; @@ -250,13 +249,13 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa break; case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received + if (status->msg_received /* Support shorter buffers than the default maximum packet size */ #if (MAVLINK_MAX_PAYLOAD_LEN < 255) - || c > MAVLINK_MAX_PAYLOAD_LEN + || c > MAVLINK_MAX_PAYLOAD_LEN #endif - ) + ) { status->buffer_overrun++; status->parse_error++; @@ -305,7 +304,8 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa break; case MAVLINK_PARSE_STATE_GOT_MSGID: - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; + _MAV_PAYLOAD_NON_CONST(rxmsg) + [status->packet_idx++] = (char)c; mavlink_update_checksum(rxmsg, c); if (status->packet_idx == rxmsg->len) { @@ -317,7 +317,8 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa #if MAVLINK_CRC_EXTRA mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); #endif - if (c != (rxmsg->checksum & 0xFF)) { + if (c != (rxmsg->checksum & 0xFF)) + { // Check first checksum byte status->parse_error++; status->msg_received = 0; @@ -332,12 +333,14 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa else { status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c; + _MAV_PAYLOAD_NON_CONST(rxmsg) + [status->packet_idx] = (char)c; } break; case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != (rxmsg->checksum >> 8)) { + if (c != (rxmsg->checksum >> 8)) + { // Check second checksum byte status->parse_error++; status->msg_received = 0; @@ -354,7 +357,8 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa // Successfully got message status->msg_received = 1; status->parse_state = MAVLINK_PARSE_STATE_IDLE; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c; + _MAV_PAYLOAD_NON_CONST(rxmsg) + [status->packet_idx + 1] = (char)c; memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); } break; @@ -364,19 +368,20 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa // If a message has been sucessfully decoded, check index if (status->msg_received == 1) { - //while(status->current_seq != rxmsg->seq) + // while(status->current_seq != rxmsg->seq) //{ // status->packet_rx_drop_count++; - // status->current_seq++; - //} + // status->current_seq++; + // } status->current_rx_seq = rxmsg->seq; // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + if (status->packet_rx_success_count == 0) + status->packet_rx_drop_count = 0; // Count this packet as received status->packet_rx_success_count++; } - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->current_rx_seq = status->current_rx_seq + 1; r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; r_mavlink_status->packet_rx_drop_count = status->parse_error; status->parse_error = 0; @@ -393,14 +398,15 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa * @param buffer packet buffer to write into * @return new position of the last used byte in the buffer */ -MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) +MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t *r_bit_index, uint8_t *buffer) { uint16_t bits_remain = bits; // Transform number into network order int32_t v; uint8_t i_bit_index, i_byte_index, curr_bits_n; #if MAVLINK_NEED_BYTE_SWAP - union { + union + { int32_t i; uint8_t b[4]; } bin, bout; @@ -447,8 +453,8 @@ MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t // we might have to pack them into more than one byte // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME + // curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 + // FIXME if (bits_remain <= (uint8_t)(8 - i_bit_index)) { // Enough space @@ -458,13 +464,13 @@ MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t { curr_bits_n = (8 - i_bit_index); } - + // Pack these n bits into the current byte // Mask out whatever was at that position with ones (xxx11111) buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); // Put content to this position, by masking out the non-used part buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); - + // Increment the bit index i_bit_index += curr_bits_n; @@ -477,10 +483,11 @@ MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t i_bit_index = 0; } } - + *r_bit_index = i_bit_index; // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; + if (i_bit_index != 7) + i_byte_index++; return i_byte_index - packet_index; } @@ -496,14 +503,14 @@ MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t void comm_send_ch(mavlink_channel_t chan, uint8_t ch) { - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } } */ @@ -516,7 +523,8 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, #else /* fallback to one byte at a time */ uint16_t i; - for (i = 0; i < len; i++) { + for (i = 0; i < len; i++) + { comm_send_ch(chan, (uint8_t)buf[i]); } #endif diff --git a/src/commser.cpp b/src/commser.cpp index 507527f..f8521e1 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -377,8 +377,8 @@ void mavlink_receiveCallback(uint8_t c) mavlink_msg_gps_global_origin_decode(&msg, &gps_org); // 解构msg数据 // home点经纬高 sprintf(buf, "{\"lng\":%d,\"lat\":%d,\"alt\":%.2f}", - gps_org.latitude, gps_org.longitude, + gps_org.latitude, (double)gps_org.altitude / 1000); if (topicPubMsg[17] != buf) @@ -494,7 +494,7 @@ void mavlink_receiveCallback(uint8_t c) topicPubMsg[1] = buf; } // 电流 - sprintf(buf, "%.2f", (double)sys_status.current_battery / 100); + sprintf(buf, "%.2f", (double)sys_status.current_battery / 100); // 解构中 1=10毫安 所以这里/100 最终单位是A 安培 if (topicPubMsg[2] != buf) { topicPubMsg[2] = buf;