mirror of
https://github.com/servalproject/serval-dna.git
synced 2024-12-24 15:26:43 +00:00
300 lines
11 KiB
C
300 lines
11 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"
|
|
#include "overlay_buffer.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++;
|
|
}
|
|
buf[length+6]=sum&0xff;
|
|
buf[length+7]=sum>>8;
|
|
|
|
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 mavlink_encode_packet(struct overlay_interface *interface)
|
|
{
|
|
int count = ob_remaining(interface->tx_packet);
|
|
int startP = !ob_position(interface->tx_packet);
|
|
int endP = 1;
|
|
if (count+6+32 > 255){
|
|
count = 255-6-32;
|
|
endP = 0;
|
|
}
|
|
|
|
interface->txbuffer[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.
|
|
*/
|
|
// we need 32 bytes for the parity, but this field assumes
|
|
// that there is a 2 byte CRC, so we can save two bytes
|
|
int len = count+32 - 2;
|
|
interface->txbuffer[1]=len;
|
|
interface->txbuffer[2]=0; // packet sequence
|
|
interface->txbuffer[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
|
|
interface->txbuffer[4]=0x40; // component ID of sender: we are reusing this to mark start,end of MDP frames
|
|
if (startP) interface->txbuffer[4]|=0x01;
|
|
if (endP) interface->txbuffer[4]|=0x02;
|
|
interface->txbuffer[5]=MAVLINK_MSG_ID_DATASTREAM; // message ID type of this frame: DATA_STREAM
|
|
// payload follows (we reuse the DATA_STREAM message type parameters)
|
|
ob_get_bytes(interface->tx_packet, &interface->txbuffer[6], count);
|
|
|
|
encode_rs_8(&interface->txbuffer[2], &interface->txbuffer[6+count], 223 - (count+4));
|
|
interface->tx_bytes_pending=len + 8;
|
|
if (endP){
|
|
ob_free(interface->tx_packet);
|
|
interface->tx_packet=NULL;
|
|
overlay_queue_schedule_next(gettime_ms());
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int mavlink_heartbeat(unsigned char *frame,int *outlen)
|
|
{
|
|
int count=9;
|
|
bzero(frame, count+8);
|
|
|
|
frame[0]=0xfe; // mavlink v1.0 frame
|
|
// Must be 9 to indicate heartbeat
|
|
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)
|
|
// Must be zero to indicate heartbeat
|
|
frame[5]=0; // message ID type of this frame: DATA_STREAM
|
|
|
|
// empty payload
|
|
|
|
// two-byte CRC follows
|
|
mavlink_crc(frame,count); // automatically adds 6 for header length
|
|
|
|
*outlen=count+8;
|
|
|
|
return 0;
|
|
}
|
|
|
|
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)
|
|
{
|
|
int packet_length=state->mavlink_payload[1];
|
|
if (packet_length==9){
|
|
if (state->mavlink_payload[5]==MAVLINK_MSG_ID_RADIO){
|
|
// we can assume that radio status packets arrive without corruption
|
|
last_radio_rssi=(1.0*state->mavlink_payload[10]-state->mavlink_payload[13])/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, buffer space = %d%%",
|
|
last_radio_rssi,
|
|
(int)((1.0*state->mavlink_payload[11]-state->mavlink_payload[14])/1.9),
|
|
state->mavlink_payload[12]);
|
|
last_rssi_time=gettime_ms();
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int data_bytes = packet_length - (32 - 2);
|
|
|
|
int errcount=decode_rs_8(&state->mavlink_payload[2], NULL, 0, 223 - (data_bytes + 4));
|
|
if (errcount==-1){
|
|
if (config.debug.mavlink)
|
|
DEBUGF("Reed-Solomon error correction failed");
|
|
state->packet_length=sizeof(state->dst)+1;
|
|
return 0;
|
|
}
|
|
|
|
if (config.debug.mavlink){
|
|
DEBUGF("Received RS protected message, len: %d, errors: %d, flags:%s%s",
|
|
data_bytes,
|
|
errcount,
|
|
state->mavlink_payload[4]&0x01?" start":"",
|
|
state->mavlink_payload[4]&0x02?" end":"");
|
|
}
|
|
|
|
if (state->mavlink_payload[4]&0x01)
|
|
state->packet_length=0;
|
|
|
|
if (state->packet_length + data_bytes > sizeof(state->dst)){
|
|
if (config.debug.mavlink)
|
|
DEBUG("Fragmented packet is too long or a previous piece was missed - discarding");
|
|
state->packet_length=sizeof(state->dst)+1;
|
|
return 0;
|
|
}
|
|
|
|
bcopy(&state->mavlink_payload[6], &state->dst[state->packet_length], data_bytes);
|
|
state->packet_length+=data_bytes;
|
|
|
|
if (state->mavlink_payload[4]&0x02) {
|
|
if (config.debug.mavlink)
|
|
DEBUGF("PDU Complete (length=%d)",state->packet_length);
|
|
state->dst_offset=0;
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int mavlink_decode(struct slip_decode_state *state, uint8_t c)
|
|
{
|
|
int ret=0;
|
|
// skip all bytes until the next packet boundary
|
|
if (state->mavlink_payload_offset==0 && c!=0xfe){
|
|
if (config.debug.mavlink)
|
|
DEBUGF("Waiting for valid header, skipped byte 0x%02x",c);
|
|
return 0;
|
|
}
|
|
|
|
if (state->mavlink_payload_offset==1){
|
|
// check that the length seems to be valid
|
|
// radio heartbeats have length==9
|
|
// RS protected frames must be >= (1 + 32 - 2) bytes
|
|
if (c+8>255 || (c<31 && c!=9)){
|
|
if (config.debug.mavlink)
|
|
DEBUGF("Waiting for valid header, skipped byte 0x%02x",c);
|
|
state->mavlink_payload_offset=0;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
state->mavlink_payload[state->mavlink_payload_offset++]=c;
|
|
|
|
if (state->mavlink_payload_offset >= state->mavlink_payload[1]+8){
|
|
ret = mavlink_parse(state);
|
|
state->mavlink_payload_offset=0;
|
|
}
|
|
|
|
return ret;
|
|
}
|