ExternalVendorCode/serval-dna
1
0
Fork 0
mirror of https://github.com/servalproject/serval-dna.git synced 2025-01-29 15:43:56 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

import mavlink.c from rfd900 radio firmware to use as basis for

Browse Source 
mavlink communications with radios.
This commit is contained in:
gardners 2013-08-29 15:26:17 +09:30 committed by Jeremy Lakeman
parent da32f6e6cd
commit 8ff14dce7a
1 changed files with 165 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

165
mavlink.c Normal file
View File

@ -0,0 +1,165 @@
// -*- Mode: C; c-basic-offset: 8; -*-
//
// Copyright (c) 2012 Andrew Tridgell, All Rights Reserved
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// o Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// o Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in
// the documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.
//
///
/// @file mavlink.c
///
/// mavlink reporting code
///
#include <stdarg.h>
#include "radio.h"
#include "packet.h"
#include "timer.h"
extern __xdata uint8_t pbuf[MAX_PACKET_LENGTH];
static __pdata uint8_t seqnum;
extern bool using_mavlink_10;
#define MAVLINK_MSG_ID_RADIO 166
#define MAVLINK_RADIO_CRC_EXTRA 21
// use '3D' for 3DRadio
#define RADIO_SOURCE_SYSTEM '3'
#define RADIO_SOURCE_COMPONENT 'D'
/*
* Calculates the MAVLink checksum on a packet in pbuf[]
* and append it after the data
*/
static void mavlink_crc(void)
{
register uint8_t length = pbuf[1];
__pdata uint16_t sum = 0xFFFF;
__pdata uint8_t i, stoplen;
stoplen = length + 6;
if (using_mavlink_10) {
// MAVLink 1.0 has an extra CRC seed
pbuf[length+6] = MAVLINK_RADIO_CRC_EXTRA;
stoplen++;
}
i = 1;
while (i<stoplen) {
register uint8_t tmp;
tmp = pbuf[i] ^ (uint8_t)(sum&0xff);
tmp ^= (tmp<<4);
sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4);
i++;
}
pbuf[length+6] = sum&0xFF;
pbuf[length+7] = sum>>8;
}
/*
we use a hand-crafted MAVLink packet based on the following
message definition
<message name="RADIO" id="166">
<description>Status generated by radio</description>
<field type="uint8_t" name="rssi">local signal strength</field>
<field type="uint8_t" name="remrssi">remote signal strength</field>
<field type="uint8_t" name="txbuf">percentage free space in transmit buffer</field>
<field type="uint8_t" name="noise">background noise level</field>
<field type="uint8_t" name="remnoise">remote background noise level</field>
<field type="uint16_t" name="rxerrors">receive errors</field>
<field type="uint16_t" name="fixed">count of error corrected packets</field>
</message>
*/
struct mavlink_RADIO_v09 {
uint8_t rssi;
uint8_t remrssi;
uint8_t txbuf;
uint8_t noise;
uint8_t remnoise;
uint16_t rxerrors;
uint16_t fixed;
};
struct mavlink_RADIO_v10 {
uint16_t rxerrors;
uint16_t fixed;
uint8_t rssi;
uint8_t remrssi;
uint8_t txbuf;
uint8_t noise;
uint8_t remnoise;
};
static void swap_bytes(__pdata uint8_t ofs, __pdata uint8_t len)
{
register uint8_t i;
for (i=ofs; i<ofs+len; i+=2) {
register uint8_t tmp = pbuf[i];
pbuf[i] = pbuf[i+1];
pbuf[i+1] = tmp;
}
}
/// send a MAVLink status report packet
void MAVLink_report(void)
{
pbuf[0] = using_mavlink_10?254:'U';
pbuf[1] = sizeof(struct mavlink_RADIO_v09);
pbuf[2] = seqnum++;
pbuf[3] = RADIO_SOURCE_SYSTEM;
pbuf[4] = RADIO_SOURCE_COMPONENT;
pbuf[5] = MAVLINK_MSG_ID_RADIO;
if (using_mavlink_10) {
struct mavlink_RADIO_v10 *m = (struct mavlink_RADIO_v10 *)&pbuf[6];
m->rxerrors = errors.rx_errors;
m->fixed = errors.corrected_packets;
m->txbuf = serial_read_space();
m->rssi = statistics.average_rssi;
m->remrssi = remote_statistics.average_rssi;
m->noise = statistics.average_noise;
m->remnoise = remote_statistics.average_noise;
} else {
struct mavlink_RADIO_v09 *m = (struct mavlink_RADIO_v09 *)&pbuf[6];
m->rxerrors = errors.rx_errors;
m->fixed = errors.corrected_packets;
m->txbuf = serial_read_space();
m->rssi = statistics.average_rssi;
m->remrssi = remote_statistics.average_rssi;
m->noise = statistics.average_noise;
m->remnoise = remote_statistics.average_noise;
swap_bytes(6+5, 4);
}
mavlink_crc();
if (serial_write_space() < sizeof(struct mavlink_RADIO_v09)+8) {
// don't cause an overflow
return;
}
serial_write_buf(pbuf, sizeof(struct mavlink_RADIO_v09)+8);
}