ExternalVendorCode/serval-dna
1
0
Fork 0
mirror of https://github.com/servalproject/serval-dna.git synced 2025-06-13 04:28:16 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Send the next packet via stream interface ASAP

Browse Source
This commit is contained in:
Jeremy Lakeman
2013-09-12 16:14:01 +09:30
parent 589c334d5b
commit 3f123e1875
6 changed files with 54 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
fakeradio.c
View File

@ -186,7 +186,9 @@ int read_bytes(struct radio_state *s)
int write_bytes(struct radio_state *s) int write_bytes(struct radio_state *s)
{ {
int wrote = write(s->fd, s->rxbuffer, s->rxb_len); int wrote=s->rxb_len;
if (s->last_char_ms)
wrote = write(s->fd, s->rxbuffer, wrote);
if (wrote>0){ if (wrote>0){
log_time(); log_time();
fprintf(stderr, "Wrote to %s\n", s->name); fprintf(stderr, "Wrote to %s\n", s->name);

68
mavlink.c
View File

@ -75,6 +75,8 @@ uint16_t mavlink_crc(unsigned char *buf,int length)
sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4); sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4);
i++; i++;
} }
buf[length+6]=sum&0xff;
buf[length+7]=sum>>8;
return sum; return sum;
} }
@ -168,6 +170,7 @@ int mavlink_encode_packet(struct overlay_interface *interface)
if (endP){ if (endP){
ob_free(interface->tx_packet); ob_free(interface->tx_packet);
interface->tx_packet=NULL; interface->tx_packet=NULL;
overlay_queue_schedule_next(gettime_ms());
} }
return 0; return 0;
} }
@ -175,6 +178,8 @@ int mavlink_encode_packet(struct overlay_interface *interface)
int mavlink_heartbeat(unsigned char *frame,int *outlen) int mavlink_heartbeat(unsigned char *frame,int *outlen)
{ {
int count=9; int count=9;
bzero(frame, count+8);
frame[0]=0xfe; // mavlink v1.0 frame frame[0]=0xfe; // mavlink v1.0 frame
// Must be 9 to indicate heartbeat // Must be 9 to indicate heartbeat
frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC
@ -184,15 +189,12 @@ int mavlink_heartbeat(unsigned char *frame,int *outlen)
// Must be zero to indicate heartbeat // Must be zero to indicate heartbeat
frame[5]=0; // message ID type of this frame: DATA_STREAM frame[5]=0; // message ID type of this frame: DATA_STREAM
// payload follows // empty payload
bzero(&frame[6],count);
// two-byte CRC follows // two-byte CRC follows
uint16_t crc=mavlink_crc(frame,count); // automatically adds 6 for header length mavlink_crc(frame,count); // automatically adds 6 for header length
frame[count+6]=crc&0xff;
frame[count+7]=crc>>8;
*outlen=count+6+2; *outlen=count+8;
return 0; return 0;
} }
@ -233,7 +235,8 @@ int mavlink_parse(struct slip_decode_state *state)
{ {
switch(state->mavlink_msgid) { switch(state->mavlink_msgid) {
case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO:
if (config.debug.mavlink) DEBUG("Received MAVLink radio report"); 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_rssi=(1.0*state->mavlink_payload[5]-state->mavlink_payload[8])/1.9;
last_radio_temperature=-999; // doesn't get reported last_radio_temperature=-999; // doesn't get reported
last_radio_rxpackets=-999; // doesn't get reported last_radio_rxpackets=-999; // doesn't get reported
@ -247,41 +250,44 @@ int mavlink_parse(struct slip_decode_state *state)
case MAVLINK_MSG_ID_DATASTREAM: case MAVLINK_MSG_ID_DATASTREAM:
// Extract and return packet, after applying Reed-Solomon error detection // Extract and return packet, after applying Reed-Solomon error detection
// and correction // and correction
if (config.debug.mavlink) DEBUG("Received MAVLink DATASTREAM message for us");
if (state->mavlink_componentid&0x01) {
if (config.debug.mavlink){ if (config.debug.mavlink){
DEBUGF("Found start of PDU mavlink-seq=0x%02x",state->mavlink_sequence); DEBUGF("Received MAVLink DATASTREAM message, len: %d, flags:%s%s",
if (state->packet_length) DEBUGF("... previous packet had not ended, discarding"); state->mavlink_payload_length,
state->mavlink_componentid&0x01?" start":"",
state->mavlink_componentid&0x02?" end":"");
} }
if (state->mavlink_componentid&0x01)
state->packet_length=0; state->packet_length=0;
} else {
if (config.debug.mavlink) { if (state->packet_length+state->mavlink_payload_length>sizeof(state->dst)){
DEBUGF("Extension PDU mavlink-seq=0x%02x",state->mavlink_sequence); if (config.debug.mavlink)
if (state->packet_length) DEBUGF("... previous packet had not ended, discarding"); DEBUG("Fragmented packet is too long or a previous piece was missed - discarding");
} state->packet_length=sizeof(state->dst)+1;
}
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; return 0;
} }
int errcount=decode_rs_8(&state->mavlink_payload[0],NULL,0, int errcount=decode_rs_8(&state->mavlink_payload[0],NULL,0,
223-(state->mavlink_payload_length-30)); 223-(state->mavlink_payload_length-30));
if (errcount==-1) if (errcount==-1){
{ if (config.debug.mavlink)
// DEBUGF("Reed-Solomon error correction failed"); DEBUGF("Reed-Solomon error correction failed");
return -1; state->packet_length=sizeof(state->dst)+1;
} // else DEBUGF("Reed-Solomon corrected %d bytes",errcount); return 0;
}
bcopy(state->mavlink_payload,&state->dst[state->packet_length], bcopy(state->mavlink_payload,&state->dst[state->packet_length],
state->mavlink_payload_length-30); state->mavlink_payload_length-30);
state->packet_length+=state->mavlink_payload_length-30; state->packet_length+=state->mavlink_payload_length-30;
if (state->mavlink_componentid&0x02) { if (state->mavlink_componentid&0x02) {
if (config.debug.mavlink) if (config.debug.mavlink)
DEBUGF("PDU Complete (length=%d)",state->packet_length); DEBUGF("PDU Complete (length=%d)",state->packet_length);
state->dst_offset=0; state->dst_offset=0;
return 1; return 1;
} else return 0; }
break; return 0;
default: default:
if (config.debug.mavlink) if (config.debug.mavlink)
DEBUGF("Received unknown MAVLink message type 0x%02x", DEBUGF("Received unknown MAVLink message type 0x%02x",
@ -296,7 +302,6 @@ int mavlink_decode(struct slip_decode_state *state,uint8_t c)
c,mavlink_describe_state(state->state)); c,mavlink_describe_state(state->state));
switch(state->state) { switch(state->state) {
case MAVLINK_STATE_LENGTH: case MAVLINK_STATE_LENGTH:
state->mavlink_crc=0;
state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1); state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
state->mavlink_payload_length=c; state->mavlink_payload_length=c;
state->mavlink_payload_offset=0; state->mavlink_payload_offset=0;
@ -338,11 +343,10 @@ int mavlink_decode(struct slip_decode_state *state,uint8_t c)
if (r==1) return 1; if (r==1) return 1;
} }
break; break;
case MAVLINK_STATE_UNKNOWN:
default: default:
if (c==0xfe) state->state=MAVLINK_STATE_LENGTH; if (c==0xfe){
else { state->state=MAVLINK_STATE_LENGTH;
state->state=MAVLINK_STATE_UNKNOWN; state->mavlink_crc=0;
} }
} }

9
overlay_interface.c
View File

@ -49,6 +49,7 @@ struct profile_total sock_any_stats;
static void overlay_interface_poll(struct sched_ent *alarm); static void overlay_interface_poll(struct sched_ent *alarm);
static int re_init_socket(int interface_index); static int re_init_socket(int interface_index);
static void write_stream_buffer(overlay_interface *interface);
#define DEBUG_packet_visualise(M,P,N) logServalPacket(LOG_LEVEL_DEBUG, __WHENCE__, (M), (P), (N)) #define DEBUG_packet_visualise(M,P,N) logServalPacket(LOG_LEVEL_DEBUG, __WHENCE__, (M), (P), (N))
@ -495,8 +496,12 @@ overlay_interface_init(const char *name, struct in_addr src_addr, struct in_addr
/* The encapsulation type should be configurable, but for now default to the one that should /* The encapsulation type should be configurable, but for now default to the one that should
be safe on the RFD900 radios, and that also allows us to receive RSSI reports inline */ be safe on the RFD900 radios, and that also allows us to receive RSSI reports inline */
interface->slip_decode_state.encapsulator=SLIP_FORMAT_MAVLINK; interface->slip_decode_state.encapsulator=SLIP_FORMAT_MAVLINK;
interface->alarm.poll.events=POLLIN; interface->alarm.poll.events=POLLIN|POLLOUT;
// Queue a hearbeat now
interface->tx_bytes_pending=0;
mavlink_heartbeat(interface->txbuffer,&interface->tx_bytes_pending);
watch(&interface->alarm); watch(&interface->alarm);
break; break;
case SOCK_FILE: case SOCK_FILE:
/* Seek to end of file as initial reading point */ /* Seek to end of file as initial reading point */
@ -796,7 +801,7 @@ static void overlay_interface_poll(struct sched_ent *alarm)
if (interface->state==INTERFACE_STATE_UP if (interface->state==INTERFACE_STATE_UP
&& interface->destination->tick_ms>0 && interface->destination->tick_ms>0
&& interface->send_broadcasts && interface->send_broadcasts
&& interface->tx_bytes_pending<=0){ && !interface->tx_packet){
if (now >= interface->destination->last_tx+interface->destination->tick_ms) if (now >= interface->destination->last_tx+interface->destination->tick_ms)
overlay_send_tick_packet(interface->destination); overlay_send_tick_packet(interface->destination);

19
overlay_packetradio.c
View File

@ -68,32 +68,13 @@ int overlay_packetradio_setup_port(overlay_interface *interface)
if (tcsetattr(interface->alarm.poll.fd, TCSANOW, &t)) if (tcsetattr(interface->alarm.poll.fd, TCSANOW, &t))
WHY_perror("Failed to set terminal parameters"); WHY_perror("Failed to set terminal parameters");
// Enable MAVLink mode to get regular RSSI reports
uint8_t buf[256];
int buflen=0;
buflen=0;
mavlink_heartbeat(buf,&buflen);
(void)write_all(interface->alarm.poll.fd,buf,buflen);
if (config.debug.packetradio) { if (config.debug.packetradio) {
tcgetattr(interface->alarm.poll.fd, &t); tcgetattr(interface->alarm.poll.fd, &t);
int in_speed=cfgetispeed(&t); int in_speed=cfgetispeed(&t);
int out_speed=cfgetospeed(&t); int out_speed=cfgetospeed(&t);
DEBUGF("Enabled MAVLink based RSSI reporting for RFD900 radios");
DEBUGF("uart speed reported as %d/%d",in_speed,out_speed); DEBUGF("uart speed reported as %d/%d",in_speed,out_speed);
} }
if (0){
// dummy write of all possible ascii values
char buff[256];
int i;
for (i=0;i<sizeof buff;i++)
buff[i]=i;
(void)write_all(interface->alarm.poll.fd,buff,sizeof buff);
}
set_nonblock(interface->alarm.poll.fd); set_nonblock(interface->alarm.poll.fd);
return 0; return 0;

4
overlay_queue.c
View File

@ -288,7 +288,7 @@ overlay_calc_queue_time(overlay_txqueue *queue, struct overlay_frame *frame){
int i; int i;
for(i=0;i<frame->destination_count;i++) for(i=0;i<frame->destination_count;i++)
{ {
if (frame->destinations[i].destination->interface->tx_bytes_pending>0) if (frame->destinations[i].destination->interface->tx_packet)
continue; continue;
time_ms_t next_packet = limit_next_allowed(&frame->destinations[i].destination->transfer_limit); time_ms_t next_packet = limit_next_allowed(&frame->destinations[i].destination->transfer_limit);
if (frame->destinations[i].transmit_time){ if (frame->destinations[i].transmit_time){
@ -401,7 +401,7 @@ overlay_stuff_packet(struct outgoing_packet *packet, overlay_txqueue *queue, tim
}else{ }else{
// skip this interface if the stream tx buffer has data // skip this interface if the stream tx buffer has data
if (dest->interface->socket_type==SOCK_STREAM if (dest->interface->socket_type==SOCK_STREAM
&& dest->interface->tx_bytes_pending>0) && dest->interface->tx_packet)
continue; continue;
// can we send a packet on this interface now? // can we send a packet on this interface now?

1
tests/routing
View File

@ -234,6 +234,7 @@ setup_simulate_extender() {
executeOk_servald config \ executeOk_servald config \
set debug.throttling on \ set debug.throttling on \
set debug.packetradio on \ set debug.packetradio on \
set debug.mavlink on \
set interfaces.1.type CATEAR \ set interfaces.1.type CATEAR \
set interfaces.1.mdp_tick_ms 5000 \ set interfaces.1.mdp_tick_ms 5000 \
set interfaces.1.socket_type STREAM \ set interfaces.1.socket_type STREAM \