diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile index 324522929e..cbc1e502b3 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -32,8 +32,51 @@ play : play.ml CC = gcc CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags` LDFLAGS=`pkg-config gtk+-2.0 --libs` -s -lgtkdatabox `pcre-config --libs` -lglibivy -ahrsview : ahrsview.c - $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) + +ahrsview : ahrsview.c sliding_plot.c + $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) + +imuview : imuview.c sliding_plot.c + $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) + +test_sliding_plot: test_sliding_plot.c sliding_plot.c + $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) + +#MORE_FLAGS = -I/usr/include/gtk-1.2 -I/usr/include/glib-1.2 -I/usr/lib/glib/include -rdynamic /home/poine/tmp/gtkglarea-1.2.3/gtkgl/.libs/libgtkgl.so -L/usr/lib -L/usr/X11R6/lib /usr/lib/libgtk.so /usr/lib/libgdk.so /usr/lib/libgmodule.so /usr/lib/libglib.so -ldl -lXi -lXext -lX11 -lm -lGLU -lGL -Wl,--rpath -Wl,/usr/local/lib +MORE_FLAGS = -I/usr/include/gtk-1.2 -I/usr/include/glib-1.2 -I/usr/lib/glib/include -rdynamic /usr/lib/libgtkgl.so -L/usr/lib -L/usr/X11R6/lib /usr/lib/libgtk.so /usr/lib/libgdk.so /usr/lib/libgmodule.so /usr/lib/libglib.so -ldl -lXi -lXext -lX11 -lm -lGLU -lGL -Wl,--rpath -Wl,/usr/local/lib `pcre-config --libs` -lglibivy + +MORE_CFLAGS = -DHAVE_DLFCN_H=1 -DSTDC_HEADERS=1 -I. -I. -I.. -g -O2 -I/usr/include/gtk-1.2 -I/usr/include/glib-1.2 -I/usr/lib/glib/include + +disp3d: disp3d.c + $(CC) $(MORE_CFLAGS) -g -o $@ $^ $(MORE_FLAGS) + +test1: test1.c + $(CC) $(MORE_CFLAGS) -g -o $@ $^ $(MORE_FLAGS) -lglut + clean: - rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play ahrsview + rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play ahrsview imuview ahrs2fg + +FGFS_ROOT = /home/poine/work/flightgear_cvs +FGFS = $(FGFS_ROOT)/bin/fgfs +FGFS_ENV = LD_LIBRARY_PATH=/usr/local/lib:$(FGFS_ROOT)/lib +FGFS_COMMON_ARGS = --fg-root=$(FGFS_ROOT) --aircraft=A320 --timeofday=noon +FGFS_IN_FDM_ARGS = $(FGFS_COMMON_ARGS) --fdm=null --native-fdm=socket,in,30,,5501,udp +FGFS_OUT_FDM_ARGS = $(FGFS_COMMON_ARGS) --native-fdm=socket,out,30,,5500,udp +FGFS_IN_GUI_ARGS = $(FGFS_COMMON_ARGS) --fdm=null --native-gui=socket,in,30,,5501,udp +FGFS_OUT_GUI_ARGS = $(FGFS_COMMON_ARGS) --native-gui=socket,out,30,,5500,udp +FGFS_IN_CTRLS_ARGS = $(FGFS_COMMON_ARGS) --native-ctrls=socket,in,30,,5501,udp +FGFS_OUT_CTRLS_ARGS = $(FGFS_COMMON_ARGS) --native-ctrls=socket,out,30,,5500,udp +FGFS_OUT_MULTIPLAY_ARGS = $(FGFS_COMMON_ARGS) --multiplay=out,10,127.0.0.1,5500 --callsign=F-POINE +FGFS_IN_MULTIPLAY_ARGS = $(FGFS_COMMON_ARGS) --multiplay=in,10,127.0.0.1,5501 --callsign=F-POINE +FGFS_RCAM_ARGS = $(FGFS_COMMON_ARGS) --fdm=null --native-ctrls=socket,out,30,,5500,udp --native-fdm=socket,in,30,,5501,udp +FGFS_GAME_ARGS = $(FGFS_COMMON_ARGS) --control=joystick + +FGFS_ARGS = $(FGFS_COMMON_ARGS) $(FGFS_IN_GUI_ARGS) +#FGFS_GAME_ARGS) + +run_fg: + $(FGFS_ENV) $(FGFS) $(FGFS_ARGS) + +ahrs2fg: ahrs2fg.c network.c flight_gear.c utils.c + $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) \ No newline at end of file diff --git a/sw/logalizer/ahrs2fg.c b/sw/logalizer/ahrs2fg.c new file mode 100644 index 0000000000..0aa02a3908 --- /dev/null +++ b/sw/logalizer/ahrs2fg.c @@ -0,0 +1,86 @@ +#include +#include +#include +#include + +#include "flight_gear.h" +#include "network.h" + +#include +#include + +gfloat biases[3]; +gfloat quat[4]; +gfloat eulers[3]; + +gboolean timeout_callback(gpointer data) { + + static double x = 0.; + static double y = 0.; + static double z = 10; + const double earth_radius = 6372795.; + + double lat = 0.656480 + asin(x/earth_radius); + double lon = -2.135537 + asin(y/earth_radius); + + struct FGNetGUI gui; + net_gui_init(&gui); + + gui.latitude = lat; + gui.longitude = lon; + gui.altitude = z; + + // printf("%f %f %f\n", eulers[0], eulers[1], eulers[2]); + gui.phi = eulers[0]; + gui.theta = eulers[1]; + gui.psi = eulers[2]; + + // net_gui_hton(&gui); + send_buf((struct FgNetChannel*)data, (char*)&gui, sizeof(gui)); + return TRUE; +} + +void quat_to_euler( gfloat* quat, gfloat* euler) { + // float q02 = quat[0] * quat[0]; + float q12 = quat[1] * quat[1]; + float q22 = quat[2] * quat[2]; + float q32 = quat[3] * quat[3]; + + euler[0] = atan2( 2*(quat[2]*quat[3] + quat[0]*quat[1]),(1-2*(q12 + q22)) ); + euler[1] = -asin(2*(quat[1]*quat[3] - quat[0]*quat[2])); + 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; + quat_to_euler(quat, eulers); +} + + +int main ( int argc, char** argv) { + struct FgNetChannel* chan = open_out_channel("127.0.0.1", 5501); + + g_timeout_add(16, 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"); + + g_main_loop_run(ml); + return 0; +} diff --git a/sw/logalizer/ahrsview.c b/sw/logalizer/ahrsview.c index 93ba89c4de..d677159f7b 100644 --- a/sw/logalizer/ahrsview.c +++ b/sw/logalizer/ahrsview.c @@ -2,78 +2,26 @@ #include #include #include - -#include -#include -#include -#include -#include -#include -#include +#include #include #include -#define NB_POINTS 2000 +#include "sliding_plot.h" -GtkWidget* databox; -static gfloat *X; -static gfloat *BX; -static gfloat *BY; -static gfloat *BZ; -static guint nb_data; +GtkWidget *biases_plot, *eulers_plot; -GtkWidget* plot_frame_new(void) { - databox = gtk_databox_new (); - - X = g_new0 (gfloat, NB_POINTS); - BX = g_new0 (gfloat, NB_POINTS); - BY = g_new0 (gfloat, NB_POINTS); - BZ = g_new0 (gfloat, NB_POINTS); - - GdkColor color; - color.red = 0; - color.green = 0; - color.blue = 0; - GtkDataboxGraph* grid = gtk_databox_grid_new (7, 7, &color, 2); - gtk_databox_graph_add (GTK_DATABOX (databox), grid); - - color.red = 0; - color.green = 65535; - color.blue = 0; - GtkDataboxGraph *graph_bx = gtk_databox_lines_new (NB_POINTS, X, BX, &color, 1); - gtk_databox_graph_add (GTK_DATABOX (databox), graph_bx); - - color.red = 65535; - color.green = 0; - color.blue = 0; - GtkDataboxGraph *graph_by = gtk_databox_lines_new(NB_POINTS, X, BY, &color, 1); - gtk_databox_graph_add (GTK_DATABOX (databox), graph_by); - - color.red = 0; - color.green = 0; - color.blue = 65535; - GtkDataboxGraph *graph_bz = gtk_databox_lines_new(NB_POINTS, X, BZ, &color, 1); - gtk_databox_graph_add (GTK_DATABOX (databox), graph_bz); - - nb_data = 0; - - return databox; +void quat_to_euler( gfloat* quat, gfloat* euler) { + // float q02 = quat[0] * quat[0]; + float q12 = quat[1] * quat[1]; + float q22 = quat[2] * quat[2]; + float q32 = quat[3] * quat[3]; + euler[0] = atan2( 2*(quat[2]*quat[3] + quat[0]*quat[1]),(1-2*(q12 + q22)) ); + euler[1] = -asin(2*(quat[1]*quat[3] - quat[0]*quat[2])); + euler[2] = atan2( 2*(quat[1]*quat[2] + quat[0]*quat[3]),(1-2*(q22 + q32)) ); } -void update_state_plot(float q0, float q1, float q2, float q3, float bx, float by, float bz) { - if (nb_data < NB_POINTS) { - X[nb_data] = nb_data; - BX[nb_data] = bx; - BY[nb_data] = by; - BZ[nb_data] = bz; - gtk_databox_auto_rescale (GTK_DATABOX (databox), 0.); - nb_data++; - } -} - - void on_AHRS_STATE(IvyClientPtr app, void *user_data, int argc, char *argv[]){ float q0 = atof(argv[0]); float q1 = atof(argv[1]); @@ -82,9 +30,14 @@ void on_AHRS_STATE(IvyClientPtr app, void *user_data, int argc, char *argv[]){ float bx = atof(argv[4]); float by = atof(argv[5]); float bz = atof(argv[6]); - update_state_plot(q0, q1, q2, q3, bx, by, bz); - - + gfloat biases[] = {bx, by, bz}; + gfloat quat[] = {q0, q1, q2, q3}; + gfloat eulers[] = {0., 0., 0.}; + quat_to_euler(quat, eulers); + // sliding_plot_update(GTK_SLIDING_PLOT(biases_plot), foo1); + // sliding_plot_update(GTK_SLIDING_PLOT(quat_plot), foo2); + sliding_plot_update(biases_plot, biases); + sliding_plot_update(eulers_plot, eulers); } int main (int argc, char** argv) { @@ -92,18 +45,28 @@ int main (int argc, char** argv) { gtk_init(&argc, &argv); GtkWidget *window = gtk_window_new (GTK_WINDOW_TOPLEVEL); - gtk_widget_set_size_request (window, 1240, 600); + gtk_widget_set_size_request (window, 1280, 480); GtkWidget *vbox1 = gtk_vbox_new (FALSE, 0); gtk_container_add (GTK_CONTAINER (window), vbox1); - gtk_box_pack_start (GTK_BOX (vbox1), plot_frame_new(), TRUE, TRUE, 0); + + GtkWidget *frame = gtk_frame_new ("Biases"); + gtk_container_set_border_width (GTK_CONTAINER (frame), 10); + gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0); + biases_plot = sliding_plot_new(3); + gtk_container_add (GTK_CONTAINER (frame), biases_plot ); + + frame = gtk_frame_new ("Eulers"); + gtk_container_set_border_width (GTK_CONTAINER (frame), 10); + gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0); + eulers_plot = sliding_plot_new(3); + gtk_container_add (GTK_CONTAINER (frame), eulers_plot ); gtk_widget_show_all(window); IvyInit ("IvyGtkButton", "IvyGtkButton READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_AHRS_STATE, NULL, "^77 AHRS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - //IvyBindMsg(on_AHRS_STATE, NULL, "^77 AHRS (.*)"); + IvyBindMsg(on_AHRS_STATE, biases_plot, "^77 AHRS_STATE (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); IvyStart("127.255.255.255"); gtk_main(); diff --git a/sw/logalizer/flight_gear.c b/sw/logalizer/flight_gear.c new file mode 100644 index 0000000000..883cc9326a --- /dev/null +++ b/sw/logalizer/flight_gear.c @@ -0,0 +1,416 @@ +#include "flight_gear.h" + +#include +#include +#include + +#include "utils.h" + +void net_fdm_ntoh (struct FGNetFDM* fdm) { + fdm->version = ntohl(fdm->version); + htond(&fdm->latitude); + htond(&fdm->longitude); + htond(&fdm->altitude); + htonf(&fdm->agl); + + htonf(&fdm->phi); + htonf(&fdm->theta); + htonf(&fdm->psi); + htonf(&fdm->alpha); + htonf(&fdm->beta); + + htonf(&fdm->phidot); + htonf(&fdm->thetadot); + htonf(&fdm->psidot); + htonf(&fdm->vcas); + htonf(&fdm->climb_rate); + + htonf(&fdm->v_north); + htonf(&fdm->v_east); + htonf(&fdm->v_down); + htonf(&fdm->v_wind_body_north); + htonf(&fdm->v_wind_body_east); + htonf(&fdm->v_wind_body_down); + + htonf(&fdm->A_X_pilot); + htonf(&fdm->A_Y_pilot); + htonf(&fdm->A_Z_pilot); + htonf(&fdm->stall_warning); + htonf(&fdm->slip_deg); + + fdm->num_engines = ntohl(fdm->num_engines); + int i; + for (i=0; ieng_state[i] = ntohl(fdm->eng_state[i]); + htonf(&fdm->rpm[i]); + htonf(&fdm->fuel_flow[i]); + htonf(&fdm->egt[i]); + htonf(&fdm->cht[i]); + htonf(&fdm->mp_osi[i]); + htonf(&fdm->tit[i]); + htonf(&fdm->oil_temp[i]); + htonf(&fdm->oil_px[i]); + } + + fdm->num_tanks = ntohl(fdm->num_tanks); + for (i=0; ifuel_quantity[i]); + } + + fdm->num_wheels = ntohl(fdm->num_wheels); + for (i=0; iwow[i] = ntohl(fdm->wow[i]); + htonf(&fdm->gear_pos[i]); + htonf(&fdm->gear_steer[i]); + htonf(&fdm->gear_compression[i]); + } + + fdm->cur_time = ntohl(fdm->cur_time); + fdm->warp = ntohl(fdm->warp); + + htonf(&fdm->visibility); + + htonf(&fdm->elevator); + htonf(&fdm->elevator_trim_tab); + htonf(&fdm->left_flap); + htonf(&fdm->right_flap); + htonf(&fdm->left_aileron); + htonf(&fdm->right_aileron); + htonf(&fdm->rudder); + htonf(&fdm->nose_wheel); + htonf(&fdm->speedbrake); + htonf(&fdm->spoilers); + +} + +void net_fdm_dump (struct FGNetFDM* fdm) { + printf("net_fdm (version %d size %d)\n",fdm->version, sizeof( *fdm)); + printf(" lat, lon, alt, agl\n [%f %f %f %f]\n", + fdm->latitude, fdm->longitude, fdm->altitude, fdm->agl); + printf(" phi, theta, psi, alpha, beta\n [%f %f %f %f %f]\n", + fdm->phi, fdm->theta, fdm->psi, fdm->alpha, fdm->beta); + printf(" phidot, thetadot, psidot, vcas, climb_rate\n [%f %f %f %f %f]\n", + fdm->phidot, fdm->thetadot, fdm->psidot, fdm->vcas, fdm->climb_rate); + printf(" v_north, v_east, v_down\n [%f %f %f]\n", + fdm->v_north, fdm->v_east, fdm->v_down); + printf(" v_wind_body_north, v_wind_body_east, v_wind_body_down\n [%f %f %f]\n", + fdm->v_wind_body_north, fdm->v_wind_body_east, fdm->v_wind_body_down); + + /* [......] */ + + printf(" cur_time, warp\n [%u %u]\n", fdm->cur_time, fdm->warp); + printf(" visibility [%f]\n", fdm->visibility); + printf(" elevator, elevator_trim_tab\n [%f %f]\n", + fdm->elevator, fdm->elevator_trim_tab); + printf(" left_flap, right_flap\n [%f %f]\n", + fdm->left_flap, fdm->right_flap); + printf(" left_aileron, right_aileron\n [%f %f]\n", + fdm->left_aileron, fdm->right_aileron); + printf(" rudder, nose_wheel\n [%f %f]\n", + fdm->rudder, fdm->nose_wheel); + printf(" speedbrake, spoilers\n [%f %f]\n", + fdm->speedbrake, fdm->spoilers); + +} + +void net_fdm_init ( struct FGNetFDM* fdm ) { + + fdm->version = FG_NET_FDM_VERSION; + + fdm->latitude = 0.656480; + fdm->longitude = -2.135537; + fdm->altitude = 2.; + fdm->agl = 1.111652; + + fdm->phi = 0.; + fdm->theta = 0.; + fdm->psi = 5.20; + fdm->alpha = 0.; + fdm->beta = 0.; + + fdm->phidot = 0.; + fdm->thetadot = 0.; + fdm->psidot = 0.; + fdm->vcas = 0.; + fdm->climb_rate = 0.; + fdm->v_north = 0.; + fdm->v_east = 0.; + fdm->v_down = 0.; + + fdm->v_wind_body_north = 0.; + fdm->v_wind_body_east = 0.; + fdm->v_wind_body_down = 0.; + + fdm->A_X_pilot = 0.; + fdm->A_Y_pilot = 0.; + fdm->A_Z_pilot = 0.; + + fdm->stall_warning = 0.; + fdm->slip_deg = 0.; + + fdm->num_engines = 2; + fdm->eng_state[0] = 0; + fdm->eng_state[1] = 0; + fdm->rpm[0] = 0.; + fdm->rpm[1] = 0.; + fdm->fuel_flow[0] = 0.; + fdm->fuel_flow[1] = 0.; + fdm->egt[0] = 0.; + fdm->egt[1] = 0.; + fdm->egt[0] = 0.; + fdm->egt[1] = 0.; + fdm->cht[0] = 0.; + fdm->cht[1] = 0.; + fdm->mp_osi[0] = 0.; + fdm->mp_osi[1] = 0.; + fdm->tit[0] = 0.; + fdm->tit[1] = 0.; + fdm->oil_temp[0] = 0.; + fdm->oil_temp[1] = 0.; + fdm->oil_px[0] = 0.; + fdm->oil_px[1] = 0.; + + fdm->num_tanks = 4; + fdm->fuel_quantity[0] = 0.; + fdm->fuel_quantity[1] = 0.; + fdm->fuel_quantity[2] = 0.; + fdm->fuel_quantity[3] = 0.; + + fdm->num_wheels = 1; + fdm->wow[0] = 1; + fdm->gear_pos[0] = 0.; + fdm->gear_steer[0] = 0.; + fdm->gear_compression[0] = 0.; + + fdm->cur_time = 3213082700ul; + fdm->warp = 0; + fdm->visibility = 1000.; + + fdm->elevator = 0.; + fdm->elevator_trim_tab = 0.; + fdm->left_flap = 0.; + fdm->right_flap = 0.; + fdm->left_aileron = 0.; + fdm->right_aileron = 0.; + fdm->rudder = 0.; + fdm->nose_wheel = 0.; + fdm->speedbrake = 0.; + fdm->spoilers = 0.; + +} + +void net_gui_init (struct FGNetGUI* gui) { + gui->version = FG_NET_GUI_VERSION; + gui->latitude = 0.656480; + gui->longitude = -2.135537; + gui->altitude = 0.807609; + gui->agl = 1.111652; + + gui->phi = 0.; + gui->theta = 0.; + gui->psi = 5.20; + + gui->vcas = 0.; + gui->climb_rate = 0.; + + gui->num_tanks = 1; + gui->fuel_quantity[0] = 0.; + + gui->cur_time = 3198060679ul; + gui->warp = 1122474394ul; + + gui->ground_elev = 0.; + + gui->tuned_freq = 125.65; + gui->nav_radial = 90.; + gui->in_range = 1; + gui->dist_nm = 10.; + gui->course_deviation_deg = 0.; + gui->gs_deviation_deg = 0.; +} + +void net_gui_hton (struct FGNetGUI* gui) { + gui->version = ntohl(gui->version); + htond(&gui->latitude); + htond(&gui->longitude); + htonf(&gui->altitude); + htonf(&gui->agl); + + htonf(&gui->phi); + htonf(&gui->theta); + htonf(&gui->psi); + +} + +void net_gui_dump (struct FGNetGUI* gui) { + printf("net_gui (version %d size %d)\n",gui->version, sizeof( *gui)); + printf(" lat, lon, alt, agl\n [%f %f %f %f]\n", + gui->latitude, gui->longitude, gui->altitude, gui->agl); + printf(" phi, theta, psi\n [%f %f %f]\n", + gui->phi, gui->theta, gui->psi); + printf(" vcas, climb_rate\n [%f %f]\n", + gui->vcas, gui->climb_rate); + printf(" num_tanks, fuel[0], fuel[1], fuel[2], fuel[3]\n [%u %f %f %f %f]\n", + gui->num_tanks, gui->fuel_quantity[0], gui->fuel_quantity[1], + gui->fuel_quantity[2], gui->fuel_quantity[3]); + printf(" cur_time, warp\n [%u %u]\n", gui->cur_time, gui->warp); + printf(" ground_elev\n [%f]\n", + gui->ground_elev); + printf(" tuned_freq, nav_radial, in_range\n [%f %f %u]\n", + gui->tuned_freq, gui->nav_radial, gui->in_range); + printf(" dist_nm, course_deviation_deg, gs_deviation_deg\n [%f %f %f]\n", + gui->dist_nm, gui->course_deviation_deg, gui->gs_deviation_deg); +} + +void net_ctrls_init(struct FGNetCtrls* ctrls) { + ctrls->version = FG_NET_CTRLS_VERSION; + ctrls->aileron = 0.; + ctrls->elevator = 0.; + ctrls->rudder = 0.; + ctrls->aileron_trim = 0.; + ctrls->elevator_trim = 0.; + ctrls->rudder_trim = 0.; + ctrls->flaps = 0.; + ctrls->spoilers = 0.; + ctrls->speedbrake = 0.; + + ctrls->flaps_power = 1; + ctrls->flap_motor_ok = 1; + + ctrls->num_engines = 2; + int i; + for (i=0; i< 2; i++) { + ctrls->master_bat[i] = 1; + ctrls->master_alt[i] = 1; + ctrls->magnetos[i] = 1; + ctrls->starter_power[i] = 0; + ctrls->throttle[i] = 0.; + ctrls->mixture[i] = 0.; + ctrls->condition[i] = 0.; + ctrls->fuel_pump_power[i] = 1; + ctrls->prop_advance[i] = 1.; + ctrls->feed_tank_to[i] = 1; + ctrls->reverse[i] = 0; + ctrls->engine_ok[i] = 1; + ctrls->mag_left_ok[i] = 1; + ctrls->mag_right_ok[i] = 1; + ctrls->spark_plugs_ok[i] = 1; + ctrls->oil_press_status[i] = 0; + ctrls->fuel_pump_ok[i] = 1; + } + ctrls->num_tanks = 1; + ctrls->fuel_selector[0] = 1; + + ctrls->xfer_pump[0] = 1; + ctrls->cross_feed = 0; + + ctrls->brake_left = 0.; + ctrls->brake_right = 0.; + ctrls->copilot_brake_left = 0.; + ctrls->copilot_brake_right = 0.; + ctrls->brake_parking = 0.; + + ctrls->gear_handle = 1; + ctrls->master_avionics = 0; + + ctrls->comm_1 = 123.4; + ctrls->comm_2 = 123.4; + ctrls->nav_1 = 123.4; + ctrls->nav_2 = 123.4; + + ctrls->wind_speed_kt = 0.; + ctrls->wind_dir_deg = 0.; + ctrls->turbulence_norm = 0.; + + ctrls->temp_c = 25.; + ctrls->press_inhg = 25.; + + ctrls->hground = 25.; + ctrls->magvar = 0.; + + ctrls->speedup = 1; + +} + +void net_ctrls_ntoh(struct FGNetCtrls* ctrls) { + ctrls->version = ntohl(ctrls->version); + htond(&ctrls->aileron); + htond(&ctrls->elevator); + htond(&ctrls->rudder); + htond(&ctrls->aileron_trim); + htond(&ctrls->elevator_trim); + htond(&ctrls->rudder_trim); + htond(&ctrls->flaps); + + +} + + +void net_ctrls_dump(struct FGNetCtrls* ctrls) { + printf("net_ctrls (version %d size %d)\n",ctrls->version, sizeof( *ctrls)); + printf(" aileron elevator rudder\n [%f %f %f]\n", + ctrls->aileron, ctrls->elevator, ctrls->rudder); + printf(" throttle\n [%f %f %f]\n", + ctrls->throttle[0], ctrls->throttle[1], ctrls->throttle[2]); + +} + +void mplay_msg_dump ( struct FGMplayMsg *msg ) { + printf("mplay_msg (size %d id %d)\n",msg->header.len, msg->header.id); + printf(" reply_addr %d\n", msg->header.reply_addr); + printf(" reply_port %d\n", msg->header.reply_port); + printf(" callsign "); + int i; + for (i=0; i < MP_MAX_CALLSIGN_LEN; i++) { + printf("%c", msg->header.callsign[i]); + } + printf("\n"); + printf(" model %-96s (%d)\n", msg->pos.model, strlen(msg->pos.model)); + printf(" time %f lag %f\n", msg->pos.time, msg->pos.lag); + printf(" position %f %f %f\n", msg->pos.position[0], msg->pos.position[1], msg->pos.position[2]); + printf(" orientation %f %f %f %f\n", msg->pos.orientation[0], msg->pos.orientation[1], msg->pos.orientation[2], msg->pos.orientation[3]); + printf(" linear_vel %f %f %f\n", msg->pos.linear_vel[0], msg->pos.linear_vel[1], msg->pos.linear_vel[2]); + printf(" angular_vel %f %f %f\n", msg->pos.angular_vel[0], msg->pos.angular_vel[1], msg->pos.angular_vel[2]); + printf(" linear_accel %f %f %f\n", msg->pos.linear_accel[0], msg->pos.linear_accel[1], msg->pos.linear_accel[2]); + printf(" angular_accel %f %f %f\n", msg->pos.angular_accel[0], msg->pos.angular_accel[1], msg->pos.angular_accel[2]); + printf("sizeof %d\n", sizeof(struct FGMplayMsg)); + +} + +void mplay_msg_ntoh ( struct FGMplayMsg* msg) { + msg->header.magic = ntohl(msg->header.magic); + msg->header.version = ntohl(msg->header.version); + msg->header.id = ntohl(msg->header.id); + msg->header.len = ntohl(msg->header.len); + msg->header.reply_addr = ntohl(msg->header.reply_addr); + msg->header.reply_port = ntohl(msg->header.reply_port); + htond(&msg->pos.time); + htond(&msg->pos.lag); + htond(&msg->pos.position[0]); + htond(&msg->pos.position[1]); + htond(&msg->pos.position[2]); + htonf(&msg->pos.orientation[0]); + htonf(&msg->pos.orientation[1]); + htonf(&msg->pos.orientation[2]); + htonf(&msg->pos.orientation[3]); + htonf(&msg->pos.linear_vel[0]); + htonf(&msg->pos.linear_vel[1]); + htonf(&msg->pos.linear_vel[2]); + htonf(&msg->pos.angular_vel[0]); + htonf(&msg->pos.angular_vel[1]); + htonf(&msg->pos.angular_vel[2]); + htonf(&msg->pos.linear_accel[0]); + htonf(&msg->pos.linear_accel[1]); + htonf(&msg->pos.linear_accel[2]); + htonf(&msg->pos.angular_accel[0]); + htonf(&msg->pos.angular_accel[1]); + htonf(&msg->pos.angular_accel[2]); +} + +void mplay_msg_init ( struct FGMplayMsg* msg) { + msg->header.magic = MP_MSG_MAGIC; + msg->header.version = MP_PROTO_VER; + msg->header.id = 7; + msg->header.len = 232; + +} diff --git a/sw/logalizer/flight_gear.h b/sw/logalizer/flight_gear.h new file mode 100644 index 0000000000..dea9105a71 --- /dev/null +++ b/sw/logalizer/flight_gear.h @@ -0,0 +1,311 @@ +#ifndef FLIGHT_GEAR_H +#define FLIGHT_GEAR_H + +#include + +#define FG_NET_CTRLS_VERSION 27 +#define FG_NET_CTRLS_MAX_ENGINES 4 +#define FG_NET_CTRLS_MAX_WHEELS 16 +#define FG_NET_CTRLS_MAX_TANKS 8 +#define FG_NET_CTRLS_RESERVED_SPACE 25 + +struct FGNetCtrls { + uint32_t version; // increment when data values change + + // Aero controls + double aileron; // -1 ... 1 + double elevator; // -1 ... 1 + double rudder; // -1 ... 1 + double aileron_trim; // -1 ... 1 + double elevator_trim; // -1 ... 1 + double rudder_trim; // -1 ... 1 + double flaps; // 0 ... 1 + double spoilers; + double speedbrake; + + // Aero control faults + uint32_t flaps_power; // true = power available + uint32_t flap_motor_ok; + + // Engine controls + uint32_t num_engines; // number of valid engines + uint32_t master_bat[FG_NET_CTRLS_MAX_ENGINES]; + uint32_t master_alt[FG_NET_CTRLS_MAX_ENGINES]; + uint32_t magnetos[FG_NET_CTRLS_MAX_ENGINES]; + uint32_t starter_power[FG_NET_CTRLS_MAX_ENGINES];// true = starter power + double throttle[FG_NET_CTRLS_MAX_ENGINES]; // 0 ... 1 + double mixture[FG_NET_CTRLS_MAX_ENGINES]; // 0 ... 1 + double condition[FG_NET_CTRLS_MAX_ENGINES]; // 0 ... 1 + uint32_t fuel_pump_power[FG_NET_CTRLS_MAX_ENGINES];// true = on + double prop_advance[FG_NET_CTRLS_MAX_ENGINES]; // 0 ... 1 + uint32_t feed_tank_to[4]; + uint32_t reverse[4]; + + // Engine faults + uint32_t engine_ok[FG_NET_CTRLS_MAX_ENGINES]; + uint32_t mag_left_ok[FG_NET_CTRLS_MAX_ENGINES]; + uint32_t mag_right_ok[FG_NET_CTRLS_MAX_ENGINES]; + uint32_t spark_plugs_ok[FG_NET_CTRLS_MAX_ENGINES]; // false = fouled plugs + uint32_t oil_press_status[FG_NET_CTRLS_MAX_ENGINES];// 0 = normal, 1 = low, 2 = full fail + uint32_t fuel_pump_ok[FG_NET_CTRLS_MAX_ENGINES]; + + // Fuel management + uint32_t num_tanks; // number of valid tanks + uint32_t fuel_selector[FG_NET_CTRLS_MAX_TANKS]; // false = off, true = on + uint32_t xfer_pump[5]; // specifies transfer from array + // value tank to tank specified by + // int value + uint32_t cross_feed; // false = off, true = on + + // Brake controls + double brake_left; + double brake_right; + double copilot_brake_left; + double copilot_brake_right; + double brake_parking; + + // Landing Gear + uint32_t gear_handle; // true=gear handle down; false= gear handle up + + // Switches + uint32_t master_avionics; + + // nav and Comm + double comm_1; + double comm_2; + double nav_1; + double nav_2; + + // wind and turbulance + double wind_speed_kt; + double wind_dir_deg; + double turbulence_norm; + + // temp and pressure + double temp_c; + double press_inhg; + + // other information about environment + double hground; // ground elevation (meters) + double magvar; // local magnetic variation in degs. + + // hazards + uint32_t icing; // icing status could me much + // more complex but I'm + // starting simple here. + + // simulation control + uint32_t speedup; // integer speedup multiplier + uint32_t freeze; // 0=normal + // 0x01=master + // 0x02=position + // 0x04=fuel + + // --- New since FlightGear 0.9.10 (FG_NET_CTRLS_VERSION = 27) + + // --- Add new variables just before this line. + + uint32_t reserved[FG_NET_CTRLS_RESERVED_SPACE]; // 100 bytes reserved for future use. +}; + + +#define FG_NET_FDM_VERSION 23 +#define FG_NET_FDM_MAX_ENGINES 4 +#define FG_NET_FDM_MAX_WHEELS 3 +#define FG_NET_FDM_MAX_TANKS 4 + + +struct FGNetFDM { + + uint32_t version; // increment when data values change + uint32_t padding; // padding + + // Positions + double longitude; // geodetic (radians) + double latitude; // geodetic (radians) + double altitude; // above sea level (meters) + float agl; // above ground level (meters) + float phi; // roll (radians) + float theta; // pitch (radians) + float psi; // yaw or true heading (radians) + float alpha; // angle of attack (radians) + float beta; // side slip angle (radians) + + // Velocities + float phidot; // roll rate (radians/sec) + float thetadot; // pitch rate (radians/sec) + float psidot; // yaw rate (radians/sec) + float vcas; // calibrated airspeed + float climb_rate; // feet per second + float v_north; // north velocity in local/body frame, fps + float v_east; // east velocity in local/body frame, fps + float v_down; // down/vertical velocity in local/body frame, fps + float v_wind_body_north; // north velocity in local/body frame + // relative to local airmass, fps + float v_wind_body_east; // east velocity in local/body frame + // relative to local airmass, fps + float v_wind_body_down; // down/vertical velocity in local/body + // frame relative to local airmass, fps + + // Accelerations + float A_X_pilot; // X accel in body frame ft/sec^2 + float A_Y_pilot; // Y accel in body frame ft/sec^2 + float A_Z_pilot; // Z accel in body frame ft/sec^2 + // Stall + float stall_warning; // 0.0 - 1.0 indicating the amount of stall + float slip_deg; // slip ball deflection + + // Pressure + + // Engine status + uint32_t num_engines; // Number of valid engines + uint32_t eng_state[FG_NET_FDM_MAX_ENGINES];// Engine state (off, cranking, running) + float rpm[FG_NET_FDM_MAX_ENGINES]; // Engine RPM rev/min + float fuel_flow[FG_NET_FDM_MAX_ENGINES]; // Fuel flow gallons/hr + float egt[FG_NET_FDM_MAX_ENGINES]; // Exhuast gas temp deg F + float cht[FG_NET_FDM_MAX_ENGINES]; // Cylinder head temp deg F + float mp_osi[FG_NET_FDM_MAX_ENGINES]; // Manifold pressure + float tit[FG_NET_FDM_MAX_ENGINES]; // Turbine Inlet Temperature + float oil_temp[FG_NET_FDM_MAX_ENGINES]; // Oil temp deg F + float oil_px[FG_NET_FDM_MAX_ENGINES]; // Oil pressure psi + + // Consumables + uint32_t num_tanks; // Max number of fuel tanks + float fuel_quantity[FG_NET_FDM_MAX_TANKS]; + + // Gear status + uint32_t num_wheels; + uint32_t wow[FG_NET_FDM_MAX_WHEELS]; + float gear_pos[FG_NET_FDM_MAX_WHEELS]; + float gear_steer[FG_NET_FDM_MAX_WHEELS]; + float gear_compression[FG_NET_FDM_MAX_WHEELS]; + + // Environment + uint32_t cur_time; // current unix time + // FIXME: make this uint64_t before 2038 + int32_t warp; // offset in seconds to unix time + float visibility; // visibility in meters (for env. effects) + + // Control surface positions (normalized values) + float elevator; + float elevator_trim_tab; + float left_flap; + float right_flap; + float left_aileron; + float right_aileron; + float rudder; + float nose_wheel; + float speedbrake; + float spoilers; +}; + +struct FGNetMiniFDM { + uint32_t version; // increment when data values change + + // Positions + double longitude; // geodetic (radians) + double latitude; // geodetic (radians) + double altitude; // above sea level (meters) + double agl; // above ground level (meters) + double phi; // roll (radians) + double theta; // pitch (radians) + double psi; // yaw or true heading (radians) + + // Velocities + double vcas; + double climb_rate; // feet per second + + // Consumables + uint32_t num_tanks; // Max number of fuel tanks + double fuel_quantity[FG_NET_FDM_MAX_TANKS]; + + // Environment + uint32_t cur_time; // current unix time + int32_t warp; // offset in seconds to unix time +}; + +#define FG_NET_GUI_VERSION 7 +#define FG_NET_GUI_MAX_TANKS 4 +struct FGNetGUI { + uint32_t version; // increment when data values change + + // Positions + double longitude; // geodetic (radians) + double latitude; // geodetic (radians) + float altitude; // above sea level (meters) + float agl; // above ground level (meters) + float phi; // roll (radians) + float theta; // pitch (radians) + float psi; // yaw or true heading (radians) + + // Velocities + float vcas; + float climb_rate; // feet per second + + // Consumables + uint32_t num_tanks; // Max number of fuel tanks + float fuel_quantity[FG_NET_GUI_MAX_TANKS]; + + // Environment + uint32_t cur_time; // current unix time + // FIXME: make this uint64_t before 2038 + uint32_t warp; // offset in seconds to unix time + float ground_elev; // ground elev (meters) + + // Approach + float tuned_freq; // currently tuned frequency + float nav_radial; // target nav radial + uint32_t in_range; // tuned navaid is in range? + float dist_nm; // distance to tuned navaid in nautical miles + float course_deviation_deg; // degrees off target course + float gs_deviation_deg; // degrees off target glide slope +}; + +extern void net_fdm_dump (struct FGNetFDM* fdm); +extern void net_fdm_ntoh (struct FGNetFDM* fdm); +extern void net_fdm_init (struct FGNetFDM* fdm); + +extern void net_gui_init (struct FGNetGUI* gui); +extern void net_gui_hton (struct FGNetGUI* gui); +extern void net_gui_dump (struct FGNetGUI* gui); + +extern void net_ctrls_init(struct FGNetCtrls* ctrls); +extern void net_ctrls_dump(struct FGNetCtrls* ctrls); +extern void net_ctrls_ntoh(struct FGNetCtrls* ctrls); + +#define MP_MAX_CALLSIGN_LEN 8 +#define MP_MAX_MODEL_NAME_LEN 96 +#define MP_MSG_MAGIC 0x46474653 +#define MP_PROTO_VER 0x00010001 + +struct FGMplayHdr { + uint32_t magic; + uint32_t version; + uint32_t id; + uint32_t len; + uint32_t reply_addr; + uint32_t reply_port; + char callsign[MP_MAX_CALLSIGN_LEN]; +}; + +struct FGMplayPosMsg { + char model[MP_MAX_MODEL_NAME_LEN]; + double time; + double lag; + double position[3]; + float orientation[4]; + float linear_vel[3]; + float angular_vel[3]; + float linear_accel[3]; + float angular_accel[3]; +}; + +struct FGMplayMsg { + struct FGMplayHdr header; + struct FGMplayPosMsg pos; +}; + +extern void mplay_msg_dump ( struct FGMplayMsg *msg ); +extern void mplay_msg_ntoh ( struct FGMplayMsg* msg ); +extern void mplay_msg_init ( struct FGMplayMsg* msg ); +#endif /* FLIGHT_GEAR_H */ diff --git a/sw/logalizer/network.c b/sw/logalizer/network.c new file mode 100644 index 0000000000..647dd5b838 --- /dev/null +++ b/sw/logalizer/network.c @@ -0,0 +1,38 @@ +#include "network.h" + +#include +#include + +#include +#include + + +struct FgNetChannel* open_out_channel( char* host, uint16_t port) { + struct FgNetChannel* chan = malloc(sizeof (struct FgNetChannel)); + int so_reuseaddr = 1; + struct protoent * pte = getprotobyname("UDP"); + chan->socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto); + setsockopt(chan->socket, SOL_SOCKET, SO_REUSEADDR, + &so_reuseaddr, sizeof(so_reuseaddr)); + + chan->addr.sin_family = PF_INET; + chan->addr.sin_port = htons(port); + chan->addr.sin_addr.s_addr = inet_addr(host); + + return chan; +} + +struct FgNetChannel* open_in_channel( char* host, uint16_t port) { + struct FgNetChannel* chan = open_out_channel(host, port); + int ret = bind(chan->socket, (struct sockaddr*)&chan->addr, sizeof(chan->addr)); + if (ret) + perror("binding\n"); + return chan; +} + + +void send_buf(struct FgNetChannel* chan, char* buf, int len) { + if (sendto(chan->socket, buf, len, 0, + (struct sockaddr*)&chan->addr, sizeof(chan->addr)) == -1) + printf("error sending\n"); +} diff --git a/sw/logalizer/network.h b/sw/logalizer/network.h new file mode 100644 index 0000000000..476e449880 --- /dev/null +++ b/sw/logalizer/network.h @@ -0,0 +1,18 @@ +#ifndef NETWORK_H +#define NETWORK_H + +#include +#include +#include + +struct FgNetChannel { + int socket; + struct sockaddr_in addr; +}; + +extern struct FgNetChannel* open_out_channel( char* host, uint16_t port ); +extern struct FgNetChannel* open_in_channel( char* host, uint16_t port ); +extern void send_buf(struct FgNetChannel* chan, char* buf, int len); + + +#endif /* NETWORK_H */ diff --git a/sw/logalizer/sliding_plot.c b/sw/logalizer/sliding_plot.c new file mode 100644 index 0000000000..4cc8aea861 --- /dev/null +++ b/sw/logalizer/sliding_plot.c @@ -0,0 +1,83 @@ +#include +#include +#include +#include +#include +#include +#include + +#include + +#define NB_POINTS 2000 +#define NB_POINTS_BY_STEP 1 +//500 + +const GdkColor colors[] = {{65535, 0, 0}, {0, 65535, 0}, {0, 0, 65535}, {0, 0, 0}}; +#define NB_COLORS 4 + +#define REFRESH_RATE 166 /* ms */ +static gboolean timeout_callback(gpointer data); + +GtkWidget* sliding_plot_new(guint nb_plot) { + GtkWidget* databox = gtk_databox_new (); + g_object_set_data(G_OBJECT(databox), "nb_plot", (gpointer)nb_plot); + gfloat *X = g_new0 (gfloat, NB_POINTS); + g_object_set_data(G_OBJECT(databox), "X", X); + gpointer *Y = g_new0 (gpointer, nb_plot); + g_object_set_data(G_OBJECT(databox), "Y", Y); + guint nb_data = 0; + g_object_set_data(G_OBJECT(databox), "nb_data", (gpointer)nb_data); + guint _time = 0; + g_object_set_data(G_OBJECT(databox), "time", (gpointer)_time); + guint i; + for (i=0; i< nb_plot; i++) { + Y[i] = g_new0 (gfloat, NB_POINTS); + GtkDataboxGraph *graph = gtk_databox_lines_new (NB_POINTS, X, Y[i], (GdkColor*)&(colors[i%NB_COLORS]), 1); + gtk_databox_graph_add (GTK_DATABOX (databox), graph); + } + + // GtkDataboxGraph *grid = gtk_databox_grid_new (10, 10, (GdkColor*)&(colors[3]), 2); + // gtk_databox_graph_add (GTK_DATABOX (databox), grid); + g_timeout_add(REFRESH_RATE, timeout_callback, databox); + + return databox; +} + +void sliding_plot_update(GtkWidget* plot, float* values) { + guint i, j; + gfloat *X = g_object_get_data(G_OBJECT(plot), "X"); + gpointer *Y = g_object_get_data(G_OBJECT(plot), "Y"); + guint nb_data = (guint)g_object_get_data(G_OBJECT(plot), "nb_data"); + guint _time = (guint)g_object_get_data(G_OBJECT(plot), "time"); + guint nb_plot = (guint)g_object_get_data(G_OBJECT(plot), "nb_plot"); + + if (nb_data >= NB_POINTS) { + nb_data -= NB_POINTS_BY_STEP; + guint mem_to_move = (NB_POINTS - NB_POINTS_BY_STEP)*sizeof(gfloat); + memmove(X, &(X[NB_POINTS_BY_STEP]), mem_to_move); + for (i=0; i< nb_plot; i++) { + gfloat* y = Y[i]; + memmove(y, &(y[NB_POINTS_BY_STEP]), mem_to_move); + } + } + + for (j=nb_data; j< NB_POINTS; j++) { + X[j] = _time; + for (i=0; i< nb_plot; i++) { + gfloat* y = Y[i]; + y[j] = values[i]; + } + } + _time++; + g_object_set_data(G_OBJECT(plot), "time", (gpointer)_time); + nb_data++; + g_object_set_data(G_OBJECT(plot), "nb_data", (gpointer)nb_data); + // gtk_databox_auto_rescale (GTK_DATABOX (plot), 0.); +} + + +static gboolean timeout_callback(gpointer user_data) { + GtkDatabox* databox = GTK_DATABOX (user_data); + gtk_databox_auto_rescale (databox, 0.); + return TRUE; +} diff --git a/sw/logalizer/sliding_plot.h b/sw/logalizer/sliding_plot.h new file mode 100644 index 0000000000..9b6f1788ea --- /dev/null +++ b/sw/logalizer/sliding_plot.h @@ -0,0 +1,9 @@ +#ifndef SLIDING_PLOT_H +#define SLIDING_PLOT_H + +#include + +extern GtkWidget* sliding_plot_new(guint nb_plot); +extern void sliding_plot_update(GtkWidget* plot, float* values); + +#endif /* SLIDING_PLOT_H */ diff --git a/sw/logalizer/utils.c b/sw/logalizer/utils.c new file mode 100644 index 0000000000..24270e43e9 --- /dev/null +++ b/sw/logalizer/utils.c @@ -0,0 +1,36 @@ +#include "utils.h" + +#include + +//#include // endian tests +void htond (double *x) +{ + // if ( sgIsLittleEndian() ) { + int *Double_Overlay; + int Holding_Buffer; + + Double_Overlay = (int *) x; + Holding_Buffer = Double_Overlay [0]; + + Double_Overlay [0] = htonl (Double_Overlay [1]); + Double_Overlay [1] = htonl (Holding_Buffer); + // } else { + // return; + // } +} + +// Float version +void htonf (float *x) +{ + // if ( sgIsLittleEndian() ) { + int *Float_Overlay; + int Holding_Buffer; + + Float_Overlay = (int *) x; + Holding_Buffer = Float_Overlay [0]; + + Float_Overlay [0] = htonl (Holding_Buffer); + // } else { + // return; + // } +} diff --git a/sw/logalizer/utils.h b/sw/logalizer/utils.h new file mode 100644 index 0000000000..f6e2673a06 --- /dev/null +++ b/sw/logalizer/utils.h @@ -0,0 +1,17 @@ +#ifndef UTILS_H +#define UTILS_H + +#include + +extern void htond (double *x); +extern void htonf (float *x); + +#define RAD_OF_DEG(d) (d*M_PI/180.) +#define NORM_ANGLE_RAD(r) { \ + while (r>2*M_PI) \ + r -= 2*M_PI; \ + while (r<0) \ + r += 2*M_PI; \ + } + +#endif /* UTILS_H */