Add init method so that the comms stream can be set after construction.

Template the pack/unpack functions, make the code a little cleaner.

Fix a few message definitions.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@770 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/APM_BinComm/APM_BinComm.cpp

@@ -47,10 +47,16 @@
 BinComm::BinComm(const BinComm::MessageHandler *handlerTable,
                  Stream *interface) :
         _handlerTable(handlerTable),
-        _interface(interface)
 {
+        init(interface);
 };
 
+void
+BinComm::init(Stream *interface)
+{
+        _interface = interface;
+}
+
 void
 BinComm::_sendMessage(void)
 {

libraries/APM_BinComm/APM_BinComm.h

@@ -71,7 +71,18 @@ public:
 	///								for telemetry communications.
 	///
 	BinComm(const MessageHandler *handlerTable,
-			Stream *interface);
+			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
@@ -101,25 +112,58 @@ 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 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, 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; }
+
+	/// Pack any scalar type
+	///
+	/// @param ptr				Buffer pointer.
+	/// @param x				Value to pack.
+	///
+	template <typename T> inline void _pack(uint8_t *&ptr, const T x) { *(T *)ptr = x; ptr += sizeof(T); }
+
+	/// Pack an array of any scalar type
+	///
+	/// @param ptr				Buffer pointer.
+	/// @param x				Array to pack.
+	/// @param count			Array size.
+	///
+	template <typename T> inline void _pack(uint8_t *&ptr, const T *values, uint8_t count) {
+		memcpy(ptr, values, count * sizeof(T));
+		ptr += count * sizeof(T);
+	}
+
+	/// Pack a string into a fixed-size buffer.
+	///
+	/// @param ptr				Buffer pointer
+	/// @param msg				The NUL-terminated string to pack.
+	/// @param size				The size of the buffer (which limits the quantity of the string
+	///							that is actually copied.
+	///
+	inline void	_pack(uint8_t *&ptr, const char *msg, uint8_t size) { strlcpy((char *)ptr, msg, size); ptr += size; }
+
+	/// Unpack any scalar type
+	///
+	/// @param buf				Buffer pointer.
+	/// @param x				Unpacked result.
+	///
+	template <typename T> 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 <typename T> 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) { strlcpy(msg, (char *)ptr, size); ptr += size; }
 	//@}
 
 public:

libraries/APM_BinComm/protocol/protocol.def

@@ -62,7 +62,8 @@
 #
 message 0x00	MSG_ACKNOWLEDGE 
 	uint8_t			msgID
-	uint16_t		msgSum
+	uint8_t			sum1
+	uint8_t			sum2
 
 #
 # System heartbeat
@@ -112,9 +113,12 @@ message 0x05	MSG_STATUS_TEXT
 message 0x06	MSG_PERF_REPORT 
 	uint32_t		interval
 	uint16_t		mainLoopCycles
-	uint8_t			mainLoopTime
+	uint8_t			mainLoopCycleTime
 	uint8_t			gyroSaturationCount
 	uint8_t			adcConstraintCount
+	uint8_t			renormSqrtCount
+	uint8_t			renormBlowupCount
+	uint8_t			gpsFixCount
 	uint16_t		imuHealth
 	uint16_t		gcsMessageCount
 
@@ -198,14 +202,20 @@ message 0x42	MSG_PID
 #
 # Radio settings and values
 #
+# The trim/radio values should be unsigned, but
+# currently aren't.
+#
 message 0x50	MSG_TRIM_STARTUP
-	uint16_t		value	8
+	int			value	8
+#	uint16_t		value	8
 
 message 0x51	MSG_TRIM_MIN
-	uint16_t		value	8
+	int			value	8
+#	uint16_t		value	8
 
 message 0x52	MSG_TRIM_MAX
-	uint16_t		value	8
+	int			value	8
+#	uint16_t		value	8
 	
 message 0x53 	MSG_SERVOS
 	int16_t			ch1

libraries/APM_BinComm/protocol/protocol.h

@@ -11,18 +11,21 @@
 /// Structure describing the payload section of the MSG_ACKNOWLEDGE message
 struct msg_acknowledge {
 	uint8_t msgID;
-	uint16_t msgSum;
+	uint8_t sum1;
+	uint8_t sum2;
 };
 
 /// Send a MSG_ACKNOWLEDGE message
 inline void
 send_msg_acknowledge(
 	const uint8_t msgID,
-	const uint16_t msgSum)
+	const uint8_t sum1,
+	const uint8_t sum2)
 {
 	uint8_t *__p = &_encodeBuf.payload[0];
 	_pack(__p, msgID);
-	_pack(__p, msgSum);
+	_pack(__p, sum1);
+	_pack(__p, sum2);
 	_encodeBuf.header.length = 3;
 	_encodeBuf.header.messageID = MSG_ACKNOWLEDGE;
 	_encodeBuf.header.messageVersion = MSG_VERSION_1;
@@ -33,11 +36,13 @@ send_msg_acknowledge(
 inline void
 unpack_msg_acknowledge(
 	uint8_t &msgID,
-	uint16_t &msgSum)
+	uint8_t &sum1,
+	uint8_t &sum2)
 {
 	uint8_t *__p = &_decodeBuf.payload[0];
 	_unpack(__p, msgID);
-	_unpack(__p, msgSum);
+	_unpack(__p, sum1);
+	_unpack(__p, sum2);
 };
 //@}
 
@@ -269,9 +274,12 @@ unpack_msg_status_text(
 struct msg_perf_report {
 	uint32_t interval;
 	uint16_t mainLoopCycles;
-	uint8_t mainLoopTime;
+	uint8_t mainLoopCycleTime;
 	uint8_t gyroSaturationCount;
 	uint8_t adcConstraintCount;
+	uint8_t renormSqrtCount;
+	uint8_t renormBlowupCount;
+	uint8_t gpsFixCount;
 	uint16_t imuHealth;
 	uint16_t gcsMessageCount;
 };
@@ -281,21 +289,27 @@ inline void
 send_msg_perf_report(
 	const uint32_t interval,
 	const uint16_t mainLoopCycles,
-	const uint8_t mainLoopTime,
+	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)
 {
 	uint8_t *__p = &_encodeBuf.payload[0];
 	_pack(__p, interval);
 	_pack(__p, mainLoopCycles);
-	_pack(__p, mainLoopTime);
+	_pack(__p, mainLoopCycleTime);
 	_pack(__p, gyroSaturationCount);
 	_pack(__p, adcConstraintCount);
+	_pack(__p, renormSqrtCount);
+	_pack(__p, renormBlowupCount);
+	_pack(__p, gpsFixCount);
 	_pack(__p, imuHealth);
 	_pack(__p, gcsMessageCount);
-	_encodeBuf.header.length = 13;
+	_encodeBuf.header.length = 16;
 	_encodeBuf.header.messageID = MSG_PERF_REPORT;
 	_encodeBuf.header.messageVersion = MSG_VERSION_1;
 	_sendMessage();
@@ -306,18 +320,24 @@ inline void
 unpack_msg_perf_report(
 	uint32_t &interval,
 	uint16_t &mainLoopCycles,
-	uint8_t &mainLoopTime,
+	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, mainLoopTime);
+	_unpack(__p, mainLoopCycleTime);
 	_unpack(__p, gyroSaturationCount);
 	_unpack(__p, adcConstraintCount);
+	_unpack(__p, renormSqrtCount);
+	_unpack(__p, renormBlowupCount);
+	_unpack(__p, gpsFixCount);
 	_unpack(__p, imuHealth);
 	_unpack(__p, gcsMessageCount);
 };
@@ -848,17 +868,17 @@ unpack_msg_pid(
 
 /// Structure describing the payload section of the MSG_TRIM_STARTUP message
 struct msg_trim_startup {
-	uint16_t value[8];
+	int value[8];
 };
 
 /// Send a MSG_TRIM_STARTUP message
 inline void
 send_msg_trim_startup(
-	const uint16_t (&value)[8])
+	const int (&value)[8])
 {
 	uint8_t *__p = &_encodeBuf.payload[0];
 	_pack(__p, value, 8);
-	_encodeBuf.header.length = 16;
+	_encodeBuf.header.length = 8;
 	_encodeBuf.header.messageID = MSG_TRIM_STARTUP;
 	_encodeBuf.header.messageVersion = MSG_VERSION_1;
 	_sendMessage();
@@ -867,7 +887,7 @@ send_msg_trim_startup(
 /// Unpack a MSG_TRIM_STARTUP message
 inline void
 unpack_msg_trim_startup(
-	uint16_t (&value)[8])
+	int (&value)[8])
 {
 	uint8_t *__p = &_decodeBuf.payload[0];
 	_unpack(__p, value, 8);
@@ -880,17 +900,17 @@ unpack_msg_trim_startup(
 
 /// Structure describing the payload section of the MSG_TRIM_MIN message
 struct msg_trim_min {
-	uint16_t value[8];
+	int value[8];
 };
 
 /// Send a MSG_TRIM_MIN message
 inline void
 send_msg_trim_min(
-	const uint16_t (&value)[8])
+	const int (&value)[8])
 {
 	uint8_t *__p = &_encodeBuf.payload[0];
 	_pack(__p, value, 8);
-	_encodeBuf.header.length = 16;
+	_encodeBuf.header.length = 8;
 	_encodeBuf.header.messageID = MSG_TRIM_MIN;
 	_encodeBuf.header.messageVersion = MSG_VERSION_1;
 	_sendMessage();
@@ -899,7 +919,7 @@ send_msg_trim_min(
 /// Unpack a MSG_TRIM_MIN message
 inline void
 unpack_msg_trim_min(
-	uint16_t (&value)[8])
+	int (&value)[8])
 {
 	uint8_t *__p = &_decodeBuf.payload[0];
 	_unpack(__p, value, 8);
@@ -912,17 +932,17 @@ unpack_msg_trim_min(
 
 /// Structure describing the payload section of the MSG_TRIM_MAX message
 struct msg_trim_max {
-	uint16_t value[8];
+	int value[8];
 };
 
 /// Send a MSG_TRIM_MAX message
 inline void
 send_msg_trim_max(
-	const uint16_t (&value)[8])
+	const int (&value)[8])
 {
 	uint8_t *__p = &_encodeBuf.payload[0];
 	_pack(__p, value, 8);
-	_encodeBuf.header.length = 16;
+	_encodeBuf.header.length = 8;
 	_encodeBuf.header.messageID = MSG_TRIM_MAX;
 	_encodeBuf.header.messageVersion = MSG_VERSION_1;
 	_sendMessage();
@@ -931,7 +951,7 @@ send_msg_trim_max(
 /// Unpack a MSG_TRIM_MAX message
 inline void
 unpack_msg_trim_max(
-	uint16_t (&value)[8])
+	int (&value)[8])
 {
 	uint8_t *__p = &_decodeBuf.payload[0];
 	_unpack(__p, value, 8);