Send the next packet via stream interface ASAP

fakeradio.c

@@ -186,7 +186,9 @@ int read_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){
     log_time();
     fprintf(stderr, "Wrote to %s\n", s->name);

mavlink.c

@@ -75,6 +75,8 @@ uint16_t mavlink_crc(unsigned char *buf,int length)
     sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4);
     i++;
   }
+  buf[length+6]=sum&0xff;
+  buf[length+7]=sum>>8;
   
   return sum;
 }
@@ -168,6 +170,7 @@ int mavlink_encode_packet(struct overlay_interface *interface)
   if (endP){
     ob_free(interface->tx_packet);
     interface->tx_packet=NULL;
+    overlay_queue_schedule_next(gettime_ms());
   }
   return 0;
 }
@@ -175,6 +178,8 @@ int mavlink_encode_packet(struct overlay_interface *interface)
 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
@@ -184,15 +189,12 @@ int mavlink_heartbeat(unsigned char *frame,int *outlen)
   // Must be zero to indicate heartbeat
   frame[5]=0; // message ID type of this frame: DATA_STREAM
 
-  // payload follows
-  bzero(&frame[6],count);
+  // empty payload
 
   // two-byte CRC follows
-  uint16_t crc=mavlink_crc(frame,count); // automatically adds 6 for header length
-  frame[count+6]=crc&0xff;
-  frame[count+7]=crc>>8;
+  mavlink_crc(frame,count); // automatically adds 6 for header length
   
-  *outlen=count+6+2;
+  *outlen=count+8;
   
   return 0;
 }
@@ -233,7 +235,8 @@ int mavlink_parse(struct slip_decode_state *state)
 {
   switch(state->mavlink_msgid) {
   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_temperature=-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:
     // Extract and return packet, after applying Reed-Solomon error detection
     // and correction
-    if (config.debug.mavlink) DEBUG("Received MAVLink DATASTREAM message for us");
-    if (state->mavlink_componentid&0x01) {
-      if (config.debug.mavlink) {
-	DEBUGF("Found start of PDU mavlink-seq=0x%02x",state->mavlink_sequence);
-	if (state->packet_length) DEBUGF("... previous packet had not ended, discarding");
-      }
-      state->packet_length=0;
-    } else {
-      if (config.debug.mavlink) {
-	DEBUGF("Extension PDU mavlink-seq=0x%02x",state->mavlink_sequence);
-	if (state->packet_length) DEBUGF("... previous packet had not ended, discarding");
-      }
+    if (config.debug.mavlink){
+      DEBUGF("Received MAVLink DATASTREAM message, len: %d, flags:%s%s", 
+	state->mavlink_payload_length,
+	state->mavlink_componentid&0x01?" start":"",
+	state->mavlink_componentid&0x02?" end":"");
     }
-    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;
-      }
+    
+    if (state->mavlink_componentid&0x01)
+      state->packet_length=0;
+    
+    if (state->packet_length+state->mavlink_payload_length>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;
+    }
+    
     int errcount=decode_rs_8(&state->mavlink_payload[0],NULL,0,
 			     223-(state->mavlink_payload_length-30));
-    if (errcount==-1)
-      {
-	// DEBUGF("Reed-Solomon error correction failed");
-	return -1;
-      } // else DEBUGF("Reed-Solomon corrected %d bytes",errcount);
+    if (errcount==-1){
+      if (config.debug.mavlink) 
+	DEBUGF("Reed-Solomon error correction failed");
+      state->packet_length=sizeof(state->dst)+1;
+      return 0;
+    }
+    
     bcopy(state->mavlink_payload,&state->dst[state->packet_length],
 	  state->mavlink_payload_length-30);
     state->packet_length+=state->mavlink_payload_length-30;
+    
     if (state->mavlink_componentid&0x02) {
       if (config.debug.mavlink) 
 	DEBUGF("PDU Complete (length=%d)",state->packet_length);
       state->dst_offset=0;
       return 1;
-    } else return 0;
-    break;
+    }
+    return 0;
+    
   default:
     if (config.debug.mavlink) 
       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));
   switch(state->state) {
   case MAVLINK_STATE_LENGTH:
-    state->mavlink_crc=0;
     state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
     state->mavlink_payload_length=c;
     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;
       }
     break;
-  case MAVLINK_STATE_UNKNOWN:
   default:
-    if (c==0xfe) state->state=MAVLINK_STATE_LENGTH; 
-    else { 
-      state->state=MAVLINK_STATE_UNKNOWN;
+    if (c==0xfe){
+      state->state=MAVLINK_STATE_LENGTH; 
+      state->mavlink_crc=0;
     }
   }
 

overlay_interface.c

@@ -49,6 +49,7 @@ struct profile_total sock_any_stats;
 
 static void overlay_interface_poll(struct sched_ent *alarm);
 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))
 
@@ -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
          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->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);
+
       break;
     case SOCK_FILE:
       /* Seek to end of file as initial reading point */
@@ -739,7 +744,7 @@ static void write_stream_buffer(overlay_interface *interface){
       int bytes = interface->tx_bytes_pending;
       if (interface->throttle_burst_write_size && bytes>bytes_allowed)
 	bytes=bytes_allowed;
-      if (config.debug.packetradio) 
+      if (config.debug.packetradio)
 	DEBUGF("Trying to write %d bytes",bytes);
       int written=write(interface->alarm.poll.fd, interface->txbuffer, bytes);
       if (written<=0)
@@ -796,7 +801,7 @@ static void overlay_interface_poll(struct sched_ent *alarm)
     if (interface->state==INTERFACE_STATE_UP 
       && interface->destination->tick_ms>0
       && interface->send_broadcasts
-      && interface->tx_bytes_pending<=0){
+      && !interface->tx_packet){
       
       if (now >= interface->destination->last_tx+interface->destination->tick_ms)
         overlay_send_tick_packet(interface->destination);

overlay_packetradio.c

@@ -68,32 +68,13 @@ int overlay_packetradio_setup_port(overlay_interface *interface)
   if (tcsetattr(interface->alarm.poll.fd, TCSANOW, &t))
     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) {
     tcgetattr(interface->alarm.poll.fd, &t);
     int in_speed=cfgetispeed(&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);
   }
   
-  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);
 
   return 0;

overlay_queue.c

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

tests/routing

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