mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
c0c28f8eae
Revert the buffer overflow test until I can work out what James was doing with it. Don't try to send a text message in response to a message we don't like; we should probably implement a NAK message instead. Improve the text string sender a bit. We need to fix the protocol generator for this to be less sucky on the send side. git-svn-id: https://arducopter.googlecode.com/svn/trunk@827 f9c3cf11-9bcb-44bc-f272-b75c42450872
113 lines
3.0 KiB
C++
113 lines
3.0 KiB
C++
// -*- 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 <string.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:
|
|
printf("0x%02x\n", c);
|
|
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();
|
|
}
|
|
}
|