• Main Page
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

/home/jgoppert/Projects/ap/libraries/APM_BinComm/test/test.cpp

Go to the documentation of this file.
00001 // -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
00002 
00003 // test harness for the APM_BinComm bits
00004 
00005 #include <stdint.h>
00006 #include <err.h>
00007 #include <unistd.h>
00008 #include <fcntl.h>
00009 #include <termios.h>
00010 #include <stdio.h>
00011 #include <string.h>
00012 
00013 #include "WProgram.h"
00014 
00015 #include "../APM_BinComm.h"
00016 
00017 static void     handler(void *arg, uint8_t messageId, uint8_t messageVersion, void *messageData);
00018 
00019 BinComm::MessageHandler handlers[] = {
00020         {BinComm::MSG_ANY,       handler,        NULL},
00021         {BinComm::MSG_NULL,      0,              0}
00022 };
00023 
00024 Stream          port;
00025 BinComm         comm(handlers, &port);
00026 
00027 int             port_fd;
00028 
00029 unsigned int
00030 millis(void)
00031 {
00032         return 0;
00033 }
00034 
00035 void
00036 Stream::write(uint8_t val)
00037 {
00038         ::write(port_fd, &val, 1);
00039 }
00040 
00041 int
00042 Stream::available(void)
00043 {
00044         return(1);
00045 }
00046 
00047 int
00048 Stream::read(void)
00049 {
00050         int     ret;
00051         uint8_t c;
00052 
00053         switch(::read(port_fd, &c, 1)) {
00054         case 1:
00055                 printf("0x%02x\n", c);
00056                 return c;
00057         case 0:
00058                 errx(1, "device disappeared");
00059 
00060         default:
00061                 // almost certainly EWOULDBLOCK
00062                 return -1;
00063         }
00064 }
00065 
00066 void
00067 handler(void *arg, uint8_t messageId, uint8_t messageVersion, void *messageData)
00068 {
00069 
00070         if (messageId == BinComm::MSG_HEARTBEAT) {
00071                 struct BinComm::msg_heartbeat *m = (struct BinComm::msg_heartbeat *)messageData;
00072                 printf("Heartbeat: mode %u  time %u  voltage %u  command %u\n",
00073                        m->flightMode, m->timeStamp, m->batteryVoltage, m->commandIndex);
00074         } else
00075         if (messageId == BinComm::MSG_ATTITUDE) {
00076                 struct BinComm::msg_attitude *m = (struct BinComm::msg_attitude *)messageData;
00077                 printf("Attitude: pitch %d  roll %d  yaw %d\n",
00078                        m->pitch, m->roll, m->yaw);
00079         } else
00080         if (messageId == BinComm::MSG_LOCATION) {
00081                 struct BinComm::msg_location *m = (struct BinComm::msg_location *)messageData;
00082                 printf("Location: lat %d  long %d  altitude %d  groundspeed %d  groundcourse %d  time %u\n",
00083                        m->latitude, m->longitude, m->altitude, m->groundSpeed, m->groundCourse, m->timeOfWeek);
00084         } else
00085         if (messageId == BinComm::MSG_STATUS_TEXT) {
00086                 struct BinComm::msg_status_text *m = (struct BinComm::msg_status_text *)messageData;
00087                 printf("Message %d: %-50s\n", m->severity, m->text);
00088         } else {
00089                 warnx("received message %d,%d", messageId, messageVersion);
00090         }
00091 }
00092 
00093 int
00094 main(int argc, char *argv[])
00095 {
00096         struct termios  t;
00097 
00098         if (2 > argc)
00099                 errx(1, "BinCommTest <port>");
00100         if (0 >= (port_fd = open(argv[1], O_RDWR | O_NONBLOCK)))
00101                 err(1, "could not open port %s", argv[1]);
00102         if (tcgetattr(port_fd, &t))
00103                 err(1, "tcgetattr");
00104         cfsetspeed(&t, 115200);
00105         if (tcsetattr(port_fd, TCSANOW, &t))
00106                 err(1, "tcsetattr");
00107 
00108         // spin listening
00109         for (;;) {
00110                 comm.update();
00111         }
00112 }

Generated for ArduPilot Libraries by doxygen