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

@@ -30,9 +30,9 @@
 #ifndef APM_BinComm_h
 #define APM_BinComm_h
 
-#include "WProgram.h"
 #include <string.h>
 #include <inttypes.h>
+#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 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 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 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 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 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 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 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 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, 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, 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, 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, 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, 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, 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, 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, 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
 	//@{

libraries/APM_BinComm/protocol/protocol.def

@@ -21,7 +21,6 @@
 #       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)
 #
 
 #
@@ -163,7 +162,7 @@ message 0x42	MSG_PID
 
 
 #
-# Radio trim settings
+# Radio settings and values
 #
 message 0x50	MSG_TRIM_STARTUP
 	uint16_t		value	8
@@ -174,6 +173,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
 #

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,

libraries/APM_BinComm/protocol/protogen.awk

@@ -6,6 +6,7 @@
 
 BEGIN {
     printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n")
+    printf("/// @file protocol.h\n")
     printf("#pragma pack(1)\n");
     
     currentMessage = ""

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)

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

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 <stdint.h>
+#include <err.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <stdio.h>
+
+#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 <port>");
+        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();
+        }
+}