Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues 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

75
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -34,34 +34,16 @@ GCS_MAVLINK::GCS_MAVLINK() :
void void
GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan) 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; _port = port;
chan = mav_chan; chan = mav_chan;
switch (chan) { mavlink_comm_port[chan] = _port;
case MAVLINK_COMM_0:
mavlink_comm_0_port = _port;
initialised = true; 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;
}
_queued_parameter = NULL; _queued_parameter = NULL;
reset_cli_timeout(); 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) bool GCS_MAVLINK::have_flow_control(void)
{ {
switch (chan) { // sanity check chan
case MAVLINK_COMM_0: if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
if (mavlink_comm_0_port == NULL) { return false;
}
if (mavlink_comm_port[chan] == NULL) {
return false; return false;
} else { } else {
// assume USB has flow control // assume USB has flow control
return hal.gpio->usb_connected() || mavlink_comm_0_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE; return hal.gpio->usb_connected() || mavlink_comm_port[chan]->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;
}
return false;
} }

110
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" #include "include/mavlink/v1.0/mavlink_helpers.h"
#endif #endif
AP_HAL::UARTDriver *mavlink_comm_0_port; AP_HAL::UARTDriver *mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
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
mavlink_system_t mavlink_system = {7,1}; 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 comm_receive_ch(mavlink_channel_t chan)
{ {
uint8_t data = 0; // sanity check chan
switch(chan) { if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
case MAVLINK_COMM_0: return 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;
return (uint8_t)mavlink_comm_port[chan]->read();
} }
/// Check for available transmit space on the nominated MAVLink channel /// 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 /// @returns Number of bytes available
uint16_t comm_get_txspace(mavlink_channel_t chan) 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) { if ((1U<<chan) & mavlink_locked_mask) {
return 0; return 0;
} }
int16_t ret = 0; int16_t ret = mavlink_comm_port[chan]->txspace();
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;
}
if (ret < 0) { if (ret < 0) {
ret = 0; ret = 0;
} }
@ -157,30 +118,14 @@ uint16_t comm_get_txspace(mavlink_channel_t chan)
/// @returns Number of bytes available /// @returns Number of bytes available
uint16_t comm_get_available(mavlink_channel_t chan) 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) { if ((1U<<chan) & mavlink_locked_mask) {
return 0; return 0;
} }
int16_t bytes = 0; int16_t bytes = mavlink_comm_port[chan]->available();
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;
}
if (bytes == -1) { if (bytes == -1) {
return 0; 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) void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
{ {
switch(chan) { // sanity check chan
case MAVLINK_COMM_0: if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
mavlink_comm_0_port->write(buf, len); return;
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;
} }
mavlink_comm_port[chan]->write(buf, len);
} }
static const uint8_t mavlink_message_crc_progmem[256] PROGMEM = MAVLINK_MESSAGE_CRCS; static const uint8_t mavlink_message_crc_progmem[256] PROGMEM = MAVLINK_MESSAGE_CRCS;

37
libraries/GCS_MAVLink/GCS_MAVLink.h
View File

@ -51,19 +51,7 @@
#include "include/mavlink/v1.0/mavlink_types.h" #include "include/mavlink/v1.0/mavlink_types.h"
/// MAVLink stream used for uartA /// MAVLink stream used for uartA
extern AP_HAL::UARTDriver *mavlink_comm_0_port; extern AP_HAL::UARTDriver *mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
/// 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
/// MAVLink system definition /// MAVLink system definition
extern mavlink_system_t mavlink_system; 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) static inline void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
{ {
switch(chan) { // sanity check chan
case MAVLINK_COMM_0: if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
mavlink_comm_0_port->write(ch); return;
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;
} }
mavlink_comm_port[chan]->write(ch);
} }
void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len); void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len);