Implement flow of mavlink packets over serial connections

Replaces upper-7 bit slip encoding
Solves reliability of reading radio status messages
This commit is contained in:
gardners 2013-08-29 15:50:13 +09:30 committed by Jeremy Lakeman
parent 8ff14dce7a
commit 7248e01567
8 changed files with 320 additions and 129 deletions

View File

@ -239,6 +239,7 @@ ATOM(bool_t, gateway, 0, boolean,, "")
ATOM(bool_t, keyring, 0, boolean,, "")
ATOM(bool_t, security, 0, boolean,, "")
ATOM(bool_t, mdprequests, 0, boolean,, "")
ATOM(bool_t, mavlink, 0, boolean,, "")
ATOM(bool_t, peers, 0, boolean,, "")
ATOM(bool_t, overlayframes, 0, boolean,, "")
ATOM(bool_t, overlayabbreviations, 0, boolean,, "")

5
lsif.c
View File

@ -54,10 +54,13 @@
#ifdef HAVE_LINUX_IF_H
#include <linux/if.h>
#else
#ifdef HAVE_NET_IF_H
#if HAVE_NET_IF_H || __MACH__ || __NetBSD__ || __OpenBSD__ || __FreeBSD__
#include <net/if.h>
#endif
#endif
#ifdef HAVE_IFADDRS_H
#include <ifaddrs.h>
#endif
/* On platforms that have variable length
ifreq use the old fixed length interface instead */

332
mavlink.c
View File

@ -1,4 +1,4 @@
// -*- Mode: C; c-basic-offset: 8; -*-
// -*- Mode: C; c-basic-offset: 2; -*-
//
// Copyright (c) 2012 Andrew Tridgell, All Rights Reserved
//
@ -26,140 +26,248 @@
// OF THE POSSIBILITY OF SUCH DAMAGE.
//
///
/// @file mavlink.c
///
/// mavlink reporting code
///
/*
Portions Copyright (C) 2013 Paul Gardner-Stephen
This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/
#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;
#include "serval.h"
#include "conf.h"
#define MAVLINK_MSG_ID_RADIO 166
#define MAVLINK_RADIO_CRC_EXTRA 21
#define MAVLINK_MSG_ID_DATASTREAM 67
int MAVLINK_MESSAGE_CRCS[]={72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7};
// 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)
uint16_t mavlink_crc(unsigned char *buf,int length)
{
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;
uint16_t sum = 0xFFFF;
uint8_t i, stoplen;
stoplen = length + 6;
// MAVLink 1.0 has an extra CRC seed
buf[length+6] = MAVLINK_MESSAGE_CRCS[buf[5]];
stoplen++;
i = 1;
while (i<stoplen) {
uint8_t tmp;
tmp = buf[i] ^ (uint8_t)(sum&0xff);
tmp ^= (tmp<<4);
sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4);
i++;
}
return sum;
}
/*
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>
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;
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;
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)
int stream_as_mavlink(int f,unsigned char *data,int count,
unsigned char *frame,int *outlen)
{
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;
}
if (count>252-6-2) return -1;
frame[0]=0xfe; // mavlink v1.0 frame
frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC
frame[2]=f; // packet sequence
frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
frame[4]=0xf1; // component ID of sender (MAV_COMP_ID_UART_BRIDGE)
frame[5]=MAVLINK_MSG_ID_DATASTREAM; // message ID type of this frame: DATA_STREAM
// payload follows (we reuse the DATA_STREAM message type parameters)
bcopy(data,&frame[6],count);
// two-byte CRC follows
uint16_t crc=mavlink_crc(frame,count); // automatically adds 6 for header length
frame[count+6]=crc&0xff;
frame[count+7]=crc>>8;
*outlen=count+6+2;
return 0;
}
/// send a MAVLink status report packet
void MAVLink_report(void)
int mavlink_heartbeat(unsigned char *frame,int *outlen)
{
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;
int count=9;
frame[0]=0xfe; // mavlink v1.0 frame
frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC
frame[2]=0; // packet sequence
frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
frame[4]=0xf1; // component ID of sender (MAV_COMP_ID_UART_BRIDGE)
frame[5]=0; // message ID type of this frame: DATA_STREAM
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();
// payload follows
bzero(&frame[6],count);
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);
// two-byte CRC follows
uint16_t crc=mavlink_crc(frame,count); // automatically adds 6 for header length
frame[count+6]=crc&0xff;
frame[count+7]=crc>>8;
*outlen=count+6+2;
return 0;
}
#define MAVLINK_STATE_UNKNOWN 0
#define MAVLINK_STATE_LENGTH 1
#define MAVLINK_STATE_SEQ 2
#define MAVLINK_STATE_SYSID 3
#define MAVLINK_STATE_COMPONENTID 4
#define MAVLINK_STATE_MSGID 5
#define MAVLINK_STATE_PAYLOAD 6
#define MAVLINK_STATE_CRC1 7
#define MAVLINK_STATE_CRC2 8
char *mavlink_describe_state(int state)
{
switch(state) {
case MAVLINK_STATE_UNKNOWN: return "MAVLINK_STATE_UNKNOWN";
case MAVLINK_STATE_LENGTH: return "MAVLINK_STATE_LENGTH";
case MAVLINK_STATE_SEQ: return "MAVLINK_STATE_SEQ";
case MAVLINK_STATE_SYSID: return "MAVLINK_STATE_SYSID";
case MAVLINK_STATE_COMPONENTID: return "MAVLINK_STATE_COMPONENTID";
case MAVLINK_STATE_MSGID: return "MAVLINK_STATE_MSGID";
case MAVLINK_STATE_PAYLOAD: return "MAVLINK_STATE_PAYLOAD";
case MAVLINK_STATE_CRC1: return "MAVLINK_STATE_CRC1";
case MAVLINK_STATE_CRC2: return "MAVLINK_STATE_CRC2";
default: return "INVALID_STATE";
}
}
extern unsigned long long last_rssi_time;
extern int last_radio_rssi;
extern int last_radio_temperature;
extern int last_radio_rxpackets;
int mavlink_parse(struct slip_decode_state *state)
{
switch(state->mavlink_msgid) {
case MAVLINK_MSG_ID_RADIO:
if (config.debug.mavlink) DEBUG("Received MAVLink radio report");
last_radio_rssi=(1.0*state->mavlink_payload[5]-state->mavlink_payload[8])/1.9;
last_radio_temperature=-999; // doesn't get reported
last_radio_rxpackets=-999; // doesn't get reported
if (config.debug.mavlink||gettime_ms()-last_rssi_time>30000) {
INFOF("Link budget = %+ddB, remote link budget = %+ddB",
last_radio_rssi,
(int)((1.0*state->mavlink_payload[6]-state->mavlink_payload[9])/1.9));
last_rssi_time=gettime_ms();
}
return 0;
case MAVLINK_MSG_ID_DATASTREAM:
// Extract and return packet
// XXX - Ignores CRC at present
if (config.debug.mavlink) DEBUG("Received MAVLink DATASTREAM message for us");
bcopy(state->mavlink_payload,state->dst,state->mavlink_payload_length);
state->packet_length=state->mavlink_payload_length;
return 1;
break;
default:
if (config.debug.mavlink)
DEBUGF("Received unknown MAVLink message type 0x%02x",
state->mavlink_msgid);
return -1;
}
}
int mavlink_decode(struct slip_decode_state *state,uint8_t c)
{
if (config.debug.mavlink) DEBUGF("RX character %02x in state %s",
c,mavlink_describe_state(state->state));
switch(state->state) {
case MAVLINK_STATE_LENGTH:
state->mavlink_payload_length=c;
state->mavlink_payload_offset=0;
state->state++;
break;
case MAVLINK_STATE_SEQ:
state->mavlink_sequence=c;
state->state++;
break;
case MAVLINK_STATE_SYSID:
state->mavlink_sysid=c;
state->state++;
break;
case MAVLINK_STATE_COMPONENTID:
state->mavlink_componentid=c;
state->state++;
break;
case MAVLINK_STATE_MSGID:
state->mavlink_msgid=c;
state->state++;
break;
case MAVLINK_STATE_PAYLOAD:
state->mavlink_payload[state->mavlink_payload_offset++]=c;
if (state->mavlink_payload_offset==state->mavlink_payload_length)
state->state++;
break;
case MAVLINK_STATE_CRC1:
state->mavlink_rxcrc=c;
state->state++;
break;
case MAVLINK_STATE_CRC2:
state->mavlink_rxcrc|=c<<8;
state->state=MAVLINK_STATE_UNKNOWN;
return mavlink_parse(state);
break;
case MAVLINK_STATE_UNKNOWN:
default:
if (c==0xfe) state->state=MAVLINK_STATE_LENGTH;
else {
state->state=MAVLINK_STATE_UNKNOWN;
}
}
return 0;
}

View File

@ -57,7 +57,7 @@ static void
overlay_interface_close(overlay_interface *interface){
link_interface_down(interface);
INFOF("Interface %s addr %s is down",
interface->name, inet_ntoa(interface->address.sin_addr));
interface->name, inet_ntoa(interface->address.sin_addr));
unschedule(&interface->alarm);
unwatch(&interface->alarm);
close(interface->alarm.poll.fd);
@ -495,7 +495,7 @@ overlay_interface_init(const char *name, struct in_addr src_addr, struct in_addr
interface->slip_decode_state.dst_offset=0;
/* The encapsulation type should be configurable, but for now default to the one that should
be safe on the RFD900 radios, and that also allows us to receive RSSI reports inline */
interface->slip_decode_state.encapsulator=SLIP_FORMAT_UPPER7;
interface->slip_decode_state.encapsulator=SLIP_FORMAT_MAVLINK;
interface->alarm.poll.events=POLLIN;
watch(&interface->alarm);
break;
@ -882,7 +882,7 @@ overlay_broadcast_ensemble(struct network_destination *destination,
unsigned char *buffer = interface->txbuffer;
int out_len=0;
int encoded = slip_encode(SLIP_FORMAT_UPPER7,
int encoded = slip_encode(SLIP_FORMAT_MAVLINK,
bytes, len, buffer+out_len, sizeof(interface->txbuffer) - out_len);
if (encoded < 0)
return WHY("Buffer overflow");
@ -891,7 +891,7 @@ overlay_broadcast_ensemble(struct network_destination *destination,
{
// Test decoding of the packet we send
struct slip_decode_state state;
state.encapsulator=SLIP_FORMAT_UPPER7;
state.encapsulator=SLIP_FORMAT_MAVLINK;
state.src_size=encoded;
state.src_offset=0;
state.src=buffer+out_len;
@ -1063,7 +1063,7 @@ void overlay_interface_discover(struct sched_ent *alarm)
int i;
for (i = 0; i < overlay_interface_count; i++)
if (overlay_interfaces[i].state==INTERFACE_STATE_UP)
overlay_interfaces[i].state=INTERFACE_STATE_DETECTING;
overlay_interfaces[i].state=INTERFACE_STATE_DETECTING;
/* Register new dummy interfaces */
int detect_real_interfaces = 0;
@ -1115,8 +1115,10 @@ void overlay_interface_discover(struct sched_ent *alarm)
// Close any interfaces that have gone away.
for(i = 0; i < overlay_interface_count; i++)
if (overlay_interfaces[i].state==INTERFACE_STATE_DETECTING)
if (overlay_interfaces[i].state==INTERFACE_STATE_DETECTING) {
DEBUGF("Closing interface stuck in DETECTING state.");
overlay_interface_close(&overlay_interfaces[i]);
}
alarm->alarm = gettime_ms()+5000;
alarm->deadline = alarm->alarm + 10000;

View File

@ -68,21 +68,20 @@ int overlay_packetradio_setup_port(overlay_interface *interface)
if (tcsetattr(interface->alarm.poll.fd, TCSANOW, &t))
WHY_perror("Failed to set terminal parameters");
// Ask radio to report RSSI
(void)write_all(interface->alarm.poll.fd,"\r",1);
sleep_ms(600);
(void)write_all(interface->alarm.poll.fd,"\r",1);
sleep_ms(600);
(void)write_all(interface->alarm.poll.fd,"+++",3);
sleep_ms(1200);
(void)write_all(interface->alarm.poll.fd,"\rAT&T\rAT&T=RSSI\rATO\r",20);
// Enable MAVLink mode to get regular RSSI reports
uint8_t buf[256];
int buflen=0;
buflen=0;
mavlink_heartbeat(buf,&buflen);
(void)write_all(interface->alarm.poll.fd,buf,buflen);
if (config.debug.packetradio) {
tcgetattr(interface->alarm.poll.fd, &t);
int in_speed=cfgetispeed(&t);
int out_speed=cfgetospeed(&t);
DEBUGF("Enabled RSSI reporting for RFD900 radios");
DEBUGF("Sent ATO to make sure we are in on-line mode");
DEBUGF("Enabled MAVLink based RSSI reporting for RFD900 radios");
DEBUGF("uart speed reported as %d/%d",in_speed,out_speed);
}

View File

@ -368,6 +368,7 @@ extern int overlayMode;
struct slip_decode_state{
#define SLIP_FORMAT_SLIP 0
#define SLIP_FORMAT_UPPER7 1
#define SLIP_FORMAT_MAVLINK 2
int encapsulator;
int state;
unsigned char *src;
@ -379,6 +380,15 @@ struct slip_decode_state{
uint32_t crc;
int src_offset;
int dst_offset;
uint8_t mavlink_payload_length;
uint8_t mavlink_payload_offset;
uint8_t mavlink_payload[256];
uint8_t mavlink_sequence;
uint8_t mavlink_sysid;
uint8_t mavlink_componentid;
uint8_t mavlink_msgid;
uint16_t mavlink_rxcrc;
};
struct overlay_interface;
@ -866,4 +876,9 @@ int link_add_destinations(struct overlay_frame *frame);
int generate_nonce(unsigned char *nonce,int bytes);
int mavlink_decode(struct slip_decode_state *state,uint8_t c);
int mavlink_heartbeat(unsigned char *frame,int *outlen);
int stream_as_mavlink(int f,unsigned char *data,int count,
unsigned char *frame,int *outlen);
#endif // __SERVALD_SERVALD_H

62
slip.c
View File

@ -1,3 +1,22 @@
/*
Serval Distributed Numbering Architecture (DNA)
Copyright (C) 2012 Paul Gardner-Stephen
This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/
#include "serval.h"
#include "conf.h"
#include "log.h"
@ -25,6 +44,32 @@ int slip_encode(int format,
unsigned char *src, int src_bytes, unsigned char *dst, int dst_len)
{
switch(format) {
case SLIP_FORMAT_MAVLINK:
{
int i;
int dst_offset=0;
for(i=0;i<src_bytes;i+=240) {
int slice_len=0;
int slice_bytes=240;
if (i+slice_bytes>src_bytes) slice_bytes=src_bytes-i;
if (dst_offset+slice_bytes+6+2>=dst_len) {
if (config.debug.mavlink)
DEBUGF("Would overflow output buffer.");
return -1;
} else {
stream_as_mavlink(0,&src[i],slice_bytes,&dst[dst_offset],&slice_len);
if (config.debug.mavlink) {
DEBUGF("Wrote %d bytes as %d byte MAVLink frame",
slice_bytes,slice_len);
dump("original data",&src[i],slice_bytes);
dump("mavlink frame",&dst[dst_offset],slice_len);
}
dst_offset+=slice_len;
}
}
return dst_offset;
}
break;
case SLIP_FORMAT_SLIP:
{
int offset=0;
@ -330,6 +375,23 @@ int upper7_decode(struct slip_decode_state *state,unsigned char byte)
int slip_decode(struct slip_decode_state *state)
{
switch(state->encapsulator) {
case SLIP_FORMAT_MAVLINK:
{
dump("rx data",state->src,state->src_size);
for(;state->src_offset<state->src_size;state->src_offset++) {
// flag complete reception of a packet
DEBUGF("src_offset=%d, src_size=%d, c=%02x",
state->src_offset,state->src_size,state->src[state->src_offset]);
if (mavlink_decode(state,state->src[state->src_offset])==1) {
// We have to increment src_offset manually here, because returning
// prevents the post-increment in the for loop from triggering
state->src_offset++;
return 1;
}
}
}
return 0;
break;
case SLIP_FORMAT_SLIP:
{
/*

View File

@ -17,6 +17,7 @@ SERVAL_SOURCES = \
$(SERVAL_BASE)log.c \
$(SERVAL_BASE)lsif.c \
$(SERVAL_BASE)main.c \
$(SERVAL_BASE)mavlink.c \
$(SERVAL_BASE)meshms.c \
$(SERVAL_BASE)mdp_client.c \
$(SERVAL_BASE)os.c \