zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
master
DrZiplok@gmail.com 15 years ago
parent
c03d4e34c5
commit
c5155aa623
7 changed files with 257 additions and 37 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 70
      libraries/APM_BinComm/APM_BinComm.h
  2. 13
      libraries/APM_BinComm/protocol/protocol.def
  3. 69
      libraries/APM_BinComm/protocol/protocol.h
  4. 3
      libraries/APM_BinComm/protocol/protogen.awk
  5. 14
      libraries/APM_BinComm/test/Makefile
  6. 15
      libraries/APM_BinComm/test/WProgram.h
  7. 110
      libraries/APM_BinComm/test/test.cpp

70
libraries/APM_BinComm/APM_BinComm.h
Unescape View File

@ -30,9 +30,9 @@ @@ -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 @@ @@ -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: @@ -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 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 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; };
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; }
//@}
public:
@ -174,21 +191,6 @@ 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
//@{

13
libraries/APM_BinComm/protocol/protocol.def
Unescape View File

@ -21,7 +21,6 @@ @@ -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 @@ -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 @@ -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

69
libraries/APM_BinComm/protocol/protocol.h
Unescape View File

@ -1,6 +1,7 @@ @@ -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( @@ -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 { @@ -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,

3
libraries/APM_BinComm/protocol/protogen.awk
Unescape View File

@ -6,8 +6,9 @@ @@ -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 = ""
}

14
libraries/APM_BinComm/test/Makefile
Unescape View File

@ -0,0 +1,14 @@ @@ -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)

15
libraries/APM_BinComm/test/WProgram.h
Unescape View File

@ -0,0 +1,15 @@ @@ -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

110
libraries/APM_BinComm/test/test.cpp
Unescape View File

@ -0,0 +1,110 @@ @@ -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();
}
}
Write Preview
Loading…
Cancel
Save