mirror of
https://github.com/servalproject/serval-dna.git
synced 2024-12-24 07:16:43 +00:00
Send the next packet via stream interface ASAP
This commit is contained in:
parent
589c334d5b
commit
3f123e1875
@ -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);
|
||||
|
76
mavlink.c
76
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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 */
|
||||
@ -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);
|
||||
|
@ -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;
|
||||
|
@ -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?
|
||||
|
@ -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 \
|
||||
|
Loading…
Reference in New Issue
Block a user