Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2006-12-03 15:37:24 +00:00
parent bbb0bd3ef7
commit fe85403ba1
11 changed files with 1093 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/logalizer/Makefile
+46 -3
View File
@@ -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)
sw/logalizer/ahrs2fg.c
+86
View File
@@ -0,0 +1,86 @@
#include <glib.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "flight_gear.h"
#include "network.h"
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
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;
}
sw/logalizer/ahrsview.c
+33 -70
View File
@@ -2,78 +2,26 @@
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gtkdatabox.h>
#include <gtkdatabox_points.h>
#include <gtkdatabox_lines.h>
#include <gtkdatabox_bars.h>
#include <gtkdatabox_grid.h>
#include <gtkdatabox_cross_simple.h>
#include <gtkdatabox_marker.h>
#include <string.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#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();
sw/logalizer/flight_gear.c
+416
View File
@@ -0,0 +1,416 @@
#include "flight_gear.h"
#include <stdio.h>
#include <netinet/in.h>
#include <string.h>
#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; i<FG_NET_FDM_MAX_ENGINES; i++) {
fdm->eng_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; i<FG_NET_FDM_MAX_TANKS; i++) {
htonf(&fdm->fuel_quantity[i]);
}
fdm->num_wheels = ntohl(fdm->num_wheels);
for (i=0; i<FG_NET_FDM_MAX_WHEELS; i++) {
fdm->wow[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;
}
sw/logalizer/flight_gear.h
+311
View File
@@ -0,0 +1,311 @@
#ifndef FLIGHT_GEAR_H
#define FLIGHT_GEAR_H
#include <stdint.h>
#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 */
sw/logalizer/network.c
+38
View File
@@ -0,0 +1,38 @@
#include "network.h"
#include <netdb.h>
#include <netinet/in.h>
#include <stdlib.h>
#include <stdio.h>
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");
}
sw/logalizer/network.h
+18
View File
@@ -0,0 +1,18 @@
#ifndef NETWORK_H
#define NETWORK_H
#include <inttypes.h>
#include <sys/socket.h>
#include <arpa/inet.h>
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 */
sw/logalizer/sliding_plot.c
+83
View File
@@ -0,0 +1,83 @@
#include <gtkdatabox.h>
#include <gtkdatabox_points.h>
#include <gtkdatabox_lines.h>
#include <gtkdatabox_bars.h>
#include <gtkdatabox_grid.h>
#include <gtkdatabox_cross_simple.h>
#include <gtkdatabox_marker.h>
#include <string.h>
#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;
}
sw/logalizer/sliding_plot.h
+9
View File
@@ -0,0 +1,9 @@
#ifndef SLIDING_PLOT_H
#define SLIDING_PLOT_H
#include <gtk/gtk.h>
extern GtkWidget* sliding_plot_new(guint nb_plot);
extern void sliding_plot_update(GtkWidget* plot, float* values);
#endif /* SLIDING_PLOT_H */
sw/logalizer/utils.c
+36
View File
@@ -0,0 +1,36 @@
#include "utils.h"
#include <netinet/in.h>
//#include <simgear/io/lowlevel.hxx> // 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;
// }
}
sw/logalizer/utils.h
+17
View File
@@ -0,0 +1,17 @@
#ifndef UTILS_H
#define UTILS_H
#include <math.h>
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 */