added comm_get_txspace() to GCS_MAVLink

this gives us the number of bytes available in the transmit buffer for
a mavlink channel

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3246 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-09-04 21:23:24 +00:00
parent c822bad359
commit 7774ac534e
2 changed files with 25 additions and 5 deletions

View File

@ -5,8 +5,8 @@
#include "GCS_MAVLink.h" #include "GCS_MAVLink.h"
Stream *mavlink_comm_0_port; FastSerial *mavlink_comm_0_port;
Stream *mavlink_comm_1_port; FastSerial *mavlink_comm_1_port;
// this might need to move to the flight software // this might need to move to the flight software
mavlink_system_t mavlink_system = {7,1,0,0}; mavlink_system_t mavlink_system = {7,1,0,0};

View File

@ -6,7 +6,7 @@
#ifndef GCS_MAVLink_h #ifndef GCS_MAVLink_h
#define GCS_MAVLink_h #define GCS_MAVLink_h
#include <Stream.h> #include <FastSerial.h>
#include "include/ardupilotmega/version.h" #include "include/ardupilotmega/version.h"
@ -17,10 +17,10 @@
#include "include/mavlink_types.h" #include "include/mavlink_types.h"
/// MAVLink stream used for HIL interaction /// MAVLink stream used for HIL interaction
extern Stream *mavlink_comm_0_port; extern FastSerial *mavlink_comm_0_port;
/// MAVLink stream used for ground control communication /// MAVLink stream used for ground control communication
extern Stream *mavlink_comm_1_port; extern FastSerial *mavlink_comm_1_port;
/// MAVLink system definition /// MAVLink system definition
extern mavlink_system_t mavlink_system; extern mavlink_system_t mavlink_system;
@ -86,6 +86,26 @@ static inline uint16_t comm_get_available(mavlink_channel_t chan)
return bytes; return bytes;
} }
/// Check for available transmit space on the nominated MAVLink channel
///
/// @param chan Channel to check
/// @returns Number of bytes available, -1 for error
static inline int comm_get_txspace(mavlink_channel_t chan)
{
switch(chan) {
case MAVLINK_COMM_0:
return mavlink_comm_0_port->txspace();
break;
case MAVLINK_COMM_1:
return mavlink_comm_1_port->txspace();
break;
default:
break;
}
return -1;
}
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#include "include/ardupilotmega/mavlink.h" #include "include/ardupilotmega/mavlink.h"