diff --git a/sw/ground_segment/tmtc/app_server.c b/sw/ground_segment/tmtc/app_server.c index 5877354da3..502b970018 100644 --- a/sw/ground_segment/tmtc/app_server.c +++ b/sw/ground_segment/tmtc/app_server.c @@ -41,8 +41,6 @@ #include #include -// assuming local path from sw/ground_segment/tmtc -char defaultPprzFolder[] = "../../.."; char defaultAppPass[] = "1234"; //4 char password to control ac's over app "pass ground stg stg stg.. char* AppPass; @@ -74,6 +72,12 @@ 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? @@ -107,8 +111,9 @@ device_names DevNames[MAXDEVICENUMB]; //Connected Clients Data Structure typedef struct { int used ; - int client_port; //do we need that?? char client_ip[MAXIPLEN]; + //Pointer for tcp connection; + gpointer ClientTcpData; } client_data; client_data ConnectedClients[MAXCLIENT]; //Holds all status of devices @@ -143,7 +148,7 @@ void remove_client(char* RemClientIpAd) { } //Record client (if new) -void add_client(char* ClientIpAd) { +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++) { @@ -160,6 +165,7 @@ void add_client(char* ClientIpAd) { //record new client ip g_stpcpy(ConnectedClients[i].client_ip,ClientIpAd); + ConnectedClients[i].ClientTcpData = connection_in; // ConnectedClients[i].used = 1; if (verbose) { @@ -191,7 +197,8 @@ int get_ac_data(char* InStr, char* RetBuf) { DevNames[AcID].color, DevNames[AcID].dl_launch_ind, DevNames[AcID].kill_thr_ind, - DevNames[AcID].flight_altitude_ind); + DevNames[AcID].flight_altitude_ind + ); } return AcID; } @@ -255,6 +262,22 @@ int get_bl_data(char* InStr, char* RetBuf) { 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) { @@ -287,6 +310,7 @@ void broadcast_to_clients () { g_object_unref(udpSocket); } } + } //Read tcp requests of connected clients @@ -298,8 +322,21 @@ gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) { if (ret == G_IO_STATUS_ERROR) { //unref connection - g_object_unref (data); - g_string_free (s,TRUE); + 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{ @@ -313,7 +350,7 @@ gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) { 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); + printf("App Server: Command passed to ivy: %s\n",incs->str); fflush(stdout); } } @@ -371,7 +408,7 @@ gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) { else { //Unknown command if (verbose) { - printf("App Server: Client send an unknown command or wrong password: %s\n",RecString); + printf("App Server: Client send an unknown command or wrong password: (%s)\n",RecString); fflush(stdout); } } @@ -380,7 +417,10 @@ gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) { if (ret == G_IO_STATUS_EOF) { //Client disconnected if (verbose) { - printf("App Server: Client disconnected without saying 'bye':(\n"); + 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); @@ -407,7 +447,7 @@ gboolean new_connection(GSocketService *service, GSocketConnection *connection, } //Record client (if new) - add_client(g_inet_address_to_string(addr)); + add_client(g_inet_address_to_string(addr), connection); g_object_ref (connection); GSocket *socket = g_socket_connection_get_socket(connection); @@ -418,11 +458,28 @@ gboolean new_connection(GSocketService *service, GSocketConnection *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.. - sprintf(ivybuffer, "%s", argv[0]); broadcast_to_clients(); + } //Parse AC flight plan xml (block & waypoint names @@ -551,7 +608,7 @@ void parse_ac_af(int DevNameIndex, char *filename) { } //Parse dl values -void parse_dl_settings(int DevNameIndex, char *filename) { +void parse_ac_settings(int DevNameIndex, char *filename) { xmlTextReaderPtr reader; int ret; @@ -608,106 +665,136 @@ void parse_dl_settings(int DevNameIndex, char *filename) { } } -//Parse ac data from conf.xml -void parse_ac_data (char *PprzFolder) { - xmlTextReaderPtr reader; - int ret; +void on_app_server_NEW_AC(IvyClientPtr app, void *user_data, int argc, char *argv[]) { + //Request config + request_ac_config(atoi(argv[0])); +} - //Create full file path - char xmlFileName[BUFLEN]; - strcpy(xmlFileName, PprzFolder); - strcat(xmlFileName, "/conf/conf.xml"); +void on_app_server_GET_CONFIG (IvyClientPtr app, void *user_data, int argc, char *argv[]) { - reader = xmlReaderForFile(xmlFileName, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */ + int i=0; - xmlChar *AcName, *AcInd, *FpPath, *AcColor, *name, *AfPath; - if (reader != NULL) { - ret = xmlTextReaderRead(reader); - int AcId; + int RmId[2]; + /* + * RmId[0] >> ProcessID of incoming MsgProcessId + * RmId[1] >> RequestID of incoming MsgProcessId + */ - while (ret == 1) { - name = xmlTextReaderName(reader); - if (name == NULL) { - name = xmlStrdup(BAD_CAST "--"); - } - //read waypoint names - - if (xmlStrEqual(name, (const xmlChar *)"aircraft")) { - - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"ac_id"); - AcInd = xmlTextReaderValue(reader); - - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); - AcName = xmlTextReaderValue(reader); - - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"airframe"); - AfPath = xmlTextReaderValue(reader); - - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"flight_plan"); - FpPath = xmlTextReaderValue(reader); - - xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"gui_color"); - AcColor = xmlTextReaderValue(reader); - - //Get Device Id - AcId = atoi(((char *) AcInd)); - - //Save Device Name - strcpy(DevNames[AcId].name, ((char *) AcName)); - - //Save color - strcpy(DevNames[AcId].color, (char *) AcColor); - - //Save Flight Plan Path - strcpy(DevNames[AcId].flight_plan_path, "/conf/"); - strcat(DevNames[AcId].flight_plan_path , (char *) FpPath); - - //Save airframe Path - strcpy(DevNames[AcId].airframe_path, "/conf/"); - strcat(DevNames[AcId].airframe_path , (char *) AfPath); - - //Save Settings Path - sprintf(DevNames[AcId].settings_path, "/var/aircrafts/%s/settings.xml", (char *) AcName); - - //parse flight plan file for waypoint and block names - char FlightPlanPath[BUFLEN]; - strcpy(FlightPlanPath, PprzFolder); - strcat(FlightPlanPath, DevNames[AcId].flight_plan_path); - parse_ac_fp(AcId, FlightPlanPath); - - //parse airframe file - char AirframePath[BUFLEN]; - strcpy(AirframePath, PprzFolder); - strcat(AirframePath, DevNames[AcId].airframe_path); - parse_ac_af(AcId, AirframePath); - - //parse dl_settings for launch & kill throttle - char SettingsPath[BUFLEN]; - strcpy(SettingsPath, PprzFolder); - strcat(SettingsPath, DevNames[AcId].settings_path); - parse_dl_settings(AcId, SettingsPath); - } - - ret = xmlTextReaderRead(reader); - } - - xmlFreeTextReader(reader); - if (ret != 0) { - if (verbose) { - printf("App Server: failed to parse %s\n", xmlFileName); - fflush(stdout); - } - } + //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, "_"); } - else{ + + //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 + strcpy(DevNames[inc_device_id].name, argv[7] ); + + //Save color + strcpy(DevNames[inc_device_id].color, argv[6]); + + //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("App Server: Unable to open %s\n", xmlFileName); + 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. + } - return; -} // end of XMLParseDoc function +} + +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() { @@ -717,24 +804,30 @@ void print_help() { 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; - // Look for paparazzi folder (PAPARAZZI_HOME or assume local path by default) - char* PprzFolder = getenv("PAPARAZZI_HOME"); - if (PprzFolder == NULL) PprzFolder = defaultPprzFolder; - // Parse options for (i = 1; i < argc; ++i) { if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) { @@ -756,6 +849,9 @@ int main(int argc, char **argv) { 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(); @@ -765,16 +861,17 @@ int main(int argc, char **argv) { if (verbose) { printf("### Paparazzi App Server ###\n"); - printf("Using Paparazzi Folder : %s\n", PprzFolder); printf("Server listen port (TCP) : %d\n", tcp_port); - printf("Server broadcast port (UDP) : %d\n", udp_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); } - //Parse conf.xml - parse_ac_data(PprzFolder); //Create tcp listener #if !GLIB_CHECK_VERSION (2, 35, 1) @@ -797,10 +894,12 @@ int main(int argc, char **argv) { 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); + IvyInit ("PPRZ_App_Server", "Papparazzi App Server Ready!", NULL, NULL, NULL, NULL); - IvyBindMsg(Ivy_All_Msgs, NULL, "(^ground .*)"); - IvyBindMsg(Ivy_All_Msgs, NULL, "(^\\S* AIRSPEED (\\S*) (\\S*) (\\S*) (\\S*))"); + 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); @@ -809,6 +908,9 @@ int main(int argc, char **argv) { printf("Starting App Server\n"); fflush(stdout); } + IvySendMsg("app_server"); + + g_timeout_add(100, request_ac_list, NULL); g_main_loop_run(loop); @@ -818,4 +920,3 @@ int main(int argc, char **argv) { } return 0; } -