Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 04:13:39 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[datalink] add a simple mavlink decoder

Browse Source
This commit is contained in:
Gautier Hattenberger
2013-02-27 16:55:51 +01:00
parent 03c78067a2
commit 7d232046b4
3 changed files with 328 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/mavlink_decoder.xml
+20
View File
@@ -0,0 +1,20 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="mavlink_decoder" dir="datalink">
<doc>
<description>A simple MAVLINK decoder</description>
<configure name="MAVLINK_PORT" value="UART0|UART1|UART2" description="Select port for Mavlink decoder"/>
<configure name="MAVLINK_BAUD" value="B9600|B57600|B115200" description="Baudrate for Mavlink decoder"/>
</doc>
<header>
<file name="mavlink_decoder.h"/>
</header>
<event fun="MavlinkDatalinkEvent()"/>
<makefile>
<define name="MAVLINK_UART" value="$(MAVLINK_PORT)"/>
<define name="USE_$(MAVLINK_PORT)"/>
<define name="$(MAVLINK_PORT)_BAUD" value="$(MAVLINK_BAUD)"/>
<file name="mavlink_decoder.c"/>
</makefile>
</module>
sw/airborne/modules/datalink/mavlink_decoder.c
+44
View File
@@ -0,0 +1,44 @@
/*
* Copyright (C) 2013 Gautier Hattenberger
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** @file modules/datalink/mavlink_decoder.c
* @brief simple decoder of mavlink message
*/
#include "modules/datalink/mavlink_decoder.h"
struct mavlink_transport mavlink_tp;
#ifndef MAVLINK_NO_CRC_EXTRA
uint8_t crc_extra[256]={50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 200, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0};
#endif
#if MAVLINK_DECODER_DEBUG
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
void mavlink_send_debug(struct mavlink_transport * t) {
DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, t->trans.payload_len, t->trans.payload);
}
#endif
sw/airborne/modules/datalink/mavlink_decoder.h
+264
View File
@@ -0,0 +1,264 @@
/*
* Copyright (C) 2012 Xavier Gibert
* Copyright (C) 2013 Gautier Hattenberger
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** @file modules/datalink/mavlink_decoder.h
* @brief simple decoder of mavlink message
*/
#ifndef MAVLINK_DECODER_H
#define MAVLINK_DECODER_H
#include "std.h"
#include "subsystems/datalink/transport.h"
#include "mcu_periph/uart.h"
/* MAVLINK Transport
*/
#define STXMAV 0xFE
/** Mavlink v1.0 message structure.
*
* Mavlink Msg:
* STXMAV + len + { packet_seq + SYS/ID + COMPONENT/ID + MSG/ID + Payload } + ck_a + ck_b
*
* Payload starts after 4 bytes.
*
*/
struct mavlink_message {
uint8_t seq;
uint8_t sys_id;
uint8_t comp_id;
uint8_t msg_id;
uint8_t* payload;
};
#define MAVLINK_PAYLOAD_OFFSET 4
#define MAVLINK_SEQ_IDX 0
#define MAVLINK_SYS_ID_IDX 1
#define MAVLINK_COMP_ID_IDX 2
#define MAVLINK_MSG_ID_IDX 3
/** MAVLINK CHECKSUM */
#define X25_INIT_CRC 0xffff
#ifndef MAVLINK_NO_CRC_EXTRA
// CRC Extra (!!! staticaly calculated !!!)
extern uint8_t crc_extra[256];
#endif
/**
* @brief Accumulate the X.25 CRC by adding one char at a time.
*
* The checksum function adds the hash of one char at a time to the
* 16 bit checksum (uint16_t).
*
* @param data new char to hash
* @param crcAccum the already accumulated checksum
*/
static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
{
/*Accumulate one byte of data into the CRC*/
uint8_t tmp;
tmp = data ^ (uint8_t)(*crcAccum &0xff);
tmp ^= (tmp<<4);
*crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
}
/**
* @brief Initiliaze the buffer for the X.25 CRC
*
* @param crcAccum the 16 bit X.25 CRC
*/
static inline void crc_init(uint16_t* crcAccum)
{
*crcAccum = X25_INIT_CRC;
}
/**
* @brief Calculates the X.25 checksum on a byte buffer
*
* @param pBuffer buffer containing the byte array to hash
* @param length length of the byte array
* @return the checksum over the buffer bytes
**/
static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
{
uint16_t crcTmp;
crc_init(&crcTmp);
while (length--) {
crc_accumulate(*pBuffer++, &crcTmp);
}
return crcTmp;
}
/** Receiving mavlink messages */
// Mavlink parsing state machine
typedef enum {
MAVLINK_PARSE_STATE_UNINIT=0,
MAVLINK_PARSE_STATE_IDLE,
MAVLINK_PARSE_STATE_GOT_STX,
MAVLINK_PARSE_STATE_GOT_LENGTH,
MAVLINK_PARSE_STATE_GOT_PAYLOAD,
MAVLINK_PARSE_STATE_GOT_CRC1
} mavlink_parse_state; ///< The state machine for the comm parser
/** Structure to submit a callback
*/
struct mavlink_msg_req {
uint8_t msg_id; ///< Requested message ID
struct mavlink_message msg; ///< Mavlink message
void (*callback)(struct mavlink_message * msg); ///< Callback function
struct mavlink_msg_req * next;
};
/** Mavlink transport protocol
*/
struct mavlink_transport {
// generic interface
struct transport trans;
// specific mavlink transport variables
mavlink_parse_state status;
uint8_t payload_idx;
uint16_t checksum;
// linked list of callbacks
struct mavlink_msg_req * req;
};
extern struct mavlink_transport mavlink_tp;
#if MAVLINK_DECODER_DEBUG
/** Send debug frame over PPRZ telemetry.
*
* Activated with MAVLINK_DECODER_DEBUG flag
*/
extern void mavlink_send_debug(struct mavlink_transport * t);
#endif
/** Register a callback for a mavlink message
*/
static inline void mavlink_register_msg(struct mavlink_transport * t, struct mavlink_msg_req * req) {
// handle linked list of requests
req->next = t->req;
t->req = req;
}
/** Mavlink character parser
*/
static inline void parse_mavlink(struct mavlink_transport * t, uint8_t c ) {
switch (t->status) {
case MAVLINK_PARSE_STATE_UNINIT:
t->status = MAVLINK_PARSE_STATE_IDLE; // directly go to idle state (no break)
case MAVLINK_PARSE_STATE_IDLE:
if (c == STXMAV) {
t->status = MAVLINK_PARSE_STATE_GOT_STX;
crc_init(&(t->checksum));
}
break;
case MAVLINK_PARSE_STATE_GOT_STX:
if (t->trans.msg_received) {
t->trans.ovrn++;
goto error;
}
t->trans.payload_len = c + MAVLINK_PAYLOAD_OFFSET; /* Not Counting STX, CRC1 and CRC2, adding LENGTH, SEQ, SYSID, COMPID, MSGID */
crc_accumulate(c, &(t->checksum));
t->status = MAVLINK_PARSE_STATE_GOT_LENGTH;
t->payload_idx = 0;
break;
case MAVLINK_PARSE_STATE_GOT_LENGTH:
t->trans.payload[t->payload_idx] = c;
crc_accumulate(c, &(t->checksum));
t->payload_idx++;
if (t->payload_idx == t->trans.payload_len)
t->status = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
break;
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
#if MAVLINK_DECODER_DEBUG
mavlink_send_debug(t);
#endif
#ifndef MAVLINK_NO_CRC_EXTRA
// add extra CRC
crc_accumulate(crc_extra[(t->trans.payload[MAVLINK_MSG_ID_IDX])], &(t->checksum));
#endif
if (c != (t->checksum & 0xFF))
goto error;
t->status = MAVLINK_PARSE_STATE_GOT_CRC1;
break;
case MAVLINK_PARSE_STATE_GOT_CRC1:
if (c != (t->checksum >> 8))
goto error;
t->trans.msg_received = TRUE;
goto restart;
default:
goto error;
}
return;
error:
t->trans.error++;
restart:
t->status = MAVLINK_PARSE_STATE_IDLE;
return;
}
static inline void mavlink_parse_payload(struct mavlink_transport * t) {
uint8_t i;
struct mavlink_msg_req * el;
// test the linked list and call callback if needed
for (el = t->req; el; el = el->next) {
if (el->msg_id == t->trans.payload[MAVLINK_MSG_ID_IDX]) {
// build message out of payload
el->msg.seq = t->trans.payload[MAVLINK_SEQ_IDX];
el->msg.sys_id = t->trans.payload[MAVLINK_SYS_ID_IDX];
el->msg.comp_id = t->trans.payload[MAVLINK_COMP_ID_IDX];
el->msg.msg_id = t->trans.payload[MAVLINK_MSG_ID_IDX];
// copy buffer
for(i = 0; i < t->trans.payload_len; i++)
el->msg.payload[i] = t->trans.payload[i+MAVLINK_PAYLOAD_OFFSET];
// callback
el->callback(&(el->msg));
}
}
}
#define MavlinkBuffer(_dev) TransportLink(_dev,ChAvailable())
#define ReadMavlinkBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans.msg_received)) parse_mavlink(&(_trans),TransportLink(_dev,Getch())); }
#define MavlinkCheckAndParse(_dev,_trans) { \
if (MavlinkBuffer(_dev)) { \
ReadMavlinkBuffer(_dev,_trans); \
if (_trans.trans.msg_received) { \
mavlink_parse_payload(&(_trans)); \
_trans.trans.msg_received = FALSE; \
} \
} \
}
/* Datalink Event Macro */
#define MavlinkDatalinkEvent() MavlinkCheckAndParse(MAVLINK_UART, mavlink_tp)
#endif /* MAVLINK_DECODER_H */