ExternalVendorCode/serval-dna
1
0
Fork 0
mirror of https://github.com/servalproject/serval-dna.git synced 2025-02-21 01:42:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Use constants instead of magic numbers

Browse Source
This commit is contained in:
Jeremy Lakeman 2013-11-25 15:12:21 +10:30
parent 99d5d9fa1f
commit cd639ba3b6
3 changed files with 83 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
overlay_interface.c
View File

@ -743,26 +743,31 @@ static void write_stream_buffer(overlay_interface *interface){
while (interface->tx_bytes_pending>0 || interface->tx_packet || interface->next_heartbeat <= now) {
if (interface->tx_bytes_pending==0){
// allocate tx buffer on first use
if (!interface->txbuffer){
interface->txbuffer=emalloc(OVERLAY_INTERFACE_RX_BUFFER_SIZE);
if (!interface->txbuffer)
break;
}
if (interface->next_heartbeat <= now){
if (!interface->txbuffer){
interface->txbuffer=emalloc(OVERLAY_INTERFACE_RX_BUFFER_SIZE);
if (!interface->txbuffer)
break;
}
// Queue a hearbeat now
radio_link_heartbeat(interface->txbuffer,&interface->tx_bytes_pending);
if (config.debug.packetradio)
DEBUGF("Sending heartbeat");
interface->next_heartbeat = now+1000;
}else if(interface->tx_packet && interface->remaining_space >= 256 + 8+9){
}else if(interface->remaining_space >= LINK_MTU + HEARTBEAT_SIZE){
// prepare a new link layer packet in txbuffer
if (radio_link_encode_packet(interface))
break;
if (interface->remaining_space - interface->tx_bytes_pending < 256 + 8+9)
if (interface->remaining_space - interface->tx_bytes_pending < LINK_MTU + HEARTBEAT_SIZE)
interface->next_heartbeat = now;
}
// nothing interesting to send, just break
if (interface->tx_bytes_pending==0)
break;
}
if (interface->next_tx_allowed > now)

127
radio_link.c
View File

@ -48,6 +48,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
#include "conf.h"
#include "overlay_buffer.h"
#include "golay.h"
#include "radio_link.h"
#define MAVLINK_MSG_ID_RADIO 166
#define MAVLINK_MSG_ID_DATASTREAM 67
@ -60,46 +61,46 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
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;
uint16_t rxerrors; // receive errors
uint16_t fixed; // count of error corrected packets
uint8_t rssi; // local signal strength
uint8_t remrssi; // remote signal strength
uint8_t txbuf; // percentage free space in transmit buffer
uint8_t noise; // background noise level
uint8_t remnoise; // remote background noise level
};
*/
#define FEC_LENGTH 32
#define FEC_BYTES 223
#define RADIO_HEADER_LENGTH 6
#define RADIO_CRC_LENGTH 2
#define LINK_PAYLOAD_MTU (LINK_MTU - FEC_LENGTH - RADIO_HEADER_LENGTH - RADIO_CRC_LENGTH)
struct radio_link_state{
// next seq for transmission
int tx_seq;
// decoded length of next link layer packet
int payload_length;
// last rx seq for reassembly
int seq;
// offset within payload that we have found a valid looking header
int payload_start;
// offset after payload_start for incoming bytes
int payload_offset;
uint8_t payload[1024];
int packet_length;
// small buffer for parsing incoming bytes from the serial interface,
// looking for recoverable link layer packets
uint8_t payload[LINK_MTU*3];
// small buffer for assembling mdp payloads.
// should be large enough to hold MDP_MTU
uint8_t dst[OVERLAY_INTERFACE_RX_BUFFER_SIZE];
int dst_offset;
// length of recovered packet
int packet_length;
};
/*
@ -136,30 +137,34 @@ int radio_link_init(struct overlay_interface *interface)
return 0;
}
// write a new link layer packet to interface->txbuffer
// consuming more bytes from the next interface->tx_packet if required
int radio_link_encode_packet(struct overlay_interface *interface)
{
// if we have nothing interesting left to send, don't create a packet at all
if (!interface->tx_packet)
return 0;
int count = ob_remaining(interface->tx_packet);
int startP = (ob_position(interface->tx_packet) == 0);
int endP = 1;
if (count+6+32 > 255){
count = 255-6-32;
if (count > LINK_PAYLOAD_MTU){
count = LINK_PAYLOAD_MTU;
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[0]=0xfe; // mavlink v1.0 magic header
// we need to add FEC_LENGTH for FEC, but the length field doesn't include the expected headers or CRC
int len = count + FEC_LENGTH - RADIO_CRC_LENGTH;
interface->txbuffer[1]=len; // mavlink payload length
interface->txbuffer[2]=(len & 0xF);
interface->txbuffer[3]=0;
// add golay encoding so that decoding the actual length is more reliable
golay_encode(&interface->txbuffer[1]);
interface->txbuffer[4]=(interface->radio_link_state->tx_seq++) & 0x3f;
if (startP) interface->txbuffer[4]|=0x40;
if (endP) interface->txbuffer[4]|=0x80;
@ -167,8 +172,8 @@ int radio_link_encode_packet(struct overlay_interface *interface)
ob_get_bytes(interface->tx_packet, &interface->txbuffer[6], count);
encode_rs_8(&interface->txbuffer[4], &interface->txbuffer[6+count], 223 - (count+2));
interface->tx_bytes_pending=len + 8;
encode_rs_8(&interface->txbuffer[4], &interface->txbuffer[6+count], FEC_BYTES - (count+2));
interface->tx_bytes_pending=len + RADIO_CRC_LENGTH + RADIO_HEADER_LENGTH;
if (endP){
ob_free(interface->tx_packet);
interface->tx_packet=NULL;
@ -177,27 +182,28 @@ int radio_link_encode_packet(struct overlay_interface *interface)
return 0;
}
int radio_link_heartbeat(unsigned char *frame,int *outlen)
int radio_link_heartbeat(unsigned char *frame, int *outlen)
{
int count=9;
bzero(frame, count+8);
bzero(frame, count + RADIO_CRC_LENGTH + RADIO_HEADER_LENGTH);
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]=(count & 0xF); // packet sequence
frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
// we're golay encoding the length to improve the probability of skipping it correctly
golay_encode(&frame[1]);
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
// extra magic number to detect remote heartbeat requests
// extra magic number to help correctly detect remote heartbeat requests
frame[14]=0x55;
frame[15]=0x05;
golay_encode(&frame[14]);
*outlen=count+8;
*outlen=count + RADIO_CRC_LENGTH + RADIO_HEADER_LENGTH;
return 0;
}
@ -245,13 +251,13 @@ static int radio_link_parse(struct overlay_interface *interface, struct radio_li
return 0;
}
int data_bytes = packet_length - (32 - 2);
int data_bytes = packet_length - (FEC_LENGTH - RADIO_CRC_LENGTH);
// preserve the last 16 bytes of data
unsigned char old_footer[32];
unsigned char *payload_footer=&payload[packet_length+8-sizeof(old_footer)];
unsigned char old_footer[FEC_LENGTH];
unsigned char *payload_footer=&payload[packet_length + RADIO_HEADER_LENGTH + RADIO_CRC_LENGTH - sizeof(old_footer)];
bcopy(payload_footer, old_footer, sizeof(old_footer));
int pad=223 - (data_bytes + 2);
int pad=FEC_BYTES - (data_bytes + RADIO_CRC_LENGTH);
int errors=decode_rs_8(&payload[4], NULL, 0, pad);
if (errors==-1){
if (config.debug.radio_link)
@ -291,13 +297,12 @@ static int radio_link_parse(struct overlay_interface *interface, struct radio_li
return 1;
}
bcopy(&payload[6], &state->dst[state->packet_length], data_bytes);
bcopy(&payload[RADIO_HEADER_LENGTH], &state->dst[state->packet_length], data_bytes);
state->packet_length+=data_bytes;
if (payload[4]&0x80) {
if (config.debug.radio_link)
DEBUGF("PDU Complete (length=%d)",state->packet_length);
state->dst_offset=0;
packetOkOverlay(interface, state->dst, state->packet_length, -1, NULL, 0);
state->packet_length=sizeof(state->dst)+1;
@ -313,7 +318,7 @@ static int decode_length(struct radio_link_state *state, unsigned char *p)
if (length<0 || ((length >>8) & 0xF) != (length&0xF))
return -1;
length=length&0xFF;
if (length!=9 && (length<31 || length+8>255))
if (length!=9 && (length <= FEC_LENGTH - RADIO_CRC_LENGTH || length + RADIO_HEADER_LENGTH + RADIO_CRC_LENGTH > LINK_MTU))
return -1;
if (config.debug.radio_link && (errs || state->payload_length!=*p))
@ -361,15 +366,15 @@ int radio_link_decode(struct overlay_interface *interface, uint8_t c)
}
// wait for a whole packet
if (!state->payload_length || state->payload_offset < state->payload_length+8)
if (!state->payload_length || state->payload_offset < state->payload_length + RADIO_HEADER_LENGTH + RADIO_CRC_LENGTH)
return 0;
if (parse_heartbeat(interface, p)){
// cut the bytes of the heartbeat out of the buffer
state->payload_offset -= state->payload_length+8;
state->payload_offset -= state->payload_length + RADIO_HEADER_LENGTH + RADIO_CRC_LENGTH;
if (state->payload_offset){
// shuffle bytes backwards
bcopy(&p[state->payload_length+8], p, state->payload_offset);
bcopy(&p[state->payload_length + RADIO_HEADER_LENGTH + RADIO_CRC_LENGTH], p, state->payload_offset);
}
// restart parsing for a valid header from the beginning of out buffer
state->payload_offset+=state->payload_start;
@ -389,10 +394,10 @@ int radio_link_decode(struct overlay_interface *interface, uint8_t c)
// If the packet is truncated by less than 16 bytes, RS protection should be enough to recover the packet,
// but we may need to examine the last few bytes to find the start of the next packet.
state->payload_offset -= state->payload_length+8-backtrack;
state->payload_offset -= state->payload_length + RADIO_HEADER_LENGTH + RADIO_CRC_LENGTH - backtrack;
if (state->payload_offset){
// shuffle all remaining bytes back to the start of the buffer
bcopy(&state->payload[state->payload_start + state->payload_length+8-backtrack],
bcopy(&state->payload[state->payload_start + state->payload_length + RADIO_HEADER_LENGTH + RADIO_CRC_LENGTH - backtrack],
state->payload, state->payload_offset);
}
state->payload_start=0;

3
radio_link.h
View File

@ -1,6 +1,9 @@
#ifndef __SERVALD_RADIO_LINK_H
#define __SERVALD_RADIO_LINK_H
#define HEARTBEAT_SIZE (8+9)
#define LINK_MTU 255
int radio_link_free(struct overlay_interface *interface);
int radio_link_init(struct overlay_interface *interface);
int radio_link_decode(struct overlay_interface *interface, uint8_t c);