Browse Source

GCS_MAVLink: remove comm_get_available

available returns an unsigned integer, so this was just weird
master
Peter Barker 6 years ago committed by Andrew Tridgell
parent
commit
8fc55b9678
  1. 2
      libraries/GCS_MAVLink/GCS.h
  2. 8
      libraries/GCS_MAVLink/GCS_Common.cpp
  3. 24
      libraries/GCS_MAVLink/GCS_MAVLink.cpp
  4. 7
      libraries/GCS_MAVLink/GCS_MAVLink.h

2
libraries/GCS_MAVLink/GCS.h

@ -214,6 +214,8 @@ public: @@ -214,6 +214,8 @@ public:
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;
// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels
static uint8_t active_channel_mask(void) { return mavlink_active; }

8
libraries/GCS_MAVLink/GCS_Common.cpp

@ -1293,6 +1293,11 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, @@ -1293,6 +1293,11 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
void
GCS_MAVLINK::update_receive(uint32_t max_time_us)
{
// do absolutely nothing if we are locked
if (locked()) {
return;
}
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
@ -1303,8 +1308,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us) @@ -1303,8 +1308,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
status.packet_rx_drop_count = 0;
// process received bytes
uint16_t nbytes = comm_get_available(chan);
const uint16_t nbytes = _port->available();
for (uint16_t i=0; i<nbytes; i++)
{
const uint8_t c = (uint8_t)_port->read();

24
libraries/GCS_MAVLink/GCS_MAVLink.cpp

@ -66,6 +66,11 @@ void GCS_MAVLINK::lock_channel(mavlink_channel_t _chan, bool lock) @@ -66,6 +66,11 @@ void GCS_MAVLINK::lock_channel(mavlink_channel_t _chan, bool lock)
}
}
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)
@ -111,25 +116,6 @@ uint16_t comm_get_txspace(mavlink_channel_t chan) @@ -111,25 +116,6 @@ uint16_t comm_get_txspace(mavlink_channel_t chan)
return (uint16_t)ret;
}
/// Check for available data on the nominated MAVLink channel
///
/// @param chan Channel to check
/// @returns Number of bytes available
uint16_t comm_get_available(mavlink_channel_t chan)
{
if (!valid_channel(chan)) {
return 0;
}
if ((1U<<chan) & mavlink_locked_mask) {
return 0;
}
int16_t bytes = mavlink_comm_port[chan]->available();
if (bytes == -1) {
return 0;
}
return (uint16_t)bytes;
}
/*
send a buffer out a MAVLink channel
*/

7
libraries/GCS_MAVLink/GCS_MAVLink.h

@ -60,13 +60,6 @@ static inline bool valid_channel(mavlink_channel_t chan) @@ -60,13 +60,6 @@ static inline bool valid_channel(mavlink_channel_t chan)
void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len);
/// Check for available data on the nominated MAVLink channel
///
/// @param chan Channel to check
/// @returns Number of bytes available
uint16_t comm_get_available(mavlink_channel_t chan);
/// Check for available transmit space on the nominated MAVLink channel
///
/// @param chan Channel to check

Loading…
Cancel
Save