Benjamin Hepp / mavlink_bridge

Mavlink bridge for Mbed devices

Dependents:   AIT_UWB_Range

mavlink_bridge.cpp@0:28183cc7963f, 2015-11-24 (annotated)

Committer:
bhepp
Date:
Tue Nov 24 16:41:11 2015 +0000
Revision:
0:28183cc7963f
Mavlink bridge for Mbed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bhepp 0:28183cc7963f 1 #include "mavlink_bridge.h"
bhepp 0:28183cc7963f 2
bhepp 0:28183cc7963f 3 using namespace ait;
bhepp 0:28183cc7963f 4
bhepp 0:28183cc7963f 5 MAVLinkBridge::MAVLinkBridge(UART_Interface* uart, mavlink_channel_t mavlink_channel, uint8_t sysid, uint8_t compid)
bhepp 0:28183cc7963f 6 : _uart(uart), _mavlink_channel(mavlink_channel), _packet_drops(0) {
bhepp 0:28183cc7963f 7 setSysId(sysid);
bhepp 0:28183cc7963f 8 setCompId(compid);
bhepp 0:28183cc7963f 9 }
bhepp 0:28183cc7963f 10
bhepp 0:28183cc7963f 11 void MAVLinkBridge::sendMessage(const mavlink_message_t& msg) {
bhepp 0:28183cc7963f 12 uint8_t buf[MAVLINK_MAX_PACKET_LEN];
bhepp 0:28183cc7963f 13 uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
bhepp 0:28183cc7963f 14 sendBuffer(buf, len);
bhepp 0:28183cc7963f 15 }
bhepp 0:28183cc7963f 16
bhepp 0:28183cc7963f 17 void MAVLinkBridge::sendBuffer(const uint8_t* buf, uint16_t len) {
bhepp 0:28183cc7963f 18 for (uint16_t i = 0; i < len; ++i) {
bhepp 0:28183cc7963f 19 _uart->writeChar(buf[i]);
bhepp 0:28183cc7963f 20 }
bhepp 0:28183cc7963f 21 }
bhepp 0:28183cc7963f 22
bhepp 0:28183cc7963f 23 bool MAVLinkBridge::tryReceiveMessage() {
bhepp 0:28183cc7963f 24 mavlink_status_t status;
bhepp 0:28183cc7963f 25 while (_uart->isCharAvailable()) {
bhepp 0:28183cc7963f 26 uint8_t c = _uart->readChar();
bhepp 0:28183cc7963f 27 // Try to get a new message
bhepp 0:28183cc7963f 28 if (mavlink_parse_char(_mavlink_channel, c, &_recv_msg, &status)) {
bhepp 0:28183cc7963f 29 return true;
bhepp 0:28183cc7963f 30 }
bhepp 0:28183cc7963f 31 }
bhepp 0:28183cc7963f 32
bhepp 0:28183cc7963f 33 // Update packet drops counter
bhepp 0:28183cc7963f 34 _packet_drops += _recv_status.packet_rx_drop_count;
bhepp 0:28183cc7963f 35
bhepp 0:28183cc7963f 36 return false;
bhepp 0:28183cc7963f 37 }
bhepp 0:28183cc7963f 38
bhepp 0:28183cc7963f 39 const mavlink_message_t& MAVLinkBridge::getLastReceivedMessage() {
bhepp 0:28183cc7963f 40 return _recv_msg;
bhepp 0:28183cc7963f 41 }