[nps] update wind from flight gear input socket

sw/simulator/nps/nps_flightgear.c

@@ -2,12 +2,14 @@
 
 #include <sys/socket.h>
 #include <sys/time.h>
+#include <errno.h>
 #include <time.h>
 #include <netdb.h>
 #include <netinet/in.h>
 #include <arpa/inet.h>
 #include <stdio.h>
 #include <math.h>
+#include <stdlib.h>
 
 
 #include "std.h"
@@ -17,6 +19,7 @@
 static struct  {
   int socket;
   struct sockaddr_in addr;
+  int socket_in;
   unsigned int initial_time;
   unsigned int time_offset;
 } flightgear;
@@ -41,17 +44,40 @@ float htonf(float x)
 }
 
 
-void nps_flightgear_init(const char *host,  unsigned int port, unsigned int time_offset)
+void nps_flightgear_init(const char *host,  unsigned int port, unsigned int port_in, unsigned int time_offset)
 {
   int so_reuseaddr = 1;
   struct protoent *pte = getprotobyname("UDP");
   flightgear.socket = socket(PF_INET, SOCK_DGRAM, pte->p_proto);
   setsockopt(flightgear.socket, SOL_SOCKET, SO_REUSEADDR,
-             &so_reuseaddr, sizeof(so_reuseaddr));
+      &so_reuseaddr, sizeof(so_reuseaddr));
   flightgear.addr.sin_family = PF_INET;
   flightgear.addr.sin_port = htons(port);
   flightgear.addr.sin_addr.s_addr = inet_addr(host);
 
+  // incoming flight gear socket
+  // only bind to socket if port_in is not zero
+  if (port_in > 0) {
+    struct sockaddr_in addr_in;
+    flightgear.socket_in = socket(PF_INET, SOCK_DGRAM, pte->p_proto);
+    setsockopt(flightgear.socket_in, SOL_SOCKET, SO_REUSEADDR,
+        &so_reuseaddr, sizeof(so_reuseaddr));
+    addr_in.sin_family = PF_INET;
+    addr_in.sin_port = htons(port_in);
+    addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
+
+    if (bind(flightgear.socket_in, (struct sockaddr*)&addr_in, sizeof(addr_in)) == -1) {
+      perror("nps_flightgear_init bind()");
+      exit(errno);
+    }
+    else{
+      printf("Bount to port %u to receive from\n", port_in);
+    }
+  }
+  else {
+    flightgear.socket_in = -1;
+  }
+
   // get current time to use as inital when computing cur_time for FG
   time_t t = time(NULL);
   flightgear.initial_time = t;
@@ -166,7 +192,6 @@ void nps_flightgear_send()
   gui.course_deviation_deg = 0.;
   gui.gs_deviation_deg = 0.;
 
-
   if (sendto(flightgear.socket, (char *)(&gui), sizeof(gui), 0,
              (struct sockaddr *)&flightgear.addr, sizeof(flightgear.addr)) == -1) {
     fprintf(stderr, "error sending to FlightGear\n");
@@ -174,3 +199,46 @@ void nps_flightgear_send()
   }
 
 }
+
+
+/**
+ * Receive Flight Gear environment messages
+ */
+void nps_flightgear_receive() {
+
+  if (flightgear.socket_in != -1) {
+    // socket is correctly opened
+
+    struct FGEnvironment env;
+    size_t s_env = sizeof(env);
+    int bytes_read;
+
+    //read first message
+    memset(&env, 0, s_env);
+    bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL);
+    while (bytes_read != -1) { // while we read a message (empty buffer)
+      if (bytes_read == (int)s_env){
+        // Update wind info
+        nps_atmosphere_set_wind_ned(
+            (double)env.wind_from_north,
+            (double)env.wind_from_east,
+            (double)env.wind_from_down);
+      }
+      else {
+        //error
+        printf("WARNING : ignoring packet with size %d (%d expected)", bytes_read, (int)sbuf);
+      }
+
+      //read next message
+      memset(&env, 0, s_env);
+      bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL);
+    }
+
+    if ((errno & (EAGAIN | EWOULDBLOCK)) == 0) {
+      perror("nps_flightgear_receive recvfrom()");
+    }
+  }
+
+}
+
+

sw/simulator/nps/nps_flightgear.h

@@ -2,8 +2,10 @@
 #define NPS_FLIGHTGEAR_H
 
 
-extern void nps_flightgear_init(const char *host,  unsigned int port, unsigned int time_offset);
+extern void nps_flightgear_init(const char* host,  unsigned int port, unsigned int port_in, unsigned int time_offset);
 extern void nps_flightgear_send();
 extern void nps_flightgear_send_fdm();
+extern void nps_flightgear_receive();
+
 
 #endif /* NPS_FLIGHTGEAR_H */

sw/simulator/nps/nps_main.c

@@ -49,6 +49,7 @@ static struct {
   double display_time;
   char *fg_host;
   unsigned int fg_port;
+  unsigned int fg_port_in;
   unsigned int fg_time_offset;
   int fg_fdm;
   char *js_dev;
@@ -143,9 +144,9 @@ static void nps_main_init(void)
   }
   nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev);
 
-  if (nps_main.fg_host) {
-    nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_time_offset);
-  }
+  if (nps_main.fg_host)
+    nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_port_in, nps_main.fg_time_offset);
+
 
 #if DEBUG_NPS_TIME
   printf("host_time_factor,host_time_elapsed,host_time_now,scaled_initial_time,sim_time_before,display_time_before,sim_time_after,display_time_after\n");
@@ -176,6 +177,7 @@ static void nps_main_display(void)
 {
   //  printf("display at %f\n", nps_main.display_time);
   nps_ivy_display();
+
   if (nps_main.fg_host) {
     if (nps_main.fg_fdm) {
       nps_flightgear_send_fdm();
@@ -183,6 +185,7 @@ static void nps_main_display(void)
       nps_flightgear_send();
     }
   }
+  nps_flightgear_receive();
 }
 
 
@@ -296,6 +299,7 @@ static bool_t nps_main_parse_options(int argc, char **argv)
 
   nps_main.fg_host = NULL;
   nps_main.fg_port = 5501;
+  nps_main.fg_port_in = 5502;
   nps_main.fg_time_offset = 0;
   nps_main.js_dev = NULL;
   nps_main.spektrum_dev = NULL;
@@ -310,6 +314,7 @@ static bool_t nps_main_parse_options(int argc, char **argv)
     "   -h                                     Display this help\n"
     "   --fg_host <flight gear host>           e.g. 127.0.0.1\n"
     "   --fg_port <flight gear port>           e.g. 5501\n"
+    "   --fg_port_in <flight gear in port>     e.g. 5502\n"
     "   --fg_time_offset <offset in seconds>   e.g. 21600 for 6h\n"
     "   -j --js_dev <optional joystick index>  e.g. 1 (default 0)\n"
     "   --spektrum_dev <spektrum device>       e.g. /dev/ttyUSB0\n"
@@ -331,6 +336,7 @@ static bool_t nps_main_parse_options(int argc, char **argv)
       {"ivy_bus", 1, NULL, 0},
       {"time_factor", 1, NULL, 0},
       {"fg_fdm", 0, NULL, 0},
+      {"fg_port_in", 1, NULL, 0},
       {0, 0, 0, 0}
     };
     int option_index = 0;
@@ -362,8 +368,9 @@ static bool_t nps_main_parse_options(int argc, char **argv)
           case 7:
             nps_main.host_time_factor = atof(optarg); break;
           case 8:
-            nps_main.fg_fdm = 1;
-            break;
+            nps_main.fg_fdm = 1; break;
+	  case 9:
+            nps_main.fg_port_in = atoi(optarg); break;
         }
         break;