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_
#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

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;