Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
DrZiplok@gmail.com 2010-10-09 18:11:24 +00:00
parent c03d4e34c5
commit c5155aa623
7 changed files with 255 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/APM_BinComm/APM_BinComm.h
View File

@ -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 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
//@{

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

@ -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
#

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

@ -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,

1
libraries/APM_BinComm/protocol/protogen.awk
View File

@ -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 = ""

14
libraries/APM_BinComm/test/Makefile Normal file
View File

@ -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 Normal file
View File

@ -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 Normal file
View File

@ -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();
}
}