Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix this so that it compiles. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@322 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2010-08-27 03:30:39 +00:00
parent b4a840a336
commit bd2ba2fcc9
4 changed files with 361 additions and 360 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/APM_BinComm/APM_BinComm.cpp
View File

@ -27,15 +27,15 @@
/// @brief Implementation of the ArduPilot Mega binary communications
/// library.
#include "WProgram.h"
#include "APM_BinComm.h"
#include "WProgram.h"
/// @name decoder state machine phases
//@{
#define DEC_WAIT_P1 0
#define DEC_WAIT_P2 1
#define DEC_WAIT_HEADER 2
#define DEC_WAIT_PACKET 3
#define DEC_WAIT_MESSAGE 3
#define DEC_WAIT_SUM_A 4
#define DEC_WAIT_SUM_B 5
//@}
@ -44,7 +44,7 @@
/// inter-byte timeout for decode (ms)
#define DEC_MESSAGE_TIMEOUT 100
BinComm::BinComm(const BinComm::BCMessageHandler *handlerTable,
BinComm::BinComm(const BinComm::MessageHandler *handlerTable,
FastSerial *interface) :
_handlerTable(handlerTable),
_interface(interface)
@ -119,7 +119,7 @@ BinComm::_decode(uint8_t inByte)
// prepare for the header
_bytesIn = 0;
_bytesExpected = sizeof(struct msg_packet_header);
_bytesExpected = sizeof(MessageHeader);
// intialise the checksum accumulators
_sumA = _sumB = 0;
@ -163,7 +163,7 @@ BinComm::_decode(uint8_t inByte)
// receiving payload data
//
case DEC_WAIT_PACKET:
case DEC_WAIT_MESSAGE:
// do checksum accumulation
_sumA += inByte;
_sumB += _sumA;
@ -181,7 +181,7 @@ BinComm::_decode(uint8_t inByte)
//
case DEC_WAIT_SUM_A:
if (inByte != _sumA) {
badPacketsReceived++;
badMessagesReceived++;
_decodePhase = DEC_WAIT_P1;
} else {
_decodePhase++;
@ -189,8 +189,8 @@ BinComm::_decode(uint8_t inByte)
break;
case DEC_WAIT_SUM_B:
if (inByte == _sumB) {
// if we got this far, we have a packet
packetsReceived++;
// if we got this far, we have a message
messagesReceived++;
// call any handler interested in this message
for (tableIndex = 0; MSG_NULL != _handlerTable[tableIndex].messageID; tableIndex++)
@ -198,7 +198,7 @@ BinComm::_decode(uint8_t inByte)
(_handlerTable[tableIndex].messageID == MSG_ANY))
_handlerTable[tableIndex].handler(_handlerTable[tableIndex].arg, _messageID, _messageVersion, &_decodeBuf);
} else {
badPacketsReceived++;
badMessagesReceived++;
}
_decodePhase = DEC_WAIT_P1;
break;

137
libraries/APM_BinComm/APM_BinComm.h
View File

@ -30,10 +30,10 @@
#ifndef APM_BinComm_h
#define APM_BinComm_h
#include <FastSerial.h>
#include "WProgram.h"
#include <string.h>
#include <inttypes.h>
#include <FastSerial.h>
///
/// @class BinComm
@ -41,11 +41,71 @@
/// Mega binary telemetry protocol.
///
class BinComm {
private:
/// OTA message header
struct MessageHeader {
uint8_t length;
uint8_t messageID;
uint8_t messageVersion;
};
/// Incoming header/packet buffer
/// XXX we could make this smaller
union {
uint8_t bytes[0];
MessageHeader header;
uint8_t payload[256];
} _decodeBuf;
/// Outgoing header/packet buffer
/// XXX we could make this smaller
struct {
MessageHeader header;
uint8_t payload[256 - sizeof(MessageHeader)];
} _encodeBuf;
//////////////////////////////////////////////////////////////////////
/// @name Message pack/unpack utility functions
///
//@{
inline void _pack(uint8_t *&ptr, const uint8_t x) { *(uint8_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const uint16_t x) { *(uint16_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const int16_t x) { *(int16_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const uint32_t x) { *(uint32_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const int32_t x) { *(int32_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const char *msg, uint8_t size) { strlcpy((char *)ptr, msg, size); ptr += size; };
inline void _pack(uint8_t *&ptr, const uint8_t *values, uint8_t count) { memcpy(ptr, values, count); ptr += count; };
inline void _pack(uint8_t *&ptr, const uint16_t *values, uint8_t count) { memcpy(ptr, values, count * 2); ptr += count * 2; };
inline void _unpack(uint8_t *&ptr, uint8_t &x) { x = *(uint8_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, uint16_t &x) { x = *(uint16_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, int16_t &x) { x = *(int16_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, uint32_t &x) { x = *(uint32_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, int32_t &x) { x = *(int32_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) { strlcpy(msg, (char *)ptr, size); ptr += size; };
inline void _unpack(uint8_t *&ptr, uint8_t *values, uint8_t count) { memcpy(values, ptr, count); ptr += count; };
inline void _unpack(uint8_t *&ptr, uint16_t *values, uint8_t count) { memcpy(values, ptr, count * 2); ptr += count * 2; };
//@}
public:
//////////////////////////////////////////////////////////////////////
/// @name Protocol definition
///
/// The protocol definition, including structures describing messages,
/// MessageID values and helper functions for packing messages are
/// automatically generated.
//@{
#include "protocol/protocol.h"
//@}
//////////////////////////////////////////////////////////////////////
/// @name Protocol magic numbers
///
/// @note The messageID enum is automatically generated and thus not described here.
/// @note The MessageID enum is automatically generated and thus not described here.
///
//@{
@ -166,9 +226,8 @@ public:
private:
/// Serial port we send/receive using.
FastSerial *_interface;
const MessageHandler *_handlerTable; ///< callout table
FastSerial *_interface; ///< Serial port we send/receive using.
/// Various magic numbers
enum MagicNumbers {
@ -177,9 +236,9 @@ private:
MSG_VERSION_1 = 1,
MSG_VARIABLE_LENGTH = 0xff
};
//////////////////////////////////////////////////////////////////////
/// @name Decoder
/// @name Decoder state
//@{
uint8_t _decodePhase; ///< decoder state machine phase
uint8_t _bytesIn; ///< bytes received in the current phase
@ -191,75 +250,17 @@ private:
uint8_t _messageVersion;///< messageVersion from the packet being received
unsigned long _lastReceived; ///< timestamp of last byte reception
//@}
/// Incoming header/packet buffer
/// XXX we could make this smaller
union {
uint8_t bytes[0];
msg_packet_header header;
uint8_t payload[256];
} _decodeBuf;
const BCMessageHandler *_handlerTable; ///< callout table
/// Decoder state machine
/// Decoder state machine.
///
/// @param inByte The byte to process.
///
void _decode(uint8_t inByte);
//@}
//////////////////////////////////////////////////////////////////////
/// @name Encoder
//@{
/// Send the packet in the encode buffer
/// Send the packet in the encode buffer.
///
void _sendMessage(void);
/// Outgoing header/packet buffer
/// XXX we could make this smaller
struct {
msg_packet_header header;
uint8_t payload[256 - sizeof(msg_packet_header)];
} _encodeBuf;
//////////////////////////////////////////////////////////////////////
/// @name Message pack/unpack utility functions
///
//@{
inline void _pack(uint8_t *&ptr, const uint8_t x) { (uint8_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const uint16 x) { (uint16_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const int16 x) { (int16_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const uint32_t x) { (uint32_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const int32_t x) { (int32_t *)ptr = x; ptr += sizeof(x); };
inline void _pack(uint8_t *&ptr, const char *msg, uint8_t size) { strlcpy(ptr, msg, size); ptr += size; };
inline void _pack(uint8_t *&ptr, const uint16_t *values, uint8_t count) { memcpy(ptr, values, count * 2); ptr += count * 2; };
inline void _unpack(uint8_t *&ptr, uint8_t &x) { x = *(uint8_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, uint16_t &x) { x = *(uint16_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, int16_t &x) { x = *(int16_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, uint32_t &x) { x = *(uint32_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, int32_t &x) { x = *(int32_t *)ptr; ptr += sizeof(x); };
inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) { strlcpy(msg, ptr, size); ptr += size; };
inline void _unpack(uint8_t *&ptr, uint16_t *values, uint8_t count) { memcpy(values, ptr, count * 2); ptr += count * 2; };
//@}
public:
//////////////////////////////////////////////////////////////////////
/// @name Protocol definition
///
/// The protocol definition, including structures describing messages,
/// messageID values and helper functions for packing messages are
/// automatically generated.
//@{
#include "protocol/protocol.h"
//@}
};
#endif // BinComm_h

546
libraries/APM_BinComm/protocol/protocol.h
View File

File diff suppressed because it is too large Load Diff

20
libraries/APM_BinComm/protocol/protogen.awk
View File

@ -16,14 +16,14 @@ END {
EMIT_MESSAGE()
#
# emit the messageID enum
# emit the MessageID enum
#
# XXX it would be elegant to sort the array here, but not
# everyone has GNU awk.
#
printf("\n//////////////////////////////////////////////////////////////////////\n")
printf("/// Message ID values\n")
printf("enum messageID {\n")
printf("enum MessageID {\n")
for (opcode in opcodes) {
printf("\t%s = 0x%x,\n", opcodes[opcode], opcode)
}
@ -60,7 +60,7 @@ function EMIT_MESSAGE(payloadSize)
# emit a routine to pack the message payload from a set of variables and send it
#
printf("/// Send a %s message\n", currentMessage)
printf("inline void\nBinComm::send_%s(\n", tolower(currentMessage))
printf("inline void\nsend_%s(\n", tolower(currentMessage))
for (i = 0; i < fieldCount; i++) {
if (counts[i]) {
printf("\tconst %s (&%s)[%d]", types[i], names[i], counts[i])
@ -71,14 +71,14 @@ function EMIT_MESSAGE(payloadSize)
printf(",\n");
}
printf(")\n{\n")
printf("\tuint8_t *p = &_encodeBuf.payload;\n")
printf("\tuint8_t *__p = &_encodeBuf.payload[0];\n")
payloadSize = 0;
for (i = 0; i < fieldCount; i++) {
if (counts[i]) {
printf("\t_pack(p, %s, %s);\n", names[i], counts[i])
printf("\t_pack(__p, %s, %s);\n", names[i], counts[i])
payloadSize += sizes[i] * counts[i]
} else {
printf("\t_pack(p, %s);\n", names[i])
printf("\t_pack(__p, %s);\n", names[i])
payloadSize += sizes[i]
}
}
@ -92,7 +92,7 @@ function EMIT_MESSAGE(payloadSize)
# emit a routine to unpack the current message into a set of variables
#
printf("/// Unpack a %s message\n", currentMessage)
printf("inline void\nBinComm::unpack_%s(\n", tolower(currentMessage))
printf("inline void\nunpack_%s(\n", tolower(currentMessage))
for (i = 0; i < fieldCount; i++) {
if (counts[i]) {
printf("\t%s (&%s)[%d]", types[i], names[i], counts[i])
@ -103,13 +103,13 @@ function EMIT_MESSAGE(payloadSize)
printf(",\n");
}
printf(")\n{\n")
printf("\tuint8_t *p = &_decodeBuf.payload;\n")
printf("\tuint8_t *__p = &_decodeBuf.payload[0];\n")
for (i = 0; i < fieldCount; i++) {
if (counts[i]) {
printf("\t_unpack(p, %s, %s);\n", names[i], counts[i])
printf("\t_unpack(__p, %s, %s);\n", names[i], counts[i])
payloadSize += sizes[i] * counts[i]
} else {
printf("\t_unpack(p, %s);\n", names[i])
printf("\t_unpack(__p, %s);\n", names[i])
payloadSize += sizes[i]
}
}