aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: mavlink_comm_port to array

Browse Source
This commit is contained in:
Randy Mackay 2015-05-15 14:40:23 +09:00
parent 757f388d62
commit 85eeba93c0
3 changed files with 45 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -34,34 +34,16 @@ GCS_MAVLINK::GCS_MAVLINK() :
void
GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan)
{
// sanity check chan
if (mav_chan >= MAVLINK_COMM_NUM_BUFFERS) {
return;
}
_port = port;
chan = mav_chan;
switch (chan) {
case MAVLINK_COMM_0:
mavlink_comm_0_port = _port;
initialised = true;
break;
case MAVLINK_COMM_1:
mavlink_comm_1_port = _port;
initialised = true;
break;
case MAVLINK_COMM_2:
#if MAVLINK_COMM_NUM_BUFFERS > 2
mavlink_comm_2_port = _port;
initialised = true;
break;
#endif
case MAVLINK_COMM_3:
#if MAVLINK_COMM_NUM_BUFFERS > 3
mavlink_comm_3_port = _port;
initialised = true;
break;
#endif
default:
// do nothing for unsupport mavlink channels
break;
}
mavlink_comm_port[chan] = _port;
initialised = true;
_queued_parameter = NULL;
reset_cli_timeout();
}
@ -419,48 +401,17 @@ void GCS_MAVLINK::handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg)
*/
bool GCS_MAVLINK::have_flow_control(void)
{
switch (chan) {
case MAVLINK_COMM_0:
if (mavlink_comm_0_port == NULL) {
return false;
} else {
// assume USB has flow control
return hal.gpio->usb_connected() || mavlink_comm_0_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
}
break;
case MAVLINK_COMM_1:
if (mavlink_comm_1_port == NULL) {
return false;
} else {
return mavlink_comm_1_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
}
break;
case MAVLINK_COMM_2:
#if MAVLINK_COMM_NUM_BUFFERS > 2
if (mavlink_comm_2_port == NULL) {
return false;
} else {
return mavlink_comm_2_port != NULL && mavlink_comm_2_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
}
break;
#endif
case MAVLINK_COMM_3:
#if MAVLINK_COMM_NUM_BUFFERS > 3
if (mavlink_comm_3_port == NULL) {
return false;
} else {
return mavlink_comm_3_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
}
break;
#endif
default:
break;
// sanity check chan
if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
return false;
}
if (mavlink_comm_port[chan] == NULL) {
return false;
} else {
// assume USB has flow control
return hal.gpio->usb_connected() || mavlink_comm_port[chan]->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
}
return false;
}

114
libraries/GCS_MAVLink/GCS_MAVLink.cpp
View File

@ -31,14 +31,7 @@ This provides some support code and variables for MAVLink enabled sketches
#include "include/mavlink/v1.0/mavlink_helpers.h"
#endif
AP_HAL::UARTDriver *mavlink_comm_0_port;
AP_HAL::UARTDriver *mavlink_comm_1_port;
#if MAVLINK_COMM_NUM_BUFFERS > 2
AP_HAL::UARTDriver *mavlink_comm_2_port;
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
AP_HAL::UARTDriver *mavlink_comm_3_port;
#endif
AP_HAL::UARTDriver *mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
mavlink_system_t mavlink_system = {7,1};
@ -91,28 +84,12 @@ uint8_t mav_var_type(enum ap_var_type t)
///
uint8_t comm_receive_ch(mavlink_channel_t chan)
{
uint8_t data = 0;
switch(chan) {
case MAVLINK_COMM_0:
data = mavlink_comm_0_port->read();
break;
case MAVLINK_COMM_1:
data = mavlink_comm_1_port->read();
break;
#if MAVLINK_COMM_NUM_BUFFERS > 2
case MAVLINK_COMM_2:
data = mavlink_comm_2_port->read();
break;
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
case MAVLINK_COMM_3:
data = mavlink_comm_3_port->read();
break;
#endif
default:
break;
}
return data;
// sanity check chan
if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
return 0;
}
return (uint8_t)mavlink_comm_port[chan]->read();
}
/// Check for available transmit space on the nominated MAVLink channel
@ -121,30 +98,14 @@ uint8_t comm_receive_ch(mavlink_channel_t chan)
/// @returns Number of bytes available
uint16_t comm_get_txspace(mavlink_channel_t chan)
{
// sanity check chan
if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
return 0;
}
if ((1U<<chan) & mavlink_locked_mask) {
return 0;
}
int16_t ret = 0;
switch(chan) {
case MAVLINK_COMM_0:
ret = mavlink_comm_0_port->txspace();
break;
case MAVLINK_COMM_1:
ret = mavlink_comm_1_port->txspace();
break;
#if MAVLINK_COMM_NUM_BUFFERS > 2
case MAVLINK_COMM_2:
ret = mavlink_comm_2_port->txspace();
break;
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
case MAVLINK_COMM_3:
ret = mavlink_comm_3_port->txspace();
break;
#endif
default:
break;
}
int16_t ret = mavlink_comm_port[chan]->txspace();
if (ret < 0) {
ret = 0;
}
@ -157,30 +118,14 @@ uint16_t comm_get_txspace(mavlink_channel_t chan)
/// @returns Number of bytes available
uint16_t comm_get_available(mavlink_channel_t chan)
{
// sanity check chan
if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
return 0;
}
if ((1U<<chan) & mavlink_locked_mask) {
return 0;
}
int16_t bytes = 0;
switch(chan) {
case MAVLINK_COMM_0:
bytes = mavlink_comm_0_port->available();
break;
case MAVLINK_COMM_1:
bytes = mavlink_comm_1_port->available();
break;
#if MAVLINK_COMM_NUM_BUFFERS > 2
case MAVLINK_COMM_2:
bytes = mavlink_comm_2_port->available();
break;
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
case MAVLINK_COMM_3:
bytes = mavlink_comm_3_port->available();
break;
#endif
default:
break;
}
int16_t bytes = mavlink_comm_port[chan]->available();
if (bytes == -1) {
return 0;
}
@ -192,26 +137,11 @@ uint16_t comm_get_available(mavlink_channel_t chan)
*/
void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
{
switch(chan) {
case MAVLINK_COMM_0:
mavlink_comm_0_port->write(buf, len);
break;
case MAVLINK_COMM_1:
mavlink_comm_1_port->write(buf, len);
break;
#if MAVLINK_COMM_NUM_BUFFERS > 2
case MAVLINK_COMM_2:
mavlink_comm_2_port->write(buf, len);
break;
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
case MAVLINK_COMM_3:
mavlink_comm_3_port->write(buf, len);
break;
#endif
default:
break;
}
// sanity check chan
if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
return;
}
mavlink_comm_port[chan]->write(buf, len);
}
static const uint8_t mavlink_message_crc_progmem[256] PROGMEM = MAVLINK_MESSAGE_CRCS;

39
libraries/GCS_MAVLink/GCS_MAVLink.h
View File

@ -51,19 +51,7 @@
#include "include/mavlink/v1.0/mavlink_types.h"
/// MAVLink stream used for uartA
extern AP_HAL::UARTDriver *mavlink_comm_0_port;
/// MAVLink stream used for uartC
extern AP_HAL::UARTDriver *mavlink_comm_1_port;
#if MAVLINK_COMM_NUM_BUFFERS > 2
/// MAVLink stream used for uartD
extern AP_HAL::UARTDriver *mavlink_comm_2_port;
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
extern AP_HAL::UARTDriver *mavlink_comm_3_port;
#endif
extern AP_HAL::UARTDriver *mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
/// MAVLink system definition
extern mavlink_system_t mavlink_system;
@ -75,26 +63,11 @@ extern mavlink_system_t mavlink_system;
///
static inline void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
{
switch(chan) {
case MAVLINK_COMM_0:
mavlink_comm_0_port->write(ch);
break;
case MAVLINK_COMM_1:
mavlink_comm_1_port->write(ch);
break;
#if MAVLINK_COMM_NUM_BUFFERS > 2
case MAVLINK_COMM_2:
mavlink_comm_2_port->write(ch);
break;
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
case MAVLINK_COMM_3:
mavlink_comm_3_port->write(ch);
break;
#endif
default:
break;
}
// sanity check chan
if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
return;
}
mavlink_comm_port[chan]->write(ch);
}
void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len);