From c5155aa623f49b035759dffc2356aea2c20b2c7a Mon Sep 17 00:00:00 2001 From: "DrZiplok@gmail.com" Date: Sat, 9 Oct 2010 18:11:24 +0000 Subject: [PATCH] Add servo output message, fixes issue #163 Check in my host-side test framework. Minor tidying. git-svn-id: https://arducopter.googlecode.com/svn/trunk@635 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/APM_BinComm/APM_BinComm.h | 66 ++++++------ libraries/APM_BinComm/protocol/protocol.def | 13 ++- libraries/APM_BinComm/protocol/protocol.h | 69 ++++++++++++ libraries/APM_BinComm/protocol/protogen.awk | 3 +- libraries/APM_BinComm/test/Makefile | 14 +++ libraries/APM_BinComm/test/WProgram.h | 15 +++ libraries/APM_BinComm/test/test.cpp | 110 ++++++++++++++++++++ 7 files changed, 255 insertions(+), 35 deletions(-) create mode 100644 libraries/APM_BinComm/test/Makefile create mode 100644 libraries/APM_BinComm/test/WProgram.h create mode 100644 libraries/APM_BinComm/test/test.cpp diff --git a/libraries/APM_BinComm/APM_BinComm.h b/libraries/APM_BinComm/APM_BinComm.h index ae8f6d07ce..22708f2dd7 100644 --- a/libraries/APM_BinComm/APM_BinComm.h +++ b/libraries/APM_BinComm/APM_BinComm.h @@ -30,9 +30,9 @@ #ifndef APM_BinComm_h #define APM_BinComm_h -#include "WProgram.h" #include #include +#include "WProgram.h" /// /// @class BinComm @@ -40,6 +40,23 @@ /// Mega binary telemetry protocol. /// 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. + /// + /// @param interface The stream that will be used + /// for telemetry communications. + /// + BinComm(const MessageHandler *handlerTable, + Stream *interface); private: /// OTA message header @@ -69,25 +86,25 @@ private: /// @name Message pack/unpack utility functions /// //@{ - inline void _pack(uint8_t *&ptr, const uint8_t x) { *(uint8_t *)ptr = x; ptr += sizeof(x); }; - inline void _pack(uint8_t *&ptr, const uint16_t x) { *(uint16_t *)ptr = x; ptr += sizeof(x); }; - inline void _pack(uint8_t *&ptr, const int16_t x) { *(int16_t *)ptr = x; ptr += sizeof(x); }; - inline void _pack(uint8_t *&ptr, const uint32_t x) { *(uint32_t *)ptr = x; ptr += sizeof(x); }; - inline void _pack(uint8_t *&ptr, const int32_t x) { *(int32_t *)ptr = x; ptr += sizeof(x); }; + inline void _pack(uint8_t *&ptr, const uint8_t x) { *(uint8_t *)ptr = x; ptr += sizeof(x); } + inline void _pack(uint8_t *&ptr, const uint16_t x) { *(uint16_t *)ptr = x; ptr += sizeof(x); } + inline void _pack(uint8_t *&ptr, const int16_t x) { *(int16_t *)ptr = x; ptr += sizeof(x); } + inline void _pack(uint8_t *&ptr, const uint32_t x) { *(uint32_t *)ptr = x; ptr += sizeof(x); } + inline void _pack(uint8_t *&ptr, const int32_t x) { *(int32_t *)ptr = x; ptr += sizeof(x); } - inline void _pack(uint8_t *&ptr, const char *msg, uint8_t size) { strlcpy((char *)ptr, msg, size); ptr += size; }; - inline void _pack(uint8_t *&ptr, const uint8_t *values, uint8_t count) { memcpy(ptr, values, count); ptr += count; }; - inline void _pack(uint8_t *&ptr, const uint16_t *values, uint8_t count) { memcpy(ptr, values, count * 2); ptr += count * 2; }; + inline void _pack(uint8_t *&ptr, const char *msg, uint8_t size) { strlcpy((char *)ptr, msg, size); ptr += size; } + inline void _pack(uint8_t *&ptr, const uint8_t *values, uint8_t count) { memcpy(ptr, values, count); ptr += count; } + inline void _pack(uint8_t *&ptr, const uint16_t *values, uint8_t count) { memcpy(ptr, values, count * 2); ptr += count * 2; } - inline void _unpack(uint8_t *&ptr, uint8_t &x) { x = *(uint8_t *)ptr; ptr += sizeof(x); }; - inline void _unpack(uint8_t *&ptr, uint16_t &x) { x = *(uint16_t *)ptr; ptr += sizeof(x); }; - inline void _unpack(uint8_t *&ptr, int16_t &x) { x = *(int16_t *)ptr; ptr += sizeof(x); }; - inline void _unpack(uint8_t *&ptr, uint32_t &x) { x = *(uint32_t *)ptr; ptr += sizeof(x); }; - inline void _unpack(uint8_t *&ptr, int32_t &x) { x = *(int32_t *)ptr; ptr += sizeof(x); }; + inline void _unpack(uint8_t *&ptr, uint8_t &x) { x = *(uint8_t *)ptr; ptr += sizeof(x); } + inline void _unpack(uint8_t *&ptr, uint16_t &x) { x = *(uint16_t *)ptr; ptr += sizeof(x); } + inline void _unpack(uint8_t *&ptr, int16_t &x) { x = *(int16_t *)ptr; ptr += sizeof(x); } + inline void _unpack(uint8_t *&ptr, uint32_t &x) { x = *(uint32_t *)ptr; ptr += sizeof(x); } + inline void _unpack(uint8_t *&ptr, int32_t &x) { x = *(int32_t *)ptr; ptr += sizeof(x); } - inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) { strlcpy(msg, (char *)ptr, size); ptr += size; }; - inline void _unpack(uint8_t *&ptr, uint8_t *values, uint8_t count) { memcpy(values, ptr, count); ptr += count; }; - inline void _unpack(uint8_t *&ptr, uint16_t *values, uint8_t count) { memcpy(values, ptr, count * 2); ptr += count * 2; }; + inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) { strlcpy(msg, (char *)ptr, size); ptr += size; } + inline void _unpack(uint8_t *&ptr, uint8_t *values, uint8_t count) { memcpy(values, ptr, count); ptr += count; } + inline void _unpack(uint8_t *&ptr, uint16_t *values, uint8_t count) { memcpy(values, ptr, count * 2); ptr += count * 2; } //@} public: @@ -174,21 +191,6 @@ public: 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 stream that will be used - /// for telemetry communications. - /// - BinComm(const MessageHandler *handlerTable, - Stream *interface); - ////////////////////////////////////////////////////////////////////// /// @name Decoder interface //@{ diff --git a/libraries/APM_BinComm/protocol/protocol.def b/libraries/APM_BinComm/protocol/protocol.def index 75f1cfa7ab..d1e465cc6f 100644 --- a/libraries/APM_BinComm/protocol/protocol.def +++ b/libraries/APM_BinComm/protocol/protocol.def @@ -21,7 +21,6 @@ # 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 -# (note that currently there is only _pack support for character arrays) # # @@ -163,7 +162,7 @@ message 0x42 MSG_PID # -# Radio trim settings +# Radio settings and values # message 0x50 MSG_TRIM_STARTUP uint16_t value 8 @@ -173,6 +172,16 @@ message 0x51 MSG_TRIM_MIN message 0x52 MSG_TRIM_MAX uint16_t value 8 + +message 0x53 MSG_SERVOS + int16_t ch1 + int16_t ch2 + int16_t ch3 + int16_t ch4 + int16_t ch5 + int16_t ch6 + int16_t ch7 + int16_t ch8 # # Direct sensor access diff --git a/libraries/APM_BinComm/protocol/protocol.h b/libraries/APM_BinComm/protocol/protocol.h index 481451a067..2da8e0f7c3 100644 --- a/libraries/APM_BinComm/protocol/protocol.h +++ b/libraries/APM_BinComm/protocol/protocol.h @@ -1,6 +1,7 @@ // // THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT // +/// @file protocol.h #pragma pack(1) ////////////////////////////////////////////////////////////////////// @@ -937,6 +938,73 @@ unpack_msg_trim_max( }; //@} +////////////////////////////////////////////////////////////////////// +/// @name MSG_SERVOS +//@{ + +/// Structure describing the payload section of the MSG_SERVOS message +struct msg_servos { + int16_t ch1; + int16_t ch2; + int16_t ch3; + int16_t ch4; + int16_t ch5; + int16_t ch6; + int16_t ch7; + int16_t ch8; +}; + +/// Send a MSG_SERVOS message +inline void +send_msg_servos( + const int16_t ch1, + const int16_t ch2, + const int16_t ch3, + const int16_t ch4, + const int16_t ch5, + const int16_t ch6, + const int16_t ch7, + const int16_t ch8) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, ch1); + _pack(__p, ch2); + _pack(__p, ch3); + _pack(__p, ch4); + _pack(__p, ch5); + _pack(__p, ch6); + _pack(__p, ch7); + _pack(__p, ch8); + _encodeBuf.header.length = 16; + _encodeBuf.header.messageID = MSG_SERVOS; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_SERVOS message +inline void +unpack_msg_servos( + int16_t &ch1, + int16_t &ch2, + int16_t &ch3, + int16_t &ch4, + int16_t &ch5, + int16_t &ch6, + int16_t &ch7, + int16_t &ch8) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, ch1); + _unpack(__p, ch2); + _unpack(__p, ch3); + _unpack(__p, ch4); + _unpack(__p, ch5); + _unpack(__p, ch6); + _unpack(__p, ch7); + _unpack(__p, ch8); +}; +//@} + ////////////////////////////////////////////////////////////////////// /// @name MSG_SENSOR //@{ @@ -1216,6 +1284,7 @@ enum MessageID { MSG_PERF_REPORT = 0x6, MSG_TRIM_MAX = 0x52, MSG_VERSION_REQUEST = 0x7, + MSG_SERVOS = 0x53, MSG_VERSION = 0x8, MSG_COMMAND_REQUEST = 0x20, MSG_COMMAND_UPLOAD = 0x21, diff --git a/libraries/APM_BinComm/protocol/protogen.awk b/libraries/APM_BinComm/protocol/protogen.awk index e063da5f8b..3b5e0b65e4 100644 --- a/libraries/APM_BinComm/protocol/protogen.awk +++ b/libraries/APM_BinComm/protocol/protogen.awk @@ -6,8 +6,9 @@ BEGIN { printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n") + printf("/// @file protocol.h\n") printf("#pragma pack(1)\n"); - + currentMessage = "" } diff --git a/libraries/APM_BinComm/test/Makefile b/libraries/APM_BinComm/test/Makefile new file mode 100644 index 0000000000..a484d5a004 --- /dev/null +++ b/libraries/APM_BinComm/test/Makefile @@ -0,0 +1,14 @@ + +PROG := BinCommTest +SRCS := test.cpp ../APM_BinComm.cpp +OBJS := $(SRCS:.cpp=.o) + +BinCommTest: $(OBJS) + c++ -g -o $@ $(OBJS) + +.cpp.o: + @echo C++ $< -> $@ + c++ -g -c -I. -o $@ $< + +clean: + rm $(PROG) $(OBJS) diff --git a/libraries/APM_BinComm/test/WProgram.h b/libraries/APM_BinComm/test/WProgram.h new file mode 100644 index 0000000000..539db09c5d --- /dev/null +++ b/libraries/APM_BinComm/test/WProgram.h @@ -0,0 +1,15 @@ + +#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 new file mode 100644 index 0000000000..e15f1827f0 --- /dev/null +++ b/libraries/APM_BinComm/test/test.cpp @@ -0,0 +1,110 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- + +// test harness for the APM_BinComm bits + +#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: + 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(); + } +}