[app_server] support tcp stream and don't read conf.xml directly

sw/ground_segment/tmtc/app_server.c

@@ -41,8 +41,6 @@
 #include <stdio.h>
 #include <libxml/xmlreader.h>
 
-// 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);
+    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 on_app_server_NEW_AC(IvyClientPtr app, void *user_data, int argc, char *argv[]) {
-void parse_ac_data (char *PprzFolder) {
+  //Request config
-  xmlTextReaderPtr reader;
+  request_ac_config(atoi(argv[0]));
-  int ret;
-
-  //Create full file path
-  char xmlFileName[BUFLEN];
-  strcpy(xmlFileName, PprzFolder);
-  strcat(xmlFileName, "/conf/conf.xml");
-
-  reader = xmlReaderForFile(xmlFileName, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */
-
-  xmlChar *AcName, *AcInd, *FpPath, *AcColor, *name, *AfPath;
-  if (reader != NULL) {
-    ret = xmlTextReaderRead(reader);
-    int AcId;
-
-    while (ret == 1) {
-      name = xmlTextReaderName(reader);
-      if (name == NULL) {
-        name = xmlStrdup(BAD_CAST "--");
 }
-      //read waypoint names
 
-      if (xmlStrEqual(name, (const xmlChar *)"aircraft")) {
+void on_app_server_GET_CONFIG (IvyClientPtr app, void *user_data, int argc, char *argv[]) {
 
-        xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"ac_id");
+  int i=0;
-        AcInd = xmlTextReaderValue(reader);
 
-        xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name");
+  int RmId[2];
-        AcName = xmlTextReaderValue(reader);
+  /*
+   * RmId[0] >> ProcessID of incoming MsgProcessId
+   * RmId[1] >> RequestID of incoming MsgProcessId
+   */
 
-        xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"airframe");
+  //Split arg0 to get process id and request id
-        AfPath = xmlTextReaderValue(reader);
+  char * mtok;
+  mtok = strtok (argv[0],"_");
+  while (mtok != NULL || i>2)
+  {
+    RmId[i]= atoi(mtok);
+    i++;
+    mtok = strtok (NULL, "_");
+  }
 
-        xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"flight_plan");
+  //Check whether process id and request id matches or not
-        FpPath = xmlTextReaderValue(reader);
+  if ( RmId[0]== ProcessID && RmId[1] <= RequestID) {
-
+    int inc_device_id=atoi(argv[1]);
-        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));
+    strcpy(DevNames[inc_device_id].name, argv[7] );
 
     //Save color
-        strcpy(DevNames[AcId].color, (char *) AcColor);
+    strcpy(DevNames[inc_device_id].color, argv[6]);
 
     //Save Flight Plan Path
-        strcpy(DevNames[AcId].flight_plan_path, "/conf/");
+    //check if file is local
-        strcat(DevNames[AcId].flight_plan_path , (char *) FpPath);
+    if ((strncmp( argv[2], "file://", strlen("file://"))) == 0) {
-
+      sprintf(DevNames[inc_device_id].flight_plan_path, "%s", argv[2]+7);
-        //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);
-      }
-    }
     }
     else {
-    if (verbose) {
+      printf("App Server: App server only works with local files! (Flight Plan Path:%s)\n", argv[2]);
-      printf("App Server: Unable to open %s\n", xmlFileName);
+      return;
-      fflush(stdout);
-    }
     }
 
+    //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;
-} // end of XMLParseDoc function
+    }
+
+    //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() {
@@ -717,24 +804,30 @@ void print_help() {
   printf("   -u <UDP port>\tfor sending AC data (default: %d)\n", udp_port);
   printf("   -b <Ivy bus>\tdefault is %s\n", defaultIvyBus);
   printf("   -p <password>\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);
+    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, "(^ground (\\S*) (\\S*) .*)");
-  IvyBindMsg(Ivy_All_Msgs, NULL, "(^\\S* AIRSPEED (\\S*) (\\S*) (\\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;
 }
-