From 84f43ef81b4ad90da0ad70752badc4e4c86f7dcf Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 Mar 2015 17:48:48 +0100 Subject: [PATCH] [logalizer] update ahrs2fg display bind to more messages update Flight Gear compatibility more command line options --- sw/logalizer/ahrs2fg.c | 153 +++++++++++++++++++++++++++++++------ sw/logalizer/flight_gear.h | 33 ++++++++ 2 files changed, 163 insertions(+), 23 deletions(-) diff --git a/sw/logalizer/ahrs2fg.c b/sw/logalizer/ahrs2fg.c index a20fedbbe6..2db694fc5d 100644 --- a/sw/logalizer/ahrs2fg.c +++ b/sw/logalizer/ahrs2fg.c @@ -2,6 +2,7 @@ #include #include #include +#include #include "flight_gear.h" #include "network.h" @@ -9,7 +10,26 @@ #include #include -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[]){ - 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; +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) { - 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); - IvyBindMsg(on_AHRS_STATE, chan, "^77 AHRS_STATE (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); + 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*)", 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; diff --git a/sw/logalizer/flight_gear.h b/sw/logalizer/flight_gear.h index 1e0b726725..5aef329600 100644 --- a/sw/logalizer/flight_gear.h +++ b/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);