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

128
lib/mavlink/mavlink_helpers.h
View File

@ -1,5 +1,5 @@
#ifndef _MAVLINK_HELPERS_H_ #ifndef _MAVLINK_HELPERS_H_
#define _MAVLINK_HELPERS_H_ #define _MAVLINK_HELPERS_H_
#include "string.h" #include "string.h"
#include "checksum.h" #include "checksum.h"
@ -12,7 +12,7 @@
/* /*
internal function to give access to the channel status for each channel 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]; static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[chan]; 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 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 #if MAVLINK_EXTERNAL_RX_BUFFER
// No m_mavlink_message array defined in function, // No m_mavlink_message array defined in function,
// has to be defined externally // 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 * @param length Message length
*/ */
#if MAVLINK_CRC_EXTRA #if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 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) uint8_t chan, uint8_t length, uint8_t crc_extra)
#else #else
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 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 chan, uint8_t length)
#endif #endif
{ {
// This code part is the same for all messages; // 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; msg->compid = component_id;
// One sequence number per component // One sequence number per component
msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; 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; 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); checksum = crc_calculate((uint8_t *)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
#if MAVLINK_CRC_EXTRA #if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum); crc_accumulate(crc_extra, &checksum);
#endif #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; return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} }
/** /**
* @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
*/ */
#if MAVLINK_CRC_EXTRA #if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 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) uint8_t length, uint8_t crc_extra)
{ {
return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
} }
#else #else
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t *msg, uint8_t system_id, uint8_t component_id,
uint8_t length) uint8_t length)
{ {
return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, 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 * @brief Finalize a MAVLink message with channel assignment and send
*/ */
#if MAVLINK_CRC_EXTRA #if MAVLINK_CRC_EXTRA
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 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) uint8_t length, uint8_t crc_extra)
#else #else
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
#endif #endif
@ -118,7 +117,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint
buf[5] = msgid; buf[5] = msgid;
status->current_tx_seq++; 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); crc_accumulate_buffer(&checksum, packet, length);
#if MAVLINK_CRC_EXTRA #if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum); 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; return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
} }
union __mavlink_bitfield { union __mavlink_bitfield
{
uint8_t uint8; uint8_t uint8;
int8_t int8; int8_t int8;
uint16_t uint16; uint16_t uint16;
@ -170,13 +170,12 @@ union __mavlink_bitfield {
int32_t int32; 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); 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); crc_accumulate(c, &msg->checksum);
} }
@ -217,12 +216,12 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
* *
* @endcode * @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 default message crc function. You can override this per-system to
put this data in a different memory segment put this data in a different memory segment
*/ */
#if MAVLINK_CRC_EXTRA #if MAVLINK_CRC_EXTRA
#ifndef MAVLINK_MESSAGE_CRC #ifndef MAVLINK_MESSAGE_CRC
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; 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
#endif #endif
mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message 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_status_t *status = mavlink_get_channel_status(chan); ///< The current decode status
int bufferIndex = 0; int bufferIndex = 0;
status->msg_received = 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; break;
case MAVLINK_PARSE_STATE_GOT_STX: case MAVLINK_PARSE_STATE_GOT_STX:
if (status->msg_received if (status->msg_received
/* Support shorter buffers than the /* Support shorter buffers than the
default maximum packet size */ default maximum packet size */
#if (MAVLINK_MAX_PAYLOAD_LEN < 255) #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
|| c > MAVLINK_MAX_PAYLOAD_LEN || c > MAVLINK_MAX_PAYLOAD_LEN
#endif #endif
) )
{ {
status->buffer_overrun++; status->buffer_overrun++;
status->parse_error++; status->parse_error++;
@ -305,7 +304,8 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
break; break;
case MAVLINK_PARSE_STATE_GOT_MSGID: 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); mavlink_update_checksum(rxmsg, c);
if (status->packet_idx == rxmsg->len) 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 #if MAVLINK_CRC_EXTRA
mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
#endif #endif
if (c != (rxmsg->checksum & 0xFF)) { if (c != (rxmsg->checksum & 0xFF))
{
// Check first checksum byte // Check first checksum byte
status->parse_error++; status->parse_error++;
status->msg_received = 0; status->msg_received = 0;
@ -332,12 +333,14 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
else else
{ {
status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; 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; break;
case MAVLINK_PARSE_STATE_GOT_CRC1: case MAVLINK_PARSE_STATE_GOT_CRC1:
if (c != (rxmsg->checksum >> 8)) { if (c != (rxmsg->checksum >> 8))
{
// Check second checksum byte // Check second checksum byte
status->parse_error++; status->parse_error++;
status->msg_received = 0; 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 // Successfully got message
status->msg_received = 1; status->msg_received = 1;
status->parse_state = MAVLINK_PARSE_STATE_IDLE; 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)); memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
} }
break; 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 a message has been sucessfully decoded, check index
if (status->msg_received == 1) if (status->msg_received == 1)
{ {
//while(status->current_seq != rxmsg->seq) // while(status->current_seq != rxmsg->seq)
//{ //{
// status->packet_rx_drop_count++; // status->packet_rx_drop_count++;
// status->current_seq++; // status->current_seq++;
//} // }
status->current_rx_seq = rxmsg->seq; status->current_rx_seq = rxmsg->seq;
// Initial condition: If no packet has been received so far, drop count is undefined // 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 // Count this packet as received
status->packet_rx_success_count++; 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_success_count = status->packet_rx_success_count;
r_mavlink_status->packet_rx_drop_count = status->parse_error; r_mavlink_status->packet_rx_drop_count = status->parse_error;
status->parse_error = 0; 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 * @param buffer packet buffer to write into
* @return new position of the last used byte in the buffer * @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; uint16_t bits_remain = bits;
// Transform number into network order // Transform number into network order
int32_t v; int32_t v;
uint8_t i_bit_index, i_byte_index, curr_bits_n; uint8_t i_bit_index, i_byte_index, curr_bits_n;
#if MAVLINK_NEED_BYTE_SWAP #if MAVLINK_NEED_BYTE_SWAP
union { union
{
int32_t i; int32_t i;
uint8_t b[4]; uint8_t b[4];
} bin, bout; } 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 // we might have to pack them into more than one byte
// First pack everything we can into the current 'open' byte // First pack everything we can into the current 'open' byte
//curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 // curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
//FIXME // FIXME
if (bits_remain <= (uint8_t)(8 - i_bit_index)) if (bits_remain <= (uint8_t)(8 - i_bit_index))
{ {
// Enough space // 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); curr_bits_n = (8 - i_bit_index);
} }
// Pack these n bits into the current byte // Pack these n bits into the current byte
// Mask out whatever was at that position with ones (xxx11111) // Mask out whatever was at that position with ones (xxx11111)
buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
// Put content to this position, by masking out the non-used part // Put content to this position, by masking out the non-used part
buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
// Increment the bit index // Increment the bit index
i_bit_index += curr_bits_n; 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; i_bit_index = 0;
} }
} }
*r_bit_index = i_bit_index; *r_bit_index = i_bit_index;
// If a partly filled byte is present, mark this as consumed // 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; 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) void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
{ {
if (chan == MAVLINK_COMM_0) if (chan == MAVLINK_COMM_0)
{ {
uart0_transmit(ch); uart0_transmit(ch);
} }
if (chan == MAVLINK_COMM_1) if (chan == MAVLINK_COMM_1)
{ {
uart1_transmit(ch); uart1_transmit(ch);
} }
} }
*/ */
@ -516,7 +523,8 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf,
#else #else
/* fallback to one byte at a time */ /* fallback to one byte at a time */
uint16_t i; uint16_t i;
for (i = 0; i < len; i++) { for (i = 0; i < len; i++)
{
comm_send_ch(chan, (uint8_t)buf[i]); comm_send_ch(chan, (uint8_t)buf[i]);
} }
#endif #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数据 mavlink_msg_gps_global_origin_decode(&msg, &gps_org); // 解构msg数据
// home点经纬高 // home点经纬高
sprintf(buf, "{\"lng\":%d,\"lat\":%d,\"alt\":%.2f}", sprintf(buf, "{\"lng\":%d,\"lat\":%d,\"alt\":%.2f}",
gps_org.latitude,
gps_org.longitude, gps_org.longitude,
gps_org.latitude,
(double)gps_org.altitude / 1000); (double)gps_org.altitude / 1000);
if (topicPubMsg[17] != buf) if (topicPubMsg[17] != buf)
@ -494,7 +494,7 @@ void mavlink_receiveCallback(uint8_t c)
topicPubMsg[1] = buf; 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) if (topicPubMsg[2] != buf)
{ {
topicPubMsg[2] = buf; topicPubMsg[2] = buf;