ExternalVendorCode/serval-dna
1
0
Fork 0
mirror of https://github.com/servalproject/serval-dna.git synced 2024-12-19 05:07: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) if (p+size+MAVLINK_HDR > bytes)
break; 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; p+=size+MAVLINK_HDR;
send=p; send=p;
continue; 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 // simulate the probability of a bit error in the packet pre-amble and drop the whole packet
for (i=0;i<PREAMBLE_LENGTH;i++){ for (i=0;i<PREAMBLE_LENGTH;i++){
if (random()<ber) if (random()<ber){
dropped=1; dropped=1;
fprintf(stderr,"Dropped the whole radio packet due to bit flip in the pre-amble\n");
break;
}
} }
if (dropped){ if (r->rxb_len + bytes + 9 > sizeof(r->rxbuffer)){
fprintf(stderr,"Dropped the whole radio packet due to bit flip in the pre-amble\n"); dropped=1;
}else{ 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++){ for (i=0;i<bytes && r->rxb_len<sizeof(r->rxbuffer);i++){
char byte = t->txbuffer[i]; char byte = t->txbuffer[i];
// introduce bit errors // introduce bit errors

4
overlay_interface.c
View File

@ -715,9 +715,7 @@ static void interface_read_stream(struct overlay_interface *interface){
} }
int i; radio_link_decode(interface, buffer, nread);
for (i=0;i<nread;i++)
radio_link_decode(interface, buffer[i]);
OUT(); 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 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{ struct radio_link_state{
// next seq for transmission // next seq for transmission
int tx_seq; 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, // small buffer for parsing incoming bytes from the serial interface,
// looking for recoverable link layer packets // looking for recoverable link layer packets
// should be large enough to hold at least one packet from the remote end // 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) int radio_link_init(struct overlay_interface *interface)
{ {
interface->radio_link_state = emalloc_zero(sizeof(struct radio_link_state)); interface->radio_link_state = emalloc_zero(sizeof(struct radio_link_state));
interface->radio_link_state->remaining_space = 512;
return 0; 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 we have nothing interesting left to send, don't create a packet at all
if (!link_state->tx_packet) if (!link_state->tx_packet)
return 0; return 0;
int count = ob_remaining(link_state->tx_packet); int count = ob_remaining(link_state->tx_packet);
int startP = (ob_position(link_state->tx_packet) == 0); int startP = (ob_position(link_state->tx_packet) == 0);
int endP = 1; int endP = 1;
@ -240,33 +248,6 @@ int radio_link_queue_packet(struct overlay_interface *interface, struct overlay_
return 0; 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 // write a new link layer packet to interface->txbuffer
// consuming more bytes from the next interface->tx_packet if required // consuming more bytes from the next interface->tx_packet if required
int radio_link_tx(struct overlay_interface *interface) int radio_link_tx(struct overlay_interface *interface)
@ -276,17 +257,28 @@ int radio_link_tx(struct overlay_interface *interface)
unschedule(&interface->alarm); unschedule(&interface->alarm);
interface->alarm.alarm = 0; interface->alarm.alarm = 0;
time_ms_t next_tick = interface->destination->last_tx+interface->destination->tick_ms; 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){ while(1){
now = gettime_ms();
if (link_state->tx_bytes){ if (link_state->tx_bytes){
if (link_state->next_tx_allowed > now){ if (link_state->next_tx_allowed > now){
interface->alarm.alarm = link_state->next_tx_allowed; interface->alarm.alarm = link_state->next_tx_allowed;
break; break;
} }
int bytes = link_state->tx_bytes;
int written=write(interface->alarm.poll.fd, &link_state->txbuffer[link_state->tx_pos], 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){ if (written<=0){
interface->alarm.poll.events|=POLLOUT; interface->alarm.poll.events|=POLLOUT;
break; break;
@ -300,24 +292,13 @@ int radio_link_tx(struct overlay_interface *interface)
continue; 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 // out of space? Don't bother to send anything interesting
// until we hear the next heartbeat response // until we hear the next heartbeat response
if (link_state->remaining_space < LINK_MTU + HEARTBEAT_SIZE){ if (link_state->remaining_space <=0){
interface->alarm.alarm = link_state->next_heartbeat; interface->alarm.alarm = now+5000;
break; break;
} }
if (link_state->remaining_space < LINK_MTU + HEARTBEAT_SIZE)
link_state->next_heartbeat = now;
if (!link_state->tx_packet){ if (!link_state->tx_packet){
// finished current packet, wait for more. // finished current packet, wait for more.
interface->alarm.alarm = next_tick; interface->alarm.alarm = next_tick;
@ -330,41 +311,13 @@ int radio_link_tx(struct overlay_interface *interface)
} }
watch(&interface->alarm); watch(&interface->alarm);
if (interface->alarm.alarm<now)
interface->alarm.alarm=now;
if (interface->alarm.alarm){ if (interface->alarm.alarm){
if (interface->alarm.alarm<now)
interface->alarm.alarm=now;
interface->alarm.deadline = interface->alarm.alarm+100; interface->alarm.deadline = interface->alarm.alarm+100;
schedule(&interface->alarm); 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; return 0;
} }
@ -394,6 +347,7 @@ static int radio_link_parse(struct overlay_interface *interface, struct radio_li
return 0; return 0;
} }
*backtrack=errors; *backtrack=errors;
data_bytes -= 2; data_bytes -= 2;
int seq=payload[4]&0x3f; int seq=payload[4]&0x3f;
@ -439,7 +393,7 @@ static int radio_link_parse(struct overlay_interface *interface, struct radio_li
return 1; 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 // look for a valid golay encoded length
int errs=0; int errs=0;
@ -453,94 +407,142 @@ static int decode_length(struct radio_link_state *state, unsigned char *p)
return -1; return -1;
if (config.debug.radio_link && (errs || state->payload_length!=*p)) 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; state->payload_length=length;
return 0; 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, const uint8_t *buffer, size_t len)
int radio_link_decode(struct overlay_interface *interface, uint8_t c)
{ {
IN(); IN();
struct radio_link_state *state=interface->radio_link_state; struct radio_link_state *state=interface->radio_link_state;
int i;
if (state->payload_start + state->payload_offset >= sizeof(state->payload)){ for (i=0;i<len;i++){
// drop one byte if we run out of space uint8_t c = buffer[i];
if (config.debug.radio_link) //DEBUGF("mode %d, %02x %d %d", state->mode, c, state->payload_start, state->payload_offset);
DEBUGF("Dropped %02x, buffer full", state->payload[0]); if (state->mode==MODE_HEADER){
bcopy(state->payload+1, state->payload, sizeof(state->payload) -1); state->radio_header[state->header_pos]=c;
state->payload_start--; /* parse the following header from the radio;
} 0 0xaa
1 0x55
unsigned char *p = &state->payload[state->payload_start]; 2 rssi
p[state->payload_offset++]=c; 3 remote rssi
4 temp
while(1){ 5 len
// look for packet length headers 6 low buff space
p = &state->payload[state->payload_start]; 7 high buff space
while(state->payload_length==0 && state->payload_offset>=6){ 8 0x55
if (p[0]==0xFE [len bytes of packet]
&& p[1]==9 */
&& p[3]==RADIO_SOURCE_SYSTEM if (state->header_pos==0 && c!=0xaa){
&& p[4]==RADIO_SOURCE_COMPONENT if (config.debug.radio_link)
&& p[5]==MAVLINK_MSG_ID_RADIO){ DEBUGF("waiting for 0xaa mode %d, %02x, %d", state->mode, c, state->header_pos);
//looks like a valid heartbeat response header, read the rest and process it continue;
state->payload_length=17; }
break;
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) if (state->header_pos>=8){
break; // end of expected header
//dump("header", state->radio_header, state->header_pos+1);
state->payload_start++; if (c==0x55){
state->payload_offset--; // we can assume that radio status packets arrive without corruption
p++; 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;
// wait for a whole packet if (state->remaining_space>=LINK_MTU){
if (!state->payload_length || state->payload_offset < state->payload_length) state->next_tx_allowed = gettime_ms();
RETURN(0); }
if (parse_heartbeat(state, p)){ if (config.debug.packetradio)
// cut the bytes of the heartbeat out of the buffer INFOF("RX len = %02x, rssi = %+ddB, remote rssi = %+ddB, buffer space = %d",
state->payload_offset -= state->payload_length; state->radio_header[5],
if (state->payload_offset){ state->radio_rssi,
// shuffle bytes backwards state->remote_rssi,
bcopy(&p[state->payload_length], p, state->payload_offset); 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->header_pos++;
state->payload_offset+=state->payload_start;
state->payload_start=0;
state->payload_length=0;
continue; continue;
} }
// is this a well formed packet? if (state->mode!=MODE_PACKET)
int backtrack=0; FATALF("Unexpected mode %d", state->mode);
if (radio_link_parse(interface, state, state->payload_length, p, &backtrack)==1){
// Since we know we've synced with the remote party, if (state->payload_start + state->payload_offset >= sizeof(state->payload)){
// and there's nothing we can do about any earlier data // drop one byte if we run out of space,
// throw away everything before the end of this packet // this shouldn't happen if we're talking to another servald
if (state->payload_start && config.debug.radio_link) if (config.debug.radio_link)
dump("Skipped", state->payload, state->payload_start); dump("overflow", state->payload, 16);
bcopy(state->payload+16, state->payload, sizeof(state->payload) - 16);
// If the packet is truncated by less than 16 bytes, RS protection should be enough to recover the packet, state->payload_start--;
// 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){ unsigned char *p = &state->payload[state->payload_start];
// shuffle all remaining bytes back to the start of the buffer p[state->payload_offset++]=c;
bcopy(&state->payload[state->payload_start + state->payload_length - backtrack],
state->payload, state->payload_offset); while(1){
} // look for packet length headers
state->payload_start=0; p = &state->payload[state->payload_start];
}else{ while(state->payload_length==0 && state->payload_offset>=6){
// ignore the first byte for now and start looking for another packet header if (decode_length(state, &p[1])==0)
// we may find a heartbeat in the middle that we need to cut out first break;
state->payload_start++;
state->payload_offset--; 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); RETURN(0);
} }

2
radio_link.h
View File

@ -6,7 +6,7 @@
int radio_link_free(struct overlay_interface *interface); int radio_link_free(struct overlay_interface *interface);
int radio_link_init(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); int radio_link_tx(struct overlay_interface *interface);
void radio_link_state_html(struct strbuf *b, 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); int radio_link_is_busy(struct overlay_interface *interface);