mirror of
https://github.com/servalproject/serval-dna.git
synced 2024-12-18 20:57:56 +00:00
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
Jeremy Lakeman
parent
8ff14dce7a
commit
7248e01567
@ -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
5
lsif.c
@ -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 */
|
||||
|
||||
248
mavlink.c
248
mavlink.c
@ -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,57 +26,56 @@
|
||||
// OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
//
|
||||
|
||||
///
|
||||
/// @file mavlink.c
|
||||
///
|
||||
/// mavlink reporting code
|
||||
///
|
||||
/*
|
||||
Portions Copyright (C) 2013 Paul Gardner-Stephen
|
||||
|
||||
#include <stdarg.h>
|
||||
#include "radio.h"
|
||||
#include "packet.h"
|
||||
#include "timer.h"
|
||||
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.
|
||||
|
||||
extern __xdata uint8_t pbuf[MAX_PACKET_LENGTH];
|
||||
static __pdata uint8_t seqnum;
|
||||
extern bool using_mavlink_10;
|
||||
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"
|
||||
|
||||
#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;
|
||||
uint16_t sum = 0xFFFF;
|
||||
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;
|
||||
buf[length+6] = MAVLINK_MESSAGE_CRCS[buf[5]];
|
||||
stoplen++;
|
||||
}
|
||||
|
||||
i = 1;
|
||||
while (i<stoplen) {
|
||||
register uint8_t tmp;
|
||||
tmp = pbuf[i] ^ (uint8_t)(sum&0xff);
|
||||
uint8_t tmp;
|
||||
tmp = buf[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;
|
||||
return sum;
|
||||
}
|
||||
|
||||
|
||||
@ -114,52 +113,161 @@ struct mavlink_RADIO_v10 {
|
||||
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;
|
||||
}
|
||||
|
||||
int mavlink_heartbeat(unsigned char *frame,int *outlen)
|
||||
{
|
||||
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
|
||||
|
||||
// payload follows
|
||||
bzero(&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;
|
||||
}
|
||||
|
||||
#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";
|
||||
}
|
||||
}
|
||||
|
||||
/// send a MAVLink status report packet
|
||||
void MAVLink_report(void)
|
||||
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)
|
||||
{
|
||||
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);
|
||||
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();
|
||||
}
|
||||
mavlink_crc();
|
||||
|
||||
if (serial_write_space() < sizeof(struct mavlink_RADIO_v09)+8) {
|
||||
// don't cause an overflow
|
||||
return;
|
||||
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;
|
||||
}
|
||||
|
||||
serial_write_buf(pbuf, sizeof(struct mavlink_RADIO_v09)+8);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
15
serval.h
15
serval.h
@ -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
62
slip.c
@ -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:
|
||||
{
|
||||
/*
|
||||
|
||||
@ -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 \
|
||||
|
||||
Loading…
Reference in New Issue
Block a user