mirror of
https://github.com/servalproject/serval-dna.git
synced 2024-12-19 05:07:56 +00:00
341 lines
12 KiB
C
341 lines
12 KiB
C
// -*- Mode: C; c-basic-offset: 2; -*-
|
|
//
|
|
// 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.
|
|
//
|
|
|
|
/*
|
|
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 "serval.h"
|
|
#include "conf.h"
|
|
|
|
#define MAVLINK_MSG_ID_RADIO 166
|
|
#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'
|
|
|
|
uint16_t mavlink_crc(unsigned char *buf,int length)
|
|
{
|
|
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>
|
|
*/
|
|
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;
|
|
};
|
|
|
|
/*
|
|
Each mavlink frame consists of 0xfe followed by a standard 6 byte header.
|
|
Normally the payload plus a 2-byte CRC follows.
|
|
We are replacing the CRC check with a Reed-Solomon code to correct as well
|
|
as detect upto 16 bytes with errors, in return for a 32-byte overhead.
|
|
|
|
The nature of the particular library we are using is that the overhead is
|
|
basically fixed, but we can shorten the data section.
|
|
|
|
Note that the mavlink headers are not protected against errors. This is a
|
|
limitation of the radio firmware at present. One day we will re-write the
|
|
radio firmware so that we can send and receive raw radio frames, and get
|
|
rid of the mavlink framing altogether, and just send R-S protected payloads.
|
|
|
|
Not ideal, but will be fine for now.
|
|
*/
|
|
|
|
#include "fec-3.0.1/fixed.h"
|
|
void encode_rs_8(data_t *data, data_t *parity,int pad);
|
|
int decode_rs_8(data_t *data, int *eras_pos, int no_eras, int pad);
|
|
|
|
int stream_as_mavlink(int sequence_number,int startP,int endP,
|
|
unsigned char *data,int count,
|
|
unsigned char *frame,int *outlen)
|
|
{
|
|
if (count>252-6-32) return -1;
|
|
|
|
frame[0]=0xfe; // mavlink v1.0 frame
|
|
/* payload len, excluding 6 byte header and 2 byte CRC.
|
|
But we use a 4-byte CRC, so need to add two to count to make packet lengths
|
|
be as expected.
|
|
Note that this construction will result in CRC errors by non-servald
|
|
programmes, which is probably more helpful than otherwise.
|
|
*/
|
|
frame[1]=count+32-2; // we need 32 bytes for the parity, but this field assumes
|
|
// that there is a 2 byte CRC, so we can save two bytes
|
|
frame[2]=sequence_number; // packet sequence
|
|
frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
|
|
frame[4]=0x40; // component ID of sender: we are reusing this to mark start,end of MDP frames
|
|
if (startP) frame[4]|=0x01;
|
|
if (endP) frame[4]|=0x02;
|
|
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);
|
|
|
|
encode_rs_8(&frame[6],&frame[6+count],(223-count));
|
|
|
|
*outlen=6+count+32;
|
|
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";
|
|
}
|
|
}
|
|
|
|
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, after applying Reed-Solomon error detection
|
|
// and correction
|
|
if (config.debug.mavlink) DEBUG("Received MAVLink DATASTREAM message for us");
|
|
if (state->mavlink_componentid&0x01) {
|
|
if (config.debug.mavlink) {
|
|
DEBUGF("Found start of PDU mavlink-seq=0x%02x",state->mavlink_sequence);
|
|
if (state->packet_length) DEBUGF("... previous packet had not ended, discarding");
|
|
}
|
|
state->packet_length=0;
|
|
} else {
|
|
if (config.debug.mavlink) {
|
|
DEBUGF("Extension PDU mavlink-seq=0x%02x",state->mavlink_sequence);
|
|
if (state->packet_length) DEBUGF("... previous packet had not ended, discarding");
|
|
}
|
|
}
|
|
if (state->packet_length+state->mavlink_payload_length>sizeof(state->dst))
|
|
{
|
|
if (config.debug.mavlink) DEBUG("Too many extension frames for packet - discarding");
|
|
return 0;
|
|
}
|
|
int errcount=decode_rs_8(&state->mavlink_payload[0],NULL,0,
|
|
223-(state->mavlink_payload_length-30));
|
|
if (errcount==-1)
|
|
{
|
|
// DEBUGF("Reed-Solomon error correction failed");
|
|
return -1;
|
|
} // else DEBUGF("Reed-Solomon corrected %d bytes",errcount);
|
|
bcopy(state->mavlink_payload,&state->dst[state->packet_length],
|
|
state->mavlink_payload_length-30);
|
|
state->packet_length+=state->mavlink_payload_length-30;
|
|
if (state->mavlink_componentid&0x02) {
|
|
if (config.debug.mavlink)
|
|
DEBUGF("PDU Complete (length=%d)",state->packet_length);
|
|
state->dst_offset=0;
|
|
return 1;
|
|
} else return 0;
|
|
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.mavlinkfsm) DEBUGF("RX character %02x in state %s",
|
|
c,mavlink_describe_state(state->state));
|
|
switch(state->state) {
|
|
case MAVLINK_STATE_LENGTH:
|
|
state->mavlink_crc=0;
|
|
state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
|
|
state->mavlink_payload_length=c;
|
|
state->mavlink_payload_offset=0;
|
|
state->state++;
|
|
break;
|
|
case MAVLINK_STATE_SEQ:
|
|
state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
|
|
state->mavlink_sequence=c;
|
|
state->state++;
|
|
break;
|
|
case MAVLINK_STATE_SYSID:
|
|
state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
|
|
state->mavlink_sysid=c;
|
|
state->state++;
|
|
break;
|
|
case MAVLINK_STATE_COMPONENTID:
|
|
state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
|
|
state->mavlink_componentid=c;
|
|
state->state++;
|
|
break;
|
|
case MAVLINK_STATE_MSGID:
|
|
state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
|
|
state->mavlink_msgid=c;
|
|
state->state++;
|
|
break;
|
|
case MAVLINK_STATE_PAYLOAD:
|
|
if (state->mavlink_payload_length-state->mavlink_payload_offset>2)
|
|
state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
|
|
if (state->mavlink_payload_length-state->mavlink_payload_offset==2)
|
|
state->mavlink_rxcrc=c;
|
|
if (state->mavlink_payload_length-state->mavlink_payload_offset==1)
|
|
state->mavlink_rxcrc|=c<<8;
|
|
state->mavlink_payload[state->mavlink_payload_offset++]=c;
|
|
if (state->mavlink_payload_offset==state->mavlink_payload_length+2)
|
|
{
|
|
int r=mavlink_parse(state);
|
|
state->mavlink_payload_length=0;
|
|
state->state=MAVLINK_STATE_UNKNOWN;
|
|
if (r==1) return 1;
|
|
}
|
|
break;
|
|
case MAVLINK_STATE_UNKNOWN:
|
|
default:
|
|
if (c==0xfe) state->state=MAVLINK_STATE_LENGTH;
|
|
else {
|
|
state->state=MAVLINK_STATE_UNKNOWN;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|