From 12c1d58511f01b12bb815e2035a022ed6f457205 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 Apr 2016 14:09:47 +1000 Subject: [PATCH] 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 | 14 ++++++-- libraries/GCS_MAVLink/GCS_Common.cpp | 36 +++++++++++--------- libraries/GCS_MAVLink/GCS_Logs.cpp | 6 ++-- libraries/GCS_MAVLink/GCS_Signing.cpp | 29 ++++++++++++++++ libraries/GCS_MAVLink/GCS_serial_control.cpp | 4 +-- libraries/GCS_MAVLink/MAVLink_routing.cpp | 10 +++--- 6 files changed, 69 insertions(+), 30 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index d919d8375e..52779f6314 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -18,8 +18,8 @@ #include // 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 }; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index cd4e5428bc..ad738f2c26 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/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 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_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_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, diff --git a/libraries/GCS_MAVLink/GCS_Logs.cpp b/libraries/GCS_MAVLink/GCS_Logs.cpp index 9628d87e4f..8b40142722 100644 --- a/libraries/GCS_MAVLink/GCS_Logs.cpp +++ b/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 (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; } diff --git a/libraries/GCS_MAVLink/GCS_Signing.cpp b/libraries/GCS_MAVLink/GCS_Signing.cpp index 6306f9d64f..54fce23306 100644 --- a/libraries/GCS_MAVLink/GCS_Signing.cpp +++ b/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 diff --git a/libraries/GCS_MAVLink/GCS_serial_control.cpp b/libraries/GCS_MAVLink/GCS_serial_control.cpp index 83bde14fc6..f382d6837f 100644 --- a/libraries/GCS_MAVLink/GCS_serial_control.cpp +++ b/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; } diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp index 0268894d98..6fae81dbf6 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.cpp +++ b/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) >= - ((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= ((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= ((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,