diff --git a/conf_schema.h b/conf_schema.h index 3257b89d..62ab4fa4 100644 --- a/conf_schema.h +++ b/conf_schema.h @@ -240,6 +240,7 @@ ATOM(bool_t, keyring, 0, boolean,, "") ATOM(bool_t, security, 0, boolean,, "") ATOM(bool_t, mdprequests, 0, boolean,, "") ATOM(bool_t, mavlink, 0, boolean,, "") +ATOM(bool_t, mavlinkfsm, 0, boolean,, "") ATOM(bool_t, peers, 0, boolean,, "") ATOM(bool_t, overlayframes, 0, boolean,, "") ATOM(bool_t, overlayabbreviations, 0, boolean,, "") diff --git a/mavlink.c b/mavlink.c index 6dc6dd03..6e1694a3 100644 --- a/mavlink.c +++ b/mavlink.c @@ -117,10 +117,16 @@ int stream_as_mavlink(int sequence_number,int startP,int endP, unsigned char *data,int count, unsigned char *frame,int *outlen) { - if (count>252-6-2) return -1; + if (count>252-6-4) return -1; frame[0]=0xfe; // mavlink v1.0 frame - frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC + /* payload len, excluding 6 byte header and 2 byte CRC. + But we use a 4-byte CRC, so need to add two to count to make packet lengths + be as expected. + Note that this construction will result in CRC errors by non-servald + programmes, which is probably more helpful than otherwise. + */ + frame[1]=count+2; frame[2]=sequence_number; // packet sequence frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC) frame[4]=0x40; // component ID of sender: we are reusing this to mark start,end of MDP frames @@ -129,12 +135,14 @@ int stream_as_mavlink(int sequence_number,int startP,int endP, frame[5]=MAVLINK_MSG_ID_DATASTREAM; // message ID type of this frame: DATA_STREAM // payload follows (we reuse the DATA_STREAM message type parameters) bcopy(data,&frame[6],count); - // 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; - - *outlen=count+6+2; + // Add our own 4-byte CRC of the data only + uint32_t crc=Crc32_ComputeBuf(0,&frame[1],-1+count+6); + frame[6+count+0]=(crc>>0)&0xff; + frame[6+count+1]=(crc>>8)&0xff; + frame[6+count+2]=(crc>>16)&0xff; + frame[6+count+3]=(crc>>24)&0xff; + + *outlen=count+6+4; return 0; } @@ -242,43 +250,61 @@ int mavlink_parse(struct slip_decode_state *state) int mavlink_decode(struct slip_decode_state *state,uint8_t c) { - if (config.debug.mavlink) DEBUGF("RX character %02x in state %s", - c,mavlink_describe_state(state->state)); + if (config.debug.mavlinkfsm) DEBUGF("RX character %02x in state %s", + 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; state->state++; break; case MAVLINK_STATE_SEQ: + state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1); state->mavlink_sequence=c; state->state++; break; case MAVLINK_STATE_SYSID: + state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1); state->mavlink_sysid=c; state->state++; break; case MAVLINK_STATE_COMPONENTID: + state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1); state->mavlink_componentid=c; state->state++; break; case MAVLINK_STATE_MSGID: + state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1); state->mavlink_msgid=c; state->state++; break; case MAVLINK_STATE_PAYLOAD: + if (state->mavlink_payload_length-state->mavlink_payload_offset>2) + state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1); + if (state->mavlink_payload_length-state->mavlink_payload_offset==2) + state->mavlink_rxcrc=c; + if (state->mavlink_payload_length-state->mavlink_payload_offset==1) + state->mavlink_rxcrc|=c<<8; state->mavlink_payload[state->mavlink_payload_offset++]=c; if (state->mavlink_payload_offset==state->mavlink_payload_length) state->state++; break; case MAVLINK_STATE_CRC1: - state->mavlink_rxcrc=c; + state->mavlink_rxcrc|=c<<16; state->state++; break; case MAVLINK_STATE_CRC2: - state->mavlink_rxcrc|=c<<8; + state->mavlink_rxcrc|=c<<24; state->state=MAVLINK_STATE_UNKNOWN; - return mavlink_parse(state); + state->mavlink_payload_length-=2; // don't count the upper 16 bits of our 32bit CRC + if (state->mavlink_rxcrc==state->mavlink_crc) + return mavlink_parse(state); + if (config.debug.mavlink) + DEBUGF("CRC mismatch on mavlink encapsulated data: rxed=%08x, calced=%08x", + state->mavlink_rxcrc,state->mavlink_crc); + state->mavlink_payload_length=0; break; case MAVLINK_STATE_UNKNOWN: default: diff --git a/overlay_interface.c b/overlay_interface.c index b850ad20..fbb9bc71 100644 --- a/overlay_interface.c +++ b/overlay_interface.c @@ -896,6 +896,8 @@ overlay_broadcast_ensemble(struct network_destination *destination, state.src_offset=0; state.src=buffer+out_len; slip_decode(&state); + // clear received packet after processing + state.packet_length=0; } out_len+=encoded; diff --git a/serval.h b/serval.h index 781c69ff..ed7cd6b9 100644 --- a/serval.h +++ b/serval.h @@ -388,7 +388,8 @@ struct slip_decode_state{ uint8_t mavlink_sysid; uint8_t mavlink_componentid; uint8_t mavlink_msgid; - uint16_t mavlink_rxcrc; + uint32_t mavlink_rxcrc; + uint32_t mavlink_crc; }; struct overlay_interface; diff --git a/slip.c b/slip.c index c882df33..5bc04db4 100644 --- a/slip.c +++ b/slip.c @@ -60,15 +60,13 @@ int slip_encode(int format, int startP=i?0:1; int endP=i+slice_bytes==src_bytes?1:0; stream_as_mavlink(0,startP,endP,&src[i],slice_bytes,&dst[dst_offset],&slice_len); - if (config.debug.mavlink) { - DEBUGF("Wrote %d bytes as %d byte MAVLink frame", - slice_bytes,slice_len); - dump("original data",&src[i],slice_bytes); - dump("mavlink frame",&dst[dst_offset],slice_len); - } dst_offset+=slice_len; } } + if (config.debug.mavlink) { + DEBUGF("Wrote %d byte packet as MAVLink frames",src_bytes); + } + return dst_offset; } break; @@ -385,6 +383,9 @@ int slip_decode(struct slip_decode_state *state) // We have to increment src_offset manually here, because returning // prevents the post-increment in the for loop from triggering state->src_offset++; + if (config.debug.mavlink) { + DEBUGF("Read %d byte packet from MAVLink frames",state->packet_length); + } return 1; } }