Skip to content
Permalink
Browse files

GCS_MAVLink: correct txspace return value issues

 - checking of space in send_to_active_channels was incorrect - did not
take into account locked status of the channel

 - corrected return value on comm_get_txspace - took a uint32_t, cast it
to int16_t, checked it for zero, then cast it to uint16_t on return.
That's just... odd.
  • Loading branch information...
peterbarker committed Sep 12, 2019
1 parent f881e4a commit 869d9e9db254c6bc902f8d492489c3cd0843f555
@@ -58,7 +58,7 @@ void GCS::send_to_active_channels(uint32_t msgid, const char *pkt)
if (!c.is_active()) {
continue;
}
if (entry->max_msg_len + c.packet_overhead() > c.get_uart()->txspace()) {
if (entry->max_msg_len + c.packet_overhead() > c.txspace()) {
// no room on this channel
continue;
}
@@ -23,9 +23,9 @@
#define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0

// check if a message will fit in the payload space available
#define PAYLOAD_SIZE(chan, id) (GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN)
#define PAYLOAD_SIZE(chan, id) (unsigned(GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN))
#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= PAYLOAD_SIZE(chan, id))
#define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN) return false
#define CHECK_PAYLOAD_SIZE(id) if (txspace() < unsigned(packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN)) return false
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false

// convenience macros for defining which ap_message ids are in which streams:
@@ -75,6 +75,14 @@ class GCS_MAVLINK
// returns true if we are requesting any items from the GCS:
bool requesting_mission_items() const;

/// Check for available transmit space
uint32_t txspace() const {
if (_locked) {
return 0;
}
return _port->txspace();
}

void send_mission_ack(const mavlink_message_t &msg,
MAV_MISSION_TYPE mission_type,
MAV_MISSION_RESULT result) const {
@@ -214,7 +222,14 @@ class GCS_MAVLINK
void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc);
void send_rpm() const;

bool locked() const;
// lock a channel, preventing use by MAVLink
void lock(bool lock) {
_locked = lock;
}
// returns true if this channel isn't available for MAVLink
bool locked() const {
return _locked;
}

// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels
@@ -692,6 +707,10 @@ class GCS_MAVLINK
#endif

uint32_t last_mavlink_stats_logged;

// true if we should NOT do MAVLink on this port (usually because
// someone's doing SERIAL_CONTROL over mavlink)
bool _locked;
};

/// @class GCS
@@ -2281,7 +2281,7 @@ bool GCS_MAVLINK::get_ap_message_interval(ap_message id, uint16_t &interval_ms)

MAV_RESULT GCS_MAVLINK::handle_command_get_message_interval(const mavlink_command_long_t &packet)
{
if (comm_get_txspace(chan) < PAYLOAD_SIZE(chan, MESSAGE_INTERVAL) + PAYLOAD_SIZE(chan, COMMAND_ACK)) {
if (txspace() < PAYLOAD_SIZE(chan, MESSAGE_INTERVAL) + PAYLOAD_SIZE(chan, COMMAND_ACK)) {
return MAV_RESULT_TEMPORARILY_REJECTED;
}

@@ -45,32 +45,9 @@ static HAL_Semaphore chan_locks[MAVLINK_COMM_NUM_BUFFERS];

mavlink_system_t mavlink_system = {7,1};

// mask of serial ports disabled to allow for SERIAL_CONTROL
static uint8_t mavlink_locked_mask;

// routing table
MAVLink_routing GCS_MAVLINK::routing;

/*
lock a channel, preventing use by MAVLink
*/
void GCS_MAVLINK::lock_channel(mavlink_channel_t _chan, bool lock)
{
if (!valid_channel(chan)) {
return;
}
if (lock) {
mavlink_locked_mask |= (1U<<(unsigned)_chan);
} else {
mavlink_locked_mask &= ~(1U<<(unsigned)_chan);
}
}

bool GCS_MAVLINK::locked() const
{
return (1U<<chan) & mavlink_locked_mask;
}

// set a channel as private. Private channels get sent heartbeats, but
// don't get broadcast packets or forwarded packets
void GCS_MAVLINK::set_channel_private(mavlink_channel_t _chan)
@@ -101,19 +78,13 @@ MAV_PARAM_TYPE GCS_MAVLINK::mav_param_type(enum ap_var_type t)
///
/// @param chan Channel to check
/// @returns Number of bytes available
uint16_t comm_get_txspace(mavlink_channel_t chan)
uint32_t comm_get_txspace(mavlink_channel_t chan)
{
if (!valid_channel(chan)) {
return 0;
}
if ((1U<<chan) & mavlink_locked_mask) {
GCS_MAVLINK *link = gcs().chan(chan);
if (link == nullptr) {
return 0;
}
int16_t ret = mavlink_comm_port[chan]->txspace();
if (ret < 0) {
ret = 0;
}
return (uint16_t)ret;
return link->txspace();
}

/*
@@ -64,7 +64,7 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len);
///
/// @param chan Channel to check
/// @returns Number of bytes available
uint16_t comm_get_txspace(mavlink_channel_t chan);
uint32_t comm_get_txspace(mavlink_channel_t chan);

#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#include "include/mavlink/v2.0/ardupilotmega/mavlink.h"
@@ -51,8 +51,8 @@ GCS_MAVLINK::queued_param_send()
if (bytes_allowed < size_for_one_param_value_msg) {
bytes_allowed = size_for_one_param_value_msg;
}
if (bytes_allowed > comm_get_txspace(chan)) {
bytes_allowed = comm_get_txspace(chan);
if (bytes_allowed > txspace()) {
bytes_allowed = txspace();
}
uint32_t count = bytes_allowed / size_for_one_param_value_msg;

@@ -44,14 +44,24 @@ void GCS_MAVLINK::handle_serial_control(const mavlink_message_t &msg)
bool exclusive = (packet.flags & SERIAL_CONTROL_FLAG_EXCLUSIVE) != 0;

switch (packet.device) {
case SERIAL_CONTROL_DEV_TELEM1:
stream = port = hal.uartC;
lock_channel(MAVLINK_COMM_1, exclusive);
case SERIAL_CONTROL_DEV_TELEM1: {
GCS_MAVLINK *link = gcs().chan(1);
if (link == nullptr) {
break;
}
stream = port = link->get_uart();
link->lock(exclusive);
break;
case SERIAL_CONTROL_DEV_TELEM2:
stream = port = hal.uartD;
lock_channel(MAVLINK_COMM_2, exclusive);
}
case SERIAL_CONTROL_DEV_TELEM2: {
GCS_MAVLINK *link = gcs().chan(2);
if (link == nullptr) {
break;
}
stream = port = link->get_uart();
link->lock(exclusive);
break;
}
case SERIAL_CONTROL_DEV_GPS1:
stream = port = hal.uartB;
AP::gps().lock_port(0, exclusive);

0 comments on commit 869d9e9

Please sign in to comment.
You can’t perform that action at this time.