diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile index 544cd326f1..d67267ed62 100644 --- a/sw/ground_segment/tmtc/Makefile +++ b/sw/ground_segment/tmtc/Makefile @@ -37,7 +37,7 @@ XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib,pprzlink,$(USE_LABELGTK) SERVERCMO = server_globals.cmo aircraft_server.cmo wind.cmo airprox.cmo kml.cmo parse_messages_v1.ml intruder.cmo server.cmo SERVERCMX = $(SERVERCMO:.cmo=.cmx) -all: link server ivy_tcp_aircraft ivy_tcp_controller ivy2udp ivy2serial app_server ivy2nmea gpsd2ivy gtk_tools +all: link server ivy_tcp_aircraft ivy_tcp_controller ivy2udp ivy2serial ivy2nmea gpsd2ivy gtk_tools ifeq ($(USE_LABELGTK),lablgtk2) gtk_tools: messages @@ -46,7 +46,7 @@ endif opt: server.opt clean: - $(Q)rm -f link server messages *.bak *~ core *.o .depend *.opt *.out *.cm* ivy_tcp_aircraft ivy_tcp_controller ivy2udp ivy2serial ivy_serial_bridge app_server gpsd2ivy c_ivy_client_example_1 c_ivy_client_example_2 ivy2nmea + $(Q)rm -f link server messages *.bak *~ core *.o .depend *.opt *.out *.cm* ivy_tcp_aircraft ivy_tcp_controller ivy2udp ivy2serial ivy_serial_bridge gpsd2ivy c_ivy_client_example_1 c_ivy_client_example_2 ivy2nmea messages : messages.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $@ @@ -115,10 +115,6 @@ ifeq ("$(UNAME)","Darwin") endif -app_server: app_server.c Makefile - @echo OL $@ - $(Q)$(CC) $(shell pkg-config libxml-2.0 gio-2.0 ivy-glib --cflags) -o $@ $< -lm -lz $(shell pkg-config libxml-2.0 gio-2.0 ivy-glib --libs) $(PCRE_LDFLAGS) - gpsd2ivy: gpsd2ivy.c Makefile ifeq (, $(shell which gpsd)) $(info GPSD not installed) diff --git a/sw/ground_segment/tmtc/app_server.c b/sw/ground_segment/tmtc/app_server.c deleted file mode 100644 index a5aa3cae10..0000000000 --- a/sw/ground_segment/tmtc/app_server.c +++ /dev/null @@ -1,936 +0,0 @@ -/* - * Copyright (C) 2014 - * - * Created by: Savas Sen - ENAC UAV Lab - * - * This file is part of paparazzi.. - * - * paparazzi 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. - * - * paparazzi 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. - * - */ - -/* - * Server application for remote devices such as Android. - * - * It forwards paparazzi ground class messages to remote app - * and receives commands from allowed clients to control the aircraft - * - * Several clients can be connected at the same time with full or restricted access - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -char defaultAppPass[] = "1234"; //4 char password to control ac's over app "pass ground stg stg stg.. -char* AppPass; - -#define BUFLEN 2048 -#define MAXCLIENT 20 -#define MAXIPLEN 50 -#define MAXNAMELENGTH 500 -#define MAXDEVICENUMB 255 - -#define MAXWPNUMB 50 //NUMBER OF WP PER AC (MAX) -#define MAXWPNUMB 50 //NUMBER OF WP PER AC (MAX) -#define MAXWPNAMELEN 50 //WP NAME LENGTH (MAX) - -// Default TCP port (listen clients commands) -int tcp_port = 5010; -// Default UDP port (broadcast data to clients) -int udp_port = 5005; - -#ifdef __APPLE__ -char defaultIvyBus[] = "224.255.255.255:2010"; -#else -char defaultIvyBus[] = "127.255.255.255:2010"; -#endif -char* IvyBus; - -char ivybuffer[BUFLEN]; - -// verbose flag -int verbose = 0; - -//TCP flag -int uTCP = 0; - -int ProcessID; -int RequestID; - -//Block structure -typedef struct { - //Will we need any more block data? - char Bl_Name[MAXWPNAMELEN]; -} bl_data; - -//Waypoint structure -typedef struct { - //Will we need any more waypoint data? - char Wp_Name[MAXWPNAMELEN]; -} wp_data; - -//Aircraft Data Structure -typedef struct { - int device_id; - char name[MAXNAMELENGTH]; - char type[MAXNAMELENGTH]; - char color[MAXNAMELENGTH]; - char flight_plan_path[MAXNAMELENGTH]; - char airframe_path[MAXNAMELENGTH]; - char settings_path[MAXNAMELENGTH]; - int dl_launch_ind; - int kill_thr_ind; - int flight_altitude_ind; - wp_data AcWp[MAXWPNUMB]; - bl_data AcBl[MAXWPNUMB]; -} device_names; - -device_names DevNames[MAXDEVICENUMB]; - -//Connected Clients Data Structure -typedef struct { - int used ; - char client_ip[MAXIPLEN]; - //Pointer for tcp connection; - gpointer ClientTcpData; -} client_data; - -client_data ConnectedClients[MAXCLIENT]; //Holds all status of devices - -//Write custom strncpy function to unsure null terminated string -char * my_strncpy(char *dest, const char *src, size_t n) { - size_t i; - for (i = 0; i < n - 1 && src[i] != '\0'; i++) - dest[i] = src[i]; - for ( ; i < n; i++) - dest[i] = '\0'; - return dest; -} - - -//Remove Client from UdpBroadcast list -void remove_client(char* RemClientIpAd) { - //remove client from client list - int i=0; - for (; i < MAXCLIENT; i++) { - //Search thru the client list - - if ( (ConnectedClients[i].used > 0) ) { - //Client list is being used - if ( g_strcmp0(RemClientIpAd , ConnectedClients[i].client_ip) == 0 ) { - //record found clean it!! - ConnectedClients[i].client_ip[0]='\0'; - ConnectedClients[i].used=0; - if (verbose) { - printf("App Server: Client removed from client list %s\n", RemClientIpAd); - fflush(stdout); - } - return; - } - } - } - //no record found! - if (verbose) { - printf("App Server: No record found to be removed from client list\n"); - fflush(stdout); - } -} - -//Record client (if new) -void add_client(char* ClientIpAd, gpointer connection_in) { - /* Check if client exists. If exists return else record it */ - int i; - for (i = 0; i < MAXCLIENT; i++) { - //Search thru the client list - if ( (ConnectedClients[i].used > 0) ) { - //Client list is being used - if ( g_strcmp0(ClientIpAd , ConnectedClients[i].client_ip) == 0 ) { - //Already in client list return - return; - } - continue; - } - //no record found! - - //record new client ip - g_stpcpy(ConnectedClients[i].client_ip,ClientIpAd); - ConnectedClients[i].ClientTcpData = connection_in; - // - ConnectedClients[i].used = 1; - if (verbose) { - printf("App Server: New client added to client list %s\n", ConnectedClients[i].client_ip); - fflush(stdout); - } - break; - } -} - -//Get aircraft name and color -int get_ac_data(char* InStr, char* RetBuf) { - const char delimiters[] = " "; - int AcID; - char StrBuf[strlen(InStr)]; - - //Make writable copy - strcpy(StrBuf, InStr); - strtok(StrBuf, delimiters); - - //Get id from command string - AcID = atoi(strtok(NULL, delimiters)); - //Get & create return string - if ( AcID > 0 ) { - //Dont search it, it is thereeee :) - sprintf(RetBuf, "AppServer ACd %d %s %s %s %d %d %d\n", AcID, - DevNames[AcID].name, - DevNames[AcID].type, - DevNames[AcID].color, - DevNames[AcID].dl_launch_ind, - DevNames[AcID].kill_thr_ind, - DevNames[AcID].flight_altitude_ind - ); - } - return AcID; -} - -//Get aircraft name waypoint names,ids -int get_wp_data(char* InStr, char* RetBuf) { - //Input command - const char delimiters[] = " "; - int AcID; - char StrBuf[strlen(InStr)]; - - //Make writable copy - strcpy(StrBuf, InStr); - strtok(StrBuf, delimiters); - - //Get id from command string - AcID = atoi(strtok(NULL, delimiters)); - //Get & create return string - if ( AcID > 0 ) { - //create wp data string - char WpStr[MAXWPNUMB*(MAXWPNAMELEN+8)] = ""; - char i = 1; - while ( strlen(DevNames[AcID].AcWp[i].Wp_Name) > 0 ) { - //Read & add wp data to return string - char wp_str[MAXWPNAMELEN+8] = ""; - snprintf(wp_str, MAXWPNAMELEN+8, "%d,%s ", i, DevNames[AcID].AcWp[i].Wp_Name); - strcat(WpStr, wp_str); - i++; - } - sprintf(RetBuf, "AppServer WPs %d %s\n", AcID, WpStr); - } - return AcID; -} - -//Get block names,ids -int get_bl_data(char* InStr, char* RetBuf) { - //Input command - const char delimiters[] = " "; - int AcID; - char StrBuf[strlen(InStr)]; - - //Make writable copy - strcpy(StrBuf, InStr); - strtok(StrBuf, delimiters); - - //Get id from command string - AcID = atoi(strtok (NULL, delimiters)); - //Get & create return string - if ( AcID > 0 ) { - //create wp data string - char BlStr[MAXWPNUMB*(MAXWPNAMELEN+5)] =""; - int i=1; - while ( strlen(DevNames[AcID].AcBl[i].Bl_Name) > 0 ) { - char bl_str[MAXWPNAMELEN+5] =""; - snprintf(bl_str, MAXWPNAMELEN+5, "%s", DevNames[AcID].AcBl[i].Bl_Name); - strcat(BlStr, bl_str); - i++; - } - sprintf(RetBuf, "AppServer BLs %d %s\n", AcID, BlStr); - } - return AcID; -} - -//Bfoadcast ivy msgs to clients -void broadcast_to_clients () { - - int i; - - if (uTCP) { - //broadcast using tcp connection - GError *error = NULL; - - for (i = 0; i < MAXCLIENT; i++) { - if (ConnectedClients[i].used > 0) { - GOutputStream * ostream = g_io_stream_get_output_stream (ConnectedClients[i].ClientTcpData); - g_output_stream_write(ostream, ivybuffer, strlen(ivybuffer), NULL, &error); - } - - } - return; - } - - i=0; - for (i = 0; i < MAXCLIENT; i++) { - if (ConnectedClients[i].used > 0) { - - //Send data - GInetAddress *udpAddress; - GSocketAddress *udpSocketAddress; - GSocket *udpSocket; - - udpAddress = g_inet_address_new_from_string(ConnectedClients[i].client_ip); - - udpSocketAddress = g_inet_socket_address_new(udpAddress, udp_port); - - udpSocket = g_socket_new(G_SOCKET_FAMILY_IPV4, G_SOCKET_TYPE_DATAGRAM, 17, NULL); - - if (g_socket_connect(udpSocket, udpSocketAddress, NULL, NULL) == FALSE) { - printf("App Server: stg wrong with socket connect\n"); - fflush(stdout); - } - if (g_socket_send(udpSocket, ivybuffer, strlen(ivybuffer) , NULL, NULL) < 0) { - printf("App Server: stg wrong with send func\n"); - fflush(stdout); - } - if (g_socket_close(udpSocket, NULL) == FALSE) { - printf("App Server: stg wrong with socket close\n"); - fflush(stdout); - } - //Unref objects - g_object_unref(udpAddress); - g_object_unref(udpSocketAddress); - g_object_unref(udpSocket); - } - } - -} - -//Read tcp requests of connected clients -gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) { - - GString *s = g_string_new(NULL); - GError *error = NULL; - GIOStatus ret = g_io_channel_read_line_string(source, s, NULL, &error); - - if (ret == G_IO_STATUS_ERROR) { - //unref connection - GSocketAddress *sockaddr = g_socket_connection_get_remote_address(data, NULL); - GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr)); - //Read sender ip - if (verbose) { - printf("App Server: Communication error.. Removing client.. ->%s\n", g_inet_address_to_string(addr)); - fflush(stdout); - } - //Remove client - remove_client(g_inet_address_to_string(addr)); - //Free objects - g_string_free(s, TRUE); - g_object_unref(sockaddr); - g_object_unref(addr); - g_object_unref(data); - //Return false to stop listening this socket - return FALSE; - } - else{ - //Read request command - gchar *RecString; - RecString=s->str; - //check client password - if ((strncmp(RecString, AppPass, strlen(AppPass))) == 0) { - //Password ok can send command - GString *incs = g_string_new(s->str); - incs = g_string_erase(s,0,(strlen(AppPass)+1)); - IvySendMsg("%s",incs->str); - if (verbose) { - printf("App Server: Command passed to ivy: %s\n",incs->str); - fflush(stdout); - } - } - //AC data request. (Ignore client password) - else if ((strncmp(RecString, "getac ", strlen("getac "))) == 0) { - //AC data request - char AcData[BUFLEN]; - //Read ac data - if (get_ac_data(RecString, AcData)) { - //Send requested data to client - GOutputStream * ostream = g_io_stream_get_output_stream (data); - g_output_stream_write(ostream, AcData, strlen(AcData), NULL, &error); - } - } - //Waypoint data request (Ignore client password) - else if ((strncmp(RecString, "getwp ", strlen("getwp "))) == 0) { - char AcData[BUFLEN]; - //Read wp data of ac - if (get_wp_data(RecString, AcData)) { - //Send requested data to client - GOutputStream * ostream = g_io_stream_get_output_stream (data); - g_output_stream_write(ostream, AcData, strlen(AcData), NULL, &error); - } - } - //Waypoint data request (Ignore client password) - else if ((strncmp(RecString, "getbl ", strlen("getbl "))) == 0) { - char AcData[BUFLEN]; - //Read block data of AC - if (get_bl_data(RecString, AcData)) { - //Send requested data to client - GOutputStream * ostream = g_io_stream_get_output_stream (data); - g_output_stream_write(ostream, AcData, strlen(AcData), NULL, &error); - } - } - - //If client sends removeme command, remove it from broadcast list - else if ((strncmp( RecString, "removeme", strlen("removeme"))) == 0) { - GSocketAddress *sockaddr = g_socket_connection_get_remote_address(data, NULL); - GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr)); - //Read sender ip - if (verbose) { - printf("App Server: need to remove %s\n", g_inet_address_to_string(addr)); - fflush(stdout); - } - //Remove client - remove_client(g_inet_address_to_string(addr)); - //Free objects - g_string_free(s, TRUE); - g_object_unref(sockaddr); - g_object_unref(addr); - g_object_unref(data); - //Return false to stop listening this socket - return FALSE; - } - else { - //Unknown command - if (verbose) { - printf("App Server: Client send an unknown command or wrong password: (%s)\n",RecString); - fflush(stdout); - } - } - } - - if (ret == G_IO_STATUS_EOF) { - //Client disconnected - if (verbose) { - GSocketAddress *sockaddr = g_socket_connection_get_remote_address(data, NULL); - GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr)); - printf("App Server: Client disconnected without saying 'bye':( ->%s\n", g_inet_address_to_string(addr)); - remove_client(g_inet_address_to_string(addr)); - fflush(stdout); - } - g_string_free(s, TRUE); - //Unref the socket and return false to allow the client to reconnect - g_object_unref(data); - return FALSE; - } - //None above.. Keep listening the socket - g_string_free(s, TRUE); - return TRUE; -} - -//New tcp conection -gboolean new_connection(GSocketService *service, GSocketConnection *connection, GObject *source_object, gpointer user_data) { - - //New client connected - GSocketAddress *sockaddr = g_socket_connection_get_remote_address(connection, NULL); - GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr)); - guint16 port = g_inet_socket_address_get_port(G_INET_SOCKET_ADDRESS(sockaddr)); - - if (verbose) { - printf("App Server: New Connection from %s:%d\n", g_inet_address_to_string(addr), port); - fflush(stdout); - } - - //Record client (if new) - add_client(g_inet_address_to_string(addr), connection); - - g_object_ref (connection); - GSocket *socket = g_socket_connection_get_socket(connection); - - gint fd = g_socket_get_fd(socket); - GIOChannel *channel = g_io_channel_unix_new(fd); - g_io_add_watch(channel, G_IO_IN, (GIOFunc) network_read, connection); - return TRUE; -} - -void request_ac_config(int ac_id_req) { - - RequestID++; - IvySendMsg("app_server %d_%d CONFIG_REQ %d" ,ProcessID, RequestID ,ac_id_req ); - //AC config requested.. - - if (verbose) { - printf("AC(id= %d) config requested.\n",ac_id_req); - fflush(stdout); - } -} - -//Ivy msg function -void Ivy_All_Msgs(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - - //For compatibility.. This will be joined in upcoming releases.. - if (uTCP) sprintf(ivybuffer, "%s\n", argv[0]); - else sprintf(ivybuffer, "%s", argv[0]); - - //Ivy msg received broadcast to clients.. - broadcast_to_clients(); - -} - -//Parse AC flight plan xml (block & waypoint names -void parse_ac_fp(int DevNameIndex, char *filename) { - xmlTextReaderPtr reader; - int ret; - - reader = xmlReaderForFile(filename, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */ - - xmlChar *name, *value; - if (reader != NULL) { - ret = xmlTextReaderRead(reader); - - int wp_queue=1; - int bl_queue=1; - while (ret == 1) { - name = xmlTextReaderName(reader); - if (name == NULL) { - name = xmlStrdup(BAD_CAST "--"); - } - - //read waypoint names - if (xmlStrEqual(name, (const xmlChar *)"waypoint")) { - //waypoint node read name attr. - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); - value = xmlTextReaderValue(reader); - //copy it to DevNames[] structure - my_strncpy(DevNames[DevNameIndex].AcWp[wp_queue].Wp_Name, (char *) value, MAXWPNAMELEN); - wp_queue++; - } - - if (xmlStrEqual(name, (const xmlChar *)"block")) { - //Read block names - if ( xmlTextReaderAttributeCount(reader) >= 1 ) { - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); - value = xmlTextReaderValue(reader); - my_strncpy(DevNames[DevNameIndex].AcBl[bl_queue].Bl_Name, (char *) value, MAXWPNAMELEN); - bl_queue++; - } - } - - ret = xmlTextReaderRead(reader); - } - - xmlFreeTextReader(reader); - if (ret != 0) { - if (verbose) { - printf("App Server: failed to parse %s\n", filename); - fflush(stdout); - } - } - } - else - { - if (verbose) { - printf("App Server: Unable to open %s\n", filename); - fflush(stdout); - } - } - -} - -//check firmware type -//return true if "acceptable" type (e.g. fixedwing or rotorcraft) -int check_firmware_type (xmlChar* firmware) { - return (xmlStrEqual(firmware, (const xmlChar *)"fixedwing") - || xmlStrEqual(firmware, (const xmlChar *)"rotorcraft")); -} - -void parse_ac_af(int DevNameIndex, char *filename) { - xmlTextReaderPtr reader; - int ret; - - reader = xmlReaderForFile(filename, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */ - - xmlChar *name, *value; - if (reader != NULL) { - ret = xmlTextReaderRead(reader); - - while (ret == 1) { - name = xmlTextReaderName(reader); - - if (name == NULL) { - name = xmlStrdup(BAD_CAST "--"); - } - - if (xmlStrEqual(name, (const xmlChar *)"firmware")) { - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); - value = xmlTextReaderValue(reader); - //check if firmware name is accaptable - if (check_firmware_type(value)>0) { - strcpy(DevNames[DevNameIndex].type, (char *) value); - } - - } - - if (xmlStrEqual(name, (const xmlChar *)"define")) { - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); - value = xmlTextReaderValue(reader); - - if (xmlStrEqual(value, (const xmlChar *)"AC_ICON")) { - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"value"); - value = xmlTextReaderValue(reader); - my_strncpy(DevNames[DevNameIndex].type, (char *) value, MAXNAMELENGTH); - return; - } - } - - ret = xmlTextReaderRead(reader); - } - - xmlFreeTextReader(reader); - if (ret != 0) { - if (verbose) { - printf("App Server: failed to parse %s\n", filename); - fflush(stdout); - } - } - } else { - if (verbose) { - printf("App Server: Unable to open %s\n", filename); - fflush(stdout); - } - } - -} - -//Parse dl values -void parse_ac_settings(int DevNameIndex, char *filename) { - - xmlTextReaderPtr reader; - int ret; - - reader = xmlReaderForFile(filename, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */ - - // Init some variables (-1 means no settings in xml file) - DevNames[DevNameIndex].dl_launch_ind = -1; - DevNames[DevNameIndex].kill_thr_ind = -1; - DevNames[DevNameIndex].flight_altitude_ind = -1; - - xmlChar *name, *value; - if (reader != NULL) { - ret = xmlTextReaderRead(reader); - - int valind = 0; - while (ret == 1) { - name = xmlTextReaderName(reader); - if (name == NULL) { - name = xmlStrdup(BAD_CAST "--"); - } - - if (xmlStrEqual(name, (const xmlChar *)"dl_setting")) { - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"var"); - - value = xmlTextReaderValue(reader); - if (xmlStrEqual(value, (const xmlChar *)"autopilot.launch")) { - DevNames[DevNameIndex].dl_launch_ind=valind; - } - if (xmlStrEqual(value, (const xmlChar *)"autopilot.kill_throttle")) { - DevNames[DevNameIndex].kill_thr_ind=valind; - } - if (xmlStrEqual(value, (const xmlChar *)"flight_altitude")) { - DevNames[DevNameIndex].flight_altitude_ind=valind; - } - xmlTextReaderNext(reader); - valind+=1; - } - ret = xmlTextReaderRead(reader); - } - xmlFreeTextReader(reader); - if (ret != 0) { - if (verbose) { - printf("App Server: %s : failed to parse\n", filename); - fflush(stdout); - } - } - } - else { - if (verbose) { - printf("App Server: Unable to open %s\n", filename); - fflush(stdout); - } - } -} - -void on_app_server_NEW_AC(IvyClientPtr app, void *user_data, int argc, char *argv[]) { - //Request config - request_ac_config(atoi(argv[0])); -} - -void on_app_server_GET_CONFIG (IvyClientPtr app, void *user_data, int argc, char *argv[]) { - - int i=0; - - int RmId[2]; - /* - * RmId[0] >> ProcessID of incoming MsgProcessId - * RmId[1] >> RequestID of incoming MsgProcessId - */ - - //Split arg0 to get process id and request id - char * mtok; - mtok = strtok (argv[0],"_"); - while (mtok != NULL || i<2) - { - RmId[i]= atoi(mtok); - i++; - mtok = strtok (NULL, "_"); - } - - //Check whether process id and request id matches or not - if ( RmId[0]== ProcessID && RmId[1] <= RequestID) { - int inc_device_id=atoi(argv[1]); - - //Save Device Name - my_strncpy(DevNames[inc_device_id].name, argv[7], MAXNAMELENGTH); - - //Save color - my_strncpy(DevNames[inc_device_id].color, argv[6], MAXWPNAMELEN); - - //Save Flight Plan Path - //check if file is local - if ((strncmp( argv[2], "file://", strlen("file://"))) == 0) { - sprintf(DevNames[inc_device_id].flight_plan_path, "%s", argv[2]+7); - } - else { - printf("App Server: App server only works with local files! (Flight Plan Path:%s)\n", argv[2]); - return; - } - - //Save Airframe Path - //check if file is local - if ((strncmp( argv[3], "file://", strlen("file://"))) == 0) { - sprintf(DevNames[inc_device_id].airframe_path, "%s", argv[3]+7); - } - else { - printf("App Server: App server works only with local files! (Airframe Path:%s)\n", argv[3]); - return; - } - - //Save Settings Path - //check if file is local - if ((strncmp( argv[5], "file://", strlen("file://"))) == 0) { - sprintf(DevNames[inc_device_id].settings_path, "%s", argv[5]+7); - } - else { - printf("App Server: App server works only with local files! (Settings Path:%s)\n", argv[5]); - return; - } - - // Init some variables (-1 means no settings in xml file) - DevNames[inc_device_id].dl_launch_ind = -1; - DevNames[inc_device_id].kill_thr_ind = -1; - DevNames[inc_device_id].flight_altitude_ind = -1; - - //Parse airframe files.. - parse_ac_af(inc_device_id, DevNames[inc_device_id].airframe_path); - - //Parse flight plan - parse_ac_fp(inc_device_id, DevNames[inc_device_id].flight_plan_path); - - //Parse settings - parse_ac_settings(inc_device_id, DevNames[inc_device_id].settings_path); - - if (verbose) { - printf("%s configuration saved. : (Id: %d Color:%s)\n", DevNames[inc_device_id].name,inc_device_id, DevNames[inc_device_id].color); - fflush(stdout); - printf("\tFlight_P Path:\t %s \n", DevNames[inc_device_id].flight_plan_path); - fflush(stdout); - printf("\tAirframe Path:\t %s \n", DevNames[inc_device_id].airframe_path); - fflush(stdout); - printf("\tSettings Path:\t %s \n", DevNames[inc_device_id].settings_path); - fflush(stdout); - } - //everything is awesome! AC data is ready to be served. - - } - -} - -void on_app_server_AIRCRAFTS (IvyClientPtr app, void *user_data, int argc, char *argv[]) { - - int i=0; - int RmId[2]; - - /* - * RmId[0] >> ProcessID of incoming MsgProcessId - * RmId[1] >> RequestID of incoming MsgProcessId - */ - - //Split arg0 to get process id and request id - char * mtok; - mtok = strtok (argv[0],"_"); - while (mtok != NULL || i<2) - { - RmId[i]= atoi(mtok); - i++; - mtok = strtok (NULL, "_"); - } - - //Check whether process id and request id matches or not - if ( RmId[0]== ProcessID && RmId[1] <= RequestID) { - i=0; - char * mtok2; - mtok2 = strtok (argv[1],","); - - while (mtok2 != NULL || i> MAXDEVICENUMB) { - request_ac_config(atoi(mtok2)); - mtok2 = strtok (NULL, ","); - i++; - } - - } - -} - -// Print help message -void print_help() { - printf("Usage: app_server [options]\n"); - printf(" Options :\n"); - printf(" -t \tfor receiving devices commands (default: %d)\n", tcp_port); - printf(" -u \tfor sending AC data (default: %d)\n", udp_port); - printf(" -b \tdefault is %s\n", defaultIvyBus); - printf(" -p \tpassword for connection with control capabilities (default is %s)\n", defaultAppPass); - printf(" -utcp \t\tUse TCP communication to send ivy messages (default: UDP )\n"); - printf(" -v\tverbose\n"); - printf(" -h --help show this help\n"); -} - -gboolean request_ac_list(gpointer data) { - RequestID++; - IvySendMsg("app_server %d_%d AIRCRAFTS_REQ" ,ProcessID, RequestID); - return FALSE; -} - -int main(int argc, char **argv) { - int i; - - //Get process id - ProcessID= getpid(); - // default password - - AppPass = defaultAppPass; - - // try environment variable first, set to default if failed - IvyBus = getenv("IVYBUS"); - if (IvyBus == NULL) IvyBus = defaultIvyBus; - - // Parse options - for (i = 1; i < argc; ++i) { - if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) { - print_help(); - exit(0); - } - else if (strcmp(argv[i], "-t") == 0) { - tcp_port = atoi(argv[++i]); - } - else if (strcmp(argv[i], "-u") == 0) { - udp_port = atoi(argv[++i]); - } - else if (strcmp(argv[i], "-b") == 0) { - IvyBus = argv[++i]; - } - else if (strcmp(argv[i], "-p") == 0) { - AppPass = argv[++i]; - } - else if (strcmp(argv[i], "-v") == 0) { - verbose = 1; - } - else if (strcmp(argv[i], "-utcp") == 0) { - uTCP = 1; - } - else { - printf("App Server: Unknown option\n"); - print_help(); - exit(0); - } - } - - if (verbose) { - printf("### Paparazzi App Server ###\n"); - printf("Server listen port (TCP) : %d\n", tcp_port); - if (uTCP) { - printf("Server using TCP communication..\n"); - }else{ - printf("Server broadcast port (UDP) : %d\n", udp_port); - } - printf("Control Pass : %s\n", AppPass); - printf("Ivy Bus : %s\n", IvyBus); - fflush(stdout); - } - - - //Create tcp listener -#if !GLIB_CHECK_VERSION (2, 35, 1) - // init GLib type system (only for older version) - g_type_init(); -#endif - GSocketService *service = g_socket_service_new(); - - GInetAddress *address = g_inet_address_new_any(G_SOCKET_FAMILY_IPV6); //G_SOCKET_FAMILY_IPV4 could be used - GSocketAddress *socket_address = g_inet_socket_address_new(address, tcp_port); - //Add listener - g_socket_listener_add_address(G_SOCKET_LISTENER(service), socket_address, G_SOCKET_TYPE_STREAM, - G_SOCKET_PROTOCOL_TCP, NULL, NULL, NULL); - - g_object_unref(socket_address); - g_object_unref(address); - g_socket_service_start(service); - - //Connect listening signal - g_signal_connect(service, "incoming", G_CALLBACK(new_connection), NULL); - - //Here comes the ivy bindings - IvyInit ("PPRZ_App_Server", "Papparazzi App Server Ready!", NULL, NULL, NULL, NULL); - - IvyBindMsg(Ivy_All_Msgs, NULL, "(^ground (\\S*) (\\S*) .*)"); - IvyBindMsg(on_app_server_NEW_AC, NULL, "ground NEW_AIRCRAFT (\\S*)"); - IvyBindMsg(on_app_server_GET_CONFIG, NULL, "(\\S*) ground CONFIG (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_app_server_AIRCRAFTS, NULL, "(\\S*) ground AIRCRAFTS (\\S*)"); - IvyStart(IvyBus); - - GMainLoop *loop = g_main_loop_new(NULL, FALSE); - - if (verbose) { - printf("Starting App Server\n"); - fflush(stdout); - } - IvySendMsg("app_server"); - - g_timeout_add(100, request_ac_list, NULL); - - g_main_loop_run(loop); - - if (verbose) { - printf("Stoping App Server\n"); - fflush(stdout); - } - return 0; -} diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile index d3fd870d3a..b9b14c0406 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -35,14 +35,14 @@ ifeq ($(USE_LABELGTK),lablgtk2) ifeq ($(LABLGTK2GLADE),) XPKG = -package lablgtk2,pprz.xlib XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib,pprzlink,$(USE_LABELGTK) -all : play plotter sd2log plotprofile openlog2tlm sdlogger_download +all : play plotter sd2log openlog2tlm sdlogger_download else XPKG = -package lablgtk2,pprz.xlib,lablgtk2.glade XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib,pprzlink,$(USE_LABELGTK) -all : play plotter logplotter sd2log plotprofile openlog2tlm sdlogger_download +all : play plotter logplotter sd2log openlog2tlm sdlogger_download endif else # no gtk2 -all: sd2log plotprofile openlog2tlm sdlogger_download +all: sd2log openlog2tlm sdlogger_download endif @@ -107,50 +107,13 @@ gtk_export.ml : export.glade CC = gcc CFLAGS=-g -O2 -Wall -PCRE_LDFLAGS= $(shell pcre2-config --libs8) - openlog2tlm: openlog2tlm.c @echo CC $@ $(Q)$(CC) $(CFLAGS) -o $@ $^ -DISP3D_CFLAGS = $(shell pkg-config --cflags ivy-glib gtk+-2.0 gtkgl-2.0) -DISP3D_LDFLAGS = $(shell pkg-config --libs ivy-glib gtk+-2.0 gtkgl-2.0) $(PCRE_LDFLAGS) - -disp3d: disp3d.c - @echo CC $@ - $(Q)$(CC) $(CFLAGS) $(DISP3D_CFLAGS) -o $@ $^ $(DISP3D_LDFLAGS) - - -IVY_C_LIBS = $(shell pkg-config --libs ivy-glib) $(PCRE_LDFLAGS) -GLIBIVY_CFLAGS = $(shell pkg-config --cflags ivy-glib) - -# -# try to find include locations on OSX -# -UNAME = $(shell uname -s) -ifeq ("$(UNAME)","Darwin") - IVY_C_LIB_INC = $(shell if test -d /opt/paparazzi/lib; then echo "-L/opt/paparazzi/lib"; elif test -d /opt/local/lib; then echo "-L/opt/local/lib"; fi) - IVY_C_INCLUDES = $(shell if test -d /opt/paparazzi/include; then echo "-I/opt/paparazzi/include"; elif test -d /opt/local/include; then echo "-I/opt/local/include"; fi) - GLIBIVY_CFLAGS += $(IVY_C_LIB_INC) $(IVY_C_INCLUDES) -endif - - -plotprofile: plotprofile.c - @echo CC $@ - $(Q)$(CC) $(CFLAGS) $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) - -ahrs2fg: ahrs2fg.c network.c flight_gear.c utils.c - $(CC) $(CFLAGS) $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) -lm - - -tmclient: tmclient.c - $(CC) -g -O1 -Wall $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) - -ffjoystick: ffjoystick.c - $(CC) -g -O2 -Wall $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) -lm clean: - $(Q)rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play ahrs2fg logplotter plotter gtk_export.ml openlog2tlm disp3d plotprofile tmclient ffjoystick sd2log sdlogger_download + $(Q)rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play logplotter plotter gtk_export.ml openlog2tlm sd2log sdlogger_download .PHONY: all clean diff --git a/sw/logalizer/ahrs2fg.c b/sw/logalizer/ahrs2fg.c deleted file mode 100644 index 0688f5430f..0000000000 --- a/sw/logalizer/ahrs2fg.c +++ /dev/null @@ -1,193 +0,0 @@ -#include -#include -#include -#include -#include - -#include "flight_gear.h" -#include "network.h" - -#include -#include - -// Default Host -char default_fg_host[] = "127.0.0.1"; -char* fg_host; -// Default TCP port -int fg_port = 5501; - -#ifdef __APPLE__ -char defaultIvyBus[] = "224.255.255.255:2010"; -#else -char defaultIvyBus[] = "127.255.255.255:2010"; -#endif -char* IvyBus; - -int verbose = FALSE; - -// quat int res -const float quat_int_res = (float)(1<<15); -// euler int res -const float euler_int_res = (float)(1<<12); - -gfloat quat[4]; -gfloat eulers[3]; - -gboolean timeout_callback(gpointer data) { - - static double x = 0.; - static double y = 0.; - static double z = 10.; - const double earth_radius = 6372795.; - - double lat = 0.656480 + asin(x/earth_radius); - double lon = -2.135537 + asin(y/earth_radius); - - struct FGNetGUI gui; - net_gui_init(&gui); - - gui.latitude = lat; - gui.longitude = lon; - gui.altitude = z; - - if (verbose) { - printf("phi:%f, theta:%f, psi:%f\n", eulers[0], eulers[1], eulers[2]); - fflush(stdout); - } - gui.phi = eulers[0]; - gui.theta = eulers[1]; - gui.psi = eulers[2]; - - // net_gui_hton(&gui); - send_buf((struct FgNetChannel*)data, (char*)&gui, sizeof(gui)); - return TRUE; -} - -void quat_to_euler( gfloat* quat, gfloat* euler) { - // float q02 = quat[0] * quat[0]; - float q12 = quat[1] * quat[1]; - float q22 = quat[2] * quat[2]; - float q32 = quat[3] * quat[3]; - - euler[0] = atan2( 2*(quat[2]*quat[3] + quat[0]*quat[1]),(1-2*(q12 + q22)) ); - euler[1] = -asin(2*(quat[1]*quat[3] - quat[0]*quat[2])); - euler[2] = atan2( 2*(quat[1]*quat[2] + quat[0]*quat[3]),(1-2*(q22 + q32)) ); -} - -void on_ATTITUDE(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - float phi = atof(argv[0]); - float psi = atof(argv[1]); - float theta = atof(argv[2]); - eulers[0] = phi; - eulers[1] = theta; - eulers[2] = psi; -} - -void on_AHRS_EULER_INT(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - float phi = atof(argv[3]); - float theta = atof(argv[4]); - float psi = atof(argv[5]); - eulers[0] = phi / euler_int_res; - eulers[1] = theta / euler_int_res; - eulers[2] = psi / euler_int_res; -} - -void on_AHRS_QUAT_INT(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - float q0 = atof(argv[5]); - float q1 = atof(argv[6]); - float q2 = atof(argv[7]); - float q3 = atof(argv[8]); - quat[0] = q0 / quat_int_res; - quat[1] = q1 / quat_int_res; - quat[2] = q2 / quat_int_res; - quat[3] = q3 / quat_int_res; - quat_to_euler(quat, eulers); -} - -void on_ROTORCRAFT_FP(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - float phi = atof(argv[6]); - float theta = atof(argv[7]); - float psi = atof(argv[8]); - eulers[0] = phi / euler_int_res; - eulers[1] = theta / euler_int_res; - eulers[2] = psi / euler_int_res; -} - -// Print help message -void print_help() { - printf("Usage: ahrs2fg [options] \n"); - printf(" Options :\n"); - printf(" -fg_host \tFlight Gear host address (default: %s)\n", fg_host); - printf(" -fg_port \tFlight Gear port (default: %d)\n", fg_port); - printf(" -b \tdefault is %s\n", defaultIvyBus); - printf(" -v\tverbose\n"); - printf(" -h --help show this help\n"); -} - - -int main ( int argc, char** argv) { - - int ac_id = -1; - - IvyBus = getenv("IVYBUS"); - if (IvyBus == NULL) IvyBus = defaultIvyBus; - - fg_host = default_fg_host; - - if (argc < 1) { - print_help(); - exit(0); - } - - // Parse options - int i; - for (i = 1; i < argc; ++i) { - if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) { - print_help(); - exit(0); - } - else if (strcmp(argv[i], "-fg_host") == 0) { - fg_host = argv[++i]; - } - else if (strcmp(argv[i], "-fg_port") == 0) { - fg_port = atoi(argv[++i]); - } - else if (strcmp(argv[i], "-b") == 0) { - IvyBus = argv[++i]; - } - else if (strcmp(argv[i], "-v") == 0) { - verbose = TRUE; - } - else { - ac_id = atoi(argv[i]); - } - } - if (ac_id == -1) { - print_help(); - exit(0); - } - - if (verbose) { - printf("FG options: %s:%d\n", fg_host, fg_port); - fflush(stdout); - } - struct FgNetChannel* chan = open_out_channel(fg_host, fg_port); - - g_timeout_add(50, timeout_callback, chan); - - GMainLoop *ml = g_main_loop_new(NULL, FALSE); - - IvyInit ("ahrs2fg", "ahrs2fg READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_ATTITUDE, chan, "^%d ATTITUDE (\\S*) (\\S*) (\\S*)", ac_id); - IvyBindMsg(on_AHRS_EULER_INT, chan, "^%d AHRS_EULER_INT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id); - IvyBindMsg(on_AHRS_QUAT_INT, chan, "^%d AHRS_QUAT_INT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id); - IvyBindMsg(on_ROTORCRAFT_FP, chan, "^%d ROTORCRAFT_FP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id); - if (verbose) { - printf("Ivy options: %s\n", IvyBus); - fflush(stdout); - } - IvyStart(IvyBus); - - g_main_loop_run(ml); - return 0; -} diff --git a/sw/logalizer/disp3d.c b/sw/logalizer/disp3d.c deleted file mode 100644 index ce72e669ac..0000000000 --- a/sw/logalizer/disp3d.c +++ /dev/null @@ -1,555 +0,0 @@ -/* - * Copyright (C) 1998 Janne Löf - * - * This library is free software; you can redistribute it and/or - * modify it under the terms of the GNU Library General Public - * License as published by the Free Software Foundation; either - * version 2 of the License, or (at your option) any later version. - * - * This library 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 - * Library General Public License for more details. - * - * You should have received a copy of the GNU Library General Public - * License along with this library; if not, write to the Free - * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - - -/* Zktor is a word that does not mean anything and is difficult to pronounce. */ -/* I apologize for horrible coding. */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -static GLfloat yellow[4] = { 0.90, 0.90, 0.00, 1.00 }; - -void on_IMU_MAG(IvyClientPtr app, void *user_data, int argc, char *argv[]); -void on_IMU_ACCEL(IvyClientPtr app, void *user_data, int argc, char *argv[]); - -#define DegOfRad(r) (r * 180. / M_PI) - -#define AXIS_X 0 -#define AXIS_Y 1 -#define AXIS_Z 2 -#define AXIS_NB 3 - - -#define REFRESH_RATE 166 /* ms */ - -float biases[3]; -float quat[4]; -float eulers[3]; -float mag[3]; -float accel[3]; -float dcm00, dcm01, dcm02, dcm12, dcm22; - -int width = 640; -int height = 480; - - -/* game state */ - -GTimer *gtimer = NULL; - -double game_time; -double game_tick; -int draw_fast = 1; - - - -GLuint fontbase = 0; - - -void game_init() { - if (!gtimer) - gtimer = g_timer_new(); - g_timer_reset(gtimer); - - game_time = g_timer_elapsed(gtimer, NULL); - game_tick = 1.0 / 60; -} - -void game_play() -{ - int i; - double time_now,tick_now; - - /* timing */ - time_now = g_timer_elapsed(gtimer, NULL); - tick_now = time_now - game_time; - if (tick_now < 0.001) tick_now = 0.001; - if (tick_now > 0.2 ) tick_now = 0.2; - game_tick = (tick_now + 4*game_tick)/5; /* average */ - game_time = time_now; - - -} - -void quat_to_euler( gfloat* quat, gfloat* euler) { - float q02 = quat[0] * quat[0]; - float q12 = quat[1] * quat[1]; - float q22 = quat[2] * quat[2]; - float q32 = quat[3] * quat[3]; - - euler[0] = atan2( 2*(quat[2]*quat[3] + quat[0]*quat[1]),(q02 - q12 - q22 + q32)); - euler[1] = -asin(2*(quat[1]*quat[3] - quat[0]*quat[2])); - euler[2] = atan2( 2*(quat[1]*quat[2] + quat[0]*quat[3]),(q02 + q12 - q22 - q32)); -} - - -float ahrs_heading_of_mag( const float* mag ) { - - const float cphi = cos( eulers[0] ); - const float sphi = sin( eulers[0] ); - const float ctheta = cos( eulers[1] ); - const float stheta = sin( eulers[1] ); - - const float mn = - ctheta* mag[0]+ - sphi*stheta* mag[1]+ - cphi*stheta* mag[2]; - const float me = - 0* mag[0]+ - cphi* mag[1]+ - -sphi* mag[2]; - - const float heading = -atan2( me, mn ); - return heading; -} - -float ahrs_roll_of_accel( const float* accel ) { - return atan2(accel[AXIS_Y], accel[AXIS_Z]); -} - -float ahrs_pitch_of_accel( float* accel) { - float g2 = - accel[AXIS_X]*accel[AXIS_X] + - accel[AXIS_Y]*accel[AXIS_Y] + - accel[AXIS_Z]*accel[AXIS_Z]; - return -asin( accel[AXIS_X] / sqrt( g2 ) ); -} - -float compute_euler_heading( void ) { - return atan2( dcm01, dcm00 ); -} - -void compute_DCM( void ) { - dcm00 = 1.0-2*(quat[2]*quat[2] + quat[3]*quat[3]); - dcm01 = 2*(quat[1]*quat[2] + quat[0]*quat[3]); - dcm02 = 2*(quat[1]*quat[3] - quat[0]*quat[2]); - dcm12 = 2*(quat[2]*quat[3] + quat[0]*quat[1]); - dcm22 = 1.0-2*(quat[1]*quat[1] + quat[2]*quat[2]); -} - - -void draw_heli ( void ) { - /* draw heli */ - glColor3f(1.0, 1.0, 0.0); - glPushMatrix(); - glRotatef(DegOfRad(eulers[AXIS_X]), 1., 0., 0.); - glRotatef(DegOfRad(eulers[AXIS_Y]), 0., 1., 0.); - glRotatef(DegOfRad(eulers[AXIS_Z]), 0., 0., 1.); - glBegin( GL_LINES ); - glVertex3f( 0.00, 1., 0. ); - glVertex3f( 10.00, 0.0, 0. ); - glVertex3f( 0.00, -1., 0. ); - glVertex3f( 10.00, 0.0, 0. ); - glVertex3f( 0.00, 1., 0. ); - glVertex3f( 0.00, -1.0, 0. ); - glVertex3f( 0.00, 0., 0. ); - glVertex3f( 10.00, 0., 0. ); - glVertex3f( 0.00, 0., 1. ); - glVertex3f( 10.00, 0., 0. ); - glVertex3f( 0.00, 0., 0. ); - glVertex3f( 0.00, 0., 1. ); - glEnd(); - glPopMatrix(); - - - /* untilt mag reading */ - float ahrs_mag_heading = ahrs_heading_of_mag(mag); - float ahrs_filt_heading = eulers[2]; - - /* display unfiltered mag heading */ - glColor3f(1., 0., 0.); - glPushMatrix(); - glRotatef(DegOfRad(ahrs_mag_heading) , 0., 0., 1.); - glBegin( GL_LINES ); - glVertex3f( 0., 0., 0. ); - glVertex3f( 5., 0 , 0); - glEnd(); - glPopMatrix(); - - - /* display filtered heading */ - glColor3f(0., 1., 1.); - glPushMatrix(); - glRotatef(DegOfRad(ahrs_filt_heading) , 0., 0., 1.); - glBegin( GL_LINES ); - glVertex3f( 0., 0., 0. ); - glVertex3f( 5., 0 , 0); - glEnd(); - glPopMatrix(); - - /* draw mag vector */ - glColor3f(0., 1.0, 1.0); - glPushMatrix(); - glRotatef(DegOfRad(eulers[0]), 1., 0., 0.); - glRotatef(DegOfRad(eulers[1]), 0., 1., 0.); - glRotatef(DegOfRad(eulers[2]), 0., 0., 1.); - float xmag = mag[0]/30.; - float ymag = mag[1]/30.; - float zmag = mag[2]/30.; - glBegin( GL_LINES ); - glVertex3f( 0., 0., 0. ); - glVertex3f( xmag, ymag, zmag ); - glEnd(); - glColor3f(0.5, 0.5, 0.5); - glBegin( GL_LINES ); - glVertex3f( xmag, ymag, zmag ); - glVertex3f( xmag, ymag, 0 ); - glVertex3f( xmag, ymag, 0 ); - glVertex3f( 0., 0., 0. ); - glEnd(); - glPopMatrix(); - - - /* draw accel */ - float ahrs_accel_phi = DegOfRad(ahrs_roll_of_accel(accel)); - float ahrs_accel_theta = DegOfRad(ahrs_pitch_of_accel(accel)); - - glColor3f(0.6, .6, 1.0); - glPushMatrix(); - glRotatef(ahrs_accel_phi, 1., 0., 0.); - glRotatef(ahrs_accel_theta, 0., 1., 0.); - // glRotatef(DegOfRad(eulers[2]), 0., 0., 1.); - glBegin( GL_LINES ); - glVertex3f( 0., 0., 0. ); - glVertex3f( accel[0]/4., accel[1]/4., accel[2]/4. ); - glEnd(); - glPopMatrix(); - - printf("mag heading %f ac heading %f err %f\n\n", DegOfRad(ahrs_mag_heading), DegOfRad(eulers[2]), - DegOfRad(ahrs_mag_heading) - DegOfRad(eulers[2])); - - -} - -#define CAM_POS 10.,10.,0. -//#define CAM_POS 0.,-10., -30. -#define HELI_POS 0., 0., 0. -//#define UP 0, 1, 0 -#define UP -1, 0, 0 - -void game_render() -{ - int i; - - /* drawmode */ - if (draw_fast) { - glDisable(GL_LINE_SMOOTH); - glDisable(GL_POINT_SMOOTH); - glDisable(GL_BLEND); - } else { - glEnable(GL_LINE_SMOOTH); - glEnable(GL_POINT_SMOOTH); - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - } - - - glClearColor(0., 0., 0.2, 0.); - glClear(GL_COLOR_BUFFER_BIT); - - glViewport(0,0,width,height); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - // glOrtho(-10.0, 10.0, -10.0, 10.0, -50.0, 50.0); - gluPerspective(45.0f,(GLfloat)width/(GLfloat)height,1.0f,1000.0f); - glEnable(GL_DEPTH_TEST); - glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); - - - - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); - // glTranslatef(HELI_POS); - gluLookAt(CAM_POS, HELI_POS, UP); - -#define NED - /* switch to nort east down */ -#ifdef NED - glPushMatrix(); - glRotatef(90., 1., 0., 0.); - glRotatef(180., 0., 0., 1.); -#endif - - /* draw frame */ - - quat_to_euler(quat, eulers); - - glColor3f(1.0, 0.0, 0.0); - glBegin( GL_LINES ); - glVertex3f( 0., 0., 0. ); - glVertex3f( 10., 0., 0. ); - glEnd(); - glColor3f(.0, 1.0, 0.0); - glBegin( GL_LINES ); - glVertex3f( 0., 0., 0. ); - glVertex3f( 0., 10., 0. ); - glEnd(); - glColor3f(0.0, 0.0, 0.9f); - glBegin( GL_LINES ); - glVertex3f( 0., 0., 0. ); - glVertex3f( 0., 0., 10. ); - glEnd(); - - compute_DCM(); - - draw_heli(); - - -#ifdef NED - glPopMatrix(); -#endif - -/* glViewport(0,0, width, height); */ -/* glMatrixMode(GL_PROJECTION); */ -/* glLoadIdentity(); */ -/* gluOrtho2D(0,100, 100,0); */ -/* glMatrixMode(GL_MODELVIEW); */ -/* glLoadIdentity(); */ - - /* textual info */ -/* if (fontbase) { */ -/* char s[200]; */ -/* g_snprintf(s, sizeof(s), "magnetometer : mx -> %d, my -> %d, mz -> %d", mx, my, mz); */ - -/* glColor3f(.8,.8,.8); */ -/* glRasterPos2f(-90, 90); */ -/* glListBase(fontbase); */ -/* glCallLists(strlen(s), GL_UNSIGNED_BYTE, s); */ - -/* } */ -} - - -gint init(GtkWidget *widget) -{ - /* OpenGL functions can be called only if makecurrent returns true */ - if (gtk_gl_area_make_current(GTK_GL_AREA(widget))) { - /* generate font display lists */ - GdkFont *font; - font = gdk_font_load("-adobe-helvetica-medium-r-normal--*-120-*-*-*-*-*-*"); - if (font) { - fontbase = glGenLists( 128 ); - gdk_gl_use_gdk_font(font, 0, 128, fontbase); - gdk_font_unref(font); - } - } - return TRUE; -} - - -/* When widget is exposed it's contents are redrawn. */ -gint draw(GtkWidget *widget, GdkEventExpose *event) -{ - /* Draw only last expose. */ - if (event->count > 0) - return TRUE; - - if (gtk_gl_area_make_current(GTK_GL_AREA(widget))) - game_render(); - - /* Swap backbuffer to front */ - gtk_gl_area_swapbuffers(GTK_GL_AREA(widget)); - - return TRUE; -} - -/* When glarea widget size changes, viewport size is set to match the new size */ -gint reshape(GtkWidget *widget, GdkEventConfigure *event) -{ - /* OpenGL functions can be called only if make_current returns true */ - if (gtk_gl_area_make_current(GTK_GL_AREA(widget))) - { - glViewport(0,0, widget->allocation.width, widget->allocation.height); - } - return TRUE; -} - - -gint animate(GtkWidget *glarea) -{ - game_play(); - gtk_widget_draw(GTK_WIDGET(glarea), NULL); - return TRUE; -} - -void on_IMU_MAG(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - int i; - // for (i=0; i< argc; i++) - // printf("[%s] ", argv[i]); - // printf("\n"); - - float mx = atoi(argv[0]); - float my = atoi(argv[1]); - float mz = atoi(argv[2]); - - mag[0] = mx; - mag[1] = my; - mag[2] = mz; - - // printf("mag %d %d %d\n", mx, my, mz); -} -void on_IMU_ACCEL(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - int i; - float ax = atoi(argv[0]); - float ay = atoi(argv[1]); - float az = atoi(argv[2]); - - accel[0] = ax; - accel[1] = ay; - accel[2] = az; -} - -void on_AHRS_STATE(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - float q0 = atof(argv[0]); - float q1 = atof(argv[1]); - float q2 = atof(argv[2]); - float q3 = atof(argv[3]); - float bx = atof(argv[4]); - float by = atof(argv[5]); - float bz = atof(argv[6]); - biases[0] = bx; - biases[1] = by; - biases[2] = bz; - quat[0] = q0; - quat[1] = q1; - quat[2] = q2; - quat[3] = q3; -} - -int main(int argc, char **argv) -{ - GtkWidget *window,*vbox,*logo,*glarea; - - /* Attribute list for gtkglarea widget. Specifies a - list of Boolean attributes and enum/integer - attribute/value pairs. The last attribute must be - GDK_GL_NONE. See glXChooseVisual manpage for further - explanation. - */ - int attrlist[] = { - GDK_GL_RGBA, - GDK_GL_RED_SIZE,1, - GDK_GL_GREEN_SIZE,1, - GDK_GL_BLUE_SIZE,1, - GDK_GL_DOUBLEBUFFER, - GDK_GL_NONE - }; - - /* initialize gtk */ - gtk_init(&argc, &argv); - - /* Check if OpenGL (GLX extension) is supported. */ - if (gdk_gl_query() == FALSE) { - g_print("OpenGL not supported\n"); - return 0; - } - - /* Create new top level window. */ - window = gtk_window_new( GTK_WINDOW_TOPLEVEL); - gtk_window_set_title(GTK_WINDOW(window), "ahrs3d"); - - /* Quit form main if got delete event */ - gtk_signal_connect(GTK_OBJECT(window), "delete_event", - GTK_SIGNAL_FUNC(gtk_main_quit), NULL); - - - /* You should always delete gtk_gl_area widgets before exit or else - GLX contexts are left undeleted, this may cause problems (=core dump) - in some systems. - Destroy method of objects is not automatically called on exit. - You need to manually enable this feature. Do gtk_quit_add_destroy() - for all your top level windows unless you are certain that they get - destroy signal by other means. - */ - gtk_quit_add_destroy(1, GTK_OBJECT(window)); - - - vbox = GTK_WIDGET(gtk_vbox_new(FALSE, 0)); - gtk_container_set_border_width(GTK_CONTAINER(vbox), 10); - - - logo = gtk_label_new("ahrs3d"); - - - /* Create new OpenGL widget. */ - glarea = GTK_WIDGET(gtk_gl_area_new(attrlist)); - /* Events for widget must be set before X Window is created */ - gtk_widget_set_events(GTK_WIDGET(glarea), - GDK_EXPOSURE_MASK); - /* set minimum size */ - /* gtk_widget_set_usize(GTK_WIDGET(glarea), 200,200); */ - /* set default size */ - gtk_gl_area_size(GTK_GL_AREA(glarea), 640,400); - - - /* Connect signal handlers */ - /* Redraw image when exposed. */ - gtk_signal_connect(GTK_OBJECT(glarea), "expose_event", - GTK_SIGNAL_FUNC(draw), NULL); - /* When window is resized viewport needs to be resized also. */ - gtk_signal_connect(GTK_OBJECT(glarea), "configure_event", - GTK_SIGNAL_FUNC(reshape), NULL); - /* Do initialization when widget has been realized. */ - gtk_signal_connect(GTK_OBJECT(glarea), "realize", - GTK_SIGNAL_FUNC(init), NULL); - /* construct widget hierarchy */ - gtk_container_add(GTK_CONTAINER(window),GTK_WIDGET(vbox)); - gtk_box_pack_start(GTK_BOX(vbox), logo, FALSE, FALSE, 0); - gtk_box_pack_start(GTK_BOX(vbox), glarea, TRUE, TRUE, 0); - - - - /* show all widgets */ - gtk_widget_show(GTK_WIDGET(glarea)); - gtk_widget_show(GTK_WIDGET(logo)); - gtk_widget_show(GTK_WIDGET(vbox)); - gtk_widget_show(window); - - /* set focus to glarea widget */ - GTK_WIDGET_SET_FLAGS(glarea, GTK_CAN_FOCUS); - gtk_widget_grab_focus(GTK_WIDGET(glarea)); - - /* animating */ - // gtk_idle_add((GtkFunction)animate, glarea); - g_timeout_add(REFRESH_RATE, (GtkFunction)animate, glarea); - - IvyInit ("IvyGtkButton", "IvyGtkButton READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_IMU_MAG, NULL, "^77 IMU_MAG (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_IMU_ACCEL, NULL, "^77 IMU_ACCEL (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_AHRS_STATE, NULL, "^77 AHRS_STATE (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); - - game_init(); - gtk_main(); - - - return 0; -} diff --git a/sw/logalizer/ffjoystick.c b/sw/logalizer/ffjoystick.c deleted file mode 100644 index d57496a415..0000000000 --- a/sw/logalizer/ffjoystick.c +++ /dev/null @@ -1,167 +0,0 @@ -/* - * based on ffmvforce.c - * - */ - -/* - * 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 of the License, 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 this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * You can contact the author by email at this address: - * Johann Deneux - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define BIT(x) (1<<(x)) -#define STR_LEN 64 -#define PPRZ_MAX 9600 -#define PPRZ_MIN (-PPRZ_MAX) -#define max(a,b) ((a)>(b)?(a):(b)) - -/* File descriptor of the force feedback /dev entry */ -static int ff_fd; -static struct ff_effect effect; - -static void welcome() -{ - const char* txt[] = { -"ffjoystick: generate force feedback reaction from Paparazzi autopilot", -"Set COMMANDS message repeat rate low in telemetry and" -"use aircraft with rudder on channel 1 and elevator on 2", -"USE WITH CARE !!! HOLD STRONGLY YOUR WHEEL OR JOYSTICK TO PREVENT DAMAGES", -"To run this program, run it with at least one argument.", -"", -NULL }; - - const char** p = txt; - - while (*p) { - printf("%s\n", *p); - p++; - } -} - -static void generate_force(double nx, double ny) -{ - static int first = 1; - double angle; - - angle = atan2(nx, -ny); -//printf("n: %4.2f %4.2f angle: %4.2f\n", nx, ny, angle); - effect.type = FF_CONSTANT; - effect.u.constant.level = 0x7fff * max(fabs(nx), fabs(ny)); - effect.direction = 0x8000 * (angle + M_PI)/M_PI; -//printf("level: %04x direction: %04x\n", (unsigned int)effect.u.constant.level, (unsigned int)effect.direction); - effect.u.constant.envelope.attack_length = 0; - effect.u.constant.envelope.attack_level = 0; - effect.u.constant.envelope.fade_length = 0; - effect.u.constant.envelope.fade_level = 0; - effect.trigger.button = 0; - effect.trigger.interval = 0; - effect.replay.length = 0xffff; - effect.replay.delay = 0; - - if (first) { - effect.id = -1; - } - - if (ioctl(ff_fd, EVIOCSFF, &effect) < 0) { -/* If updates are sent to frequently, they can be refused */ - } - - /* If first time, start to play the effect */ - if (first) { - struct input_event play; - play.type = EV_FF; - play.code = effect.id; - play.value = 1; - - if (write(ff_fd, (const void*) &play, sizeof(play)) == -1) { - perror("Play effect"); - exit(1); - } - } - - first = 0; -} - -void on_commands(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - - char* start = argv[0]; - char* stop; - int x, y; - - strtol(start, &stop, 10); - start=stop+1; - x = -strtol(start, &stop, 10); - start=stop+1; - y = strtol(start, &stop, 10); - - if (x>PPRZ_MAX) x=PPRZ_MAX; - if (xPPRZ_MAX) y=PPRZ_MAX; - if (y -#include -#include - -#include "utils.h" - -void net_fdm_ntoh (struct FGNetFDM* fdm) { - fdm->version = ntohl(fdm->version); - htond(&fdm->latitude); - htond(&fdm->longitude); - htond(&fdm->altitude); - htonf(&fdm->agl); - - htonf(&fdm->phi); - htonf(&fdm->theta); - htonf(&fdm->psi); - htonf(&fdm->alpha); - htonf(&fdm->beta); - - htonf(&fdm->phidot); - htonf(&fdm->thetadot); - htonf(&fdm->psidot); - htonf(&fdm->vcas); - htonf(&fdm->climb_rate); - - htonf(&fdm->v_north); - htonf(&fdm->v_east); - htonf(&fdm->v_down); - htonf(&fdm->v_wind_body_north); - htonf(&fdm->v_wind_body_east); - htonf(&fdm->v_wind_body_down); - - htonf(&fdm->A_X_pilot); - htonf(&fdm->A_Y_pilot); - htonf(&fdm->A_Z_pilot); - htonf(&fdm->stall_warning); - htonf(&fdm->slip_deg); - - fdm->num_engines = ntohl(fdm->num_engines); - int i; - for (i=0; ieng_state[i] = ntohl(fdm->eng_state[i]); - htonf(&fdm->rpm[i]); - htonf(&fdm->fuel_flow[i]); - htonf(&fdm->egt[i]); - htonf(&fdm->cht[i]); - htonf(&fdm->mp_osi[i]); - htonf(&fdm->tit[i]); - htonf(&fdm->oil_temp[i]); - htonf(&fdm->oil_px[i]); - } - - fdm->num_tanks = ntohl(fdm->num_tanks); - for (i=0; ifuel_quantity[i]); - } - - fdm->num_wheels = ntohl(fdm->num_wheels); - for (i=0; iwow[i] = ntohl(fdm->wow[i]); - htonf(&fdm->gear_pos[i]); - htonf(&fdm->gear_steer[i]); - htonf(&fdm->gear_compression[i]); - } - - fdm->cur_time = ntohl(fdm->cur_time); - fdm->warp = ntohl(fdm->warp); - - htonf(&fdm->visibility); - - htonf(&fdm->elevator); - htonf(&fdm->elevator_trim_tab); - htonf(&fdm->left_flap); - htonf(&fdm->right_flap); - htonf(&fdm->left_aileron); - htonf(&fdm->right_aileron); - htonf(&fdm->rudder); - htonf(&fdm->nose_wheel); - htonf(&fdm->speedbrake); - htonf(&fdm->spoilers); - -} - -void net_fdm_dump (struct FGNetFDM* fdm) { - printf("net_fdm (version %d size %d)\n",fdm->version, sizeof( *fdm)); - printf(" lat, lon, alt, agl\n [%f %f %f %f]\n", - fdm->latitude, fdm->longitude, fdm->altitude, fdm->agl); - printf(" phi, theta, psi, alpha, beta\n [%f %f %f %f %f]\n", - fdm->phi, fdm->theta, fdm->psi, fdm->alpha, fdm->beta); - printf(" phidot, thetadot, psidot, vcas, climb_rate\n [%f %f %f %f %f]\n", - fdm->phidot, fdm->thetadot, fdm->psidot, fdm->vcas, fdm->climb_rate); - printf(" v_north, v_east, v_down\n [%f %f %f]\n", - fdm->v_north, fdm->v_east, fdm->v_down); - printf(" v_wind_body_north, v_wind_body_east, v_wind_body_down\n [%f %f %f]\n", - fdm->v_wind_body_north, fdm->v_wind_body_east, fdm->v_wind_body_down); - - /* [......] */ - - printf(" cur_time, warp\n [%u %u]\n", fdm->cur_time, fdm->warp); - printf(" visibility [%f]\n", fdm->visibility); - printf(" elevator, elevator_trim_tab\n [%f %f]\n", - fdm->elevator, fdm->elevator_trim_tab); - printf(" left_flap, right_flap\n [%f %f]\n", - fdm->left_flap, fdm->right_flap); - printf(" left_aileron, right_aileron\n [%f %f]\n", - fdm->left_aileron, fdm->right_aileron); - printf(" rudder, nose_wheel\n [%f %f]\n", - fdm->rudder, fdm->nose_wheel); - printf(" speedbrake, spoilers\n [%f %f]\n", - fdm->speedbrake, fdm->spoilers); - -} - -void net_fdm_init ( struct FGNetFDM* fdm ) { - - fdm->version = FG_NET_FDM_VERSION; - - fdm->latitude = 0.656480; - fdm->longitude = -2.135537; - fdm->altitude = 2.; - fdm->agl = 1.111652; - - fdm->phi = 0.; - fdm->theta = 0.; - fdm->psi = 5.20; - fdm->alpha = 0.; - fdm->beta = 0.; - - fdm->phidot = 0.; - fdm->thetadot = 0.; - fdm->psidot = 0.; - fdm->vcas = 0.; - fdm->climb_rate = 0.; - fdm->v_north = 0.; - fdm->v_east = 0.; - fdm->v_down = 0.; - - fdm->v_wind_body_north = 0.; - fdm->v_wind_body_east = 0.; - fdm->v_wind_body_down = 0.; - - fdm->A_X_pilot = 0.; - fdm->A_Y_pilot = 0.; - fdm->A_Z_pilot = 0.; - - fdm->stall_warning = 0.; - fdm->slip_deg = 0.; - - fdm->num_engines = 2; - fdm->eng_state[0] = 0; - fdm->eng_state[1] = 0; - fdm->rpm[0] = 0.; - fdm->rpm[1] = 0.; - fdm->fuel_flow[0] = 0.; - fdm->fuel_flow[1] = 0.; - fdm->egt[0] = 0.; - fdm->egt[1] = 0.; - fdm->egt[0] = 0.; - fdm->egt[1] = 0.; - fdm->cht[0] = 0.; - fdm->cht[1] = 0.; - fdm->mp_osi[0] = 0.; - fdm->mp_osi[1] = 0.; - fdm->tit[0] = 0.; - fdm->tit[1] = 0.; - fdm->oil_temp[0] = 0.; - fdm->oil_temp[1] = 0.; - fdm->oil_px[0] = 0.; - fdm->oil_px[1] = 0.; - - fdm->num_tanks = 4; - fdm->fuel_quantity[0] = 0.; - fdm->fuel_quantity[1] = 0.; - fdm->fuel_quantity[2] = 0.; - fdm->fuel_quantity[3] = 0.; - - fdm->num_wheels = 1; - fdm->wow[0] = 1; - fdm->gear_pos[0] = 0.; - fdm->gear_steer[0] = 0.; - fdm->gear_compression[0] = 0.; - - fdm->cur_time = 3213082700ul; - fdm->warp = 0; - fdm->visibility = 1000.; - - fdm->elevator = 0.; - fdm->elevator_trim_tab = 0.; - fdm->left_flap = 0.; - fdm->right_flap = 0.; - fdm->left_aileron = 0.; - fdm->right_aileron = 0.; - fdm->rudder = 0.; - fdm->nose_wheel = 0.; - fdm->speedbrake = 0.; - fdm->spoilers = 0.; - -} - -void net_gui_init (struct FGNetGUI* gui) { - gui->version = FG_NET_GUI_VERSION; - gui->latitude = 0.656480; - gui->longitude = -2.135537; - gui->altitude = 0.807609; - gui->agl = 1.111652; - - gui->phi = 0.; - gui->theta = 0.; - gui->psi = 5.20; - - gui->vcas = 0.; - gui->climb_rate = 0.; - - gui->num_tanks = 1; - gui->fuel_quantity[0] = 0.; - - gui->cur_time = 3198060679ul; - gui->warp = 1122474394ul; - - gui->ground_elev = 0.; - - gui->tuned_freq = 125.65; - gui->nav_radial = 90.; - gui->in_range = 1; - gui->dist_nm = 10.; - gui->course_deviation_deg = 0.; - gui->gs_deviation_deg = 0.; -} - -void net_gui_hton (struct FGNetGUI* gui) { - gui->version = ntohl(gui->version); - htond(&gui->latitude); - htond(&gui->longitude); - htonf(&gui->altitude); - htonf(&gui->agl); - - htonf(&gui->phi); - htonf(&gui->theta); - htonf(&gui->psi); - -} - -void net_gui_dump (struct FGNetGUI* gui) { - printf("net_gui (version %d size %d)\n",gui->version, sizeof( *gui)); - printf(" lat, lon, alt, agl\n [%f %f %f %f]\n", - gui->latitude, gui->longitude, gui->altitude, gui->agl); - printf(" phi, theta, psi\n [%f %f %f]\n", - gui->phi, gui->theta, gui->psi); - printf(" vcas, climb_rate\n [%f %f]\n", - gui->vcas, gui->climb_rate); - printf(" num_tanks, fuel[0], fuel[1], fuel[2], fuel[3]\n [%u %f %f %f %f]\n", - gui->num_tanks, gui->fuel_quantity[0], gui->fuel_quantity[1], - gui->fuel_quantity[2], gui->fuel_quantity[3]); - printf(" cur_time, warp\n [%u %u]\n", gui->cur_time, gui->warp); - printf(" ground_elev\n [%f]\n", - gui->ground_elev); - printf(" tuned_freq, nav_radial, in_range\n [%f %f %u]\n", - gui->tuned_freq, gui->nav_radial, gui->in_range); - printf(" dist_nm, course_deviation_deg, gs_deviation_deg\n [%f %f %f]\n", - gui->dist_nm, gui->course_deviation_deg, gui->gs_deviation_deg); -} - -void net_ctrls_init(struct FGNetCtrls* ctrls) { - ctrls->version = FG_NET_CTRLS_VERSION; - ctrls->aileron = 0.; - ctrls->elevator = 0.; - ctrls->rudder = 0.; - ctrls->aileron_trim = 0.; - ctrls->elevator_trim = 0.; - ctrls->rudder_trim = 0.; - ctrls->flaps = 0.; - ctrls->spoilers = 0.; - ctrls->speedbrake = 0.; - - ctrls->flaps_power = 1; - ctrls->flap_motor_ok = 1; - - ctrls->num_engines = 2; - int i; - for (i=0; i< 2; i++) { - ctrls->master_bat[i] = 1; - ctrls->master_alt[i] = 1; - ctrls->magnetos[i] = 1; - ctrls->starter_power[i] = 0; - ctrls->throttle[i] = 0.; - ctrls->mixture[i] = 0.; - ctrls->condition[i] = 0.; - ctrls->fuel_pump_power[i] = 1; - ctrls->prop_advance[i] = 1.; - ctrls->feed_tank_to[i] = 1; - ctrls->reverse[i] = 0; - ctrls->engine_ok[i] = 1; - ctrls->mag_left_ok[i] = 1; - ctrls->mag_right_ok[i] = 1; - ctrls->spark_plugs_ok[i] = 1; - ctrls->oil_press_status[i] = 0; - ctrls->fuel_pump_ok[i] = 1; - } - ctrls->num_tanks = 1; - ctrls->fuel_selector[0] = 1; - - ctrls->xfer_pump[0] = 1; - ctrls->cross_feed = 0; - - ctrls->brake_left = 0.; - ctrls->brake_right = 0.; - ctrls->copilot_brake_left = 0.; - ctrls->copilot_brake_right = 0.; - ctrls->brake_parking = 0.; - - ctrls->gear_handle = 1; - ctrls->master_avionics = 0; - - ctrls->comm_1 = 123.4; - ctrls->comm_2 = 123.4; - ctrls->nav_1 = 123.4; - ctrls->nav_2 = 123.4; - - ctrls->wind_speed_kt = 0.; - ctrls->wind_dir_deg = 0.; - ctrls->turbulence_norm = 0.; - - ctrls->temp_c = 25.; - ctrls->press_inhg = 25.; - - ctrls->hground = 25.; - ctrls->magvar = 0.; - - ctrls->speedup = 1; - -} - -void net_ctrls_ntoh(struct FGNetCtrls* ctrls) { - ctrls->version = ntohl(ctrls->version); - htond(&ctrls->aileron); - htond(&ctrls->elevator); - htond(&ctrls->rudder); - htond(&ctrls->aileron_trim); - htond(&ctrls->elevator_trim); - htond(&ctrls->rudder_trim); - htond(&ctrls->flaps); - - -} - - -void net_ctrls_dump(struct FGNetCtrls* ctrls) { - printf("net_ctrls (version %d size %d)\n",ctrls->version, sizeof( *ctrls)); - printf(" aileron elevator rudder\n [%f %f %f]\n", - ctrls->aileron, ctrls->elevator, ctrls->rudder); - printf(" throttle\n [%f %f %f]\n", - ctrls->throttle[0], ctrls->throttle[1], ctrls->throttle[2]); - -} - -void mplay_msg_dump ( struct FGMplayMsg *msg ) { - printf("mplay_msg (size %d id %d)\n",msg->header.len, msg->header.id); - printf(" reply_addr %d\n", msg->header.reply_addr); - printf(" reply_port %d\n", msg->header.reply_port); - printf(" callsign "); - int i; - for (i=0; i < MP_MAX_CALLSIGN_LEN; i++) { - printf("%c", msg->header.callsign[i]); - } - printf("\n"); - printf(" model %-96s (%d)\n", msg->pos.model, strlen(msg->pos.model)); - printf(" time %f lag %f\n", msg->pos.time, msg->pos.lag); - printf(" position %f %f %f\n", msg->pos.position[0], msg->pos.position[1], msg->pos.position[2]); - printf(" orientation %f %f %f %f\n", msg->pos.orientation[0], msg->pos.orientation[1], msg->pos.orientation[2], msg->pos.orientation[3]); - printf(" linear_vel %f %f %f\n", msg->pos.linear_vel[0], msg->pos.linear_vel[1], msg->pos.linear_vel[2]); - printf(" angular_vel %f %f %f\n", msg->pos.angular_vel[0], msg->pos.angular_vel[1], msg->pos.angular_vel[2]); - printf(" linear_accel %f %f %f\n", msg->pos.linear_accel[0], msg->pos.linear_accel[1], msg->pos.linear_accel[2]); - printf(" angular_accel %f %f %f\n", msg->pos.angular_accel[0], msg->pos.angular_accel[1], msg->pos.angular_accel[2]); - printf("sizeof %d\n", sizeof(struct FGMplayMsg)); - -} - -void mplay_msg_ntoh ( struct FGMplayMsg* msg) { - msg->header.magic = ntohl(msg->header.magic); - msg->header.version = ntohl(msg->header.version); - msg->header.id = ntohl(msg->header.id); - msg->header.len = ntohl(msg->header.len); - msg->header.reply_addr = ntohl(msg->header.reply_addr); - msg->header.reply_port = ntohl(msg->header.reply_port); - htond(&msg->pos.time); - htond(&msg->pos.lag); - htond(&msg->pos.position[0]); - htond(&msg->pos.position[1]); - htond(&msg->pos.position[2]); - htonf(&msg->pos.orientation[0]); - htonf(&msg->pos.orientation[1]); - htonf(&msg->pos.orientation[2]); - htonf(&msg->pos.orientation[3]); - htonf(&msg->pos.linear_vel[0]); - htonf(&msg->pos.linear_vel[1]); - htonf(&msg->pos.linear_vel[2]); - htonf(&msg->pos.angular_vel[0]); - htonf(&msg->pos.angular_vel[1]); - htonf(&msg->pos.angular_vel[2]); - htonf(&msg->pos.linear_accel[0]); - htonf(&msg->pos.linear_accel[1]); - htonf(&msg->pos.linear_accel[2]); - htonf(&msg->pos.angular_accel[0]); - htonf(&msg->pos.angular_accel[1]); - htonf(&msg->pos.angular_accel[2]); -} - -void mplay_msg_init ( struct FGMplayMsg* msg) { - msg->header.magic = MP_MSG_MAGIC; - msg->header.version = MP_PROTO_VER; - msg->header.id = 7; - msg->header.len = 232; - -} diff --git a/sw/logalizer/flight_gear.h b/sw/logalizer/flight_gear.h deleted file mode 100644 index 453ee0ea17..0000000000 --- a/sw/logalizer/flight_gear.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef FLIGHT_GEAR_H -#define FLIGHT_GEAR_H - -#include -#include "../simulator/flight_gear.h" - - -#define MP_MAX_CALLSIGN_LEN 8 -#define MP_MAX_MODEL_NAME_LEN 96 -#define MP_MSG_MAGIC 0x46474653 -#define MP_PROTO_VER 0x00010001 - -struct FGMplayHdr { - uint32_t magic; - uint32_t version; - uint32_t id; - uint32_t len; - uint32_t reply_addr; - uint32_t reply_port; - char callsign[MP_MAX_CALLSIGN_LEN]; -}; - -struct FGMplayPosMsg { - char model[MP_MAX_MODEL_NAME_LEN]; - double time; - double lag; - double position[3]; - float orientation[4]; - float linear_vel[3]; - float angular_vel[3]; - float linear_accel[3]; - float angular_accel[3]; -}; - -struct FGMplayMsg { - struct FGMplayHdr header; - struct FGMplayPosMsg pos; -}; - -extern void mplay_msg_dump ( struct FGMplayMsg *msg ); -extern void mplay_msg_ntoh ( struct FGMplayMsg* msg ); -extern void mplay_msg_init ( struct FGMplayMsg* msg ); -#endif /* FLIGHT_GEAR_H */ diff --git a/sw/logalizer/network.c b/sw/logalizer/network.c deleted file mode 100644 index 8a4ce2c8a2..0000000000 --- a/sw/logalizer/network.c +++ /dev/null @@ -1,38 +0,0 @@ -#include "network.h" - -#include -#include - -#include -#include - - -struct FgNetChannel* open_out_channel( char* host, uint16_t port) { - struct FgNetChannel* chan = malloc(sizeof (struct FgNetChannel)); - int so_reuseaddr = 1; - struct protoent * pte = getprotobyname("UDP"); - chan->socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto); - setsockopt(chan->socket, SOL_SOCKET, SO_REUSEADDR, - &so_reuseaddr, sizeof(so_reuseaddr)); - - chan->addr.sin_family = PF_INET; - chan->addr.sin_port = htons(port); - chan->addr.sin_addr.s_addr = inet_addr(host); - - return chan; -} - -struct FgNetChannel* open_in_channel( char* host, uint16_t port) { - struct FgNetChannel* chan = open_out_channel(host, port); - int ret = bind(chan->socket, (struct sockaddr*)&chan->addr, sizeof(chan->addr)); - if (ret) - perror("binding\n"); - return chan; -} - - -void send_buf(struct FgNetChannel* chan, char* buf, int len) { - if (sendto(chan->socket, buf, len, 0, - (struct sockaddr*)&chan->addr, sizeof(chan->addr)) == -1) - printf("error sending\n"); -} diff --git a/sw/logalizer/network.h b/sw/logalizer/network.h deleted file mode 100644 index 476e449880..0000000000 --- a/sw/logalizer/network.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef NETWORK_H -#define NETWORK_H - -#include -#include -#include - -struct FgNetChannel { - int socket; - struct sockaddr_in addr; -}; - -extern struct FgNetChannel* open_out_channel( char* host, uint16_t port ); -extern struct FgNetChannel* open_in_channel( char* host, uint16_t port ); -extern void send_buf(struct FgNetChannel* chan, char* buf, int len); - - -#endif /* NETWORK_H */ diff --git a/sw/logalizer/plot3dparse.c b/sw/logalizer/plot3dparse.c deleted file mode 100644 index a748abac79..0000000000 --- a/sw/logalizer/plot3dparse.c +++ /dev/null @@ -1,200 +0,0 @@ - -/* - -This is a quick hack - just to show how it might work. It gives a 3D view of -the GPS positions with an additional value (here: attitude phi) as colour. - -Start this with your favorite .data telemetry file as argument and pipe it -to output.dat, then run gnuplot with: - -#set terminal png nocrop enhanced size 800,600 -#set output 'output.png' -set view 64, 8, 1, 1 -set isosamples 50, 10 -set hidden3d offset 1 trianglepattern 3 undefined 1 altdiagonal bentover -set palette -set xlabel "m" -set ylabel "m" -set zlabel "m" -splot "output.dat" using 1:2:3:4 title "roll" with linespoints palette -pause -1 - -*/ - -#include -#include -#include -#include -#include -#include -#include -#include - -int main( int argc, char* argv[] ) -{ -/* - - - - - - - - - - - - - -7.73 11 GPS 0 55577549 665183336 0 -4310 0 0 345957748 31 0 - - - - - - - - -230.41 23 ATTITUDE -22 0 -4 - - - - - - - -7.77 23 NAVIGATION_REF 567835 5790977 32 - -*/ - - char line_in[256]; - - float rtime; - char id_name[255]; - unsigned int ac_id; - - unsigned int byte_mask; - int utm_east; - int utm_north; - int course; - int alt; - int speed; - int climb; - unsigned int itow; - unsigned int utm_zone; - unsigned int gps_nb_err; - - unsigned int phi; - unsigned int psi; - unsigned int theta; - - unsigned int nr_utm_east; - unsigned int nr_utm_north; - unsigned int nr_utm_zone; - - unsigned int humid; - unsigned int temp; - - char got_navref = 0; - - FILE *fd; - - if (argc != 2) - { - printf("plot3dparse 'flight_file.data' > output.dat\n"); - exit(0); - } - - fd = fopen( argv[1], "r" ); - - if (!fd) exit(0); - - while (!feof(fd)) - { - - fgets( line_in, 256, fd ); - - if (sscanf( line_in, "%e %i %s", &rtime, &ac_id, id_name )) - { - if (!strcmp( "NAVIGATION_REF", id_name)) - { - sscanf( line_in, "%e %i %s %i %i %i\n", - &rtime, - &ac_id, - id_name, - &nr_utm_east, - &nr_utm_north, - &nr_utm_zone ); - - got_navref = 1; - } - -#if 0 - if (!strcmp( "DPICCO_STATUS", id_name)) - { - #define DPICCO_HUMID_MAX 0x7FFF - #define DPICCO_HUMID_RANGE 100.0 - - #define DPICCO_TEMP_MAX 0x7FFF - #define DPICCO_TEMP_RANGE 165.0 - #define DPICCO_TEMP_OFFS -40.0 - - float fhumid, ftemp; - - sscanf( line_in, "%e %i %s %i %i %e %e\n", - &rtime, - &ac_id, - id_name, - &humid, - &temp ); - - fhumid = (dpicco_val[0] * DPICCO_HUMID_RANGE) / DPICCO_HUMID_MAX; - ftemp = ((dpicco_val[1] * DPICCO_TEMP_RANGE) / DPICCO_TEMP_MAX) + DPICCO_TEMP_OFFS; - - if (temp != 0) printf("%f %f %f\n", rtime, fhumid, ftemp); - } -#endif - - if (!strcmp( "GPS", id_name)) - { - sscanf( line_in, "%e %i %s %i %i %i %i %i %i %i %i %i %i\n", - &rtime, - &ac_id, - id_name, - &byte_mask, - &utm_east, - &utm_north, - &course, - &alt, - &speed, - &climb, - &itow, - &utm_zone, - &gps_nb_err ); - - } - - if (!strcmp( "ATTITUDE", id_name)) - { - sscanf( line_in, "%e %i %s %i %i %i\n", - &rtime, - &ac_id, - id_name, - &phi, - &psi, - &theta ); - - if (got_navref) printf("%i %i %i %i\n\n", - utm_east/100 - nr_utm_east, - utm_north/100 - nr_utm_north, - alt / 100, - phi ); - } - - } - } - - fclose( fd ); - - return(0); -} diff --git a/sw/logalizer/plotprofile.c b/sw/logalizer/plotprofile.c deleted file mode 100644 index 2eb6e0e936..0000000000 --- a/sw/logalizer/plotprofile.c +++ /dev/null @@ -1,138 +0,0 @@ - -/* - -http://users.softlab.ntua.gr/~ttsiod/gnuplotStreaming.html - -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define HEIGHT_SPAN 20000 - -FILE *Gplt, *Gplh; -int32_t alt = 0; -int32_t temp[HEIGHT_SPAN] = {0}; -int32_t humid[HEIGHT_SPAN] = {0}; - -void on_GPS(IvyClientPtr app, void *user_data, int argc, char *argv[]){ -/* - - - - - - - - - - - - - - -7.73 11 GPS 0 55577549 665183336 0 -4310 0 0 1642 345957748 31 0 -*/ - - int32_t _alt; - - _alt = atoi(argv[5]); - alt = _alt / 100; -// if ((_alt/100) < HEIGHT_SPAN) alt = _alt; - -// printf("alt %f\n", (float) _alt/100.); -} - -void on_TMP_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]){ -/* - - - - -*/ - - float _temp; - int i; - - _temp = atof(argv[2]); - if (alt < HEIGHT_SPAN) temp[alt] = _temp * 100; - -// printf("temp %f\n", _temp); - fprintf(Gplt, "plot '-' w points pt 0 title \"Temp\"\n"); - for (i = 0; i < HEIGHT_SPAN; i++){ - if (temp[i] != 0) fprintf(Gplt, "%f %d\n", temp[i]/100., i); - } - fprintf(Gplt,"e\n"); -} - -void on_SHT_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]){ -/* - - - - - - -*/ - - float _humid; - int i; - - _humid = atof(argv[3]); - if (alt < HEIGHT_SPAN) humid[alt] = _humid * 100; - -// printf("humid %f\n", _humid); - fprintf(Gplh, "plot '-' w points pt 0 title \"Humid\"\n"); - for (i = 0; i < HEIGHT_SPAN; i++){ - if (humid[i] != 0) fprintf(Gplh, "%f %d\n", humid[i]/100., i); - } - fprintf(Gplh,"e\n"); -} - -int main( int argc, char* argv[] ) -{ - double xmint, xmaxt, xminh, xmaxh, ymin, ymax; - GMainLoop *ml; - - ml = g_main_loop_new(NULL, FALSE); - - IvyInit ("IvyPlotProfile", "IvyPlotProfile READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_GPS, NULL, "^(\\S*) GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_TMP_STATUS, NULL, "^(\\S*) TMP_STATUS (\\S*) (\\S*)"); - IvyBindMsg(on_SHT_STATUS, NULL, "^(\\S*) SHT_STATUS (\\S*) (\\S*) (\\S*) (\\S*)"); -// IvyBindMsg(on_SHT_STATUS, NULL, "^(\\S*) DPICCO_STATUS (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); - - xmint = 5; - xmaxt = 35; - xminh = 0; - xmaxh = 100; - ymin = 500; - ymax = 2300; - - Gplt = popen("gnuplot -geometry 300x300 -noraise","w"); - setlinebuf(Gplt); - fprintf(Gplt, "set xrange[%f:%f]\n", xmint, xmaxt); - fprintf(Gplt, "set yrange[%f:%f]\n", ymin, ymax); - - Gplh = popen("gnuplot -geometry 300x300 -noraise","w"); - setlinebuf(Gplh); - fprintf(Gplh, "set xrange[%f:%f]\n", xminh, xmaxh); - fprintf(Gplh, "set yrange[%f:%f]\n", ymin, ymax); - - g_main_loop_run(ml); - - fclose(Gplt); - fclose(Gplh); - return 0; -} diff --git a/sw/logalizer/sliding_plot.c b/sw/logalizer/sliding_plot.c deleted file mode 100644 index 0f81ab3768..0000000000 --- a/sw/logalizer/sliding_plot.c +++ /dev/null @@ -1,83 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include - -#define NB_POINTS 2000 -#define NB_POINTS_BY_STEP 1 -//500 - -const GdkColor colors[] = {{65535, 0, 0}, {0, 65535, 0}, {0, 0, 65535}, {0, 0, 0}}; -#define NB_COLORS 4 - -#define REFRESH_RATE 166 /* ms */ -static gboolean timeout_callback(gpointer data); - -GtkWidget* sliding_plot_new(guint nb_plot) { - GtkWidget* databox = gtk_databox_new (); - g_object_set_data(G_OBJECT(databox), "nb_plot", (gpointer)nb_plot); - gfloat *X = g_new0 (gfloat, NB_POINTS); - g_object_set_data(G_OBJECT(databox), "X", X); - gpointer *Y = g_new0 (gpointer, nb_plot); - g_object_set_data(G_OBJECT(databox), "Y", Y); - guint nb_data = 0; - g_object_set_data(G_OBJECT(databox), "nb_data", (gpointer)nb_data); - guint _time = 0; - g_object_set_data(G_OBJECT(databox), "time", (gpointer)_time); - guint i; - for (i=0; i< nb_plot; i++) { - Y[i] = g_new0 (gfloat, NB_POINTS); - GtkDataboxGraph *graph = gtk_databox_lines_new (NB_POINTS, X, Y[i], (GdkColor*)&(colors[i%NB_COLORS]), 1); - gtk_databox_graph_add (GTK_DATABOX (databox), graph); - } - - // GtkDataboxGraph *grid = gtk_databox_grid_new (10, 10, (GdkColor*)&(colors[3]), 2); - // gtk_databox_graph_add (GTK_DATABOX (databox), grid); - g_timeout_add(REFRESH_RATE, timeout_callback, databox); - - return databox; -} - -void sliding_plot_update(GtkWidget* plot, float* values) { - guint i, j; - gfloat *X = g_object_get_data(G_OBJECT(plot), "X"); - gpointer *Y = g_object_get_data(G_OBJECT(plot), "Y"); - guint nb_data = (guint)g_object_get_data(G_OBJECT(plot), "nb_data"); - guint _time = (guint)g_object_get_data(G_OBJECT(plot), "time"); - guint nb_plot = (guint)g_object_get_data(G_OBJECT(plot), "nb_plot"); - - if (nb_data >= NB_POINTS) { - nb_data -= NB_POINTS_BY_STEP; - guint mem_to_move = (NB_POINTS - NB_POINTS_BY_STEP)*sizeof(gfloat); - memmove(X, &(X[NB_POINTS_BY_STEP]), mem_to_move); - for (i=0; i< nb_plot; i++) { - gfloat* y = Y[i]; - memmove(y, &(y[NB_POINTS_BY_STEP]), mem_to_move); - } - } - - for (j=nb_data; j< NB_POINTS; j++) { - X[j] = _time; - for (i=0; i< nb_plot; i++) { - gfloat* y = Y[i]; - y[j] = values[i]; - } - } - _time++; - g_object_set_data(G_OBJECT(plot), "time", (gpointer)_time); - nb_data++; - g_object_set_data(G_OBJECT(plot), "nb_data", (gpointer)nb_data); - // gtk_databox_auto_rescale (GTK_DATABOX (plot), 0.); -} - - -static gboolean timeout_callback(gpointer user_data) { - GtkDatabox* databox = GTK_DATABOX (user_data); - gtk_databox_auto_rescale (databox, 0.); - return TRUE; -} diff --git a/sw/logalizer/sliding_plot.h b/sw/logalizer/sliding_plot.h deleted file mode 100644 index 9b6f1788ea..0000000000 --- a/sw/logalizer/sliding_plot.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef SLIDING_PLOT_H -#define SLIDING_PLOT_H - -#include - -extern GtkWidget* sliding_plot_new(guint nb_plot); -extern void sliding_plot_update(GtkWidget* plot, float* values); - -#endif /* SLIDING_PLOT_H */ diff --git a/sw/logalizer/tmclient.c b/sw/logalizer/tmclient.c deleted file mode 100644 index 9d646ae9d3..0000000000 --- a/sw/logalizer/tmclient.c +++ /dev/null @@ -1,131 +0,0 @@ -/* $Id$ - * - * tmclient, an telemetry client 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 - -#include -#include -#include - -#define PORT 7123 -#define HOSTADDR "localhost" - -#define PACKET_SIZE 9 - -int sock, length; -struct sockaddr_in server; -char hostaddr[4096] = {HOSTADDR}; - -unsigned int uvalue[PACKET_SIZE]; -unsigned char cvalue[4*PACKET_SIZE]; - -static const char usage_str[] = -"client [options]\n" -"options:\n" -" -s HOST server address\n"; - -void on_FLIGHT_PARAM_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]) -{ - int count, cnt; - float ftemp; - - if (argc != 10) return; - - uvalue[0] = 42;//atoi(argv[0]); - cvalue[0] = uvalue[0] & 0xFF; - cvalue[1] = 0; - cvalue[2] = 'P'; - cvalue[3] = 'Z'; - - for (count=1; count < PACKET_SIZE; count++) { - ftemp = atof(argv[count]); - uvalue[count] = *(unsigned int*) &ftemp; - uvalue[count] = htonl(uvalue[count]); - for (cnt=0; cnt<4; cnt++) { - cvalue[count*4 + cnt] = (uvalue[count] >> (cnt*8)) & 0xFF; - } - } - - /* send data as binary 32bit floats through UDP */ - count = sendto(sock, cvalue, PACKET_SIZE*4, 0, (struct sockaddr*) &server, length); - if (count != PACKET_SIZE*4) perror("sendto"); -} - - -int main ( int argc, char** argv) { - - struct hostent *hent; - int c; - GMainLoop *ml = g_main_loop_new(NULL, FALSE); - - while ((c = getopt(argc, argv, "h:s:")) != EOF) { - switch (c) { - - case 'h': - printf(usage_str); - break; - - case 's': - strncpy(hostaddr, optarg, strlen(optarg)+1); - break; - } - } - - IvyInit ("IvyFlightParams", "IvyFlightParams READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_FLIGHT_PARAM_STATUS, NULL, "^\\S* FLIGHT_PARAM (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); - - printf("Using server: %s\n", hostaddr); - - sock= socket(AF_INET, SOCK_DGRAM, 0); - if (sock < 0) perror("socket"); - - server.sin_family = AF_INET; - hent = gethostbyname(hostaddr); - if (hent == 0) { - perror("unknown host %s"); - exit(1); - } - - memcpy((char *)&server.sin_addr, - (char *)hent->h_addr, - hent->h_length); - - server.sin_port = htons(PORT); - length=sizeof(struct sockaddr_in); - - g_main_loop_run(ml); - - return 0; -} diff --git a/sw/logalizer/tmdata.c b/sw/logalizer/tmdata.c deleted file mode 100644 index 2730d75879..0000000000 --- a/sw/logalizer/tmdata.c +++ /dev/null @@ -1,139 +0,0 @@ -/* $Id$ - * - * tmdata, an telemetry client 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 -#include - -#define PORT_OUT 7124 -#define HOSTADDR "localhost" -#define BUFSIZE 0x10000 - -int sock, length, fromlen; -struct sockaddr_in server; -char hostaddr[4096] = {HOSTADDR}; - -static const char usage_str[] = -"tmdata [options] [parameter]\n" -"options:\n" -" -s HOST server address\n"; - -int main ( int argc, char** argv) { - - struct hostent *hent; - char buf[BUFSIZE]; - int c, count; - fd_set fds; - struct timeval tvt; - char noparam = 0; - char *param = &noparam; - - while ((c = getopt(argc, argv, "hs:")) != EOF) - { - switch (c) - { - case 'h': - printf(usage_str); - break; - - case 's': - strncpy(hostaddr, optarg, strlen(optarg)+1); - break; - } - } - - if (argv[optind]) - { - param = argv[optind]; - } - - sock = socket(PF_INET, SOCK_STREAM, 0); - if (sock < 0) - { - perror("socket"); - exit(1); - } - - server.sin_family = AF_INET; - hent = gethostbyname(hostaddr); - if (hent == 0) - { - perror("unknown host"); - exit(1); - } - - memcpy((char *)hent->h_addr, - (char *)&server.sin_addr, - hent->h_length); - - server.sin_port = htons(PORT_OUT); - length=sizeof(struct sockaddr_in); - - if (connect(sock, &server, length)) - { - perror("connect"); - exit(1); - } - - /* send some data to trigger output */ - count = send(sock, param, strlen(param), 0); - - /* we will wait for data 500ms */ - tvt.tv_sec = 0; - tvt.tv_usec = 500000; - - FD_CLEAR(&fds); - FD_SET(sock, &fds); - - count = select(sock+1, &fds, NULL, NULL, &tvt); - - if (count > 0) - { - /* receive data packet containing formatted data */ - count = recv(sock, buf, sizeof(buf), 0); - - if (count < 0) - { - perror("recv"); - exit(1); - } - - write(1, buf, count); - } - else - { - perror("select"); - } - - return 0; -} diff --git a/sw/logalizer/tmserver.c b/sw/logalizer/tmserver.c deleted file mode 100644 index a34021e3fe..0000000000 --- a/sw/logalizer/tmserver.c +++ /dev/null @@ -1,378 +0,0 @@ -/* $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://localhost/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); -#if 0 - 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"); -#endif - 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,"Cache-Control: no-cache\n"); - pbuf += sprintf(pbuf,"Pragma: no-cache\n"); - 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); -#if 0 - 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"); -#endif - 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://localhost/maps/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://localhost/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,"Cache-Control: no-cache\n"); - pbuf += sprintf(pbuf,"Pragma: no-cache\n"); - 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; -} diff --git a/sw/logalizer/utils.c b/sw/logalizer/utils.c deleted file mode 100644 index 466ff4b027..0000000000 --- a/sw/logalizer/utils.c +++ /dev/null @@ -1,36 +0,0 @@ -#include "utils.h" - -#include - -//#include // endian tests -void htond (double *x) -{ - // if ( sgIsLittleEndian() ) { - int *Double_Overlay; - int Holding_Buffer; - - Double_Overlay = (int *) x; - Holding_Buffer = Double_Overlay [0]; - - Double_Overlay [0] = htonl (Double_Overlay [1]); - Double_Overlay [1] = htonl (Holding_Buffer); - // } else { - // return; - // } -} - -// Float version -void htonf (float *x) -{ - // if ( sgIsLittleEndian() ) { - int *Float_Overlay; - int Holding_Buffer; - - Float_Overlay = (int *) x; - Holding_Buffer = Float_Overlay [0]; - - Float_Overlay [0] = htonl (Holding_Buffer); - // } else { - // return; - // } -} diff --git a/sw/logalizer/utils.h b/sw/logalizer/utils.h deleted file mode 100644 index 056ad9560e..0000000000 --- a/sw/logalizer/utils.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef UTILS_H -#define UTILS_H - -#include - -extern void htond (double *x); -extern void htonf (float *x); - -#define RAD_OF_DEG(d) (d*M_PI/180.) -#define NORM_ANGLE_RAD(r) { \ - while (r>2*M_PI) \ - r -= 2*M_PI; \ - while (r<0) \ - r += 2*M_PI; \ - } - -#define NORM_ANGLE_DEG(d) { \ - while (d>180) \ - d -= 360; \ - while (d<=-180) \ - d += 360; \ - } - - -#endif /* UTILS_H */