ExternalVendorCode/serval-dna
1
0
Fork 0
mirror of https://github.com/servalproject/serval-dna.git synced 2025-04-07 11:08:36 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Send and track mavlink sequence number

Browse Source
This commit is contained in:
Jeremy Lakeman 2013-09-20 13:49:44 +09:30
parent e3a5e8c353
commit 80c1da28b0
2 changed files with 30 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
mavlink.c
View File

@ -158,15 +158,15 @@ int mavlink_encode_packet(struct overlay_interface *interface)
// that there is a 2 byte CRC, so we can save two bytes
int len = count+32 - 2;
interface->txbuffer[1]=len;
interface->txbuffer[2]=(len & 0xF); // packet sequence
interface->txbuffer[3]=0; // system ID of sender (MAV_TYPE_GENERIC)
interface->txbuffer[2]=(len & 0xF);
interface->txbuffer[3]=0;
golay_encode(&interface->txbuffer[1]);
interface->txbuffer[4]=0x40; // component ID of sender: we are reusing this to mark start,end of MDP frames
if (startP) interface->txbuffer[4]|=0x01;
if (endP) interface->txbuffer[4]|=0x02;
interface->txbuffer[5]=MAVLINK_MSG_ID_DATASTREAM; // message ID type of this frame: DATA_STREAM
// payload follows (we reuse the DATA_STREAM message type parameters)
interface->txbuffer[4]=(interface->mavlink_seq++) & 0x3f;
if (startP) interface->txbuffer[4]|=0x40;
if (endP) interface->txbuffer[4]|=0x80;
interface->txbuffer[5]=MAVLINK_MSG_ID_DATASTREAM;
ob_get_bytes(interface->tx_packet, &interface->txbuffer[6], count);
encode_rs_8(&interface->txbuffer[4], &interface->txbuffer[6+count], 223 - (count+2));
@ -269,19 +269,32 @@ static int mavlink_parse(struct overlay_interface *interface, struct slip_decode
}
*backtrack=errors;
int seq=payload[4]&0x3f;
if (config.debug.mavlink){
DEBUGF("Received RS protected message, len: %d, errors: %d, flags:%s%s",
DEBUGF("Received RS protected message, len: %d, errors: %d, seq: %d, flags:%s%s",
data_bytes,
errors,
payload[4]&0x01?" start":"",
payload[4]&0x02?" end":"");
seq,
payload[4]&0x40?" start":"",
payload[4]&0x80?" end":"");
}
if (payload[4]&0x01)
state->packet_length=0;
if (state->packet_length + data_bytes > sizeof(state->dst)){
if (seq != ((state->mavlink_seq+1)&0x3f)){
// reject partial packet if we missed a sequence number
if (config.debug.mavlink)
DEBUGF("Rejecting packet, sequence jumped from %d to %d", state->mavlink_seq, seq);
state->packet_length=sizeof(state->dst)+1;
}
if (payload[4]&0x40){
// start a new packet
state->packet_length=0;
}
state->mavlink_seq=payload[4]&0x3f;
if (state->packet_length + data_bytes > 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 1;
@ -290,7 +303,7 @@ static int mavlink_parse(struct overlay_interface *interface, struct slip_decode
bcopy(&payload[6], &state->dst[state->packet_length], data_bytes);
state->packet_length+=data_bytes;
if (payload[4]&0x02) {
if (payload[4]&0x80) {
if (config.debug.mavlink)
DEBUGF("PDU Complete (length=%d)",state->packet_length);
state->dst_offset=0;

2
serval.h
View File

@ -382,6 +382,7 @@ struct slip_decode_state{
int dst_offset;
int mavlink_payload_length;
int mavlink_seq;
int mavlink_payload_start;
int mavlink_payload_offset;
uint8_t mavlink_payload[1024];
@ -458,6 +459,7 @@ typedef struct overlay_interface {
uint64_t next_tx_allowed;
int32_t remaining_space;
time_ms_t next_heartbeat;
int mavlink_seq;
struct slip_decode_state slip_decode_state;