mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-11 17:13:56 -03:00
Library wrapper for MAVLink protocol bits.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@924 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
0037b40329
commit
7582353b08
12
libraries/GCS_MAVLink/GCS_MAVLink.cpp
Normal file
12
libraries/GCS_MAVLink/GCS_MAVLink.cpp
Normal file
@ -0,0 +1,12 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/// @file GCS_MAVLink.cpp
|
||||
/// @brief Supporting bits for MAVLink.
|
||||
|
||||
#include "GCS_MAVLink.h"
|
||||
|
||||
Stream *mavlink_comm_0_port;
|
||||
Stream *mavlink_comm_1_port;
|
||||
|
||||
// this might need to move to the flight software
|
||||
mavlink_system_t mavlink_system = {7,1,0,0};
|
79
libraries/GCS_MAVLink/GCS_MAVLink.h
Normal file
79
libraries/GCS_MAVLink/GCS_MAVLink.h
Normal file
@ -0,0 +1,79 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/// @file GCS_MAVLink.h
|
||||
/// @brief One size fits all header for MAVLink integration.
|
||||
|
||||
#ifndef GCS_MAVLink_h
|
||||
#define GCS_MAVlink_h
|
||||
|
||||
#include <Stream.h>
|
||||
#include "include/mavlink_types.h"
|
||||
|
||||
/// MAVLink stream used for HIL interaction
|
||||
extern Stream *mavlink_comm_0_port;
|
||||
|
||||
/// MAVLink stream used for ground control communication
|
||||
extern Stream *mavlink_comm_1_port;
|
||||
|
||||
/// MAVLink system definition
|
||||
extern mavlink_system_t mavlink_system;
|
||||
|
||||
/// 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 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;
|
||||
}
|
||||
}
|
||||
|
||||
/// Read a byte from the nominated MAVLink channel
|
||||
///
|
||||
/// @param chan Channel to receive on
|
||||
/// @returns Byte read
|
||||
///
|
||||
static inline 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;
|
||||
}
|
||||
return data;
|
||||
}
|
||||
|
||||
/// Check for available data on the nominated MAVLink channel
|
||||
///
|
||||
/// @param chan Channel to check
|
||||
/// @returns Number of bytes available
|
||||
static inline uint16_t comm_get_available(mavlink_channel_t chan)
|
||||
{
|
||||
uint16_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;
|
||||
}
|
||||
return bytes;
|
||||
}
|
||||
|
||||
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
#include "include/ardupilotmega/mavlink.h"
|
||||
|
||||
#endif // GCS_MAVLink_h
|
Loading…
Reference in New Issue
Block a user