mirror of https://github.com/ArduPilot/ardupilot
Library implementing the lower layers of the APM binary communications protocol.
Based on work done for the ArduStationM firmware and influenced by Randall Mackay's ArduCopter BinComm code. This is a work in progress; discussion is welcome. git-svn-id: https://arducopter.googlecode.com/svn/trunk@318 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com
parent
884098b74d
commit
71efe28851
|
|
@ -0,0 +1,206 @@
|
|||
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
|
||||
//
|
||||
// Copyright (c) 2010 Michael Smith. All rights reserved.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions
|
||||
// are met:
|
||||
// 1. Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
// 2. Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
// OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
// SUCH DAMAGE.
|
||||
|
||||
/// @file BinComm.cpp
|
||||
/// @brief Implementation of the ArduPilot Mega binary communications
|
||||
/// library.
|
||||
|
||||
#include "WProgram.h"
|
||||
#include "APM_BinComm.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_SUM_A 4
|
||||
#define DEC_WAIT_SUM_B 5
|
||||
//@}
|
||||
|
||||
|
||||
/// inter-byte timeout for decode (ms)
|
||||
#define DEC_MESSAGE_TIMEOUT 100
|
||||
|
||||
BinComm::BinComm(const BinComm::BCMessageHandler *handlerTable,
|
||||
FastSerial *interface) :
|
||||
_handlerTable(handlerTable),
|
||||
_interface(interface)
|
||||
{
|
||||
};
|
||||
|
||||
void
|
||||
BinComm::_sendMessage(void)
|
||||
{
|
||||
uint8_t bytesToSend;
|
||||
uint8_t sumA, sumB;
|
||||
const uint8_t *p;
|
||||
|
||||
// send the preamble first
|
||||
_interface->write((uint8_t)MSG_PREAMBLE_1);
|
||||
_interface->write((uint8_t)MSG_PREAMBLE_2);
|
||||
|
||||
// set up to send the payload
|
||||
bytesToSend = _encodeBuf.header.length + sizeof(_encodeBuf.header);
|
||||
sumA = sumB = 0;
|
||||
p = (const uint8_t *)&_encodeBuf;
|
||||
|
||||
// send message bytes and compute checksum on the fly
|
||||
while (bytesToSend--) {
|
||||
sumA += *p;
|
||||
sumB += sumA;
|
||||
_interface->write(*p++);
|
||||
}
|
||||
|
||||
// send the checksum
|
||||
_interface->write(sumA);
|
||||
_interface->write(sumB);
|
||||
}
|
||||
|
||||
void
|
||||
BinComm::update(void)
|
||||
{
|
||||
uint8_t count;
|
||||
|
||||
// Ensure that we don't spend too long here by only processing
|
||||
// the bytes that were available when we started.
|
||||
//
|
||||
// XXX we might want to further constrain this count
|
||||
count = _interface->available();
|
||||
while (count--)
|
||||
_decode(_interface->read());
|
||||
}
|
||||
|
||||
void
|
||||
BinComm::_decode(uint8_t inByte)
|
||||
{
|
||||
uint8_t tableIndex;
|
||||
|
||||
// handle inter-byte timeouts (resync after link loss, etc.)
|
||||
//
|
||||
if ((millis() - _lastReceived) > DEC_MESSAGE_TIMEOUT)
|
||||
_decodePhase = DEC_WAIT_P1;
|
||||
|
||||
// run the decode state machine
|
||||
//
|
||||
switch (_decodePhase) {
|
||||
|
||||
// Preamble detection
|
||||
//
|
||||
// Note the fallthrough from P2 to P1 deals with the case where
|
||||
// we see 0x34, 0x34, 0x44 where the first 0x34 is garbage or
|
||||
// a SUM_B byte we never looked at.
|
||||
//
|
||||
case DEC_WAIT_P2:
|
||||
if (MSG_PREAMBLE_2 == inByte) {
|
||||
_decodePhase++;
|
||||
|
||||
// prepare for the header
|
||||
_bytesIn = 0;
|
||||
_bytesExpected = sizeof(struct msg_packet_header);
|
||||
|
||||
// intialise the checksum accumulators
|
||||
_sumA = _sumB = 0;
|
||||
|
||||
break;
|
||||
}
|
||||
_decodePhase = DEC_WAIT_P1;
|
||||
// FALLTHROUGH
|
||||
case DEC_WAIT_P1:
|
||||
if (MSG_PREAMBLE_1 == inByte) {
|
||||
_decodePhase++;
|
||||
}
|
||||
break;
|
||||
|
||||
// receiving the header
|
||||
//
|
||||
case DEC_WAIT_HEADER:
|
||||
// do checksum accumulation
|
||||
_sumA += inByte;
|
||||
_sumB += _sumA;
|
||||
|
||||
// store the byte
|
||||
_decodeBuf.bytes[_bytesIn++] = inByte;
|
||||
|
||||
// check for complete header received
|
||||
if (_bytesIn == _bytesExpected) {
|
||||
_decodePhase++;
|
||||
|
||||
// prepare for the payload
|
||||
// variable-length data?
|
||||
_bytesIn = 0;
|
||||
_bytesExpected = _decodeBuf.header.length;
|
||||
_messageID = _decodeBuf.header.messageID;
|
||||
_messageVersion = _decodeBuf.header.messageVersion;
|
||||
|
||||
// sanity check to avoid buffer overflow - revert back to waiting
|
||||
if (_bytesExpected > sizeof(_decodeBuf))
|
||||
_decodePhase = DEC_WAIT_P1;
|
||||
}
|
||||
break;
|
||||
|
||||
// receiving payload data
|
||||
//
|
||||
case DEC_WAIT_PACKET:
|
||||
// do checksum accumulation
|
||||
_sumA += inByte;
|
||||
_sumB += _sumA;
|
||||
|
||||
// store the byte
|
||||
_decodeBuf.bytes[_bytesIn++] = inByte;
|
||||
|
||||
// check for complete payload received
|
||||
if (_bytesIn == _bytesExpected) {
|
||||
_decodePhase++;
|
||||
}
|
||||
break;
|
||||
|
||||
// waiting for the checksum bytes
|
||||
//
|
||||
case DEC_WAIT_SUM_A:
|
||||
if (inByte != _sumA) {
|
||||
badPacketsReceived++;
|
||||
_decodePhase = DEC_WAIT_P1;
|
||||
} else {
|
||||
_decodePhase++;
|
||||
}
|
||||
break;
|
||||
case DEC_WAIT_SUM_B:
|
||||
if (inByte == _sumB) {
|
||||
// if we got this far, we have a packet
|
||||
packetsReceived++;
|
||||
|
||||
// call any handler interested in this message
|
||||
for (tableIndex = 0; MSG_NULL != _handlerTable[tableIndex].messageID; tableIndex++)
|
||||
if ((_handlerTable[tableIndex].messageID == _messageID) ||
|
||||
(_handlerTable[tableIndex].messageID == MSG_ANY))
|
||||
_handlerTable[tableIndex].handler(_handlerTable[tableIndex].arg, _messageID, _messageVersion, &_decodeBuf);
|
||||
} else {
|
||||
badPacketsReceived++;
|
||||
}
|
||||
_decodePhase = DEC_WAIT_P1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,265 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
//
|
||||
// Copyright (c) 2010 Michael Smith. All rights reserved.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions
|
||||
// are met:
|
||||
// 1. Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
// 2. Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
// OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
// SUCH DAMAGE.
|
||||
|
||||
/// @file BinComm.h
|
||||
/// @brief Definitions for the ArduPilot Mega binary communications
|
||||
/// library.
|
||||
|
||||
#ifndef APM_BinComm_h
|
||||
#define APM_BinComm_h
|
||||
|
||||
#include "WProgram.h"
|
||||
#include <string.h>
|
||||
#include <inttypes.h>
|
||||
#include <FastSerial.h>
|
||||
|
||||
///
|
||||
/// @class BinComm
|
||||
/// @brief Class providing protocol en/decoding services for the ArduPilot
|
||||
/// Mega binary telemetry protocol.
|
||||
///
|
||||
class BinComm {
|
||||
public:
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
/// @name Protocol magic numbers
|
||||
///
|
||||
/// @note The messageID enum is automatically generated and thus not described here.
|
||||
///
|
||||
//@{
|
||||
|
||||
/// Variables defined
|
||||
/// XXX these should probably be handled by the database/MIB?
|
||||
enum variableID {
|
||||
MSG_VAR_ROLL_MODE = 0x00,
|
||||
MSG_VAR_PITCH_MODE = 0x01,
|
||||
MSG_VAR_THROTTLE_MODE = 0x02,
|
||||
MSG_VAR_YAW_MODE = 0x03,
|
||||
MSG_VAR_ELEVON_TRIM_1 = 0x04,
|
||||
MSG_VAR_ELEVON_TRIM_2 = 0x05,
|
||||
|
||||
MSG_VAR_INTEGRATOR_0 = 0x10,
|
||||
MSG_VAR_INTEGRATOR_1 = 0x11,
|
||||
MSG_VAR_INTEGRATOR_2 = 0x12,
|
||||
MSG_VAR_INTEGRATOR_3 = 0x13,
|
||||
MSG_VAR_INTEGRATOR_4 = 0x14,
|
||||
MSG_VAR_INTEGRATOR_5 = 0x15,
|
||||
MSG_VAR_INTEGRATOR_6 = 0x16,
|
||||
MSG_VAR_INTEGRATOR_7 = 0x17,
|
||||
|
||||
MSG_VAR_KFF_0 = 0x1a,
|
||||
MSG_VAR_KFF_1 = 0x1b,
|
||||
MSG_VAR_KFF_2 = 0x1c,
|
||||
|
||||
MSG_VAR_TARGET_BEARING = 0x20,
|
||||
MSG_VAR_NAV_BEARING = 0x21,
|
||||
MSG_VAR_BEARING_ERROR = 0x22,
|
||||
MSG_VAR_CROSSTRACK_BEARING = 0x23,
|
||||
MSG_VAR_CROSSTRACK_ERROR = 0x24,
|
||||
MSG_VAR_ALTITUDE_ERROR = 0x25,
|
||||
MSG_VAR_WP_RADIUS = 0x26,
|
||||
MSG_VAR_LOITER_RADIUS = 0x27,
|
||||
MSG_VAR_WP_MODE = 0x28,
|
||||
MSG_VAR_LOOP_COMMANDS = 0x29,
|
||||
MSG_VAR_NAV_GAIN_SCALER = 0x2a,
|
||||
};
|
||||
|
||||
/// PID sets defined
|
||||
enum PIDSet {
|
||||
MSG_SERVO_ROLL = 0,
|
||||
MSG_SERVO_PITCH = 1,
|
||||
MSG_SERVO_RUDDER = 2,
|
||||
MSG_SERVO_NAV_ROLL = 3,
|
||||
MSG_SERVO_NAV_PITCH_ASP = 4,
|
||||
MSG_SERVO_NAV_PITCH_ALT = 5,
|
||||
MSG_SERVO_TE_THROTTLE = 6,
|
||||
MSG_SERVO_ALT_THROTTLE = 7,
|
||||
MSG_SERVO_ELEVATOR = 8 // Added by Randy
|
||||
};
|
||||
|
||||
//@}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
/// Message reception callout descriptor
|
||||
///
|
||||
/// An array of these handlers is passed to the constructor to delegate
|
||||
/// processing for received messages.
|
||||
///
|
||||
struct MessageHandler {
|
||||
MessageID messageID; ///< messageID for which the handler will be called
|
||||
void (* handler)(void *arg,
|
||||
uint8_t messageId,
|
||||
uint8_t messageVersion,
|
||||
void *messageData); ///< function to be called
|
||||
void *arg; ///< argument passed to function
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
/// Constructor.
|
||||
///
|
||||
/// @param handlerTable Array of callout functions to which
|
||||
/// received messages will be sent. More than
|
||||
/// one handler for a given messageID may be
|
||||
/// registered; handlers are called in the order
|
||||
/// they appear in the table.
|
||||
///
|
||||
/// @param interface The FastSerial interface that will be used
|
||||
/// for telemetry communications.
|
||||
///
|
||||
BinComm(const MessageHandler *handlerTable,
|
||||
FastSerial *interface);
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
/// @name Decoder interface
|
||||
//@{
|
||||
|
||||
/// Consume bytes from the interface and feed them to the decoder.
|
||||
///
|
||||
/// If a packet is completed, then any callbacks associated
|
||||
/// with the packet's messageID will be called.
|
||||
///
|
||||
/// If no bytes are passed to the decoder for a period determined
|
||||
/// by DEC_MESSAGE_TIMEOUT, the decode state machine will reset
|
||||
/// before processing the next byte. This can help re-synchronise
|
||||
/// after a link loss or in-flight failure.
|
||||
///
|
||||
|
||||
void update(void);
|
||||
|
||||
uint32_t messagesReceived; ///< statistics
|
||||
uint32_t badMessagesReceived; ///< statistics
|
||||
|
||||
//@}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
/// @name Encoder interface
|
||||
///
|
||||
/// Messages are normally encoded and sent using the
|
||||
/// send_msg_* functions defined in protocol/protocol.h.
|
||||
/// For each message type MSG_* there is a corresponding send_msg_*
|
||||
/// function which will construct and transmit the message.
|
||||
///
|
||||
//@{
|
||||
uint32_t messagesSent; ///< statistics
|
||||
//@}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
/// Serial port we send/receive using.
|
||||
FastSerial *_interface;
|
||||
|
||||
/// Various magic numbers
|
||||
enum MagicNumbers {
|
||||
MSG_PREAMBLE_1 = 0x34,
|
||||
MSG_PREAMBLE_2 = 0x44,
|
||||
MSG_VERSION_1 = 1,
|
||||
MSG_VARIABLE_LENGTH = 0xff
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
/// @name Decoder
|
||||
//@{
|
||||
uint8_t _decodePhase; ///< decoder state machine phase
|
||||
uint8_t _bytesIn; ///< bytes received in the current phase
|
||||
uint8_t _bytesExpected; ///< bytes expected in the current phase
|
||||
uint8_t _sumA; ///< sum of incoming bytes
|
||||
uint8_t _sumB; ///< sum of _sumA values
|
||||
|
||||
uint8_t _messageID; ///< messageID from the packet being received
|
||||
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
|
||||
///
|
||||
/// @param inByte The byte to process.
|
||||
///
|
||||
void _decode(uint8_t inByte);
|
||||
|
||||
//@}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
/// @name Encoder
|
||||
//@{
|
||||
|
||||
/// 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
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
APM_BinComm KEYWORD1
|
||||
update KEYWORD2
|
||||
messagesReceived KEYWORD2
|
||||
badMessagesReceived KEYWORD2
|
||||
messagesSent KEYWORD2
|
||||
|
||||
|
|
@ -0,0 +1,215 @@
|
|||
#
|
||||
# Message definitions for the ArduPilot Mega binary communications protocol.
|
||||
#
|
||||
# Process this file using:
|
||||
#
|
||||
# awk -f protogen.awk protocol.def > protocol.h
|
||||
#
|
||||
# Messages are declared with
|
||||
#
|
||||
# message <MESSAGE_ID> <MESSAGE_NAME>
|
||||
#
|
||||
# <MESSAGE_NAME> is a valid member of BinComm::MessageID.
|
||||
# <MESSAGE_ID> is the message ID byte
|
||||
#
|
||||
# Following a message declaration the fields of the message are
|
||||
# defined in the format:
|
||||
#
|
||||
# <TYPE> <NAME> [<COUNT>]
|
||||
#
|
||||
# <TYPE> is a C type corresponding to the field. The type must be a single
|
||||
# word, either an integer from <inttypes.h> or "char".
|
||||
# <NAME> is the name of the field; it should be unique within the message
|
||||
# <COUNT> is an optional array count for fields that are arrays
|
||||
# (note that currently there is only _pack support for character arrays)
|
||||
#
|
||||
|
||||
#
|
||||
# Acknowledge message
|
||||
#
|
||||
message 0x00 MSG_ACKNOWLEDGE
|
||||
uint8_t msgID
|
||||
uint16_t msgSum
|
||||
|
||||
#
|
||||
# System heartbeat
|
||||
#
|
||||
message 0x01 MSG_HEARTBEAT
|
||||
uint8_t flightMode
|
||||
uint16_t timeStamp
|
||||
uint16_t batteryVoltage
|
||||
uint16_t commandIndex
|
||||
|
||||
#
|
||||
# Attitude report
|
||||
#
|
||||
message 0x02 MSG_ATTITUDE
|
||||
int16_t roll
|
||||
int16_t pitch
|
||||
int16_t yaw
|
||||
|
||||
#
|
||||
# Location report
|
||||
#
|
||||
message 0x03 MSG_LOCATION
|
||||
int32_t latitude
|
||||
int32_t longitude
|
||||
int16_t altitude
|
||||
int16_t groundSpeed
|
||||
int16_t groundCourse
|
||||
uint16_t timeOfWeek
|
||||
|
||||
#
|
||||
# Optional pressure-based location report
|
||||
#
|
||||
message 0x04 MSG_PRESSURE
|
||||
uint16_t pressureAltitude
|
||||
uint16_t airSpeed
|
||||
|
||||
#
|
||||
# Text status message
|
||||
#
|
||||
message 0x05 MSG_STATUS_TEXT
|
||||
uint8_t severity
|
||||
char text 50
|
||||
|
||||
#
|
||||
# Algorithm performance report
|
||||
#
|
||||
message 0x06 MSG_PERF_REPORT
|
||||
uint32_t interval
|
||||
uint16_t mainLoopCycles
|
||||
uint8_t mainLoopTime
|
||||
uint8_t gyroSaturationCount
|
||||
uint8_t adcConstraintCount
|
||||
uint16_t imuHealth
|
||||
uint16_t gcsMessageCount
|
||||
|
||||
#
|
||||
# System version messages
|
||||
#
|
||||
message 0x07 MSG_VERSION_REQUEST
|
||||
uint8_t systemType
|
||||
uint8_t systemID
|
||||
|
||||
message 0x08 MSG_VERSION
|
||||
uint8_t systemType
|
||||
uint8_t systemID
|
||||
uint8_t firmwareVersion 3
|
||||
|
||||
#
|
||||
# Flight command operations
|
||||
#
|
||||
message 0x20 MSG_COMMAND_REQUEST
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
message 0x21 MSG_COMMAND_UPLOAD
|
||||
uint8_t action
|
||||
uint16_t itemNumber
|
||||
int listLength
|
||||
uint8_t commandID
|
||||
uint8_t p1
|
||||
uint16_t p2
|
||||
uint32_t p3
|
||||
uint32_t p4
|
||||
|
||||
message 0x22 MSG_COMMAND_LIST
|
||||
int itemNumber
|
||||
int listLength
|
||||
uint8_t commandID
|
||||
uint8_t p1
|
||||
uint16_t p2
|
||||
uint32_t p3
|
||||
uint32_t p4
|
||||
|
||||
message 0x23 MSG_COMMAND_MODE_CHANGE
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
#
|
||||
# Parameter operations
|
||||
#
|
||||
message 0x30 MSG_VALUE_REQUEST
|
||||
uint8_t valueID
|
||||
uint8_t broadcast
|
||||
|
||||
|
||||
message 0x31 MSG_VALUE_SET
|
||||
uint8_t valueID
|
||||
uint32_t value
|
||||
|
||||
message 0x32 MSG_VALUE
|
||||
uint8_t valueID
|
||||
uint32_t value
|
||||
|
||||
#
|
||||
# PID adjustments
|
||||
#
|
||||
message 0x40 MSG_PID_REQUEST
|
||||
uint8_t pidSet
|
||||
|
||||
message 0x41 MSG_PID_SET
|
||||
uint8_t pidSet
|
||||
int32_t p
|
||||
int32_t i
|
||||
int32_t d
|
||||
int16_t integratorMax
|
||||
|
||||
message 0x42 MSG_PID
|
||||
uint8_t pidSet
|
||||
int32_t p
|
||||
int32_t i
|
||||
int32_t d
|
||||
int16_t integratorMax
|
||||
|
||||
|
||||
#
|
||||
# Radio trim settings
|
||||
#
|
||||
message 0x50 MSG_TRIM_STARTUP
|
||||
uint16_t value 8
|
||||
|
||||
message 0x51 MSG_TRIM_MIN
|
||||
uint16_t value 8
|
||||
|
||||
message 0x52 MSG_TRIM_MAX
|
||||
uint16_t value 8
|
||||
|
||||
#
|
||||
# Direct sensor access
|
||||
#
|
||||
message 0x60 MSG_SENSOR
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
#
|
||||
# Simulation-related messages
|
||||
#
|
||||
message 0x70 MSG_SIM
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
#
|
||||
# Direct I/O pin control
|
||||
#
|
||||
message 0x80 MSG_PIN_REQUEST
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
message 0x81 MSG_PIN_SET
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
#
|
||||
# Dataflash operations
|
||||
#
|
||||
message 0x90 MSG_DATAFLASH_REQUEST
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
message 0x91 MSG_DATAFLASH_SET
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
#
|
||||
# EEPROM operations
|
||||
#
|
||||
message 0xa0 MSG_EEPROM_REQUEST
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
message 0xa1 MSG_EEPROM_SET
|
||||
uint16_t UNSPECIFIED
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,173 @@
|
|||
#
|
||||
# Process the protocol specification and emit functions to pack and unpack buffers.
|
||||
#
|
||||
# See protocol.def for a description of the definition format.
|
||||
#
|
||||
|
||||
BEGIN {
|
||||
printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n")
|
||||
printf("#pragma pack(1)\n");
|
||||
|
||||
currentMessage = ""
|
||||
}
|
||||
|
||||
END {
|
||||
# finalise the last message definition
|
||||
EMIT_MESSAGE()
|
||||
|
||||
#
|
||||
# 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")
|
||||
for (opcode in opcodes) {
|
||||
printf("\t%s = 0x%x,\n", opcodes[opcode], opcode)
|
||||
}
|
||||
printf("\tMSG_ANY = 0xfe,\n")
|
||||
printf("\tMSG_NULL = 0xff\n")
|
||||
printf("};\n")
|
||||
|
||||
printf("#pragma pack(pop)\n")
|
||||
}
|
||||
|
||||
#
|
||||
# Emit definitions for one message
|
||||
#
|
||||
function EMIT_MESSAGE(payloadSize)
|
||||
{
|
||||
if (currentMessage != "") {
|
||||
printf("\n//////////////////////////////////////////////////////////////////////\n")
|
||||
printf("/// @name %s \n//@{\n\n", currentMessage)
|
||||
|
||||
#
|
||||
# emit a structure defining the message payload
|
||||
#
|
||||
printf("/// Structure describing the payload section of the %s message\n", currentMessage)
|
||||
printf("struct %s {\n", tolower(currentMessage))
|
||||
for (i = 0; i < fieldCount; i++) {
|
||||
printf("\t%s %s", types[i], names[i])
|
||||
if (counts[i])
|
||||
printf("[%s]", counts[i])
|
||||
printf(";\n")
|
||||
}
|
||||
printf("};\n\n")
|
||||
|
||||
#
|
||||
# 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))
|
||||
for (i = 0; i < fieldCount; i++) {
|
||||
if (counts[i]) {
|
||||
printf("\tconst %s (&%s)[%d]", types[i], names[i], counts[i])
|
||||
} else {
|
||||
printf("\tconst %s %s", types[i], names[i])
|
||||
}
|
||||
if (i < (fieldCount -1))
|
||||
printf(",\n");
|
||||
}
|
||||
printf(")\n{\n")
|
||||
printf("\tuint8_t *p = &_encodeBuf.payload;\n")
|
||||
payloadSize = 0;
|
||||
for (i = 0; i < fieldCount; i++) {
|
||||
if (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])
|
||||
payloadSize += sizes[i]
|
||||
}
|
||||
}
|
||||
printf("\t_encodeBuf.header.length = %s;\n", payloadSize)
|
||||
printf("\t_encodeBuf.header.messageID = %s;\n", currentMessage)
|
||||
printf("\t_encodeBuf.header.messageVersion = MSG_VERSION_1;\n")
|
||||
printf("\t_sendMessage();\n")
|
||||
printf("};\n\n")
|
||||
|
||||
#
|
||||
# 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))
|
||||
for (i = 0; i < fieldCount; i++) {
|
||||
if (counts[i]) {
|
||||
printf("\t%s (&%s)[%d]", types[i], names[i], counts[i])
|
||||
} else {
|
||||
printf("\t%s &%s", types[i], names[i])
|
||||
}
|
||||
if (i < (fieldCount -1))
|
||||
printf(",\n");
|
||||
}
|
||||
printf(")\n{\n")
|
||||
printf("\tuint8_t *p = &_decodeBuf.payload;\n")
|
||||
for (i = 0; i < fieldCount; i++) {
|
||||
if (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])
|
||||
payloadSize += sizes[i]
|
||||
}
|
||||
}
|
||||
printf("};\n")
|
||||
|
||||
# close the Doxygen group
|
||||
printf("//@}\n")
|
||||
}
|
||||
}
|
||||
|
||||
# skip lines containing comments
|
||||
$1=="#" {
|
||||
next
|
||||
}
|
||||
|
||||
#
|
||||
# process a new message declaration
|
||||
#
|
||||
$1=="message" {
|
||||
|
||||
# emit any previous message
|
||||
EMIT_MESSAGE()
|
||||
|
||||
# save the current opcode and message name
|
||||
currentOpcode = $2
|
||||
currentMessage = $3
|
||||
opcodes[$2] = $3
|
||||
|
||||
# set up for the coming fields
|
||||
fieldCount = 0
|
||||
delete types
|
||||
delete names
|
||||
delete sizes
|
||||
delete counts
|
||||
|
||||
next
|
||||
}
|
||||
|
||||
#
|
||||
# process a field inside a message definition
|
||||
#
|
||||
NF >= 2 {
|
||||
|
||||
# save the field definition
|
||||
types[fieldCount] = $1
|
||||
names[fieldCount] = $2
|
||||
|
||||
# guess the field size, note that we only support <inttypes.h> and "char"
|
||||
sizes[fieldCount] = 1
|
||||
if ($1 ~ ".*16.*")
|
||||
sizes[fieldCount] = 2
|
||||
if ($1 ~ ".*32.*")
|
||||
sizes[fieldCount] = 4
|
||||
|
||||
# if an array size was supplied, save it
|
||||
if (NF >= 3) {
|
||||
counts[fieldCount] = $3
|
||||
}
|
||||
|
||||
fieldCount++
|
||||
}
|
||||
Loading…
Reference in New Issue