zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

GCS_MAVLink: cope with different packet overheads with signing 

use common macros for determining if there is sufficient space to send
a packet
master
Andrew Tridgell 9 years ago
parent
d4cb7b8970
commit
12c1d58511
6 changed files with 69 additions and 30 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 12
      libraries/GCS_MAVLink/GCS.h
  2. 36
      libraries/GCS_MAVLink/GCS_Common.cpp
  3. 6
      libraries/GCS_MAVLink/GCS_Logs.cpp
  4. 29
      libraries/GCS_MAVLink/GCS_Signing.cpp
  5. 4
      libraries/GCS_MAVLink/GCS_serial_control.cpp
  6. 10
      libraries/GCS_MAVLink/MAVLink_routing.cpp

12
libraries/GCS_MAVLink/GCS.h
Unescape View File

@ -18,8 +18,8 @@
#include <AP_HAL/utility/RingBuffer.h> #include <AP_HAL/utility/RingBuffer.h>
// check if a message will fit in the payload space available // 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 #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 #if HAL_CPU_CLASS <= HAL_CPU_CLASS_150 || CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -207,6 +207,9 @@ public:
// update signing timestamp on GPS lock // update signing timestamp on GPS lock
static void update_signing_timestamp(uint64_t timestamp_usec); 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: private:
float adjust_rate_for_stream_trigger(enum streams stream_num); 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_save(const struct SigningKey &key);
bool signing_key_load(struct SigningKey &key); bool signing_key_load(struct SigningKey &key);
void load_signing_key(void); 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 #endif // MAVLINK_PROTOCOL_VERSION
}; };

36
libraries/GCS_MAVLink/GCS_Common.cpp
Unescape View File

@ -106,8 +106,11 @@ GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager
load_signing_key(); load_signing_key();
mavlink_status_t *status = mavlink_get_channel_status(chan); 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; status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
} }
#endif #endif
@ -134,7 +137,7 @@ GCS_MAVLINK::queued_param_send()
if (bytes_allowed > comm_get_txspace(chan)) { if (bytes_allowed > comm_get_txspace(chan)) {
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 // when we don't have flow control we really need to keep the
// param download very slow, or it tends to stall // 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 { } else {
waypoint_timelast_request = AP_HAL::millis(); waypoint_timelast_request = AP_HAL::millis();
// if we have enough space, then send the next WP immediately // 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(); queued_waypoint_send();
} else { } else {
send_message(MSG_NEXT_WAYPOINT); send_message(MSG_NEXT_WAYPOINT);
@ -935,6 +937,9 @@ GCS_MAVLINK::update(run_cli_fn run_cli)
#if MAVLINK_PROTOCOL_VERSION >= 2 #if MAVLINK_PROTOCOL_VERSION >= 2
if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
(status.flags & MAVLINK_STATUS_FLAG_OUT_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); mavlink_status_t *cstatus = mavlink_get_channel_status(chan);
if (cstatus != NULL) { if (cstatus != NULL) {
cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; 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) 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); gps.send_mavlink_gps_raw(chan);
} else { } else {
return false; return false;
} }
if (gps.highest_supported_status(0) > AP_GPS::GPS_OK_FIX_3D) { 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); 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 (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); gps.send_mavlink_gps2_raw(chan);
} }
if (gps.highest_supported_status(1) > AP_GPS::GPS_OK_FIX_3D) { 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); gps.send_mavlink_gps2_rtk(chan);
} }
} }
@ -1049,8 +1052,7 @@ void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi)
values[7], values[7],
receiver_rssi); 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( mavlink_msg_rc_channels_send(
chan, chan,
now, now,
@ -1314,7 +1316,7 @@ void GCS_MAVLINK::service_statustext(void)
if (statustext->bitmask & chan_bit) { if (statustext->bitmask & chan_bit) {
// something is queued on a port and that's the port index we're looped at // 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); 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 // 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); mavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text);
statustext->bitmask &= ~chan_bit; 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++) { for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) { if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i); 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( mavlink_msg_param_value_send(
chan, chan,
param_name, 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 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}; const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
mavlink_msg_home_position_send( mavlink_msg_home_position_send(
chan, 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++) { for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) { if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i); 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( mavlink_msg_home_position_send(
chan, chan,
home.lat, home.lat,

6
libraries/GCS_MAVLink/GCS_Logs.cpp
Unescape View File

@ -180,8 +180,7 @@ void GCS_MAVLINK::handle_log_send(DataFlash_Class &dataflash)
*/ */
void GCS_MAVLINK::handle_log_send_listing(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 // no space
return; 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) 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 // no space
return false; return false;
} }

29
libraries/GCS_MAVLink/GCS_Signing.cpp
Unescape View File

@ -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 #else
void GCS_MAVLINK::update_signing_timestamp(uint64_t timestamp_usec) {} 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 #endif // MAVLINK_PROTOCOL_VERSION

4
libraries/GCS_MAVLink/GCS_serial_control.cpp
Unescape View File

@ -132,11 +132,11 @@ more_data:
} }
if (packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) { 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); hal.scheduler->delay(1);
} }
} else { } 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 // no space for reply
return; return;
} }

10
libraries/GCS_MAVLink/MAVLink_routing.cpp
Unescape View File

@ -135,8 +135,8 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
target_component == routes[i].compid || target_component == routes[i].compid ||
!match_system))) { !match_system))) {
if (in_channel != routes[i].channel && !sent_to_chan[routes[i].channel]) { 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 #if ROUTING_DEBUG
::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n", ::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n",
msg->msgid, msg->msgid,
@ -172,7 +172,8 @@ void MAVLink_routing::send_to_components(const mavlink_message_t* msg)
// check learned routes // check learned routes
for (uint8_t i=0; i<num_routes; i++) { for (uint8_t i=0; i<num_routes; i++) {
if ((routes[i].sysid == mavlink_system.sysid) && !sent_to_chan[routes[i].channel]) { 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 #if ROUTING_DEBUG
::printf("send msg %u on chan %u sysid=%u compid=%u\n", ::printf("send msg %u on chan %u sysid=%u compid=%u\n",
msg->msgid, 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++) { for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if (mask & (1U<<i)) { if (mask & (1U<<i)) {
mavlink_channel_t channel = (mavlink_channel_t)(MAVLINK_COMM_0 + 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 #if ROUTING_DEBUG
::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n", ::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n",
(unsigned)in_channel, (unsigned)in_channel,

Write Preview
Loading…
Cancel
Save