diff --git a/sw/logalizer/tmserver.c b/sw/logalizer/tmserver.c new file mode 100644 index 0000000000..6888725fae --- /dev/null +++ b/sw/logalizer/tmserver.c @@ -0,0 +1,368 @@ +/* $Id$ + * + * tmserver, an telemetry server to distribute paparazzi location data + * Copyright (C) 2007 Martin Mueller + * + * This file is part of paparazzi. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define MAX(a, b) ((a > b) ? a : b) + +#define PORT_IN 7123 +#define PORT_OUT 7124 +#define BUFSIZE 0x10000 + +#define PACKET_SIZE 9 + +#define AC_NUM 10 +#define AC_TIMEOUT 5 + +typedef struct +{ + long addr; + struct timeval tvvalid; + unsigned char ac_id; + float fval[PACKET_SIZE]; +} ac_dat_t; + +static char ppfg[] = {"ppfg"}; +static char ppua[] = {"ppua"}; +static char ppac[] = {"ppac"}; + +int getac(ac_dat_t * ac_dati, unsigned char ac_id, long addr) +{ + int count; + + for(count=0; count < AC_NUM; count++) + { + if ((ac_dati[count].ac_id == ac_id) && + (ac_dati[count].addr == addr)) + { + return count; + } + } + for(count=0; count < AC_NUM; count++) + { + if (ac_dati[count].ac_id == 0) return count; + } + return -1; +} + +int getnumac(ac_dat_t * ac_dati) +{ + int count, num=0; + + for(count=0; count < AC_NUM; count++) + { + if (ac_dati[count].ac_id != 0) num++; + } + return num; +} + +int cleanupac(ac_dat_t * ac_dati) +{ + int count, num=0; + struct timeval tvcur; + + gettimeofday(&tvcur, NULL); + + for(count=0; count < AC_NUM; count++) + { + if (ac_dati[count].tvvalid.tv_sec + AC_TIMEOUT < tvcur.tv_sec) + { + ac_dati[count].ac_id = 0; + } + } + return num; +} + +int main(int argc, char *argv[]) +{ + int length, fromlen, tolen, n, count, cnt, fdmax, ac; + int sourcesock, sinksock, consock; + struct sockaddr_in sourceaddr, sinkaddr, from; + char buf[BUFSIZE]; + char *pbuf; + unsigned int itemp; + fd_set fds; + struct timeval tvt, tvcur; + ac_dat_t ac_dat[AC_NUM]; + int on = 1; + + memset(ac_dat, 0, sizeof(ac_dat)); + + /* open sockets */ + sourcesock = socket(AF_INET, SOCK_DGRAM, 0); + sinksock = socket(PF_INET, SOCK_STREAM, 0); + consock = 0; + if ((sourcesock < 0) || (sinksock < 0)) + { + perror("socket"); + exit(1); + } + + tolen = sizeof(struct sockaddr_in); + length = sizeof(sourceaddr); + memset(&sourceaddr, 0, sizeof(sourceaddr)); + memset(&sinkaddr, 0, sizeof(sinkaddr)); + + sourceaddr.sin_family = AF_INET; + sourceaddr.sin_addr.s_addr = INADDR_ANY; + sourceaddr.sin_port = htons(PORT_IN); + sinkaddr.sin_family = AF_INET; + sinkaddr.sin_addr.s_addr = INADDR_ANY; + sinkaddr.sin_port = htons(PORT_OUT); + + setsockopt(sinksock, SOL_SOCKET, SO_REUSEADDR, (void *)&on, sizeof(on)) ; + + if ((bind(sourcesock, (struct sockaddr *)&sourceaddr, length) < 0) || + (bind(sinksock, (struct sockaddr *)&sinkaddr, length) < 0)) + { + perror("bind"); + exit(1); + } + + if (listen(sinksock, 3)) + { + perror("listen"); + exit(1); + } + + fdmax = MAX(sourcesock, sinksock); + FD_ZERO(&fds); + + while (1) + { + FD_SET(sourcesock, &fds); + FD_SET(sinksock, &fds); + + /* loop every second */ + tvt.tv_sec = 1; + tvt.tv_usec = 0; + + count = select(fdmax+1, &fds, NULL, NULL, &tvt); + + if (FD_ISSET(sourcesock, &fds)) + { + gettimeofday(&tvcur, NULL); + n = recvfrom(sourcesock, buf, sizeof(buf), 0, (struct sockaddr *)&sourceaddr, &fromlen); + if (n < 0) perror("recvfrom"); + + if (n == PACKET_SIZE*4) + { + ac = getac(ac_dat, *(unsigned char*)(buf), ntohl(sourceaddr.sin_addr.s_addr)); + if (ac >= 0) + { + ac_dat[ac].ac_id = *(unsigned char*)(buf); + ac_dat[ac].addr = ntohl(sourceaddr.sin_addr.s_addr); + ac_dat[ac].tvvalid.tv_sec = tvcur.tv_sec; + + for (count=1; count\n"); + pbuf += sprintf(pbuf,"\n"); + pbuf += sprintf(pbuf,"\n"); + pbuf += sprintf(pbuf," \n"); + pbuf += sprintf(pbuf," http://uav-flight.de/maps/fg_server_xml.cgi?ppac\n"); + + for (count = 0; count < ac; count++) + { + if (ac_dat[count].ac_id > 0) + { + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n", count); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[4]); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[3]); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[7]); + pbuf += sprintf(pbuf, " 100\n"); + pbuf += sprintf(pbuf, " 55\n"); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[6]); + pbuf += sprintf(pbuf, " absolute\n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[3]); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[4]); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[7]); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[6]); + pbuf += sprintf(pbuf, " %f\n", -ac_dat[count].fval[2]); + pbuf += sprintf(pbuf, " %f\n", -ac_dat[count].fval[1]); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + } + } + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, "\n"); + pbuf += sprintf(pbuf, "\n"); + } + else if (strncmp(ppac, buf, 4) == 0) + { + pbuf = buf; + ac = getnumac(ac_dat); + pbuf += sprintf(pbuf,"Content-type: text/xml\n\n"); + pbuf += sprintf(pbuf,"\n"); + pbuf += sprintf(pbuf,"\n"); + pbuf += sprintf(pbuf," \n"); + pbuf += sprintf(pbuf," Paparazzi live aircrafts\n"); + pbuf += sprintf(pbuf," 1\n"); + + for (count = 0; count < ac; count++) + { + if (ac_dat[count].ac_id > 0) + { + pbuf += sprintf(pbuf, " \n", count); + pbuf += sprintf(pbuf, " %i\n", ac_dat[count].ac_id); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[4]); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[3]); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[7]); + pbuf += sprintf(pbuf, " 500\n"); + pbuf += sprintf(pbuf, " 55\n"); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[7]); + pbuf += sprintf(pbuf, " absolute\n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " Paparazzi\n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " absolute\n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[3]); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[4]); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[7]); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " %f\n", ac_dat[count].fval[6]); + pbuf += sprintf(pbuf, " %f\n", -ac_dat[count].fval[2]); + pbuf += sprintf(pbuf, " %f\n", -ac_dat[count].fval[1]); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " 150.0\n"); + pbuf += sprintf(pbuf, " 150.0\n"); + pbuf += sprintf(pbuf, " 150.0\n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " http://pigeond.net/flightgear/fgmap/ge/daes/c172p/c172p.dae\n"); + pbuf += sprintf(pbuf, " onChange\n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n\n"); + } + } + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " Update\n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " http://uav-flight.de/maps/fg_server_xml.cgi?ppua\n"); + pbuf += sprintf(pbuf, " onInterval\n"); + pbuf += sprintf(pbuf, " 1\n"); + pbuf += sprintf(pbuf, " \n"); + pbuf += sprintf(pbuf, " \n\n"); + pbuf += sprintf(pbuf, "\n"); + } + else //if strncmp(ppfg, buf, 4) == 0) + { + pbuf=buf; + ac = getnumac(ac_dat); + + pbuf += sprintf(pbuf, "Content-type: text/xml\n\n\n", ac); + + for (count = 0; count < ac; count++) + { + if (ac_dat[count].ac_id > 0) + { + pbuf += sprintf(pbuf, "\n", + ac_dat[count].ac_id, + (unsigned char)(ac_dat[count].addr >> 24), + (unsigned char)(ac_dat[count].addr >> 16), + (unsigned char)(ac_dat[count].addr >> 8), + (unsigned char)(ac_dat[count].addr & 0xff), + ac_dat[count].fval[3], + ac_dat[count].fval[4], + ac_dat[count].fval[7], + ac_dat[count].fval[6], + ac_dat[count].fval[2], + ac_dat[count].fval[1]); + } + } + pbuf += sprintf(pbuf, "\n"); + } +// else buf[0]=0; + + if (ntohl(sinkaddr.sin_addr.s_addr) == INADDR_LOOPBACK) + { + count = send(consock, buf, strlen(buf), 0); + } + close(consock); + FD_CLR(consock, &fds); + fdmax = MAX(sourcesock, sinksock); + } + + cleanupac(ac_dat); + } + + return 0; +}