mirror of https://github.com/ArduPilot/ardupilot
GCS_MAVLink: cope with different packet overheads with signing
use common macros for determining if there is sufficient space to send a packet
This commit is contained in:
parent
d4cb7b8970
commit
12c1d58511
|
@ -18,8 +18,8 @@
|
|||
#include <AP_HAL/utility/RingBuffer.h>
|
||||
|
||||
// check if a message will fit in the payload space available
|
||||
#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN)
|
||||
#define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false
|
||||
#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN)
|
||||
#define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN) return false
|
||||
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
|
||||
|
||||
#if HAL_CPU_CLASS <= HAL_CPU_CLASS_150 || CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
|
@ -206,7 +206,10 @@ public:
|
|||
|
||||
// update signing timestamp on GPS lock
|
||||
static void update_signing_timestamp(uint64_t timestamp_usec);
|
||||
|
||||
|
||||
// return current packet overhead for a channel
|
||||
static uint8_t packet_overhead_chan(mavlink_channel_t chan);
|
||||
|
||||
private:
|
||||
float adjust_rate_for_stream_trigger(enum streams stream_num);
|
||||
|
||||
|
@ -363,5 +366,10 @@ private:
|
|||
bool signing_key_save(const struct SigningKey &key);
|
||||
bool signing_key_load(struct SigningKey &key);
|
||||
void load_signing_key(void);
|
||||
bool signing_enabled(void) const;
|
||||
uint8_t packet_overhead(void) const { return packet_overhead_chan(chan); }
|
||||
#else
|
||||
bool signing_enabled(void) const { return false; }
|
||||
uint8_t packet_overhead(void) const { return MAVLINK_NUM_NON_PAYLOAD_BYTES; }
|
||||
#endif // MAVLINK_PROTOCOL_VERSION
|
||||
};
|
||||
|
|
|
@ -106,8 +106,11 @@ GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager
|
|||
load_signing_key();
|
||||
|
||||
mavlink_status_t *status = mavlink_get_channel_status(chan);
|
||||
if (status && status->signing == NULL) {
|
||||
// if signing is off start by sending MAVLink1
|
||||
if ((status && status->signing == NULL) ||
|
||||
chan == MAVLINK_COMM_0) {
|
||||
// if signing is off start by sending MAVLink1.
|
||||
// Always start with MAVLink1 on first port for now, to allow for recovery
|
||||
// after experiments with MAVLink2
|
||||
status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
|
||||
}
|
||||
#endif
|
||||
|
@ -134,7 +137,7 @@ GCS_MAVLINK::queued_param_send()
|
|||
if (bytes_allowed > comm_get_txspace(chan)) {
|
||||
bytes_allowed = comm_get_txspace(chan);
|
||||
}
|
||||
count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
|
||||
count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + packet_overhead());
|
||||
|
||||
// when we don't have flow control we really need to keep the
|
||||
// param download very slow, or it tends to stall
|
||||
|
@ -814,8 +817,7 @@ bool GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &missio
|
|||
} else {
|
||||
waypoint_timelast_request = AP_HAL::millis();
|
||||
// if we have enough space, then send the next WP immediately
|
||||
if (comm_get_txspace(chan) >=
|
||||
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_MISSION_ITEM_LEN) {
|
||||
if (HAVE_PAYLOAD_SPACE(chan, MISSION_ITEM)) {
|
||||
queued_waypoint_send();
|
||||
} else {
|
||||
send_message(MSG_NEXT_WAYPOINT);
|
||||
|
@ -935,6 +937,9 @@ GCS_MAVLINK::update(run_cli_fn run_cli)
|
|||
#if MAVLINK_PROTOCOL_VERSION >= 2
|
||||
if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
|
||||
(status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
|
||||
// if we receive any MAVLink2 packets on a connection
|
||||
// currently sending MAVLink1 then switch to sending
|
||||
// MAVLink2
|
||||
mavlink_status_t *cstatus = mavlink_get_channel_status(chan);
|
||||
if (cstatus != NULL) {
|
||||
cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
|
||||
|
@ -978,15 +983,14 @@ GCS_MAVLINK::update(run_cli_fn run_cli)
|
|||
*/
|
||||
bool GCS_MAVLINK::send_gps_raw(AP_GPS &gps)
|
||||
{
|
||||
if (comm_get_txspace(chan) >=
|
||||
MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
|
||||
if (HAVE_PAYLOAD_SPACE(chan, GPS_RAW_INT)) {
|
||||
gps.send_mavlink_gps_raw(chan);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (gps.highest_supported_status(0) > AP_GPS::GPS_OK_FIX_3D) {
|
||||
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS_RTK_LEN) {
|
||||
if (HAVE_PAYLOAD_SPACE(chan, GPS_RTK)) {
|
||||
gps.send_mavlink_gps_rtk(chan);
|
||||
}
|
||||
|
||||
|
@ -994,13 +998,12 @@ bool GCS_MAVLINK::send_gps_raw(AP_GPS &gps)
|
|||
|
||||
if (gps.num_sensors() > 1 && gps.status(1) > AP_GPS::NO_GPS) {
|
||||
|
||||
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS2_RAW_LEN) {
|
||||
if (HAVE_PAYLOAD_SPACE(chan, GPS2_RAW)) {
|
||||
gps.send_mavlink_gps2_raw(chan);
|
||||
}
|
||||
|
||||
if (gps.highest_supported_status(1) > AP_GPS::GPS_OK_FIX_3D) {
|
||||
if (comm_get_txspace(chan) >=
|
||||
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS2_RTK_LEN) {
|
||||
if (HAVE_PAYLOAD_SPACE(chan, GPS2_RTK)) {
|
||||
gps.send_mavlink_gps2_rtk(chan);
|
||||
}
|
||||
}
|
||||
|
@ -1049,8 +1052,7 @@ void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi)
|
|||
values[7],
|
||||
receiver_rssi);
|
||||
|
||||
if (hal.rcin->num_channels() > 8 &&
|
||||
comm_get_txspace(chan) >= MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
|
||||
if (hal.rcin->num_channels() > 8 && HAVE_PAYLOAD_SPACE(chan, RC_CHANNELS)) {
|
||||
mavlink_msg_rc_channels_send(
|
||||
chan,
|
||||
now,
|
||||
|
@ -1314,7 +1316,7 @@ void GCS_MAVLINK::service_statustext(void)
|
|||
if (statustext->bitmask & chan_bit) {
|
||||
// something is queued on a port and that's the port index we're looped at
|
||||
mavlink_channel_t chan_index = (mavlink_channel_t)(MAVLINK_COMM_0+i);
|
||||
if (comm_get_txspace(chan_index) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
|
||||
if (HAVE_PAYLOAD_SPACE(chan_index, STATUSTEXT)) {
|
||||
// we have space so send then clear that channel bit on the mask
|
||||
mavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text);
|
||||
statustext->bitmask &= ~chan_bit;
|
||||
|
@ -1339,7 +1341,7 @@ void GCS_MAVLINK::send_parameter_value_all(const char *param_name, ap_var_type p
|
|||
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
|
||||
if ((1U<<i) & mavlink_active) {
|
||||
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
|
||||
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
|
||||
if (HAVE_PAYLOAD_SPACE(chan, PARAM_VALUE)) {
|
||||
mavlink_msg_param_value_send(
|
||||
chan,
|
||||
param_name,
|
||||
|
@ -1530,7 +1532,7 @@ void GCS_MAVLINK::send_vibration(const AP_InertialSensor &ins) const
|
|||
|
||||
void GCS_MAVLINK::send_home(const Location &home) const
|
||||
{
|
||||
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_HOME_POSITION_LEN) {
|
||||
if (HAVE_PAYLOAD_SPACE(chan, HOME_POSITION)) {
|
||||
const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
mavlink_msg_home_position_send(
|
||||
chan,
|
||||
|
@ -1549,7 +1551,7 @@ void GCS_MAVLINK::send_home_all(const Location &home)
|
|||
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
|
||||
if ((1U<<i) & mavlink_active) {
|
||||
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
|
||||
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_HOME_POSITION_LEN) {
|
||||
if (HAVE_PAYLOAD_SPACE(chan, HOME_POSITION)) {
|
||||
mavlink_msg_home_position_send(
|
||||
chan,
|
||||
home.lat,
|
||||
|
|
|
@ -180,8 +180,7 @@ void GCS_MAVLINK::handle_log_send(DataFlash_Class &dataflash)
|
|||
*/
|
||||
void GCS_MAVLINK::handle_log_send_listing(DataFlash_Class &dataflash)
|
||||
{
|
||||
uint16_t txspace = comm_get_txspace(chan);
|
||||
if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_LOG_ENTRY_LEN) {
|
||||
if (!HAVE_PAYLOAD_SPACE(chan, LOG_ENTRY)) {
|
||||
// no space
|
||||
return;
|
||||
}
|
||||
|
@ -210,8 +209,7 @@ void GCS_MAVLINK::handle_log_send_listing(DataFlash_Class &dataflash)
|
|||
*/
|
||||
bool GCS_MAVLINK::handle_log_send_data(DataFlash_Class &dataflash)
|
||||
{
|
||||
uint16_t txspace = comm_get_txspace(chan);
|
||||
if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_LOG_DATA_LEN) {
|
||||
if (!HAVE_PAYLOAD_SPACE(chan, LOG_DATA)) {
|
||||
// no space
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -176,6 +176,35 @@ void GCS_MAVLINK::update_signing_timestamp(uint64_t timestamp_usec)
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
return true if signing is enabled on this channel
|
||||
*/
|
||||
bool GCS_MAVLINK::signing_enabled(void) const
|
||||
{
|
||||
const mavlink_status_t *status = mavlink_get_channel_status(chan);
|
||||
if (status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
return packet overhead in bytes for a channel
|
||||
*/
|
||||
uint8_t GCS_MAVLINK::packet_overhead_chan(mavlink_channel_t chan)
|
||||
{
|
||||
const mavlink_status_t *status = mavlink_get_channel_status(chan);
|
||||
if (status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) {
|
||||
return MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN;
|
||||
}
|
||||
return MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
#else
|
||||
void GCS_MAVLINK::update_signing_timestamp(uint64_t timestamp_usec) {}
|
||||
|
||||
uint8_t packet_overhead_chan(mavlink_channel_t chan)
|
||||
{
|
||||
return MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
#endif // MAVLINK_PROTOCOL_VERSION
|
||||
|
|
|
@ -132,11 +132,11 @@ more_data:
|
|||
}
|
||||
|
||||
if (packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) {
|
||||
while (comm_get_txspace(chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_SERIAL_CONTROL) {
|
||||
while (!HAVE_PAYLOAD_SPACE(chan, SERIAL_CONTROL)) {
|
||||
hal.scheduler->delay(1);
|
||||
}
|
||||
} else {
|
||||
if (comm_get_txspace(chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_SERIAL_CONTROL) {
|
||||
if (!HAVE_PAYLOAD_SPACE(chan, SERIAL_CONTROL)) {
|
||||
// no space for reply
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -135,8 +135,8 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
|
|||
target_component == routes[i].compid ||
|
||||
!match_system))) {
|
||||
if (in_channel != routes[i].channel && !sent_to_chan[routes[i].channel]) {
|
||||
if (comm_get_txspace(routes[i].channel) >=
|
||||
((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
|
||||
if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) +
|
||||
GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
|
||||
#if ROUTING_DEBUG
|
||||
::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n",
|
||||
msg->msgid,
|
||||
|
@ -172,7 +172,8 @@ void MAVLink_routing::send_to_components(const mavlink_message_t* msg)
|
|||
// check learned routes
|
||||
for (uint8_t i=0; i<num_routes; i++) {
|
||||
if ((routes[i].sysid == mavlink_system.sysid) && !sent_to_chan[routes[i].channel]) {
|
||||
if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
|
||||
if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) +
|
||||
GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
|
||||
#if ROUTING_DEBUG
|
||||
::printf("send msg %u on chan %u sysid=%u compid=%u\n",
|
||||
msg->msgid,
|
||||
|
@ -278,7 +279,8 @@ void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavli
|
|||
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
|
||||
if (mask & (1U<<i)) {
|
||||
mavlink_channel_t channel = (mavlink_channel_t)(MAVLINK_COMM_0 + i);
|
||||
if (comm_get_txspace(channel) >= ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
|
||||
if (comm_get_txspace(channel) >= ((uint16_t)msg->len) +
|
||||
GCS_MAVLINK::packet_overhead_chan(channel)) {
|
||||
#if ROUTING_DEBUG
|
||||
::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n",
|
||||
(unsigned)in_channel,
|
||||
|
|
Loading…
Reference in New Issue