Browse Source

GCS_MAVLink: remove intermediate functions

GCS_MAVLink: remove comm_receive_ch

This is a lot of sanity checking in a static function which has been
called from a place where things are guaranteed

GCS_MAVLink: remove unused comm_is_idle

GCS_MAVLink: remove unused comm_send_ch

GCS_MAVLink: remove unused hal reference

GCS_MAVLink: remove unused #includes
master
Peter Barker 7 years ago committed by Andrew Tridgell
parent
commit
1aaf7e96af
  1. 2
      libraries/GCS_MAVLink/GCS_Common.cpp
  2. 26
      libraries/GCS_MAVLink/GCS_MAVLink.cpp
  3. 28
      libraries/GCS_MAVLink/GCS_MAVLink.h

2
libraries/GCS_MAVLink/GCS_Common.cpp

@ -895,7 +895,7 @@ GCS_MAVLINK::update(uint32_t max_time_us) @@ -895,7 +895,7 @@ GCS_MAVLINK::update(uint32_t max_time_us)
uint16_t nbytes = comm_get_available(chan);
for (uint16_t i=0; i<nbytes; i++)
{
uint8_t c = comm_receive_ch(chan);
const uint8_t c = (uint8_t)_port->read();
const uint32_t protocol_timeout = 4000;
if (alternative.handler &&

26
libraries/GCS_MAVLink/GCS_MAVLink.cpp

@ -82,20 +82,6 @@ uint8_t mav_var_type(enum ap_var_type t) @@ -82,20 +82,6 @@ uint8_t mav_var_type(enum ap_var_type t)
}
/// Read a byte from the nominated MAVLink channel
///
/// @param chan Channel to receive on
/// @returns Byte read
///
uint8_t comm_receive_ch(mavlink_channel_t chan)
{
if (!valid_channel(chan)) {
return 0;
}
return (uint8_t)mavlink_comm_port[chan]->read();
}
/// Check for available transmit space on the nominated MAVLink channel
///
/// @param chan Channel to check
@ -148,15 +134,3 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len) @@ -148,15 +134,3 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
}
mavlink_comm_port[chan]->write(buf, len);
}
extern const AP_HAL::HAL& hal;
/*
return true if the MAVLink parser is idle, so there is no partly parsed
MAVLink message being processed
*/
bool comm_is_idle(mavlink_channel_t chan)
{
mavlink_status_t *status = mavlink_get_channel_status(chan);
return status == nullptr || status->parse_state <= MAVLINK_PARSE_STATE_IDLE;
}

28
libraries/GCS_MAVLink/GCS_MAVLink.h

@ -2,9 +2,7 @@ @@ -2,9 +2,7 @@
/// @brief One size fits all header for MAVLink integration.
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
// we have separate helpers disabled to make it possible
// to select MAVLink 1.0 in the arduino GUI build
@ -53,28 +51,8 @@ static inline bool valid_channel(mavlink_channel_t chan) @@ -53,28 +51,8 @@ static inline bool valid_channel(mavlink_channel_t chan)
#pragma clang diagnostic pop
}
/// Send a byte to the nominated MAVLink channel
///
/// @param chan Channel to send to
/// @param ch Byte to send
///
static inline void comm_send_ch(mavlink_channel_t chan, uint8_t chr)
{
if (!valid_channel(chan)) {
return;
}
mavlink_comm_port[chan]->write(chr);
}
void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len);
/// Read a byte from the nominated MAVLink channel
///
/// @param chan Channel to receive on
/// @returns Byte read
///
uint8_t comm_receive_ch(mavlink_channel_t chan);
/// Check for available data on the nominated MAVLink channel
///
/// @param chan Channel to check
@ -88,12 +66,6 @@ uint16_t comm_get_available(mavlink_channel_t chan); @@ -88,12 +66,6 @@ uint16_t comm_get_available(mavlink_channel_t chan);
/// @returns Number of bytes available
uint16_t comm_get_txspace(mavlink_channel_t chan);
/*
return true if the MAVLink parser is idle, so there is no partly parsed
MAVLink message being processed
*/
bool comm_is_idle(mavlink_channel_t chan);
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#include "include/mavlink/v2.0/ardupilotmega/mavlink.h"

Loading…
Cancel
Save