ExternalVendorCode/serval-dna
1
0
Fork 0
mirror of https://github.com/servalproject/serval-dna.git synced 2024-12-19 05:07:56 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

implement fragmentation of MDP frames over mavlink serial link.

Browse Source
This commit is contained in:
gardners 2013-08-30 10:59:40 +09:30 committed by Jeremy Lakeman
parent e2c3f1a8fb
commit 61c219a1f9
3 changed files with 30 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
mavlink.c
View File

@ -113,16 +113,19 @@ struct mavlink_RADIO_v10 {
uint8_t remnoise;
};
int stream_as_mavlink(int f,unsigned char *data,int count,
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;
frame[0]=0xfe; // mavlink v1.0 frame
frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC
frame[2]=f; // packet sequence
frame[2]=sequence_number; // packet sequence
frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
frame[4]=0xf1; // component ID of sender (MAV_COMP_ID_UART_BRIDGE)
frame[4]=0x40; // component ID of sender: we are reusing this to mark start,end of MDP frames
if (startP) frame[4]|=0x01;
if (endP) frame[4]|=0x02;
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);
@ -209,9 +212,25 @@ int mavlink_parse(struct slip_decode_state *state)
// Extract and return packet
// XXX - Ignores CRC at present
if (config.debug.mavlink) DEBUG("Received MAVLink DATASTREAM message for us");
bcopy(state->mavlink_payload,state->dst,state->mavlink_payload_length);
state->packet_length=state->mavlink_payload_length;
if (state->mavlink_componentid&0x01) {
if (config.debug.mavlink) {
DEBUG("Found start of PDU");
if (state->packet_length) DEBUGF("... previous packet had not ended, discarding");
}
state->packet_length=0;
}
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;
}
bcopy(state->mavlink_payload,&state->dst[state->packet_length],
state->mavlink_payload_length);
state->packet_length+=state->mavlink_payload_length;
if (state->mavlink_componentid&0x02) {
if (config.debug.mavlink) DEBUG("Found end of PDU");
return 1;
} else return 0;
break;
default:
if (config.debug.mavlink)

3
serval.h
View File

@ -878,7 +878,8 @@ int generate_nonce(unsigned char *nonce,int bytes);
int mavlink_decode(struct slip_decode_state *state,uint8_t c);
int mavlink_heartbeat(unsigned char *frame,int *outlen);
int stream_as_mavlink(int f,unsigned char *data,int count,
int stream_as_mavlink(int sequence_number,int startP,int endP,
unsigned char *data,int count,
unsigned char *frame,int *outlen);
#endif // __SERVALD_SERVALD_H

7
slip.c
View File

@ -57,7 +57,9 @@ int slip_encode(int format,
DEBUGF("Would overflow output buffer.");
return -1;
} else {
stream_as_mavlink(0,&src[i],slice_bytes,&dst[dst_offset],&slice_len);
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);
@ -377,11 +379,8 @@ int slip_decode(struct slip_decode_state *state)
switch(state->encapsulator) {
case SLIP_FORMAT_MAVLINK:
{
dump("rx data",state->src,state->src_size);
for(;state->src_offset<state->src_size;state->src_offset++) {
// flag complete reception of a packet
DEBUGF("src_offset=%d, src_size=%d, c=%02x",
state->src_offset,state->src_size,state->src[state->src_offset]);
if (mavlink_decode(state,state->src[state->src_offset])==1) {
// We have to increment src_offset manually here, because returning
// prevents the post-increment in the for loop from triggering