GCS_MAVLink: cope with different packet overheads with signing

use common macros for determining if there is sufficient space to send
a packet

libraries/GCS_MAVLink/GCS.h

@@ -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 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) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false
+#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
 };

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -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 ((status && status->signing == NULL) ||
-        // if signing is off start by sending MAVLink1
+        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) >= 
+        if (HAVE_PAYLOAD_SPACE(chan, MISSION_ITEM)) {
-            MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_MISSION_ITEM_LEN) {
             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) >= 
+    if (HAVE_PAYLOAD_SPACE(chan, GPS_RAW_INT)) {
-        MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
         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) >= 
+            if (HAVE_PAYLOAD_SPACE(chan, GPS2_RTK)) {
-                MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS2_RTK_LEN) {
                 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 && 
+    if (hal.rcin->num_channels() > 8 && HAVE_PAYLOAD_SPACE(chan, RC_CHANNELS)) {
-        comm_get_txspace(chan) >= MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
         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,

libraries/GCS_MAVLink/GCS_Logs.cpp

@@ -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 (!HAVE_PAYLOAD_SPACE(chan, LOG_ENTRY)) {
-    if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_LOG_ENTRY_LEN) {
         // 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 (!HAVE_PAYLOAD_SPACE(chan, LOG_DATA)) {
-    if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_LOG_DATA_LEN) {
         // no space
         return false;
     }

libraries/GCS_MAVLink/GCS_Signing.cpp

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

libraries/GCS_MAVLink/GCS_serial_control.cpp

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

libraries/GCS_MAVLink/MAVLink_routing.cpp

@@ -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) >= 
+                if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) +
-                    ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
+                    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,