[logalizer] update ahrs2fg display

bind to more messages
update Flight Gear compatibility
more command line options

sw/logalizer/ahrs2fg.c

@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include "flight_gear.h"
 #include "network.h"
@@ -9,7 +10,26 @@
 #include <Ivy/ivy.h>
 #include <Ivy/ivyglibloop.h>
 
-gfloat biases[3];
+// 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];
 
@@ -17,7 +37,7 @@ gboolean timeout_callback(gpointer data) {
 
   static double x = 0.;
   static double y = 0.;
-  static double z = 10;
+  static double z = 10.;
   const double earth_radius = 6372795.;
 
   double lat =  0.656480 + asin(x/earth_radius);
@@ -30,7 +50,10 @@ gboolean timeout_callback(gpointer data) {
   gui.longitude = lon;
   gui.altitude = z;
 
-  //  printf("%f %f %f\n", eulers[0], eulers[1], eulers[2]);
+  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];
@@ -51,35 +74,119 @@ void quat_to_euler( gfloat* quat, gfloat* euler) {
   euler[2] = atan2( 2*(quat[1]*quat[2] + quat[0]*quat[3]),(1-2*(q22 + q32)) );
 }
 
-void on_AHRS_STATE(IvyClientPtr app, void *user_data, int argc, char *argv[]){
+void on_ATTITUDE(IvyClientPtr app, void *user_data, int argc, char *argv[]){
-  float q0 = atof(argv[0]);
+  float phi = atof(argv[0]);
-  float q1 = atof(argv[1]);
+  float psi = atof(argv[1]);
-  float q2 = atof(argv[2]);
+  float theta = atof(argv[2]);
-  float q3 = atof(argv[3]);
+  eulers[0] = phi;
-  float bx = atof(argv[4]);
+  eulers[1] = theta;
-  float by = atof(argv[5]);
+  eulers[2] = psi;
-  float bz = atof(argv[6]);
+}
-  biases[0] = bx;
+
-  biases[1] = by;
+void on_AHRS_EULER_INT(IvyClientPtr app, void *user_data, int argc, char *argv[]){
-  biases[2] = bz;
+  float phi = atof(argv[3]);
-  quat[0] = q0;
+  float theta = atof(argv[4]);
-  quat[1] = q1;
+  float psi = atof(argv[5]);
-  quat[2] = q2;
+  eulers[0] = phi / euler_int_res;
-  quat[3] = q3;
+  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] <AC_ID>\n");
+  printf(" Options :\n");
+  printf("   -fg_host <host>\tFlight Gear host address (default: %s)\n", fg_host);
+  printf("   -fg_port <port>\tFlight Gear port (default: %d)\n", fg_port);
+  printf("   -b <Ivy bus>\tdefault is %s\n", defaultIvyBus);
+  printf("   -v\tverbose\n");
+  printf("   -h --help show this help\n");
+}
+
 
 int main ( int argc, char** argv) {
-  struct FgNetChannel* chan = open_out_channel("127.0.0.1", 5501);
 
-  g_timeout_add(16, timeout_callback, chan);
+  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 ("IvyGtkButton", "IvyGtkButton READY", NULL, NULL, NULL, NULL);
+  IvyInit ("ahrs2fg", "ahrs2fg READY", NULL, NULL, NULL, NULL);
-  IvyBindMsg(on_AHRS_STATE, chan, "^77 AHRS_STATE (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
+  IvyBindMsg(on_ATTITUDE, chan, "^%d ATTITUDE (\\S*) (\\S*) (\\S*)", ac_id);
-  IvyStart("127.255.255.255");
+  IvyBindMsg(on_AHRS_EULER_INT, chan, "^%d AHRS_EULER_INT (\\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*)", 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;

sw/logalizer/flight_gear.h

@@ -114,6 +114,7 @@ struct FGNetCtrls {
 #define FG_NET_FDM_MAX_WHEELS  3
 #define FG_NET_FDM_MAX_TANKS   4
 
+#ifndef _NET_FDM_HXX
 
 struct FGNetFDM {
 
@@ -199,6 +200,8 @@ struct FGNetFDM {
   float spoilers;
 };
 
+#endif
+
 struct FGNetMiniFDM {
    uint32_t version;           // increment when data values change
 
@@ -224,10 +227,33 @@ struct FGNetMiniFDM {
     int32_t warp;                 // offset in seconds to unix time
 };
 
+#if FG_2_4
 #define FG_NET_GUI_VERSION 7
+#else
+#define FG_NET_GUI_VERSION 8
+#endif /*FG_2_4*/
+
 #define FG_NET_GUI_MAX_TANKS 4
+
+// Prior to FG_NET_GUI_VERSION 8, OS X needed #pragma pack(4) to
+// properly display FG visualization data. In version 8 they added
+// a padding1 element to ensure proper data alignment, so this is
+// no longer required. The rest of this struct is based on FG source
+// in src/Network/net_gui.hxx
+
+#if FG_2_4
+#ifdef __x86_64__
+#pragma pack(push)
+#ifdef __APPLE__
+#pragma pack(4)
+#else
+#pragma pack(8)
+#endif /*__APPLE__*/
+#endif /*__x86_64__*/
+#endif /*FG_2_4*/
 struct FGNetGUI {
   uint32_t version;           // increment when data values change
+  uint32_t padding1;
 
   // Positions
   double longitude;           // geodetic (radians)
@@ -260,6 +286,13 @@ struct FGNetGUI {
   float course_deviation_deg; // degrees off target course
   float gs_deviation_deg;     // degrees off target glide slope
 };
+#if FG_2_4
+#ifdef __x86_64__
+#pragma pack(push)
+#pragma pack(pop)
+#endif /*__x86_64__*/
+#endif /*FG_2_4*/
+
 
 extern void net_fdm_dump (struct FGNetFDM* fdm);
 extern void net_fdm_ntoh (struct FGNetFDM* fdm);