ExternalVendorCode/serval-dna
1
0
Fork 0
mirror of https://github.com/servalproject/serval-dna.git synced 2024-12-18 20:57:56 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Simulate new radio firmware with packet headers

Browse Source
This commit is contained in:
Jeremy Lakeman 2013-11-27 12:30:56 +10:30
parent d45470ce81
commit 41155ce751
4 changed files with 176 additions and 163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
fakeradio.c
View File

@ -305,11 +305,6 @@ int transfer_bytes(struct radio_state *radios)
if (p+size+MAVLINK_HDR > bytes)
break;
// detect when we are about to transmit a heartbeat frame
if (size==9 && t->txbuffer[p+5]==0){
// reply to the host with a heartbeat
build_heartbeat(t);
}
p+=size+MAVLINK_HDR;
send=p;
continue;
@ -348,13 +343,31 @@ int transfer_bytes(struct radio_state *radios)
// simulate the probability of a bit error in the packet pre-amble and drop the whole packet
for (i=0;i<PREAMBLE_LENGTH;i++){
if (random()<ber)
if (random()<ber){
dropped=1;
fprintf(stderr,"Dropped the whole radio packet due to bit flip in the pre-amble\n");
break;
}
}
if (dropped){
fprintf(stderr,"Dropped the whole radio packet due to bit flip in the pre-amble\n");
}else{
if (r->rxb_len + bytes + 9 > sizeof(r->rxbuffer)){
dropped=1;
fprintf(stderr,"Dropped the whole radio packet due to insufficient space\n");
}
if (!dropped){
// build new firmware header
r->rxbuffer[r->rxb_len++]=0xaa;
r->rxbuffer[r->rxb_len++]=0x55;
r->rxbuffer[r->rxb_len++]=0x42; // rssi
r->rxbuffer[r->rxb_len++]=0x44; // remote rsi
r->rxbuffer[r->rxb_len++]=0x12; // temperature
r->rxbuffer[r->rxb_len++]=bytes; // packet length (can be zero)
int remaining_space = sizeof(r->txbuffer) - r->txb_len;
r->rxbuffer[r->rxb_len++]=remaining_space & 0xFF;
r->rxbuffer[r->rxb_len++]=remaining_space >> 8;
r->rxbuffer[r->rxb_len++]=0x55;
for (i=0;i<bytes && r->rxb_len<sizeof(r->rxbuffer);i++){
char byte = t->txbuffer[i];
// introduce bit errors

4
overlay_interface.c
View File

@ -715,9 +715,7 @@ static void interface_read_stream(struct overlay_interface *interface){
}
int i;
for (i=0;i<nread;i++)
radio_link_decode(interface, buffer[i]);
radio_link_decode(interface, buffer, nread);
OUT();
}

302
radio_link.c
View File

@ -81,10 +81,18 @@ struct mavlink_RADIO_v10 {
#define LINK_PAYLOAD_MTU (LINK_MTU - FEC_LENGTH - RADIO_HEADER_LENGTH - RADIO_CRC_LENGTH)
#define MODE_HEADER 0
#define MODE_PACKET 1
struct radio_link_state{
// next seq for transmission
int tx_seq;
int mode;
// small buffer for receiving incoming radio packet header
uint8_t radio_header[10];
int header_pos;
// small buffer for parsing incoming bytes from the serial interface,
// looking for recoverable link layer packets
// should be large enough to hold at least one packet from the remote end
@ -161,6 +169,7 @@ int radio_link_free(struct overlay_interface *interface)
int radio_link_init(struct overlay_interface *interface)
{
interface->radio_link_state = emalloc_zero(sizeof(struct radio_link_state));
interface->radio_link_state->remaining_space = 512;
return 0;
}
@ -178,7 +187,6 @@ static int radio_link_encode_packet(struct radio_link_state *link_state)
// if we have nothing interesting left to send, don't create a packet at all
if (!link_state->tx_packet)
return 0;
int count = ob_remaining(link_state->tx_packet);
int startP = (ob_position(link_state->tx_packet) == 0);
int endP = 1;
@ -240,33 +248,6 @@ int radio_link_queue_packet(struct overlay_interface *interface, struct overlay_
return 0;
}
static int build_heartbeat(struct radio_link_state *link_state)
{
int count=9;
bzero(link_state->txbuffer, count + RADIO_CRC_LENGTH + RADIO_HEADER_LENGTH);
link_state->txbuffer[0]=0xfe; // mavlink v1.0 link_state->txbuffer
// Must be 9 to indicate heartbeat
link_state->txbuffer[1]=count; // payload len, excluding 6 byte header and 2 byte CRC
link_state->txbuffer[2]=(count & 0xF); // packet sequence
link_state->txbuffer[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(&link_state->txbuffer[1]);
link_state->txbuffer[4]=0xf1; // component ID of sender (MAV_COMP_ID_UART_BRIDGE)
// Must be zero to indicate heartbeat
link_state->txbuffer[5]=0; // message ID type of this link_state->txbuffer: DATA_STREAM
// extra magic number to help correctly detect remote heartbeat requests
link_state->txbuffer[14]=0x55;
link_state->txbuffer[15]=0x05;
golay_encode(&link_state->txbuffer[14]);
link_state->tx_bytes = count + RADIO_CRC_LENGTH + RADIO_HEADER_LENGTH;
if (config.debug.radio_link)
DEBUGF("Produced heartbeat");
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_tx(struct overlay_interface *interface)
@ -276,17 +257,28 @@ int radio_link_tx(struct overlay_interface *interface)
unschedule(&interface->alarm);
interface->alarm.alarm = 0;
time_ms_t next_tick = interface->destination->last_tx+interface->destination->tick_ms;
time_ms_t now = gettime_ms();
interface->alarm.poll.events&=~POLLOUT;
time_ms_t now;
while(1){
now = gettime_ms();
if (link_state->tx_bytes){
if (link_state->next_tx_allowed > now){
interface->alarm.alarm = link_state->next_tx_allowed;
break;
}
int written=write(interface->alarm.poll.fd, &link_state->txbuffer[link_state->tx_pos], link_state->tx_bytes);
int bytes = link_state->tx_bytes;
if (bytes > link_state->remaining_space){
bytes = link_state->remaining_space;
if (!bytes){
interface->alarm.alarm = now+5000;
break;
}
}
int written=write(interface->alarm.poll.fd, &link_state->txbuffer[link_state->tx_pos], bytes);
if (written<=0){
interface->alarm.poll.events|=POLLOUT;
break;
@ -300,24 +292,13 @@ int radio_link_tx(struct overlay_interface *interface)
continue;
}
interface->alarm.poll.events&=~POLLOUT;
if (link_state->next_heartbeat<=now){
build_heartbeat(link_state);
link_state->next_heartbeat = now + 1000;
continue;
}
// out of space? Don't bother to send anything interesting
// until we hear the next heartbeat response
if (link_state->remaining_space < LINK_MTU + HEARTBEAT_SIZE){
interface->alarm.alarm = link_state->next_heartbeat;
if (link_state->remaining_space <=0){
interface->alarm.alarm = now+5000;
break;
}
if (link_state->remaining_space < LINK_MTU + HEARTBEAT_SIZE)
link_state->next_heartbeat = now;
if (!link_state->tx_packet){
// finished current packet, wait for more.
interface->alarm.alarm = next_tick;
@ -330,41 +311,13 @@ int radio_link_tx(struct overlay_interface *interface)
}
watch(&interface->alarm);
if (interface->alarm.alarm<now)
interface->alarm.alarm=now;
if (interface->alarm.alarm){
if (interface->alarm.alarm<now)
interface->alarm.alarm=now;
interface->alarm.deadline = interface->alarm.alarm+100;
schedule(&interface->alarm);
}
return 0;
}
static int parse_heartbeat(struct radio_link_state *state, const unsigned char *payload)
{
if (payload[0]==0xFE
&& payload[1]==9
&& payload[3]==RADIO_SOURCE_SYSTEM
&& payload[4]==RADIO_SOURCE_COMPONENT
&& payload[5]==MAVLINK_MSG_ID_RADIO){
// we can assume that radio status packets arrive without corruption
state->radio_rssi=(1.0*payload[10]-payload[13])/1.9;
state->remote_rssi=(1.0*payload[11] - payload[14])/1.9;
int free_space = payload[12];
int free_bytes = (free_space * 1280) / 100 - 30;
state->remaining_space = free_bytes;
if (free_bytes>0)
state->next_tx_allowed = gettime_ms();
if (free_bytes>720)
state->next_heartbeat=gettime_ms()+1000;
if (config.debug.packetradio)
INFOF("Link budget = %+ddB, remote link budget = %+ddB, buffer space = %d%% (approx %d)",
state->radio_rssi,
state->remote_rssi,
free_space, free_bytes);
return 1;
}
return 0;
}
@ -394,6 +347,7 @@ static int radio_link_parse(struct overlay_interface *interface, struct radio_li
return 0;
}
*backtrack=errors;
data_bytes -= 2;
int seq=payload[4]&0x3f;
@ -439,7 +393,7 @@ static int radio_link_parse(struct overlay_interface *interface, struct radio_li
return 1;
}
static int decode_length(struct radio_link_state *state, unsigned char *p)
static int decode_length(struct radio_link_state *state, uint8_t *p)
{
// look for a valid golay encoded length
int errs=0;
@ -453,94 +407,142 @@ static int decode_length(struct radio_link_state *state, unsigned char *p)
return -1;
if (config.debug.radio_link && (errs || state->payload_length!=*p))
DEBUGF("Decoded length %d to %d with %d errs", *p, length, errs);
DEBUGF("Decoded length %02x (+8) to %02x with %d errs", *p, length, errs);
state->payload_length=length;
return 0;
}
// add one byte at a time from the serial link, and attempt to decode packets
int radio_link_decode(struct overlay_interface *interface, uint8_t c)
int radio_link_decode(struct overlay_interface *interface, const uint8_t *buffer, size_t len)
{
IN();
struct radio_link_state *state=interface->radio_link_state;
if (state->payload_start + state->payload_offset >= sizeof(state->payload)){
// drop one byte if we run out of space
if (config.debug.radio_link)
DEBUGF("Dropped %02x, buffer full", state->payload[0]);
bcopy(state->payload+1, state->payload, sizeof(state->payload) -1);
state->payload_start--;
}
unsigned char *p = &state->payload[state->payload_start];
p[state->payload_offset++]=c;
while(1){
// look for packet length headers
p = &state->payload[state->payload_start];
while(state->payload_length==0 && state->payload_offset>=6){
if (p[0]==0xFE
&& p[1]==9
&& p[3]==RADIO_SOURCE_SYSTEM
&& p[4]==RADIO_SOURCE_COMPONENT
&& p[5]==MAVLINK_MSG_ID_RADIO){
//looks like a valid heartbeat response header, read the rest and process it
state->payload_length=17;
break;
int i;
for (i=0;i<len;i++){
uint8_t c = buffer[i];
//DEBUGF("mode %d, %02x %d %d", state->mode, c, state->payload_start, state->payload_offset);
if (state->mode==MODE_HEADER){
state->radio_header[state->header_pos]=c;
/* parse the following header from the radio;
0 0xaa
1 0x55
2 rssi
3 remote rssi
4 temp
5 len
6 low buff space
7 high buff space
8 0x55
[len bytes of packet]
*/
if (state->header_pos==0 && c!=0xaa){
if (config.debug.radio_link)
DEBUGF("waiting for 0xaa mode %d, %02x, %d", state->mode, c, state->header_pos);
continue;
}
if (state->header_pos==1 && c!=0x55){
// whoops, header magic bytes weren't there, try again
state->header_pos=0;
if (config.debug.radio_link)
DEBUGF("expected 0x55 mode %d, %02x, %d", state->mode, c, state->header_pos);
continue;
}
if (decode_length(state, &p[1])==0)
break;
state->payload_start++;
state->payload_offset--;
p++;
}
// wait for a whole packet
if (!state->payload_length || state->payload_offset < state->payload_length)
RETURN(0);
if (parse_heartbeat(state, p)){
// cut the bytes of the heartbeat out of the buffer
state->payload_offset -= state->payload_length;
if (state->payload_offset){
// shuffle bytes backwards
bcopy(&p[state->payload_length], p, state->payload_offset);
if (state->header_pos>=8){
// end of expected header
//dump("header", state->radio_header, state->header_pos+1);
if (c==0x55){
// we can assume that radio status packets arrive without corruption
state->radio_rssi=state->radio_header[2];//(1.0*payload[10]-payload[13])/1.9;
state->remote_rssi=state->radio_header[3];//(1.0*payload[11] - payload[14])/1.9;
state->remaining_space = ((state->radio_header[7]<<8) | state->radio_header[6]) - 64;
if (state->remaining_space>=LINK_MTU){
state->next_tx_allowed = gettime_ms();
}
if (config.debug.packetradio)
INFOF("RX len = %02x, rssi = %+ddB, remote rssi = %+ddB, buffer space = %d",
state->radio_header[5],
state->radio_rssi,
state->remote_rssi,
state->remaining_space);
if (state->radio_header[5])
state->mode=MODE_PACKET;
}else if (config.debug.radio_link)
DEBUGF("expected 0x55 mode %d, %02x, %d", state->mode, c, state->header_pos);
state->header_pos=0;
continue;
}
// restart parsing for a valid header from the beginning of out buffer
state->payload_offset+=state->payload_start;
state->payload_start=0;
state->payload_length=0;
state->header_pos++;
continue;
}
// is this a well formed packet?
int backtrack=0;
if (radio_link_parse(interface, state, state->payload_length, p, &backtrack)==1){
// Since we know we've synced with the remote party,
// and there's nothing we can do about any earlier data
// throw away everything before the end of this packet
if (state->payload_start && config.debug.radio_link)
dump("Skipped", state->payload, state->payload_start);
// 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 - 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 - backtrack],
state->payload, state->payload_offset);
}
state->payload_start=0;
}else{
// ignore the first byte for now and start looking for another packet header
// we may find a heartbeat in the middle that we need to cut out first
state->payload_start++;
state->payload_offset--;
if (state->mode!=MODE_PACKET)
FATALF("Unexpected mode %d", state->mode);
if (state->payload_start + state->payload_offset >= sizeof(state->payload)){
// drop one byte if we run out of space,
// this shouldn't happen if we're talking to another servald
if (config.debug.radio_link)
dump("overflow", state->payload, 16);
bcopy(state->payload+16, state->payload, sizeof(state->payload) - 16);
state->payload_start--;
}
unsigned char *p = &state->payload[state->payload_start];
p[state->payload_offset++]=c;
while(1){
// look for packet length headers
p = &state->payload[state->payload_start];
while(state->payload_length==0 && state->payload_offset>=6){
if (decode_length(state, &p[1])==0)
break;
state->payload_start++;
state->payload_offset--;
p++;
}
// wait for a whole packet
if (!state->payload_length || state->payload_offset < state->payload_length)
break;
// is this a well formed packet?
int backtrack=0;
if (radio_link_parse(interface, state, state->payload_length, p, &backtrack)==1){
// Since we know we've synced with the remote party,
// and there's nothing we can do about any earlier data
// throw away everything before the end of this packet
if (state->payload_start && config.debug.radio_link)
dump("Skipped between packets", state->payload, state->payload_start);
// 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 - 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 - backtrack],
state->payload, state->payload_offset);
}
state->payload_start=0;
}else{
// ignore the first byte for now and start looking for another packet header
// we may find a heartbeat in the middle that we need to cut out first
state->payload_start++;
state->payload_offset--;
}
state->payload_length=0;
}
// decrement packet length until we have processed all incoming bytes
state->radio_header[5]--;
if (state->radio_header[5]==0){
state->mode=MODE_HEADER;
state->header_pos=0;
}
state->payload_length=0;
};
RETURN(0);
}

2
radio_link.h
View File

@ -6,7 +6,7 @@
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);
int radio_link_decode(struct overlay_interface *interface, const uint8_t *buffer, size_t len);
int radio_link_tx(struct overlay_interface *interface);
void radio_link_state_html(struct strbuf *b, struct overlay_interface *interface);
int radio_link_is_busy(struct overlay_interface *interface);