From e579f4a6aac963896a94afbb9739d01966770f33 Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Mon, 18 Apr 2011 02:51:19 +0000 Subject: [PATCH] Removing BinComm because we're using MavLink now git-svn-id: https://arducopter.googlecode.com/svn/trunk@1905 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/APM_BinComm/APM_BinComm.cpp | 236 --- libraries/APM_BinComm/APM_BinComm.h | 355 ----- libraries/APM_BinComm/keywords.txt | 6 - libraries/APM_BinComm/protocol/protocol.def | 279 ---- libraries/APM_BinComm/protocol/protocol.h | 1501 ------------------- libraries/APM_BinComm/protocol/protogen.awk | 183 --- libraries/APM_BinComm/test/Makefile | 14 - libraries/APM_BinComm/test/WProgram.h | 15 - libraries/APM_BinComm/test/test.cpp | 112 -- 9 files changed, 2701 deletions(-) delete mode 100644 libraries/APM_BinComm/APM_BinComm.cpp delete mode 100644 libraries/APM_BinComm/APM_BinComm.h delete mode 100644 libraries/APM_BinComm/keywords.txt delete mode 100644 libraries/APM_BinComm/protocol/protocol.def delete mode 100644 libraries/APM_BinComm/protocol/protocol.h delete mode 100644 libraries/APM_BinComm/protocol/protogen.awk delete mode 100644 libraries/APM_BinComm/test/Makefile delete mode 100644 libraries/APM_BinComm/test/WProgram.h delete mode 100644 libraries/APM_BinComm/test/test.cpp diff --git a/libraries/APM_BinComm/APM_BinComm.cpp b/libraries/APM_BinComm/APM_BinComm.cpp deleted file mode 100644 index d51a0bc7c5..0000000000 --- a/libraries/APM_BinComm/APM_BinComm.cpp +++ /dev/null @@ -1,236 +0,0 @@ -// -*- 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.cpp -/// @brief Implementation of the ArduPilot Mega binary communications -/// library. - -#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_MESSAGE 3 -#define DEC_WAIT_SUM_A 4 -#define DEC_WAIT_SUM_B 5 -//@} - - -/// inter-byte timeout for decode (ms) -#define DEC_MESSAGE_TIMEOUT 1000 - -BinComm::BinComm(const BinComm::MessageHandler *handlerTable, - Stream *interface) : - _handlerTable(handlerTable), - _decodePhase(DEC_WAIT_P1), - _lastReceived(millis()) -{ - init(interface); -}; - -void -BinComm::init(Stream *interface) -{ - _interface = interface; -} - -void -BinComm::_startMessage(uint8_t messageId, uint8_t messageLength, uint8_t messageVersion) -{ - // send the preamble first - _interface->write((uint8_t)MSG_PREAMBLE_1); - _interface->write((uint8_t)MSG_PREAMBLE_2); - - // initialise the checksum accumulators - _encoderSumA = _encoderSumB = 0; - - // send the header - _emit(messageLength); - _emit(messageId); - _emit(messageVersion); -} - -void -BinComm::_send(const void *bytes, uint8_t count) -{ - const uint8_t *p = (const uint8_t *)bytes; - uint8_t c; - - while (count--) { - c = *p++; - _encoderSumA += c; - _encoderSumB += _encoderSumA; - _interface->write(c); - } -} - -void -BinComm::_endMessage(void) -{ - _interface->write(_encoderSumA); - _interface->write(_encoderSumB); -} - -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; - _lastReceived = millis(); - - // 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(MessageHeader); - - // intialise the checksum accumulators - _decoderSumA = _decoderSumB = 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 - _decoderSumA += inByte; - _decoderSumB += _decoderSumA; - - // 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_MESSAGE: - // do checksum accumulation - _decoderSumA += inByte; - _decoderSumB += _decoderSumA; - - // 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 != _decoderSumA) { - badMessagesReceived++; - _decodePhase = DEC_WAIT_P1; - } else { - _decodePhase++; - } - break; - case DEC_WAIT_SUM_B: - if (inByte == _decoderSumB) { - // 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++) { - if(_handlerTable[tableIndex].messageID == MSG_ANY || - _handlerTable[tableIndex].messageID == _messageID ) { - _handlerTable[tableIndex].handler(_handlerTable[tableIndex].arg, - _messageID, _messageVersion, &_decodeBuf); - // dont' acknowledge an acknowledgement, will echo infinetly - if (_messageID != MSG_ACKNOWLEDGE) - send_msg_acknowledge(_messageID, _decoderSumA, _decoderSumB); - } else { - // XXX should send a NAK of some sort here - } - } - } else { - badMessagesReceived++; - } - // FALLTHROUGH - default: - _decodePhase = DEC_WAIT_P1; - break; - - } -} diff --git a/libraries/APM_BinComm/APM_BinComm.h b/libraries/APM_BinComm/APM_BinComm.h deleted file mode 100644 index b76f6ad18a..0000000000 --- a/libraries/APM_BinComm/APM_BinComm.h +++ /dev/null @@ -1,355 +0,0 @@ -// -*- 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 -#include -#include "WProgram.h" - -/// -/// @class BinComm -/// @brief Class providing protocol en/decoding services for the ArduPilot -/// Mega binary telemetry protocol. -/// -/// The protocol definition, including structures describing -/// messages, MessageID values and helper functions for sending -/// and unpacking messages are automatically generated. -/// -/// See protocol/protocol.def for a description of the message -/// definitions, and protocol/protocol.h for the generated -/// definitions. -/// -/// Protocol messages are sent using the send_* functions defined in -/// protocol/protocol.h, and handled on reception by functions defined -/// in the handlerTable array passed to the constructor. -/// -class BinComm { -public: - struct MessageHandler; - - ////////////////////////////////////////////////////////////////////// - /// 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. A single handler - /// may be registered for more than one message, - /// as the message ID is passed to the handler - /// when it is received. - /// - /// @param interface The stream that will be used - /// for telemetry communications. - /// - /// @param rxBuffSize Size of receive buffer allocated by interface. - /// This is used to warn for buffer overflow. - /// - - /// - BinComm(const MessageHandler *handlerTable, - Stream *interface = NULL); - - /// - /// Optional initialiser. - /// - /// If the interface stream isn't known at construction time, it - /// can be set here instead. - /// - /// @param interface The stream that will be used for telemetry - /// communications. - /// - void init(Stream *interface); - -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; - - ////////////////////////////////////////////////////////////////////// - /// @name Message pack/unpack utility functions - /// - //@{ - - /// Emit any scalar type. - /// - /// @param x Value to emit. - /// - template inline void _emit(const T x) { _send(&x, sizeof(T)); } - - /// Emit an array of any scalar type. - /// - /// @param x Array to emit. - /// @param count Array size. - /// - template inline void _emit(const T *values, uint8_t count) { _send(values, count * sizeof(T)); } - - - /// Emit a fixed-size string, from a NUL-terminated buffer. - /// - /// The string is NUL-padded if the buffer is larger than the string. - /// - /// @param msg The NUL-terminated string to emit. - /// @param size The maximum length of the string to emit. - /// - inline void _emit(const char *msg, uint8_t size) { - while (size--) { - char c = *msg; - _emit(c); - if (0 != c) - msg++; - } - } - - /// Unpack any scalar type. - /// - /// @param buf Buffer pointer. - /// @param x Unpacked result. - /// - template inline void _unpack(uint8_t *&ptr, T &x) { x = *(T *)ptr; ptr += sizeof(T); } - - /// Unpack an array of any scalar type. - /// - /// @param buf Buffer pointer. - /// @param values Array to receive the unpacked values. - /// - template inline void _unpack(uint8_t *&ptr, T *values, uint8_t count) { - memcpy(values, ptr, count * sizeof(T)); - ptr += count * sizeof(T); - } - - /// Unpack a string from a fixed-size buffer. - /// - /// @param ptr Buffer pointer. - /// @param msg Pointer to the result buffer. - /// @param size The size of the buffer. - /// - inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) { - strncpy(msg, (char *)ptr, size); msg[size-1] = '\0'; ptr += size; - } - //@} - -public: - ////////////////////////////////////////////////////////////////////// - /// @name Protocol definition - /// - //@{ -#include "protocol/protocol.h" - //@} - - ////////////////////////////////////////////////////////////////////// - /// @name Protocol magic numbers - /// - /// @note The MessageID enum is automatically generated and thus not described here. - /// - //@{ - - /// Message serverities - enum severities - { - SEVERITY_LOW = 1, - SEVERITY_MEDIUM = 2, - SEVERITY_HIGH = 3, - SEVERITY_CRITICAL = 4, - }; - - /// 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 - }; - - ////////////////////////////////////////////////////////////////////// - /// @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: - const MessageHandler *_handlerTable; ///< callout table - Stream *_interface; ///< Serial port we send/receive using. - - /// Various magic numbers - enum MagicNumbers { - MSG_PREAMBLE_1 = 0x34, - MSG_PREAMBLE_2 = 0x44, - MSG_VERSION_1 = 1, - MSG_VARIABLE_LENGTH = 0xff - }; - - ////////////////////////////////////////////////////////////////////// - /// @name Decoder state - //@{ - 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 _decoderSumA; ///< sum of incoming bytes - uint8_t _decoderSumB; ///< 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 - //@} - - /// Decoder state machine. - /// - /// @param inByte The byte to process. - /// - void _decode(uint8_t inByte); - - ////////////////////////////////////////////////////////////////////// - /// @name Encoder state - //@{ - uint8_t _encoderSumA; ///< sum of outgoing bytes - uint8_t _encoderSumB; ///< sum of _sumA values - //@} - - /// Start transmitting a message. - /// - /// @param messageId The ID of the message to be sent - /// @param messageLength The protocol-defined length of the message in bytes - /// @param messageVersion The message version (optional) - /// - void _startMessage(uint8_t messageId, uint8_t messageLength, uint8_t messageVersion = 1); - - /// Send bytes as part of a message. - /// - /// @param bytes Pointer to the byte(s) to send. - /// @param count Count of bytes to send. - void _send(const void *bytes, uint8_t count); - - /// Finalise message transmission. - /// - void _endMessage(void); -}; - -#endif // BinComm_h diff --git a/libraries/APM_BinComm/keywords.txt b/libraries/APM_BinComm/keywords.txt deleted file mode 100644 index 3d75c5372d..0000000000 --- a/libraries/APM_BinComm/keywords.txt +++ /dev/null @@ -1,6 +0,0 @@ -APM_BinComm KEYWORD1 -update KEYWORD2 -messagesReceived KEYWORD2 -badMessagesReceived KEYWORD2 -messagesSent KEYWORD2 - diff --git a/libraries/APM_BinComm/protocol/protocol.def b/libraries/APM_BinComm/protocol/protocol.def deleted file mode 100644 index 287595c092..0000000000 --- a/libraries/APM_BinComm/protocol/protocol.def +++ /dev/null @@ -1,279 +0,0 @@ -# -# Message definitions for the ArduPilot Mega binary communications protocol. -# -# Process this file to generate protocol.h, using: -# -# awk -f protogen.awk protocol.def > protocol.h -# -# Messages are declared with -# -# message -# -# is a unique name by which the message will -# be known. -# -# is the message ID byte -# -# Following a message declaration the fields of the message are -# defined in the format: -# -# [] -# -# is a C type corresponding to the field. The type must be a single -# word, either an integer from or "char". -# is the name of the field; it should be unique within the message -# is an optional array count for fields that are arrays -# -# Each message causes the definition of several items in the APM_BinComm -# class. Note that is uppercase, and is -# lowercase. -# -# BinComm:: -# -# An enumeration with the value -# -# BinComm:: -# -# A structure which corresponds to the layout of the message payload. -# In a message reception callout function the messageData pointer can -# be cast to this type to directly access specific elements of the -# message. Do not modify fields in the structure in this case. -# -# BinComm::send_( [, ...]) -# -# A function which takes arguments as listed in the message definition -# and which constructs and sends the message. -# The send_ functions queue data for transmission but -# do not wait for the message to be completed before returning, as long -# as there is space in the Stream buffer for the message. -# These functions are not re-entrant, and must not be called from an -# interrupt handler. -# -# BinComm::unpack_( &[, &...]) -# -# A function which unpacks the message. Use this instead of casting -# to the structure when you intend to use most of the values from the -# message. Must only be called inside the message reception callout -# function, as it references the message receive buffer directly. -# - -# -# Acknowledge message -# Required by protocol, don't change -message 0x00 MSG_ACKNOWLEDGE - uint8_t msgID - uint8_t sum1 - uint8_t sum2 -# -# Text status message -# Required by protocol, don't change -message 0x05 MSG_STATUS_TEXT - uint8_t severity - char text 50 - -# -# 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 - uint16_t yaw - -# -# Location report -# -message 0x03 MSG_LOCATION - int32_t latitude - int32_t longitude - int32_t altitude - uint16_t groundSpeed - uint16_t groundCourse - uint32_t timeOfWeek - -# -# Optional pressure-based location report -# -message 0x04 MSG_PRESSURE - int32_t pressureAltitude - int16_t airSpeed - -# -# Algorithm performance report -# -message 0x06 MSG_PERF_REPORT - uint32_t interval - uint16_t mainLoopCycles - uint8_t mainLoopCycleTime - uint8_t gyroSaturationCount - uint8_t adcConstraintCount - uint8_t renormSqrtCount - uint8_t renormBlowupCount - uint8_t gpsFixCount - 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 - uint16_t listLength - uint8_t commandID - uint8_t p1 - int32_t p2 - int32_t p3 - int32_t p4 - -message 0x22 MSG_COMMAND_LIST - uint16_t itemNumber - uint16_t listLength - uint8_t commandID - uint8_t p1 - int32_t p2 - int32_t p3 - int32_t p4 - -# this message is not in the standard -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 settings and values -# -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 - -message 0x53 MSG_RADIO_OUT - uint16_t value 8 - -# -# Direct sensor access -# -message 0x60 MSG_SENSOR - uint16_t UNSPECIFIED - -# -# Simulation-related messages -# -message 0x70 MSG_SERVO_OUT - int16_t value 8 - -# -# 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 - -# -# Navigation Augmentation -# -message 0xb0 MSG_POSITION_CORRECT - int16_t latError - int16_t lonError - int16_t altError - int16_t groundSpeedError - -message 0xb1 MSG_ATTITUDE_CORRECT - int16_t rollError - int16_t pitchError - int16_t yawError - -message 0xb2 MSG_POSITION_SET - int32_t latitude - int32_t longitude - int32_t altitude - uint16_t heading - -message 0xb3 MSG_ATTITUDE_SET - int16_t roll - int16_t pitch - uint16_t yaw diff --git a/libraries/APM_BinComm/protocol/protocol.h b/libraries/APM_BinComm/protocol/protocol.h deleted file mode 100644 index d81257b0e9..0000000000 --- a/libraries/APM_BinComm/protocol/protocol.h +++ /dev/null @@ -1,1501 +0,0 @@ -// -// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT -// -/// @file protocol.h -#pragma pack(push) -#pragma pack(1) - -////////////////////////////////////////////////////////////////////// -/// @name MSG_ACKNOWLEDGE -//@{ - -/// Structure describing the payload section of the MSG_ACKNOWLEDGE message -struct msg_acknowledge { - uint8_t msgID; - uint8_t sum1; - uint8_t sum2; -}; - -/// Send a MSG_ACKNOWLEDGE message -inline void -send_msg_acknowledge( - const uint8_t msgID, - const uint8_t sum1, - const uint8_t sum2) -{ - _startMessage(MSG_ACKNOWLEDGE, - sizeof(msgID) + - sizeof(sum1) + - sizeof(sum2) + 0); - _emit(msgID); - _emit(sum1); - _emit(sum2); - _endMessage(); -}; - -/// Unpack a MSG_ACKNOWLEDGE message -inline void -unpack_msg_acknowledge( - uint8_t &msgID, - uint8_t &sum1, - uint8_t &sum2) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, msgID); - _unpack(__p, sum1); - _unpack(__p, sum2); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_STATUS_TEXT -//@{ - -/// Structure describing the payload section of the MSG_STATUS_TEXT message -struct msg_status_text { - uint8_t severity; - char text[50]; -}; - -/// Send a MSG_STATUS_TEXT message -inline void -send_msg_status_text( - const uint8_t severity, - const char (&text)[50]) -{ - _startMessage(MSG_STATUS_TEXT, - sizeof(severity) + - (sizeof(text[0]) * 50) + 0); - _emit(severity); - _emit(text, 50); - _endMessage(); -}; - -/// Unpack a MSG_STATUS_TEXT message -inline void -unpack_msg_status_text( - uint8_t &severity, - char (&text)[50]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, severity); - _unpack(__p, text, 50); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_HEARTBEAT -//@{ - -/// Structure describing the payload section of the MSG_HEARTBEAT message -struct msg_heartbeat { - uint8_t flightMode; - uint16_t timeStamp; - uint16_t batteryVoltage; - uint16_t commandIndex; -}; - -/// Send a MSG_HEARTBEAT message -inline void -send_msg_heartbeat( - const uint8_t flightMode, - const uint16_t timeStamp, - const uint16_t batteryVoltage, - const uint16_t commandIndex) -{ - _startMessage(MSG_HEARTBEAT, - sizeof(flightMode) + - sizeof(timeStamp) + - sizeof(batteryVoltage) + - sizeof(commandIndex) + 0); - _emit(flightMode); - _emit(timeStamp); - _emit(batteryVoltage); - _emit(commandIndex); - _endMessage(); -}; - -/// Unpack a MSG_HEARTBEAT message -inline void -unpack_msg_heartbeat( - uint8_t &flightMode, - uint16_t &timeStamp, - uint16_t &batteryVoltage, - uint16_t &commandIndex) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, flightMode); - _unpack(__p, timeStamp); - _unpack(__p, batteryVoltage); - _unpack(__p, commandIndex); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_ATTITUDE -//@{ - -/// Structure describing the payload section of the MSG_ATTITUDE message -struct msg_attitude { - int16_t roll; - int16_t pitch; - uint16_t yaw; -}; - -/// Send a MSG_ATTITUDE message -inline void -send_msg_attitude( - const int16_t roll, - const int16_t pitch, - const uint16_t yaw) -{ - _startMessage(MSG_ATTITUDE, - sizeof(roll) + - sizeof(pitch) + - sizeof(yaw) + 0); - _emit(roll); - _emit(pitch); - _emit(yaw); - _endMessage(); -}; - -/// Unpack a MSG_ATTITUDE message -inline void -unpack_msg_attitude( - int16_t &roll, - int16_t &pitch, - uint16_t &yaw) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, roll); - _unpack(__p, pitch); - _unpack(__p, yaw); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_LOCATION -//@{ - -/// Structure describing the payload section of the MSG_LOCATION message -struct msg_location { - int32_t latitude; - int32_t longitude; - int32_t altitude; - uint16_t groundSpeed; - uint16_t groundCourse; - uint32_t timeOfWeek; -}; - -/// Send a MSG_LOCATION message -inline void -send_msg_location( - const int32_t latitude, - const int32_t longitude, - const int32_t altitude, - const uint16_t groundSpeed, - const uint16_t groundCourse, - const uint32_t timeOfWeek) -{ - _startMessage(MSG_LOCATION, - sizeof(latitude) + - sizeof(longitude) + - sizeof(altitude) + - sizeof(groundSpeed) + - sizeof(groundCourse) + - sizeof(timeOfWeek) + 0); - _emit(latitude); - _emit(longitude); - _emit(altitude); - _emit(groundSpeed); - _emit(groundCourse); - _emit(timeOfWeek); - _endMessage(); -}; - -/// Unpack a MSG_LOCATION message -inline void -unpack_msg_location( - int32_t &latitude, - int32_t &longitude, - int32_t &altitude, - uint16_t &groundSpeed, - uint16_t &groundCourse, - uint32_t &timeOfWeek) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, latitude); - _unpack(__p, longitude); - _unpack(__p, altitude); - _unpack(__p, groundSpeed); - _unpack(__p, groundCourse); - _unpack(__p, timeOfWeek); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PRESSURE -//@{ - -/// Structure describing the payload section of the MSG_PRESSURE message -struct msg_pressure { - int32_t pressureAltitude; - int16_t airSpeed; -}; - -/// Send a MSG_PRESSURE message -inline void -send_msg_pressure( - const int32_t pressureAltitude, - const int16_t airSpeed) -{ - _startMessage(MSG_PRESSURE, - sizeof(pressureAltitude) + - sizeof(airSpeed) + 0); - _emit(pressureAltitude); - _emit(airSpeed); - _endMessage(); -}; - -/// Unpack a MSG_PRESSURE message -inline void -unpack_msg_pressure( - int32_t &pressureAltitude, - int16_t &airSpeed) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, pressureAltitude); - _unpack(__p, airSpeed); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PERF_REPORT -//@{ - -/// Structure describing the payload section of the MSG_PERF_REPORT message -struct msg_perf_report { - uint32_t interval; - uint16_t mainLoopCycles; - uint8_t mainLoopCycleTime; - uint8_t gyroSaturationCount; - uint8_t adcConstraintCount; - uint8_t renormSqrtCount; - uint8_t renormBlowupCount; - uint8_t gpsFixCount; - uint16_t imuHealth; - uint16_t gcsMessageCount; -}; - -/// Send a MSG_PERF_REPORT message -inline void -send_msg_perf_report( - const uint32_t interval, - const uint16_t mainLoopCycles, - const uint8_t mainLoopCycleTime, - const uint8_t gyroSaturationCount, - const uint8_t adcConstraintCount, - const uint8_t renormSqrtCount, - const uint8_t renormBlowupCount, - const uint8_t gpsFixCount, - const uint16_t imuHealth, - const uint16_t gcsMessageCount) -{ - _startMessage(MSG_PERF_REPORT, - sizeof(interval) + - sizeof(mainLoopCycles) + - sizeof(mainLoopCycleTime) + - sizeof(gyroSaturationCount) + - sizeof(adcConstraintCount) + - sizeof(renormSqrtCount) + - sizeof(renormBlowupCount) + - sizeof(gpsFixCount) + - sizeof(imuHealth) + - sizeof(gcsMessageCount) + 0); - _emit(interval); - _emit(mainLoopCycles); - _emit(mainLoopCycleTime); - _emit(gyroSaturationCount); - _emit(adcConstraintCount); - _emit(renormSqrtCount); - _emit(renormBlowupCount); - _emit(gpsFixCount); - _emit(imuHealth); - _emit(gcsMessageCount); - _endMessage(); -}; - -/// Unpack a MSG_PERF_REPORT message -inline void -unpack_msg_perf_report( - uint32_t &interval, - uint16_t &mainLoopCycles, - uint8_t &mainLoopCycleTime, - uint8_t &gyroSaturationCount, - uint8_t &adcConstraintCount, - uint8_t &renormSqrtCount, - uint8_t &renormBlowupCount, - uint8_t &gpsFixCount, - uint16_t &imuHealth, - uint16_t &gcsMessageCount) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, interval); - _unpack(__p, mainLoopCycles); - _unpack(__p, mainLoopCycleTime); - _unpack(__p, gyroSaturationCount); - _unpack(__p, adcConstraintCount); - _unpack(__p, renormSqrtCount); - _unpack(__p, renormBlowupCount); - _unpack(__p, gpsFixCount); - _unpack(__p, imuHealth); - _unpack(__p, gcsMessageCount); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VERSION_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_VERSION_REQUEST message -struct msg_version_request { - uint8_t systemType; - uint8_t systemID; -}; - -/// Send a MSG_VERSION_REQUEST message -inline void -send_msg_version_request( - const uint8_t systemType, - const uint8_t systemID) -{ - _startMessage(MSG_VERSION_REQUEST, - sizeof(systemType) + - sizeof(systemID) + 0); - _emit(systemType); - _emit(systemID); - _endMessage(); -}; - -/// Unpack a MSG_VERSION_REQUEST message -inline void -unpack_msg_version_request( - uint8_t &systemType, - uint8_t &systemID) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, systemType); - _unpack(__p, systemID); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VERSION -//@{ - -/// Structure describing the payload section of the MSG_VERSION message -struct msg_version { - uint8_t systemType; - uint8_t systemID; - uint8_t firmwareVersion[3]; -}; - -/// Send a MSG_VERSION message -inline void -send_msg_version( - const uint8_t systemType, - const uint8_t systemID, - const uint8_t (&firmwareVersion)[3]) -{ - _startMessage(MSG_VERSION, - sizeof(systemType) + - sizeof(systemID) + - (sizeof(firmwareVersion[0]) * 3) + 0); - _emit(systemType); - _emit(systemID); - _emit(firmwareVersion, 3); - _endMessage(); -}; - -/// Unpack a MSG_VERSION message -inline void -unpack_msg_version( - uint8_t &systemType, - uint8_t &systemID, - uint8_t (&firmwareVersion)[3]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, systemType); - _unpack(__p, systemID); - _unpack(__p, firmwareVersion, 3); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_COMMAND_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_COMMAND_REQUEST message -struct msg_command_request { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_COMMAND_REQUEST message -inline void -send_msg_command_request( - const uint16_t UNSPECIFIED) -{ - _startMessage(MSG_COMMAND_REQUEST, - sizeof(UNSPECIFIED) + 0); - _emit(UNSPECIFIED); - _endMessage(); -}; - -/// Unpack a MSG_COMMAND_REQUEST message -inline void -unpack_msg_command_request( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_COMMAND_UPLOAD -//@{ - -/// Structure describing the payload section of the MSG_COMMAND_UPLOAD message -struct msg_command_upload { - uint8_t action; - uint16_t itemNumber; - uint16_t listLength; - uint8_t commandID; - uint8_t p1; - int32_t p2; - int32_t p3; - int32_t p4; -}; - -/// Send a MSG_COMMAND_UPLOAD message -inline void -send_msg_command_upload( - const uint8_t action, - const uint16_t itemNumber, - const uint16_t listLength, - const uint8_t commandID, - const uint8_t p1, - const int32_t p2, - const int32_t p3, - const int32_t p4) -{ - _startMessage(MSG_COMMAND_UPLOAD, - sizeof(action) + - sizeof(itemNumber) + - sizeof(listLength) + - sizeof(commandID) + - sizeof(p1) + - sizeof(p2) + - sizeof(p3) + - sizeof(p4) + 0); - _emit(action); - _emit(itemNumber); - _emit(listLength); - _emit(commandID); - _emit(p1); - _emit(p2); - _emit(p3); - _emit(p4); - _endMessage(); -}; - -/// Unpack a MSG_COMMAND_UPLOAD message -inline void -unpack_msg_command_upload( - uint8_t &action, - uint16_t &itemNumber, - uint16_t &listLength, - uint8_t &commandID, - uint8_t &p1, - int32_t &p2, - int32_t &p3, - int32_t &p4) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, action); - _unpack(__p, itemNumber); - _unpack(__p, listLength); - _unpack(__p, commandID); - _unpack(__p, p1); - _unpack(__p, p2); - _unpack(__p, p3); - _unpack(__p, p4); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_COMMAND_LIST -//@{ - -/// Structure describing the payload section of the MSG_COMMAND_LIST message -struct msg_command_list { - uint16_t itemNumber; - uint16_t listLength; - uint8_t commandID; - uint8_t p1; - int32_t p2; - int32_t p3; - int32_t p4; -}; - -/// Send a MSG_COMMAND_LIST message -inline void -send_msg_command_list( - const uint16_t itemNumber, - const uint16_t listLength, - const uint8_t commandID, - const uint8_t p1, - const int32_t p2, - const int32_t p3, - const int32_t p4) -{ - _startMessage(MSG_COMMAND_LIST, - sizeof(itemNumber) + - sizeof(listLength) + - sizeof(commandID) + - sizeof(p1) + - sizeof(p2) + - sizeof(p3) + - sizeof(p4) + 0); - _emit(itemNumber); - _emit(listLength); - _emit(commandID); - _emit(p1); - _emit(p2); - _emit(p3); - _emit(p4); - _endMessage(); -}; - -/// Unpack a MSG_COMMAND_LIST message -inline void -unpack_msg_command_list( - uint16_t &itemNumber, - uint16_t &listLength, - uint8_t &commandID, - uint8_t &p1, - int32_t &p2, - int32_t &p3, - int32_t &p4) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, itemNumber); - _unpack(__p, listLength); - _unpack(__p, commandID); - _unpack(__p, p1); - _unpack(__p, p2); - _unpack(__p, p3); - _unpack(__p, p4); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_COMMAND_MODE_CHANGE -//@{ - -/// Structure describing the payload section of the MSG_COMMAND_MODE_CHANGE message -struct msg_command_mode_change { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_COMMAND_MODE_CHANGE message -inline void -send_msg_command_mode_change( - const uint16_t UNSPECIFIED) -{ - _startMessage(MSG_COMMAND_MODE_CHANGE, - sizeof(UNSPECIFIED) + 0); - _emit(UNSPECIFIED); - _endMessage(); -}; - -/// Unpack a MSG_COMMAND_MODE_CHANGE message -inline void -unpack_msg_command_mode_change( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VALUE_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_VALUE_REQUEST message -struct msg_value_request { - uint8_t valueID; - uint8_t broadcast; -}; - -/// Send a MSG_VALUE_REQUEST message -inline void -send_msg_value_request( - const uint8_t valueID, - const uint8_t broadcast) -{ - _startMessage(MSG_VALUE_REQUEST, - sizeof(valueID) + - sizeof(broadcast) + 0); - _emit(valueID); - _emit(broadcast); - _endMessage(); -}; - -/// Unpack a MSG_VALUE_REQUEST message -inline void -unpack_msg_value_request( - uint8_t &valueID, - uint8_t &broadcast) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, valueID); - _unpack(__p, broadcast); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VALUE_SET -//@{ - -/// Structure describing the payload section of the MSG_VALUE_SET message -struct msg_value_set { - uint8_t valueID; - uint32_t value; -}; - -/// Send a MSG_VALUE_SET message -inline void -send_msg_value_set( - const uint8_t valueID, - const uint32_t value) -{ - _startMessage(MSG_VALUE_SET, - sizeof(valueID) + - sizeof(value) + 0); - _emit(valueID); - _emit(value); - _endMessage(); -}; - -/// Unpack a MSG_VALUE_SET message -inline void -unpack_msg_value_set( - uint8_t &valueID, - uint32_t &value) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, valueID); - _unpack(__p, value); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VALUE -//@{ - -/// Structure describing the payload section of the MSG_VALUE message -struct msg_value { - uint8_t valueID; - uint32_t value; -}; - -/// Send a MSG_VALUE message -inline void -send_msg_value( - const uint8_t valueID, - const uint32_t value) -{ - _startMessage(MSG_VALUE, - sizeof(valueID) + - sizeof(value) + 0); - _emit(valueID); - _emit(value); - _endMessage(); -}; - -/// Unpack a MSG_VALUE message -inline void -unpack_msg_value( - uint8_t &valueID, - uint32_t &value) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, valueID); - _unpack(__p, value); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PID_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_PID_REQUEST message -struct msg_pid_request { - uint8_t pidSet; -}; - -/// Send a MSG_PID_REQUEST message -inline void -send_msg_pid_request( - const uint8_t pidSet) -{ - _startMessage(MSG_PID_REQUEST, - sizeof(pidSet) + 0); - _emit(pidSet); - _endMessage(); -}; - -/// Unpack a MSG_PID_REQUEST message -inline void -unpack_msg_pid_request( - uint8_t &pidSet) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, pidSet); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PID_SET -//@{ - -/// Structure describing the payload section of the MSG_PID_SET message -struct msg_pid_set { - uint8_t pidSet; - int32_t p; - int32_t i; - int32_t d; - int16_t integratorMax; -}; - -/// Send a MSG_PID_SET message -inline void -send_msg_pid_set( - const uint8_t pidSet, - const int32_t p, - const int32_t i, - const int32_t d, - const int16_t integratorMax) -{ - _startMessage(MSG_PID_SET, - sizeof(pidSet) + - sizeof(p) + - sizeof(i) + - sizeof(d) + - sizeof(integratorMax) + 0); - _emit(pidSet); - _emit(p); - _emit(i); - _emit(d); - _emit(integratorMax); - _endMessage(); -}; - -/// Unpack a MSG_PID_SET message -inline void -unpack_msg_pid_set( - uint8_t &pidSet, - int32_t &p, - int32_t &i, - int32_t &d, - int16_t &integratorMax) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, pidSet); - _unpack(__p, p); - _unpack(__p, i); - _unpack(__p, d); - _unpack(__p, integratorMax); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PID -//@{ - -/// Structure describing the payload section of the MSG_PID message -struct msg_pid { - uint8_t pidSet; - int32_t p; - int32_t i; - int32_t d; - int16_t integratorMax; -}; - -/// Send a MSG_PID message -inline void -send_msg_pid( - const uint8_t pidSet, - const int32_t p, - const int32_t i, - const int32_t d, - const int16_t integratorMax) -{ - _startMessage(MSG_PID, - sizeof(pidSet) + - sizeof(p) + - sizeof(i) + - sizeof(d) + - sizeof(integratorMax) + 0); - _emit(pidSet); - _emit(p); - _emit(i); - _emit(d); - _emit(integratorMax); - _endMessage(); -}; - -/// Unpack a MSG_PID message -inline void -unpack_msg_pid( - uint8_t &pidSet, - int32_t &p, - int32_t &i, - int32_t &d, - int16_t &integratorMax) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, pidSet); - _unpack(__p, p); - _unpack(__p, i); - _unpack(__p, d); - _unpack(__p, integratorMax); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_TRIM_STARTUP -//@{ - -/// Structure describing the payload section of the MSG_TRIM_STARTUP message -struct msg_trim_startup { - uint16_t value[8]; -}; - -/// Send a MSG_TRIM_STARTUP message -inline void -send_msg_trim_startup( - const uint16_t (&value)[8]) -{ - _startMessage(MSG_TRIM_STARTUP, - (sizeof(value[0]) * 8) + 0); - _emit(value, 8); - _endMessage(); -}; - -/// Unpack a MSG_TRIM_STARTUP message -inline void -unpack_msg_trim_startup( - uint16_t (&value)[8]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, value, 8); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_TRIM_MIN -//@{ - -/// Structure describing the payload section of the MSG_TRIM_MIN message -struct msg_trim_min { - uint16_t value[8]; -}; - -/// Send a MSG_TRIM_MIN message -inline void -send_msg_trim_min( - const uint16_t (&value)[8]) -{ - _startMessage(MSG_TRIM_MIN, - (sizeof(value[0]) * 8) + 0); - _emit(value, 8); - _endMessage(); -}; - -/// Unpack a MSG_TRIM_MIN message -inline void -unpack_msg_trim_min( - uint16_t (&value)[8]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, value, 8); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_TRIM_MAX -//@{ - -/// Structure describing the payload section of the MSG_TRIM_MAX message -struct msg_trim_max { - uint16_t value[8]; -}; - -/// Send a MSG_TRIM_MAX message -inline void -send_msg_trim_max( - const uint16_t (&value)[8]) -{ - _startMessage(MSG_TRIM_MAX, - (sizeof(value[0]) * 8) + 0); - _emit(value, 8); - _endMessage(); -}; - -/// Unpack a MSG_TRIM_MAX message -inline void -unpack_msg_trim_max( - uint16_t (&value)[8]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, value, 8); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_RADIO_OUT -//@{ - -/// Structure describing the payload section of the MSG_RADIO_OUT message -struct msg_radio_out { - uint16_t value[8]; -}; - -/// Send a MSG_RADIO_OUT message -inline void -send_msg_radio_out( - const uint16_t (&value)[8]) -{ - _startMessage(MSG_RADIO_OUT, - (sizeof(value[0]) * 8) + 0); - _emit(value, 8); - _endMessage(); -}; - -/// Unpack a MSG_RADIO_OUT message -inline void -unpack_msg_radio_out( - uint16_t (&value)[8]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, value, 8); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_SENSOR -//@{ - -/// Structure describing the payload section of the MSG_SENSOR message -struct msg_sensor { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_SENSOR message -inline void -send_msg_sensor( - const uint16_t UNSPECIFIED) -{ - _startMessage(MSG_SENSOR, - sizeof(UNSPECIFIED) + 0); - _emit(UNSPECIFIED); - _endMessage(); -}; - -/// Unpack a MSG_SENSOR message -inline void -unpack_msg_sensor( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_SERVO_OUT -//@{ - -/// Structure describing the payload section of the MSG_SERVO_OUT message -struct msg_servo_out { - int16_t value[8]; -}; - -/// Send a MSG_SERVO_OUT message -inline void -send_msg_servo_out( - const int16_t (&value)[8]) -{ - _startMessage(MSG_SERVO_OUT, - (sizeof(value[0]) * 8) + 0); - _emit(value, 8); - _endMessage(); -}; - -/// Unpack a MSG_SERVO_OUT message -inline void -unpack_msg_servo_out( - int16_t (&value)[8]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, value, 8); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PIN_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_PIN_REQUEST message -struct msg_pin_request { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_PIN_REQUEST message -inline void -send_msg_pin_request( - const uint16_t UNSPECIFIED) -{ - _startMessage(MSG_PIN_REQUEST, - sizeof(UNSPECIFIED) + 0); - _emit(UNSPECIFIED); - _endMessage(); -}; - -/// Unpack a MSG_PIN_REQUEST message -inline void -unpack_msg_pin_request( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PIN_SET -//@{ - -/// Structure describing the payload section of the MSG_PIN_SET message -struct msg_pin_set { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_PIN_SET message -inline void -send_msg_pin_set( - const uint16_t UNSPECIFIED) -{ - _startMessage(MSG_PIN_SET, - sizeof(UNSPECIFIED) + 0); - _emit(UNSPECIFIED); - _endMessage(); -}; - -/// Unpack a MSG_PIN_SET message -inline void -unpack_msg_pin_set( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_DATAFLASH_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_DATAFLASH_REQUEST message -struct msg_dataflash_request { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_DATAFLASH_REQUEST message -inline void -send_msg_dataflash_request( - const uint16_t UNSPECIFIED) -{ - _startMessage(MSG_DATAFLASH_REQUEST, - sizeof(UNSPECIFIED) + 0); - _emit(UNSPECIFIED); - _endMessage(); -}; - -/// Unpack a MSG_DATAFLASH_REQUEST message -inline void -unpack_msg_dataflash_request( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_DATAFLASH_SET -//@{ - -/// Structure describing the payload section of the MSG_DATAFLASH_SET message -struct msg_dataflash_set { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_DATAFLASH_SET message -inline void -send_msg_dataflash_set( - const uint16_t UNSPECIFIED) -{ - _startMessage(MSG_DATAFLASH_SET, - sizeof(UNSPECIFIED) + 0); - _emit(UNSPECIFIED); - _endMessage(); -}; - -/// Unpack a MSG_DATAFLASH_SET message -inline void -unpack_msg_dataflash_set( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_EEPROM_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_EEPROM_REQUEST message -struct msg_eeprom_request { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_EEPROM_REQUEST message -inline void -send_msg_eeprom_request( - const uint16_t UNSPECIFIED) -{ - _startMessage(MSG_EEPROM_REQUEST, - sizeof(UNSPECIFIED) + 0); - _emit(UNSPECIFIED); - _endMessage(); -}; - -/// Unpack a MSG_EEPROM_REQUEST message -inline void -unpack_msg_eeprom_request( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_EEPROM_SET -//@{ - -/// Structure describing the payload section of the MSG_EEPROM_SET message -struct msg_eeprom_set { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_EEPROM_SET message -inline void -send_msg_eeprom_set( - const uint16_t UNSPECIFIED) -{ - _startMessage(MSG_EEPROM_SET, - sizeof(UNSPECIFIED) + 0); - _emit(UNSPECIFIED); - _endMessage(); -}; - -/// Unpack a MSG_EEPROM_SET message -inline void -unpack_msg_eeprom_set( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_POSITION_CORRECT -//@{ - -/// Structure describing the payload section of the MSG_POSITION_CORRECT message -struct msg_position_correct { - int16_t latError; - int16_t lonError; - int16_t altError; - int16_t groundSpeedError; -}; - -/// Send a MSG_POSITION_CORRECT message -inline void -send_msg_position_correct( - const int16_t latError, - const int16_t lonError, - const int16_t altError, - const int16_t groundSpeedError) -{ - _startMessage(MSG_POSITION_CORRECT, - sizeof(latError) + - sizeof(lonError) + - sizeof(altError) + - sizeof(groundSpeedError) + 0); - _emit(latError); - _emit(lonError); - _emit(altError); - _emit(groundSpeedError); - _endMessage(); -}; - -/// Unpack a MSG_POSITION_CORRECT message -inline void -unpack_msg_position_correct( - int16_t &latError, - int16_t &lonError, - int16_t &altError, - int16_t &groundSpeedError) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, latError); - _unpack(__p, lonError); - _unpack(__p, altError); - _unpack(__p, groundSpeedError); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_ATTITUDE_CORRECT -//@{ - -/// Structure describing the payload section of the MSG_ATTITUDE_CORRECT message -struct msg_attitude_correct { - int16_t rollError; - int16_t pitchError; - int16_t yawError; -}; - -/// Send a MSG_ATTITUDE_CORRECT message -inline void -send_msg_attitude_correct( - const int16_t rollError, - const int16_t pitchError, - const int16_t yawError) -{ - _startMessage(MSG_ATTITUDE_CORRECT, - sizeof(rollError) + - sizeof(pitchError) + - sizeof(yawError) + 0); - _emit(rollError); - _emit(pitchError); - _emit(yawError); - _endMessage(); -}; - -/// Unpack a MSG_ATTITUDE_CORRECT message -inline void -unpack_msg_attitude_correct( - int16_t &rollError, - int16_t &pitchError, - int16_t &yawError) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, rollError); - _unpack(__p, pitchError); - _unpack(__p, yawError); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_POSITION_SET -//@{ - -/// Structure describing the payload section of the MSG_POSITION_SET message -struct msg_position_set { - int32_t latitude; - int32_t longitude; - int32_t altitude; - uint16_t heading; -}; - -/// Send a MSG_POSITION_SET message -inline void -send_msg_position_set( - const int32_t latitude, - const int32_t longitude, - const int32_t altitude, - const uint16_t heading) -{ - _startMessage(MSG_POSITION_SET, - sizeof(latitude) + - sizeof(longitude) + - sizeof(altitude) + - sizeof(heading) + 0); - _emit(latitude); - _emit(longitude); - _emit(altitude); - _emit(heading); - _endMessage(); -}; - -/// Unpack a MSG_POSITION_SET message -inline void -unpack_msg_position_set( - int32_t &latitude, - int32_t &longitude, - int32_t &altitude, - uint16_t &heading) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, latitude); - _unpack(__p, longitude); - _unpack(__p, altitude); - _unpack(__p, heading); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_ATTITUDE_SET -//@{ - -/// Structure describing the payload section of the MSG_ATTITUDE_SET message -struct msg_attitude_set { - int16_t roll; - int16_t pitch; - uint16_t yaw; -}; - -/// Send a MSG_ATTITUDE_SET message -inline void -send_msg_attitude_set( - const int16_t roll, - const int16_t pitch, - const uint16_t yaw) -{ - _startMessage(MSG_ATTITUDE_SET, - sizeof(roll) + - sizeof(pitch) + - sizeof(yaw) + 0); - _emit(roll); - _emit(pitch); - _emit(yaw); - _endMessage(); -}; - -/// Unpack a MSG_ATTITUDE_SET message -inline void -unpack_msg_attitude_set( - int16_t &roll, - int16_t &pitch, - uint16_t &yaw) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, roll); - _unpack(__p, pitch); - _unpack(__p, yaw); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// Message ID values -enum MessageID { - MSG_PID = 0x42, - MSG_DATAFLASH_REQUEST = 0x90, - MSG_DATAFLASH_SET = 0x91, - MSG_SENSOR = 0x60, - MSG_VALUE_REQUEST = 0x30, - MSG_VALUE_SET = 0x31, - MSG_VALUE = 0x32, - MSG_PIN_REQUEST = 0x80, - MSG_PIN_SET = 0x81, - MSG_POSITION_CORRECT = 0xb0, - MSG_ACKNOWLEDGE = 0x0, - MSG_ATTITUDE_CORRECT = 0xb1, - MSG_HEARTBEAT = 0x1, - MSG_POSITION_SET = 0xb2, - MSG_ATTITUDE = 0x2, - MSG_ATTITUDE_SET = 0xb3, - MSG_LOCATION = 0x3, - MSG_PRESSURE = 0x4, - MSG_TRIM_STARTUP = 0x50, - MSG_STATUS_TEXT = 0x5, - MSG_TRIM_MIN = 0x51, - MSG_PERF_REPORT = 0x6, - MSG_TRIM_MAX = 0x52, - MSG_VERSION_REQUEST = 0x7, - MSG_RADIO_OUT = 0x53, - MSG_VERSION = 0x8, - MSG_COMMAND_REQUEST = 0x20, - MSG_COMMAND_UPLOAD = 0x21, - MSG_COMMAND_LIST = 0x22, - MSG_COMMAND_MODE_CHANGE = 0x23, - MSG_SERVO_OUT = 0x70, - MSG_EEPROM_REQUEST = 0xa0, - MSG_EEPROM_SET = 0xa1, - MSG_PID_REQUEST = 0x40, - MSG_PID_SET = 0x41, - MSG_ANY = 0xfe, - MSG_NULL = 0xff -}; - -////////////////////////////////////////////////////////////////////// -/// Message buffer sizing -union _binCommBufferSizer { - struct msg_acknowledge msg_acknowledge; - struct msg_status_text msg_status_text; - struct msg_heartbeat msg_heartbeat; - struct msg_attitude msg_attitude; - struct msg_location msg_location; - struct msg_pressure msg_pressure; - struct msg_perf_report msg_perf_report; - struct msg_version_request msg_version_request; - struct msg_version msg_version; - struct msg_command_request msg_command_request; - struct msg_command_upload msg_command_upload; - struct msg_command_list msg_command_list; - struct msg_command_mode_change msg_command_mode_change; - struct msg_value_request msg_value_request; - struct msg_value_set msg_value_set; - struct msg_value msg_value; - struct msg_pid_request msg_pid_request; - struct msg_pid_set msg_pid_set; - struct msg_pid msg_pid; - struct msg_trim_startup msg_trim_startup; - struct msg_trim_min msg_trim_min; - struct msg_trim_max msg_trim_max; - struct msg_radio_out msg_radio_out; - struct msg_sensor msg_sensor; - struct msg_servo_out msg_servo_out; - struct msg_pin_request msg_pin_request; - struct msg_pin_set msg_pin_set; - struct msg_dataflash_request msg_dataflash_request; - struct msg_dataflash_set msg_dataflash_set; - struct msg_eeprom_request msg_eeprom_request; - struct msg_eeprom_set msg_eeprom_set; - struct msg_position_correct msg_position_correct; - struct msg_attitude_correct msg_attitude_correct; - struct msg_position_set msg_position_set; - struct msg_attitude_set msg_attitude_set; -}; -#define BINCOMM_MAX_MESSAGE_SIZE sizeof(union _binCommBufferSizer) - -#pragma pack(pop) diff --git a/libraries/APM_BinComm/protocol/protogen.awk b/libraries/APM_BinComm/protocol/protogen.awk deleted file mode 100644 index 556eece10a..0000000000 --- a/libraries/APM_BinComm/protocol/protogen.awk +++ /dev/null @@ -1,183 +0,0 @@ -# -# 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("/// @file protocol.h\n") - printf("#pragma pack(push)\n"); - printf("#pragma pack(1)\n"); - - currentMessage = "" - structureCount = 0 -} - -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("\n//////////////////////////////////////////////////////////////////////\n") - printf("/// Message buffer sizing\n") - printf("union _binCommBufferSizer {\n"); - for (i = 0; i < structureCount; i++) { - printf("\tstruct %s %s;\n", structs[i], structs[i]); - } - printf("};\n"); - printf("#define BINCOMM_MAX_MESSAGE_SIZE sizeof(union _binCommBufferSizer)\n\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") - - # - # Record the structure name for later use sizing the receive buffer. - # - structs[structureCount] = tolower(currentMessage); - structureCount++; - - # - # emit a routine to emit the message payload from a set of variables - # - printf("/// Send a %s message\n", 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]) - } else { - printf("\tconst %s %s", types[i], names[i]) - } - if (i < (fieldCount -1)) - printf(",\n"); - } - printf(")\n{\n") - printf("\t_startMessage(%s,", currentMessage); - for (i = 0; i < fieldCount; i++) { - if (counts[i]) { - printf("\n\t\t(sizeof(%s[0]) * %d) +", names[i], counts[i]); - } else { - printf("\n\t\tsizeof(%s) +", names[i]); - } - } - printf(" 0);\n"); - for (i = 0; i < fieldCount; i++) { - if (counts[i]) { - printf("\t_emit(%s, %s);\n", names[i], counts[i]) - } else { - printf("\t_emit(%s);\n", names[i]) - } - } - printf("\t_endMessage();\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\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]) - } else { - printf("\t%s &%s", types[i], names[i]) - } - if (i < (fieldCount -1)) - printf(",\n"); - } - printf(")\n{\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]) - } else { - printf("\t_unpack(__p, %s);\n", names[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 counts - - next -} - -# -# process a field inside a message definition -# -NF >= 2 { - - # save the field definition - types[fieldCount] = $1 - names[fieldCount] = $2 - - # if an array size was supplied, save it - if (NF >= 3) { - counts[fieldCount] = $3 - } - - fieldCount++ -} diff --git a/libraries/APM_BinComm/test/Makefile b/libraries/APM_BinComm/test/Makefile deleted file mode 100644 index f61e128ee4..0000000000 --- a/libraries/APM_BinComm/test/Makefile +++ /dev/null @@ -1,14 +0,0 @@ - -PROG := BinCommTest -SRCS := test.cpp ../APM_BinComm.cpp -OBJS := $(SRCS:.cpp=.o) - -BinCommTest: $(OBJS) - /Volumes/Data/Users/msmith/llvm/bin/clang++ -g -o $@ $(OBJS) - -.cpp.o: - @echo C++ $< -> $@ - /Volumes/Data/Users/msmith/llvm/bin/clang++ -g -c -I. -o $@ $< - -clean: - rm $(PROG) $(OBJS) diff --git a/libraries/APM_BinComm/test/WProgram.h b/libraries/APM_BinComm/test/WProgram.h deleted file mode 100644 index 539db09c5d..0000000000 --- a/libraries/APM_BinComm/test/WProgram.h +++ /dev/null @@ -1,15 +0,0 @@ - -#ifndef WPROGRAM_H -#define WPROGRAM_H - -class Stream { -public: - void write(uint8_t val); - int available(void); - int read(void); -}; - - -extern unsigned int millis(void); - -#endif diff --git a/libraries/APM_BinComm/test/test.cpp b/libraries/APM_BinComm/test/test.cpp deleted file mode 100644 index a7e8ba5952..0000000000 --- a/libraries/APM_BinComm/test/test.cpp +++ /dev/null @@ -1,112 +0,0 @@ -// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- - -// test harness for the APM_BinComm bits - -#include -#include -#include -#include -#include -#include -#include - -#include "WProgram.h" - -#include "../APM_BinComm.h" - -static void handler(void *arg, uint8_t messageId, uint8_t messageVersion, void *messageData); - -BinComm::MessageHandler handlers[] = { - {BinComm::MSG_ANY, handler, NULL}, - {BinComm::MSG_NULL, 0, 0} -}; - -Stream port; -BinComm comm(handlers, &port); - -int port_fd; - -unsigned int -millis(void) -{ - return 0; -} - -void -Stream::write(uint8_t val) -{ - ::write(port_fd, &val, 1); -} - -int -Stream::available(void) -{ - return(1); -} - -int -Stream::read(void) -{ - int ret; - uint8_t c; - - switch(::read(port_fd, &c, 1)) { - case 1: - printf("0x%02x\n", c); - return c; - case 0: - errx(1, "device disappeared"); - - default: - // almost certainly EWOULDBLOCK - return -1; - } -} - -void -handler(void *arg, uint8_t messageId, uint8_t messageVersion, void *messageData) -{ - - if (messageId == BinComm::MSG_HEARTBEAT) { - struct BinComm::msg_heartbeat *m = (struct BinComm::msg_heartbeat *)messageData; - printf("Heartbeat: mode %u time %u voltage %u command %u\n", - m->flightMode, m->timeStamp, m->batteryVoltage, m->commandIndex); - } else - if (messageId == BinComm::MSG_ATTITUDE) { - struct BinComm::msg_attitude *m = (struct BinComm::msg_attitude *)messageData; - printf("Attitude: pitch %d roll %d yaw %d\n", - m->pitch, m->roll, m->yaw); - } else - if (messageId == BinComm::MSG_LOCATION) { - struct BinComm::msg_location *m = (struct BinComm::msg_location *)messageData; - printf("Location: lat %d long %d altitude %d groundspeed %d groundcourse %d time %u\n", - m->latitude, m->longitude, m->altitude, m->groundSpeed, m->groundCourse, m->timeOfWeek); - } else - if (messageId == BinComm::MSG_STATUS_TEXT) { - struct BinComm::msg_status_text *m = (struct BinComm::msg_status_text *)messageData; - printf("Message %d: %-50s\n", m->severity, m->text); - } else { - warnx("received message %d,%d", messageId, messageVersion); - } -} - -int -main(int argc, char *argv[]) -{ - struct termios t; - - if (2 > argc) - errx(1, "BinCommTest "); - if (0 >= (port_fd = open(argv[1], O_RDWR | O_NONBLOCK))) - err(1, "could not open port %s", argv[1]); - if (tcgetattr(port_fd, &t)) - err(1, "tcgetattr"); - cfsetspeed(&t, 115200); - if (tcsetattr(port_fd, TCSANOW, &t)) - err(1, "tcsetattr"); - - // spin listening - for (;;) { - comm.update(); - } -}