From ce08e1a86946fec74c472aaf39c10ec52b009fe9 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Mon, 24 Oct 2011 17:05:11 +0200 Subject: [PATCH] IVY 2 Serial relay --- .gitignore | 1 + sw/ground_segment/misc/Makefile | 6 +- sw/ground_segment/misc/ivy2serial.c | 217 ++++++++++++++++++++++++++++ 3 files changed, 223 insertions(+), 1 deletion(-) create mode 100644 sw/ground_segment/misc/ivy2serial.c diff --git a/.gitignore b/.gitignore index 4e3b07d886..9c0e518c98 100644 --- a/.gitignore +++ b/.gitignore @@ -80,6 +80,7 @@ /sw/ground_segment/tmtc/ivy2udp /sw/ground_segment/tmtc/server /sw/ground_segment/tmtc/diadec +/sw/ground_segment/misc/ivy2serial # /sw/ground_segment/joystick /sw/ground_segment/joystick/input2ivy diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile index 35ff00d8c9..d2c1539164 100644 --- a/sw/ground_segment/misc/Makefile +++ b/sw/ground_segment/misc/Makefile @@ -7,7 +7,7 @@ else endif -all: davis2ivy kestrel2ivy +all: davis2ivy kestrel2ivy ivy2serial clean: rm *.o davis2ivy kestrel2ivy @@ -18,5 +18,9 @@ davis2ivy: davis2ivy.o kestrel2ivy: kestrel2ivy.o g++ -o kestrel2ivy kestrel2ivy.o $(LIBRARYS) -livy +ivy2serial: ivy2serial.o + g++ -o ivy2serial ivy2serial.o $(LIBRARYS) -livy + + %.o : %.c gcc -c -O2 -Wall -I /opt/local/include/ $< diff --git a/sw/ground_segment/misc/ivy2serial.c b/sw/ground_segment/misc/ivy2serial.c new file mode 100644 index 0000000000..e006499f82 --- /dev/null +++ b/sw/ground_segment/misc/ivy2serial.c @@ -0,0 +1,217 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////// +// SETTINGS +////////////////////////////////////////////////////////////////////////////////// + +// Serial Repeat Rate +long delay = 1000; + +// local_uav Number +unsigned char send_ac_id = 5; + +////////////////////////////////////////////////////////////////////////////////// +// local_uav DATA +////////////////////////////////////////////////////////////////////////////////// + + +struct _uav_type_ +{ + // Header + unsigned char header; + + // Data + unsigned char ac_id; + short int phi, theta, psi; + int utm_east,utm_north,utm_z; + unsigned char utm_zone; + + // Footer + unsigned char footer; +} +__attribute__((packed)) + +local_uav, remote_uav; + +////////////////////////////////////////////////////////////////////////////////// +// IVY Reader +////////////////////////////////////////////////////////////////////////////////// + + +static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[]) +{ + unsigned char id = atoi(argv[0]); + + if (id != send_ac_id) + { + printf("NEGLECT: %d\n",id); + return; + } + +/* + + + + + +*/ + + local_uav.ac_id = id; + local_uav.phi = (short int) (atof(argv[1]) * 1000.0); + local_uav.theta = (short int) (atof(argv[3]) * 1000.0); + local_uav.psi = (short int) (atof(argv[2]) * 1000.0); + +} + +static void on_Gps(IvyClientPtr app, void *user_data, int argc, char *argv[]) +{ + unsigned char id = atoi(argv[0]); + + if (id != send_ac_id) + { + printf("NEGLECT: %d\n",id); + return; + } + +/* + + + + + + + + + + + + + +*/ + + local_uav.ac_id = id; + local_uav.utm_east = atoi(argv[2]); + local_uav.utm_north = atoi(argv[3]); + local_uav.utm_z = atoi(argv[5]); + local_uav.utm_zone = atoi(argv[10]); + + printf("ATTITUDE ac=%d phi=%d theta=%d psi=%d ",local_uav.ac_id, local_uav.phi, local_uav.theta, local_uav.psi); + printf("GPS ac=%d %d %d %d %d\n",local_uav.ac_id, local_uav.utm_east, local_uav.utm_north, local_uav.utm_z, local_uav.utm_zone); + +} + +////////////////////////////////////////////////////////////////////////////////// +// SERIAL PORT +////////////////////////////////////////////////////////////////////////////////// + +// pointer +int fd; + +/// Open +void open_port(const char* device) { + fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd == -1) { + fprintf(stderr, "open_port: unable to open device %s - ", device); + perror(NULL); + exit(EXIT_FAILURE); + } + // setup connection options + struct termios options; + + // get the current options + tcgetattr(fd, &options); + + // set local mode, enable receiver, set comm. options: + // 8 data bits, 1 stop bit, no parity, 9600 Baud + options.c_cflag = CLOCAL | CREAD | CS8 | B9600; + + // write options back to port + tcsetattr(fd, TCSANOW, &options); +} + +void send_port(void) +{ + int bytes = write(fd, &local_uav, sizeof(local_uav)); + printf("SENT: %d bytes\n",bytes); +} + +void read_port(void) +{ +} + +void close_port(void) +{ + close(fd); +} + +////////////////////////////////////////////////////////////////////////////////// +// TIMER +////////////////////////////////////////////////////////////////////////////////// + +// Timer +void handle_timer (TimerId id, void *data, unsigned long delta) { + printf("TIMER\n"); + send_port(); +} + +TimerId tid; + +/// Handler for Ctrl-C, exits the main loop +void sigint_handler(int sig) { + printf("\nCLEAN STOP\n"); + IvyStop(); + TimerRemove(tid); + close_port(); +} + +////////////////////////////////////////////////////////////////////////////////// +// MAIN +////////////////////////////////////////////////////////////////////////////////// + +int main ( int argc, char** argv) +{ + int s = sizeof(local_uav); + + if (argc < 3) + { + printf("Use: ivy2serial ac_id serial_device\n"); + return -1; + } + + send_ac_id = atoi(argv[1]); + + printf("Listening to AC=%d, \nSending Size of Data = %d \n",send_ac_id, s); + + // make Ctrl-C stop the main loop and clean up properly + signal(SIGINT, sigint_handler); + + open_port(argv[2]); + + // create timer (Ivy) + tid = TimerRepeatAfter (0, delay, handle_timer, 0); + + + IvyInit ("IVY <-> Serial", "IVY <-> Serial READY", NULL, NULL, NULL, NULL); + IvyBindMsg(on_Attitude, NULL, "^(\\S*) ATTITUDE (\\S*) (\\S*) (\\S*)"); + IvyBindMsg(on_Gps, NULL, "^(\\S*) GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); + IvyStart("127.255.255.255"); + + IvyMainLoop (); + + return 0; +} +