mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-20 23:58:43 -04:00
71efe28851
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
266 lines
8.8 KiB
C++
266 lines
8.8 KiB
C++
// -*- 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
|