Food/PullupDev
3
0
Fork 0
Code Issues 8 Pull Requests Actions Packages Projects Releases Wiki Activity

【类 型】：style

Browse Source 
【原  因】：
【过  程】：
【影  响】：
This commit is contained in:
tk 2024-08-19 14:34:12 +08:00
parent d713db3c1d
commit ceba245d46
2 changed files with 70 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
lib/mavlink/mavlink_helpers.h
View File

@ -75,7 +75,6 @@ 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
*/
@ -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,7 +170,6 @@ union __mavlink_bitfield {
int32_t int32;
};
MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t *msg)
{
crc_init(&msg->checksum);
@ -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;
@ -371,7 +375,8 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
// }
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++;
}
@ -400,7 +405,8 @@ MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t
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;
@ -480,7 +486,8 @@ MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t
*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;
}
@ -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

4
src/commser.cpp
View File

@ -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;