From 3f123e1875e2fcbf6dfdecf13a9fbb9aa04a493f Mon Sep 17 00:00:00 2001 From: Jeremy Lakeman Date: Thu, 12 Sep 2013 16:14:01 +0930 Subject: [PATCH] Send the next packet via stream interface ASAP --- fakeradio.c | 4 ++- mavlink.c | 76 +++++++++++++++++++++++-------------------- overlay_interface.c | 11 +++++-- overlay_packetradio.c | 19 ----------- overlay_queue.c | 4 +-- tests/routing | 1 + 6 files changed, 54 insertions(+), 61 deletions(-) diff --git a/fakeradio.c b/fakeradio.c index 3206f5c4..1b50f420 100644 --- a/fakeradio.c +++ b/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); diff --git a/mavlink.c b/mavlink.c index 80470a2d..2908780d 100644 --- a/mavlink.c +++ b/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; } } diff --git a/overlay_interface.c b/overlay_interface.c index b0e8d60c..b0502a7d 100644 --- a/overlay_interface.c +++ b/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); diff --git a/overlay_packetradio.c b/overlay_packetradio.c index 4151a218..587f92e6 100644 --- a/overlay_packetradio.c +++ b/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;ialarm.poll.fd,buff,sizeof buff); - } - set_nonblock(interface->alarm.poll.fd); return 0; diff --git a/overlay_queue.c b/overlay_queue.c index fd2f4c79..e48a1bd7 100644 --- a/overlay_queue.c +++ b/overlay_queue.c @@ -288,7 +288,7 @@ overlay_calc_queue_time(overlay_txqueue *queue, struct overlay_frame *frame){ int i; for(i=0;idestination_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? diff --git a/tests/routing b/tests/routing index 243a7ee6..440f9066 100755 --- a/tests/routing +++ b/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 \