Browse Source
It seems like nobody uses this app because it can't even get compiled. That's why I think it should go away.master
Staroselskii Georgii
9 years ago
committed by
Andrew Tridgell
4 changed files with 0 additions and 890 deletions
@ -1,682 +0,0 @@ |
|||||||
// -*- tab-width: 8; Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
|
|
||||||
|
|
||||||
//
|
|
||||||
// Copyright (c) 2010 Michael Smith. All rights reserved.
|
|
||||||
//
|
|
||||||
// Redistribution and use in source and binary forms, with or without
|
|
||||||
// modification, are permitted provided that the following conditions
|
|
||||||
// are met:
|
|
||||||
// 1. Redistributions of source code must retain the above copyright
|
|
||||||
// notice, this list of conditions and the following disclaimer.
|
|
||||||
// 2. Redistributions in binary form must reproduce the above copyright
|
|
||||||
// notice, this list of conditions and the following disclaimer in the
|
|
||||||
// documentation and/or other materials provided with the distribution.
|
|
||||||
//
|
|
||||||
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
|
||||||
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
||||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
||||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
|
||||||
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
||||||
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
|
||||||
// OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
|
||||||
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
|
||||||
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
|
||||||
// SUCH DAMAGE.
|
|
||||||
//
|
|
||||||
|
|
||||||
//
|
|
||||||
// This is a serial port proxy shim for use with MAVLink-capable MAV
|
|
||||||
// controllers. It allows the controller to interact with FlightGear
|
|
||||||
// and control a MAV inside the simulation.
|
|
||||||
//
|
|
||||||
// In addition to bridging between MAVLink and FlightGear, MAVLink data
|
|
||||||
// is forwarded to QGroundControl if it is running on the local system.
|
|
||||||
//
|
|
||||||
// The controller must be willing to send the RC_CHANNELS_SCALED
|
|
||||||
// stream in response to a request, and it should be able to operate
|
|
||||||
// using the data supplied by the GPS_RAW and RAW_IMU messages.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include <sys/errno.h> |
|
||||||
#include <sys/types.h> |
|
||||||
#include <sys/signal.h> |
|
||||||
#include <sys/sysctl.h> |
|
||||||
#include <sys/time.h> |
|
||||||
#include <sys/uio.h> |
|
||||||
|
|
||||||
#include <netinet/in.h> |
|
||||||
|
|
||||||
#include <err.h> |
|
||||||
#include <fcntl.h> |
|
||||||
#include <cmath> |
|
||||||
#include <pthread.h> |
|
||||||
#include <signal.h> |
|
||||||
#include <string.h> |
|
||||||
#include <stdlib.h> |
|
||||||
#include <stdio.h> |
|
||||||
#include <termios.h> |
|
||||||
#include <unistd.h> |
|
||||||
|
|
||||||
#pragma pack(1) |
|
||||||
#include <GCS_MAVLink/include/mavlink/v1.0/mavlink_types.h> |
|
||||||
|
|
||||||
mavlink_system_t mavlink_system; |
|
||||||
void comm_send_ch(mavlink_channel_t chan, uint8_t ch); |
|
||||||
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS |
|
||||||
#include <GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink.h> |
|
||||||
|
|
||||||
#define TARGET_SYSTEM 7 /* XXX what should these really be? */ |
|
||||||
#define TARGET_COMPONENT 1 |
|
||||||
|
|
||||||
/*
|
|
||||||
* Binary packet as exchanged with FG. |
|
||||||
* |
|
||||||
* Described in MAVLink.xml |
|
||||||
*/ |
|
||||||
struct fgIMUData { |
|
||||||
// GPS
|
|
||||||
double latitude; |
|
||||||
double longitude; |
|
||||||
double altitude; |
|
||||||
double heading; |
|
||||||
double velocityN; |
|
||||||
double velocityE; |
|
||||||
|
|
||||||
// IMU
|
|
||||||
double accelX; |
|
||||||
double accelY; |
|
||||||
double accelZ; |
|
||||||
double rateRoll; |
|
||||||
double ratePitch; |
|
||||||
double rateYaw; |
|
||||||
|
|
||||||
// trailer
|
|
||||||
#define MSG_MAGIC 0x4c56414d |
|
||||||
uint32_t magic; |
|
||||||
} __attribute__((packed)); |
|
||||||
|
|
||||||
struct fgControlData { |
|
||||||
double aileron; |
|
||||||
double elevator; |
|
||||||
double rudder; |
|
||||||
double throttle; |
|
||||||
} __attribute__((packed)); |
|
||||||
|
|
||||||
/* utility functions for transforming FG blobs */ |
|
||||||
void swap32(void *p); |
|
||||||
void swap64(void *p); |
|
||||||
|
|
||||||
/* diagnostic tool */ |
|
||||||
void hexdump(void *p, size_t s); |
|
||||||
|
|
||||||
/* local port addresses for communication with FlightGear - these are the FG defaults */ |
|
||||||
#define IMU_LISTEN_PORT 5501 |
|
||||||
#define CTRL_SEND_PORT 5500 |
|
||||||
|
|
||||||
/* local port addresses for communication with QGroundControl */ |
|
||||||
#define QGCS_LISTEN_PORT 14551 |
|
||||||
#define QGCS_SEND_PORT 14550 |
|
||||||
|
|
||||||
/*
|
|
||||||
* debug logs |
|
||||||
*/ |
|
||||||
#define FAC_MAIN 0 |
|
||||||
#define FAC_CTRL 1 |
|
||||||
#define FAC_IMU 2 |
|
||||||
#define FAC_QGCS 3 |
|
||||||
char *fac_names[] = {"", "CTRL: ", "IMU: ", "QGCS: "}; |
|
||||||
|
|
||||||
#define log(_fac, _fmt, _args...) \ |
|
||||||
do { \
|
|
||||||
fprintf(stderr, "%s" _fmt "\n", fac_names[FAC_##_fac], ##_args); \
|
|
||||||
} while(0) |
|
||||||
|
|
||||||
#define debug(_fac, _lvl, _fmt, _args...) \ |
|
||||||
do { \
|
|
||||||
if (gDebug >= _lvl) fprintf(stderr, "%s" _fmt "\n", fac_names[FAC_##_fac], ##_args); \
|
|
||||||
} while(0) |
|
||||||
|
|
||||||
int gShouldQuit; /* set to 1 if threads should exit */ |
|
||||||
int gDebug; /* set to 1 if -d is passed */ |
|
||||||
|
|
||||||
void usage(void); /* print usage message */ |
|
||||||
void shouldQuit(int sig); /* signal that the program should quit */ |
|
||||||
|
|
||||||
/*
|
|
||||||
* Thread handling aircraft control instructions from the MAV controller to FlightGear. |
|
||||||
* Also forwards MAVLink packets to QGroundControl. |
|
||||||
*/ |
|
||||||
pthread_t gCTRLThread; |
|
||||||
void *ctrlThread(void *arg); |
|
||||||
void ctrlHandleMessage(mavlink_message_t *msg); |
|
||||||
|
|
||||||
/* thread handling pseudo-IMU data from FlightGear to the MAV controller */ |
|
||||||
pthread_t gIMUThread; |
|
||||||
void *imuThread(void *arg); |
|
||||||
int imuGetMessage(int sock, struct fgIMUData *msg); |
|
||||||
void imuSendPacket(struct fgIMUData *msg); |
|
||||||
|
|
||||||
/* thread handling MAVLink traffic from QGroundControl to the MAV controller */ |
|
||||||
pthread_t gQGCSThread; |
|
||||||
void *qgsThread(void *arg); |
|
||||||
|
|
||||||
int fgSock; /* socket used for flightgear comms */ |
|
||||||
struct sockaddr_in fgAddr = {sizeof(fgAddr), AF_INET}; |
|
||||||
|
|
||||||
int qgcsSock; /* socket used for qgroundcontrol comms */ |
|
||||||
struct sockaddr_in qgcsAddr = {sizeof(qgcsAddr), AF_INET}; |
|
||||||
|
|
||||||
int port; /* serial port connected to MAV controller */ |
|
||||||
pthread_mutex_t portMutex; /* lock that must be held while writing to the serial port */ |
|
||||||
|
|
||||||
int |
|
||||||
main(int argc, char *argv[]) |
|
||||||
{ |
|
||||||
int ch; |
|
||||||
struct sockaddr_in sin = {sizeof(sin), AF_INET}; |
|
||||||
int opt; |
|
||||||
struct termios t; |
|
||||||
|
|
||||||
/* handle arguments */ |
|
||||||
while ((ch = getopt(argc, argv, "ds:")) != -1) { |
|
||||||
switch(ch) { |
|
||||||
case 'd': |
|
||||||
gDebug++; |
|
||||||
break; |
|
||||||
|
|
||||||
case '?': |
|
||||||
default: |
|
||||||
usage(); |
|
||||||
} |
|
||||||
} |
|
||||||
argc -= optind; |
|
||||||
argv += optind; |
|
||||||
|
|
||||||
if (pthread_mutex_init(&portMutex, NULL)) |
|
||||||
errx(1, "port mutex"); |
|
||||||
|
|
||||||
/*
|
|
||||||
* set up the flightgear socket |
|
||||||
*/ |
|
||||||
fgSock = socket(AF_INET, SOCK_DGRAM, 0); |
|
||||||
if (fgSock < 0) |
|
||||||
err(1, "IMU socket"); |
|
||||||
sin.sin_port = htons(IMU_LISTEN_PORT); |
|
||||||
sin.sin_addr.s_addr = htonl(INADDR_LOOPBACK); |
|
||||||
fgAddr.sin_port = htons(CTRL_SEND_PORT); |
|
||||||
fgAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); |
|
||||||
|
|
||||||
/* bind the listening side of the socket */ |
|
||||||
for (;;) { |
|
||||||
if (bind(fgSock, (struct sockaddr *)&sin, sizeof(sin)) < 0) { |
|
||||||
if (errno != EADDRINUSE) |
|
||||||
err(1, "IMU socket bind"); |
|
||||||
log(IMU, "socket in use, waiting..."); |
|
||||||
sleep(5); |
|
||||||
} else { |
|
||||||
break; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
/* set socket options */ |
|
||||||
opt = 1; |
|
||||||
if (setsockopt(fgSock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) |
|
||||||
err(1, "IMU setsockopt SO_REUSEADDR"); |
|
||||||
|
|
||||||
/*
|
|
||||||
* set up the qgroundcontrol socket |
|
||||||
*/ |
|
||||||
qgcsSock = socket(AF_INET, SOCK_DGRAM, 0); |
|
||||||
if (qgcsSock < 0) |
|
||||||
err(1, "QGroundControl socket"); |
|
||||||
qgcsAddr.sin_port = htons(QGCS_SEND_PORT); |
|
||||||
qgcsAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); |
|
||||||
|
|
||||||
sin.sin_port = htons(QGCS_LISTEN_PORT); |
|
||||||
sin.sin_addr.s_addr = htonl(INADDR_LOOPBACK); |
|
||||||
|
|
||||||
/* bind the listening side of the socket */ |
|
||||||
for (;;) { |
|
||||||
if (bind(qgcsSock, (struct sockaddr *)&sin, sizeof(sin)) < 0) { |
|
||||||
if (errno != EADDRINUSE) |
|
||||||
err(1, "QGroundControl socket bind"); |
|
||||||
log(QGCS, "socket in use, waiting..."); |
|
||||||
sleep(5); |
|
||||||
} else { |
|
||||||
break; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
/* set socket options */ |
|
||||||
opt = 1; |
|
||||||
if (setsockopt(qgcsSock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) |
|
||||||
err(1, "QGroundControl setsockopt SO_REUSEADDR"); |
|
||||||
|
|
||||||
/*
|
|
||||||
* set up the serial port at 57600 |
|
||||||
*/ |
|
||||||
if (argc < 1) |
|
||||||
errx(1, "missing serial port name"); |
|
||||||
if (0 >= (port = open(argv[0], O_RDWR | O_NONBLOCK))) |
|
||||||
err(1, "could not open port %s", argv[0]); |
|
||||||
if (tcgetattr(port, &t)) |
|
||||||
err(1, "tcgetattr"); |
|
||||||
cfmakeraw(&t); |
|
||||||
t.c_cflag |= CLOCAL; |
|
||||||
cfsetspeed(&t, 57600); |
|
||||||
|
|
||||||
if (tcsetattr(port, TCSANOW, &t)) |
|
||||||
err(1, "tcsetattr"); |
|
||||||
|
|
||||||
/* start worker threads */ |
|
||||||
if (pthread_create(&gIMUThread, NULL, imuThread, NULL)) |
|
||||||
err(1, "IMU thread"); |
|
||||||
if (pthread_create(&gCTRLThread, NULL, ctrlThread, NULL)) |
|
||||||
err(1, "CTRL thread"); |
|
||||||
if (pthread_create(&gQGCSThread, NULL, qgsThread, NULL)) |
|
||||||
err(1, "QGS thread"); |
|
||||||
|
|
||||||
/* don't install new handlers until the threads are ready */ |
|
||||||
signal(SIGHUP, shouldQuit); |
|
||||||
siginterrupt(SIGHUP, 1); |
|
||||||
signal(SIGINT, shouldQuit); |
|
||||||
siginterrupt(SIGINT, 1); |
|
||||||
signal(SIGTERM, shouldQuit); |
|
||||||
siginterrupt(SIGTERM, 1); |
|
||||||
signal(SIGQUIT, shouldQuit); |
|
||||||
siginterrupt(SIGQUIT, 1); |
|
||||||
|
|
||||||
pthread_join(gIMUThread, NULL); |
|
||||||
log(IMU, "service terminated"); |
|
||||||
pthread_join(gCTRLThread, NULL); |
|
||||||
log(CTRL, "service terminated"); |
|
||||||
pthread_join(gQGCSThread, NULL); |
|
||||||
log(QGCS, "service terminated"); |
|
||||||
|
|
||||||
close(port); |
|
||||||
close(fgSock); |
|
||||||
close(qgcsSock); |
|
||||||
|
|
||||||
return(0); |
|
||||||
} |
|
||||||
|
|
||||||
#if __linux__ |
|
||||||
#define getprogname program_invocation_short_name |
|
||||||
#endif |
|
||||||
|
|
||||||
void |
|
||||||
usage() |
|
||||||
{ |
|
||||||
fprintf(stderr, "usage: %s [-d[-d]]\n", getprogname()); |
|
||||||
exit(1); |
|
||||||
} |
|
||||||
|
|
||||||
void |
|
||||||
shouldQuit(int sig) |
|
||||||
{ |
|
||||||
if (!gShouldQuit) { |
|
||||||
fputs("\b\b \b\b", stderr); /* overwrite tty's ^C */ |
|
||||||
if (sig) |
|
||||||
debug(MAIN, 1, "caught signal %d, cleaning up", sig); |
|
||||||
pthread_kill(gIMUThread, SIGINT); |
|
||||||
pthread_kill(gCTRLThread, SIGINT); |
|
||||||
pthread_kill(gQGCSThread, SIGINT); |
|
||||||
gShouldQuit = 1; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
/*
|
|
||||||
* IMU thread listens for datagrams from FlightGear and forwards them out |
|
||||||
* the serial port. |
|
||||||
*/ |
|
||||||
void * |
|
||||||
imuThread(void *arg) |
|
||||||
{ |
|
||||||
struct fgIMUData msg; |
|
||||||
|
|
||||||
/* do not take signals on this thread */ |
|
||||||
pthread_sigmask(SIG_BLOCK, NULL, NULL); |
|
||||||
|
|
||||||
log(IMU, "Listening for FlightGear binary data on port %d", IMU_LISTEN_PORT); |
|
||||||
|
|
||||||
/* handle IMU messages */ |
|
||||||
while (!gShouldQuit) { |
|
||||||
if (imuGetMessage(fgSock, &msg)) |
|
||||||
imuSendPacket(&msg); |
|
||||||
} |
|
||||||
|
|
||||||
return(NULL); |
|
||||||
} |
|
||||||
|
|
||||||
int |
|
||||||
imuGetMessage(int fgSock, struct fgIMUData *msg) |
|
||||||
{ |
|
||||||
ssize_t received; |
|
||||||
|
|
||||||
/* get a message */ |
|
||||||
received = recvfrom(fgSock, msg, sizeof(*msg), MSG_WAITALL, NULL, NULL); |
|
||||||
if (received != sizeof(*msg)) { |
|
||||||
if (received < 0) { |
|
||||||
log(IMU, "receive error: %s", strerror(errno)); |
|
||||||
shouldQuit(0); |
|
||||||
return(0); |
|
||||||
} else { |
|
||||||
if (0 == received) { |
|
||||||
log(IMU, "Received zero-length data packet, check that MAVLink.xml is correctly installed."); |
|
||||||
shouldQuit(0); |
|
||||||
} else { |
|
||||||
log(IMU, "received %ld instead of %lu", received, sizeof(*msg)); |
|
||||||
} |
|
||||||
return(0); |
|
||||||
} |
|
||||||
} |
|
||||||
swap32(&msg->magic); |
|
||||||
if (msg->magic != MSG_MAGIC) { |
|
||||||
log(IMU, "bad magic 0x%08x not 0x%08x", msg->magic, MSG_MAGIC); |
|
||||||
return(0); |
|
||||||
} |
|
||||||
|
|
||||||
/* endian-swap message fields */ |
|
||||||
swap64(&msg->latitude); |
|
||||||
swap64(&msg->longitude); |
|
||||||
swap64(&msg->altitude); |
|
||||||
swap64(&msg->heading); |
|
||||||
swap64(&msg->velocityN); |
|
||||||
swap64(&msg->velocityE); |
|
||||||
swap64(&msg->accelX); |
|
||||||
swap64(&msg->accelY); |
|
||||||
swap64(&msg->accelZ); |
|
||||||
swap64(&msg->rateRoll); |
|
||||||
swap64(&msg->ratePitch); |
|
||||||
swap64(&msg->rateYaw); |
|
||||||
|
|
||||||
return(1); |
|
||||||
} |
|
||||||
|
|
||||||
void |
|
||||||
imuSendPacket(struct fgIMUData *msg) |
|
||||||
{ |
|
||||||
struct timeval tv; |
|
||||||
uint64_t usec; |
|
||||||
int i; |
|
||||||
|
|
||||||
gettimeofday(&tv, NULL); |
|
||||||
usec = (uint64_t)tv.tv_sec + tv.tv_usec; |
|
||||||
|
|
||||||
pthread_mutex_lock(&portMutex); |
|
||||||
|
|
||||||
debug(IMU, 2, "***"); |
|
||||||
debug(IMU, 2, "lat %f lon %f alt %f", msg->latitude, msg->longitude, msg->altitude); |
|
||||||
debug(IMU, 2, "head %f velocityN %f velocityE %f", msg->heading, msg->velocityN, msg->velocityE); |
|
||||||
debug(IMU, 2, "accX %f accY %f accZ %f", msg->accelX, msg->accelY, msg->accelZ); |
|
||||||
debug(IMU, 2, "roll %f pitch %f yaw %f", msg->rateRoll, msg->ratePitch, msg->rateYaw); |
|
||||||
|
|
||||||
|
|
||||||
#define ft2m(_x) ((_x) * 0.3408) /* feet to metres */ |
|
||||||
#define dps2mrps(_x) ((_x) * 17.453293) /* degrees per second to milliradians per second */ |
|
||||||
#define fpss2mg(_x) ((_x) * 1000/ 32.2) /* feet per second per second to milligees */ |
|
||||||
#define rad2deg(_x) fmod((((_x) * 57.29578) + 360), 360) /* radians to degrees */ |
|
||||||
|
|
||||||
mavlink_msg_gps_raw_send(0, |
|
||||||
usec, |
|
||||||
3, /* 3D fix */ |
|
||||||
msg->latitude, |
|
||||||
msg->longitude, |
|
||||||
ft2m(msg->altitude), |
|
||||||
0, /* no uncertainty */ |
|
||||||
0, /* no uncertainty */ |
|
||||||
ft2m(sqrt((msg->velocityN * msg->velocityN) + |
|
||||||
(msg->velocityE * msg->velocityE))), |
|
||||||
rad2deg(atan2(msg->velocityE, msg->velocityN))); |
|
||||||
|
|
||||||
mavlink_msg_raw_imu_send(0, |
|
||||||
usec, |
|
||||||
fpss2mg(msg->accelX), |
|
||||||
fpss2mg(msg->accelY), |
|
||||||
fpss2mg(msg->accelZ), |
|
||||||
dps2mrps(msg->rateRoll), |
|
||||||
dps2mrps(msg->ratePitch), |
|
||||||
dps2mrps(msg->rateYaw), |
|
||||||
0, /* xmag */ |
|
||||||
0, /* ymag */ |
|
||||||
0); /* zmag */ |
|
||||||
|
|
||||||
pthread_mutex_unlock(&portMutex); |
|
||||||
|
|
||||||
} |
|
||||||
|
|
||||||
void * |
|
||||||
ctrlThread(void *arg) |
|
||||||
{ |
|
||||||
fd_set readset, errorset; |
|
||||||
mavlink_message_t msg; |
|
||||||
mavlink_status_t status; |
|
||||||
int result; |
|
||||||
|
|
||||||
/* do not take signals on this thread */ |
|
||||||
pthread_sigmask(SIG_BLOCK, NULL, NULL); |
|
||||||
|
|
||||||
log(CTRL, "Listening for MAVLink packets and sending to %d", CTRL_SEND_PORT); |
|
||||||
|
|
||||||
/* loop handling data arriving on the serial port */ |
|
||||||
while (!gShouldQuit) { |
|
||||||
|
|
||||||
/* wait for data or an exceptional condition */ |
|
||||||
FD_ZERO(&readset); |
|
||||||
FD_ZERO(&errorset); |
|
||||||
FD_SET(port, &readset); |
|
||||||
FD_SET(port, &errorset); |
|
||||||
if (select(port + 1, &readset, NULL, &errorset, NULL) < 0) { |
|
||||||
if (errno != EINTR) |
|
||||||
log(CTRL, "select error: %s", strerror(errno)); |
|
||||||
goto abort; |
|
||||||
}
|
|
||||||
|
|
||||||
/* exception? */ |
|
||||||
if (FD_ISSET(port, &errorset)) { |
|
||||||
log(CTRL, "serial port error"); |
|
||||||
goto abort; |
|
||||||
} |
|
||||||
|
|
||||||
/* no data available for reading? */ |
|
||||||
if (!FD_ISSET(port, &readset)) |
|
||||||
continue; |
|
||||||
|
|
||||||
/* read data in chunks and process */ |
|
||||||
for (;;) { |
|
||||||
uint8_t buf[128]; /* 11ms @ 115200 */ |
|
||||||
ssize_t cnt; |
|
||||||
uint8_t *ch; |
|
||||||
|
|
||||||
/* port is nonblocking, cnt may be as small as 1 byte */ |
|
||||||
cnt = read(port, buf, sizeof(buf)); |
|
||||||
if (cnt < 0) { |
|
||||||
/* have we run out of data? */ |
|
||||||
if (errno == EWOULDBLOCK) |
|
||||||
break; |
|
||||||
|
|
||||||
/* some other error */ |
|
||||||
log(CTRL, "serial port error"); |
|
||||||
goto abort; |
|
||||||
}
|
|
||||||
|
|
||||||
/* port closed for some reason */ |
|
||||||
if (cnt == 0) { |
|
||||||
log(CTRL, "serial port closed"); |
|
||||||
goto abort; |
|
||||||
} |
|
||||||
|
|
||||||
/* process characters, possibly handle messages */ |
|
||||||
for (ch = buf; cnt; ch++, cnt--) |
|
||||||
if (mavlink_parse_char(0, *ch, &msg, &status)) |
|
||||||
ctrlHandleMessage(&msg); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
abort: |
|
||||||
shouldQuit(0); |
|
||||||
return(NULL); |
|
||||||
} |
|
||||||
|
|
||||||
void |
|
||||||
ctrlHandleMessage(mavlink_message_t *msg) |
|
||||||
{ |
|
||||||
struct fgControlData cdata; |
|
||||||
time_t now; |
|
||||||
static time_t lastRCMessage; |
|
||||||
uint8_t buf[1024]; |
|
||||||
int len; |
|
||||||
|
|
||||||
time(&now); |
|
||||||
|
|
||||||
switch(msg->msgid) { |
|
||||||
case MAVLINK_MSG_ID_HEARTBEAT: |
|
||||||
/*
|
|
||||||
* If we have received a heartbeat, we are connected |
|
||||||
* to something. If we haven't seen an RC_CHANNELS_SCALED |
|
||||||
* message in the last couple of seconds, ask for it to |
|
||||||
* be added to the stream. |
|
||||||
*/ |
|
||||||
if ((now - lastRCMessage) > 2) { |
|
||||||
debug(CTRL, 1, "got heartbeat, requesting RC channel datastream"); |
|
||||||
pthread_mutex_lock(&portMutex); |
|
||||||
mavlink_msg_request_data_stream_send(0, |
|
||||||
TARGET_SYSTEM, |
|
||||||
TARGET_COMPONENT,
|
|
||||||
MAV_DATA_STREAM_RAW_CONTROLLER, |
|
||||||
10, /* 10Hz enough? */ |
|
||||||
1); /* start */ |
|
||||||
pthread_mutex_unlock(&portMutex); |
|
||||||
/* suppress re-sending for 2 seconds */ |
|
||||||
lastRCMessage = now; |
|
||||||
} |
|
||||||
break; |
|
||||||
|
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: |
|
||||||
|
|
||||||
/* build the control message for FG */ |
|
||||||
/* XXX ArduPilotMega channel ordering */ |
|
||||||
cdata.aileron = (double)mavlink_msg_rc_channels_scaled_get_chan1_scaled(msg) / 10000; |
|
||||||
cdata.elevator = (double)mavlink_msg_rc_channels_scaled_get_chan2_scaled(msg) / 10000; |
|
||||||
cdata.throttle = (double)mavlink_msg_rc_channels_scaled_get_chan3_scaled(msg) / 10000; |
|
||||||
cdata.rudder = (double)mavlink_msg_rc_channels_scaled_get_chan4_scaled(msg) / 10000; |
|
||||||
|
|
||||||
debug(CTRL, 2, "%+6.4f %+6.4f %+6.4f %+6.4f", cdata.aileron, cdata.elevator, cdata.throttle, cdata.rudder); |
|
||||||
|
|
||||||
/* swap and send it to FG */ |
|
||||||
swap64(&cdata.aileron); |
|
||||||
swap64(&cdata.elevator); |
|
||||||
swap64(&cdata.throttle); |
|
||||||
swap64(&cdata.rudder); |
|
||||||
sendto(fgSock, &cdata, sizeof(cdata), 0, (struct sockaddr *)&fgAddr, sizeof(fgAddr)); |
|
||||||
|
|
||||||
/* update our timestamp so that we don't re-request the stream */ |
|
||||||
lastRCMessage = now; |
|
||||||
} |
|
||||||
|
|
||||||
/* pass the message on to QGCS */ |
|
||||||
len = mavlink_msg_to_send_buffer(buf, msg); |
|
||||||
sendto(qgcsSock, buf, len, 0, (struct sockaddr *)&qgcsAddr, sizeof(qgcsAddr)); |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
/*
|
|
||||||
* QGS thread listens for datagrams from QGroundControl and forwards them out |
|
||||||
* the serial port. |
|
||||||
*/ |
|
||||||
void * |
|
||||||
qgsThread(void *arg) |
|
||||||
{ |
|
||||||
struct fgIMUData msg; |
|
||||||
ssize_t received; |
|
||||||
uint8_t buf[1024]; |
|
||||||
int sent, result; |
|
||||||
|
|
||||||
/* do not take signals on this thread */ |
|
||||||
pthread_sigmask(SIG_BLOCK, NULL, NULL); |
|
||||||
|
|
||||||
log(QGCS, "Listening for QGroundControl data on port %d", QGCS_LISTEN_PORT); |
|
||||||
|
|
||||||
while (!gShouldQuit) { |
|
||||||
|
|
||||||
/* get a message */ |
|
||||||
received = recvfrom(qgcsSock, buf, sizeof(buf), MSG_WAITALL, NULL, NULL); |
|
||||||
|
|
||||||
/* XXX might want to intercept messages here that would turn off data that we need */ |
|
||||||
|
|
||||||
/* and forward it */ |
|
||||||
pthread_mutex_lock(&portMutex); |
|
||||||
sent = 0; |
|
||||||
while (sent < received) { |
|
||||||
if (gShouldQuit) |
|
||||||
break; |
|
||||||
result = write(port, buf + sent, received - sent); |
|
||||||
if (result < 0) { |
|
||||||
if (result == EAGAIN) |
|
||||||
continue; |
|
||||||
warn("serial passthrough write failed"); |
|
||||||
goto abort; |
|
||||||
} |
|
||||||
if (result == 0) { |
|
||||||
warnx("serial port closed"); |
|
||||||
goto abort; |
|
||||||
} |
|
||||||
sent += result; |
|
||||||
} |
|
||||||
pthread_mutex_unlock(&portMutex); |
|
||||||
} |
|
||||||
|
|
||||||
abort: |
|
||||||
shouldQuit(0); |
|
||||||
return(NULL); |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
void comm_send_ch(mavlink_channel_t chan, uint8_t ch) |
|
||||||
{ |
|
||||||
if (write(port, &ch, 1) != 1) |
|
||||||
warn("serial packet write failed"); |
|
||||||
} |
|
||||||
|
|
||||||
void |
|
||||||
swap32(void *p) |
|
||||||
{ |
|
||||||
*(int32_t *)p = htonl(*(int32_t *)p); |
|
||||||
} |
|
||||||
|
|
||||||
union temp64 { |
|
||||||
int64_t ll; |
|
||||||
int32_t l[2]; |
|
||||||
}; |
|
||||||
|
|
||||||
void |
|
||||||
swap64(void *p) |
|
||||||
{ |
|
||||||
union temp64 *f, t; |
|
||||||
|
|
||||||
f = (union temp64 *)p; |
|
||||||
|
|
||||||
t.l[0] = htonl(f->l[1]); |
|
||||||
t.l[1] = htonl(f->l[0]); |
|
||||||
|
|
||||||
f->ll = t.ll; |
|
||||||
} |
|
||||||
|
|
||||||
void |
|
||||||
hexdump(void *p, size_t s) |
|
||||||
{ |
|
||||||
int i, j, lim; |
|
||||||
uint8_t *c = (uint8_t *)p; |
|
||||||
|
|
||||||
for (i = 0; i < s; i += 32) { |
|
||||||
printf("%04x:", i); |
|
||||||
lim = s - i; |
|
||||||
if (lim > 32) |
|
||||||
lim = 32; |
|
||||||
for (j = 0; j < lim; j++) |
|
||||||
printf(" %02x", *c++); |
|
||||||
printf("\n"); |
|
||||||
} |
|
||||||
} |
|
||||||
@ -1,119 +0,0 @@ |
|||||||
<?xml version="1.0"?> |
|
||||||
|
|
||||||
<PropertyList> |
|
||||||
|
|
||||||
<generic> |
|
||||||
|
|
||||||
<!-- template |
|
||||||
<chunk> |
|
||||||
<name></name> |
|
||||||
<type>double</type> |
|
||||||
<node></node> |
|
||||||
</chunk> |
|
||||||
--> |
|
||||||
|
|
||||||
<input> |
|
||||||
<binary_mode>true</binary_mode> |
|
||||||
|
|
||||||
<!-- ##### Flight Controls --> |
|
||||||
<chunk> |
|
||||||
<name>aileron</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/controls/flight/aileron</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>elevator</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/controls/flight/elevator</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>rudder</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/controls/flight/rudder</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>throttle</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/controls/engines/engine[0]/throttle</node> |
|
||||||
</chunk> |
|
||||||
|
|
||||||
</input> |
|
||||||
|
|
||||||
<output> |
|
||||||
<binary_mode>true</binary_mode> |
|
||||||
<binary_footer>magic,0x4c56414d</binary_footer> |
|
||||||
|
|
||||||
<!-- ##### GPS ##### --> |
|
||||||
<chunk> |
|
||||||
<name>latitude</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/position/latitude-deg</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>longitude</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/position/longitude-deg</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>altitude</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/position/altitude-ft</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>heading</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/orientation/heading-deg</node> |
|
||||||
</chunk> |
|
||||||
|
|
||||||
<!-- ground course = atan(ve/vn), speed = sqrt((ve*ve) + (vn*vn)) --> |
|
||||||
<chunk> |
|
||||||
<name>speed - north</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/velocities/speed-north-fps</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>speed - east</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/velocities/speed-east-fps</node> |
|
||||||
</chunk> |
|
||||||
|
|
||||||
|
|
||||||
<!-- ##### IMU ##### --> |
|
||||||
<chunk> |
|
||||||
<name>x-accel</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/accelerations/pilot/x-accel-fps_sec</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>y-accel</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/accelerations/pilot/y-accel-fps_sec</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>z-accel</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/accelerations/pilot/z-accel-fps_sec</node> |
|
||||||
</chunk> |
|
||||||
|
|
||||||
<chunk> |
|
||||||
<name>roll-rate</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/orientation/roll-rate-degps</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>pitch-rate</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/orientation/pitch-rate-degps</node> |
|
||||||
</chunk> |
|
||||||
<chunk> |
|
||||||
<name>yaw-rate</name> |
|
||||||
<type>double</type> |
|
||||||
<node>/orientation/yaw-rate-degps</node> |
|
||||||
</chunk> |
|
||||||
|
|
||||||
|
|
||||||
</output> |
|
||||||
|
|
||||||
</generic> |
|
||||||
|
|
||||||
</PropertyList> |
|
||||||
@ -1,28 +0,0 @@ |
|||||||
#!/usr/bin/make
|
|
||||||
#
|
|
||||||
# Requires GNU Make
|
|
||||||
#
|
|
||||||
|
|
||||||
OS := $(shell uname)
|
|
||||||
|
|
||||||
ifeq ($(OS),Darwin) |
|
||||||
CC := cc
|
|
||||||
CFLAGS := -std=c99
|
|
||||||
LDFLAGS :=
|
|
||||||
endif |
|
||||||
|
|
||||||
ifeq ($(OS),Linux) |
|
||||||
CC := cc
|
|
||||||
CFLAGS := -std=c99 -D_XOPEN_SOURCE=600
|
|
||||||
LDFLAGS := -lpthread -lm
|
|
||||||
endif |
|
||||||
|
|
||||||
CFLAGS += -I./GCS_MAVLink/include
|
|
||||||
SRCS := FGShim.c
|
|
||||||
|
|
||||||
FGShim: $(SRCS) |
|
||||||
$(CC) -o $@ $(SRCS) $(CFLAGS) $(LDFLAGS)
|
|
||||||
|
|
||||||
|
|
||||||
clean: |
|
||||||
rm -f FGShim *~
|
|
||||||
@ -1,61 +0,0 @@ |
|||||||
#!/bin/sh |
|
||||||
|
|
||||||
case `uname` in |
|
||||||
Darwin) |
|
||||||
FG_DIR=/Volumes/Data/Applications/FlightGear.app/Contents/Resources |
|
||||||
export DYLD_LIBRARY_PATH=${FG_DIR}/plugins |
|
||||||
export LD_LIBRARY_PATH=${FG_DIR}/plugins |
|
||||||
export FG_ROOT=${FG_DIR}/data |
|
||||||
FGFS=${FG_DIR}/fgfs |
|
||||||
FGFSOPTIONS= |
|
||||||
;; |
|
||||||
Linux) |
|
||||||
FGFS=fgfs |
|
||||||
FGFSOPTIONS= |
|
||||||
;; |
|
||||||
esac |
|
||||||
|
|
||||||
if [ $# != 1 ] |
|
||||||
then |
|
||||||
echo usage: $0 aircraft |
|
||||||
echo choose one from below: |
|
||||||
${FGFS} --show-aircraft |
|
||||||
exit |
|
||||||
else |
|
||||||
aircraft=$1 |
|
||||||
fi |
|
||||||
|
|
||||||
if [ ! -z "${FG_DIR}" ]; then |
|
||||||
if [ -d ${FG_DIR}/data/Protocol ]; then |
|
||||||
cp -v MAVLink.xml ${FG_DIR}/data/Protocol/ |
|
||||||
else |
|
||||||
echo "FlightGear protocol definition directory ${FG_DIR}/data/Protocol doesn't exist." |
|
||||||
exit 1 |
|
||||||
fi |
|
||||||
fi |
|
||||||
|
|
||||||
${FGFS} \ |
|
||||||
${FGFSOPTIONS} \ |
|
||||||
--aircraft=$aircraft \ |
|
||||||
--geometry=400x300 \ |
|
||||||
--generic=socket,out,20,,5501,udp,MAVLink \ |
|
||||||
--generic=socket,in,50,,5500,udp,MAVLink \ |
|
||||||
--vc=30 \ |
|
||||||
--altitude=10000 \ |
|
||||||
--heading=90 \ |
|
||||||
--roll=0 \ |
|
||||||
--pitch=0 \ |
|
||||||
--wind=0@0 \ |
|
||||||
--turbulence=0.0 \ |
|
||||||
--prop:/sim/frame-rate-throttle-hz=30 \ |
|
||||||
--timeofday=noon \ |
|
||||||
--shading-flat \ |
|
||||||
--fog-disable \ |
|
||||||
--disable-specular-highlight \ |
|
||||||
--disable-skyblend \ |
|
||||||
--disable-random-objects \ |
|
||||||
--disable-panel \ |
|
||||||
--disable-horizon-effect \ |
|
||||||
--disable-clouds \ |
|
||||||
--disable-anti-alias-hud |
|
||||||
;; |
|
||||||
Loading…
Reference in new issue