From b742e5f968416916a05d2445aeb277f93a175fbf Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 22 Oct 2014 20:17:13 +0200 Subject: [PATCH] [logalizer] remove cruft and fix compilation of disp3d maybe just remove disp3d and some of the other remaining stuff as well... --- .gitignore | 6 +- sw/logalizer/Makefile | 95 +++----------- sw/logalizer/ahrs_utils.h | 230 --------------------------------- sw/logalizer/ahrsview.c | 74 ----------- sw/logalizer/imuview.c | 79 ------------ sw/logalizer/ivy_example.c | 27 ---- sw/logalizer/motor_bench.c | 231 ---------------------------------- sw/logalizer/plot_roll_loop.c | 60 --------- sw/logalizer/test_2.c | 109 ---------------- 9 files changed, 17 insertions(+), 894 deletions(-) delete mode 100644 sw/logalizer/ahrs_utils.h delete mode 100644 sw/logalizer/ahrsview.c delete mode 100644 sw/logalizer/imuview.c delete mode 100644 sw/logalizer/ivy_example.c delete mode 100644 sw/logalizer/motor_bench.c delete mode 100644 sw/logalizer/plot_roll_loop.c delete mode 100644 sw/logalizer/test_2.c diff --git a/.gitignore b/.gitignore index 071ee5336a..12dd5d209c 100644 --- a/.gitignore +++ b/.gitignore @@ -122,14 +122,12 @@ paparazzi.sublime-workspace /sw/logalizer/gtk_export.ml /sw/logalizer/sd2log /sw/logalizer/plotprofile -/sw/logalizer/ahrsview /sw/logalizer/ctrlstick /sw/logalizer/ffjoystick -/sw/logalizer/imuview -/sw/logalizer/ivy_example -/sw/logalizer/motor_bench /sw/logalizer/tmclient /sw/logalizer/openlog2tlm +/sw/logalizer/ahrs2fg +/sw/logalizer/disp3d # /sw/simulator/ /sw/simulator/gaia diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile index dc2a56a1a9..e568d836e6 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -52,13 +52,6 @@ sd2log : sd2log.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $^ -CC = gcc -CFLAGS=-g -O2 -Wall -LDFLAGS= - -openlog2tlm: openlog2tlm.c - @echo CC $@ - $(Q)$(CC) $(CFLAGS) -g -o $@ $^ # Target for bytecode executable (if ocamlopt is not available) # plot : log_file.cmo gtk_export.cmo export.cmo plot.cmo @@ -93,27 +86,20 @@ gtk_export.ml : export.glade $(Q)rm -f $($@_TMP) -pt : ahrsview imuview ahrs2fg CC = gcc -CFLAGS=-g -O2 -Wall $(shell pkg-config gtk+-2.0 --cflags) -LDFLAGS=$(shell pkg-config gtk+-2.0 --libs) -s -lgtkdatabox $(shell pkg-config --libs ivy-glib) +CFLAGS=-g -O2 -Wall -motor_bench : motor_bench.c sliding_plot.c - $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) +openlog2tlm: openlog2tlm.c + @echo CC $@ + $(Q)$(CC) $(CFLAGS) -o $@ $^ -ahrsview : ahrsview.c sliding_plot.c - $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) - -imuview : imuview.c sliding_plot.c - $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) - -plot_roll_loop : plot_roll_loop.c sliding_plot.c - $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) - -test_sliding_plot: test_sliding_plot.c sliding_plot.c - $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) +DISP3D_CFLAGS = $(shell pkg-config --cflags ivy-glib gtk+-2.0 gtkgl-2.0) +DISP3D_LDFLAGS = $(shell pkg-config --libs ivy-glib gtk+-2.0 gtkgl-2.0) $(shell pcre-config --libs) +disp3d: disp3d.c + @echo CC $@ + $(Q)$(CC) $(CFLAGS) $(DISP3D_CFLAGS) -o $@ $^ $(DISP3D_LDFLAGS) IVY_C_LIBS = $(shell pkg-config --libs ivy-glib) $(shell pcre-config --libs) @@ -130,65 +116,13 @@ ifeq ("$(UNAME)","Darwin") endif - -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 -lglibivy $(shell pcre-config --libs) - -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 - @echo CC $@ - $(Q)$(CC) $(MORE_CFLAGS) -g -o $@ $^ $(MORE_FLAGS) - plotprofile: plotprofile.c @echo CC $@ - $(Q)$(CC) -g -O2 -Wall $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) - -test1: test1.c - @echo CC $@ - $(Q)$(CC) $(MORE_CFLAGS) -g -o $@ $^ $(MORE_FLAGS) -lglut - -test3: test3.c sliding_plot.c - @echo CC $@ - $(Q)$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) - - -#FGFS_PREFIX=/home/poine/local -FGFS_PREFIX=/home/poine/flightgear -#FGFS_PREFIX=/usr/local - -#FGFS_ROOT = /home/poine/local -FGFS_ROOT = /home/poine/flightgear -FGFS = $(FGFS_PREFIX)/bin/fgfs -#FGFS = /usr/games/fgfs -#FGFS_ENV = LD_LIBRARY_PATH=/usr/local/lib:$(FGFS_ROOT)/lib -FGFS_ENV = LD_LIBRARY_PATH=$(FGFS_ROOT)/lib -FGFS_COMMON_ARGS = --fg-root=$(FGFS_ROOT) --aircraft=A320 --timeofday=noon -#FGFS_COMMON_ARGS = --aircraft=737-300 --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) + $(Q)$(CC) $(CFLAGS) $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) ahrs2fg: ahrs2fg.c network.c flight_gear.c utils.c - $(CC) -g -O2 -Wall $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) -lm + $(CC) $(CFLAGS) $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) -lm -test_samere: test_samere.c network.c flight_gear.c utils.c - $(CC) $(CFLAGS) -I../airborne -I../airborne/test -I../include -g -o $@ $^ $(LDFLAGS) - -ivy_example: ivy_example.c - $(CC) -g -O2 -Wall $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) tmclient: tmclient.c $(CC) -g -O1 -Wall $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) @@ -199,10 +133,11 @@ ffjoystick: ffjoystick.c ctrlstick: ctrlstick.c $(CC) -g -O2 -Wall $(GLIBIVY_CFLAGS) -o $@ $^ $(IVY_C_LIBS) -clean: - $(Q)rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play ahrsview imuview ahrs2fg plot plotter gtk_export.ml openlog2tlm disp3d plotprofile test1 test3 test_samere ivy_example tmclient ffjoystick ctrlstick sd2log motor_bench -.PHONY: all clean pt run_fg +clean: + $(Q)rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play ahrs2fg plot plotter gtk_export.ml openlog2tlm disp3d plotprofile tmclient ffjoystick ctrlstick sd2log + +.PHONY: all clean # # Dependencies diff --git a/sw/logalizer/ahrs_utils.h b/sw/logalizer/ahrs_utils.h deleted file mode 100644 index c6bbbee053..0000000000 --- a/sw/logalizer/ahrs_utils.h +++ /dev/null @@ -1,230 +0,0 @@ -#ifndef AHRS_UTILS_H -#define AHRS_UTILS_H - -#include "frames.h" - -#ifdef DEBUG -#include - -#define PrintEuler() { \ - printf("euler %f %f %f\n\n", phi, theta, psi); \ - } - -#define PrintQuat() { \ - float norm = sqrt ( q0*q0 + q1*q1 + q2*q2 + q3*q3); \ - printf("quat %f %f %f %f (%f)\n\n", q0, q1, q2, q3, norm); \ - } - -#define PrintDCM() { \ - printf("DCM %.2f %.2f %.2f\n", dcm00, dcm01, dcm02); \ - printf(" XXXX XXXX %.2f\n", dcm12); \ - printf(" XXXX XXXX %.2f\n\n", dcm22); \ - } - -#define PrintC() { \ - printf("C %.2f %.2f %.2f %.2f\n\n", C[0], C[1], C[2], C[3]); \ - } -#endif /* DEBUG */ - -extern float C[4]; - -extern float dcm00; -extern float dcm01; -extern float dcm02; -extern float dcm12; -extern float dcm22; - - -/* - * Compute the five elements of the DCM that we use for our - * rotations and Jacobians. This is used by several other functions - * to speedup their computations. - */ -static inline void DCM_of_quat( void ) { - dcm00 = 1.0-2*(q2*q2 + q3*q3); - dcm01 = 2*(q1*q2 + q0*q3); - dcm02 = 2*(q1*q3 - q0*q2); - dcm12 = 2*(q2*q3 + q0*q1); - dcm22 = 1.0-2*(q1*q1 + q2*q2); -} - -static inline void eulers_of_DCM ( void ) { - ahrs_phi = atan2( dcm12, dcm22 ); - ahrs_theta = -asin( dcm02 ); - ahrs_psi = atan2( dcm01, dcm00 ); -} - - -/* - * initialise a quaternion from a set of eulers - */ -static inline void quat_of_eulers ( void ) { - const float phi2 = ahrs_phi / 2.0; - const float theta2 = ahrs_theta / 2.0; - const float psi2 = ahrs_psi / 2.0; - - const float sinphi2 = sin( phi2 ); - const float cosphi2 = cos( phi2 ); - - const float sintheta2 = sin( theta2 ); - const float costheta2 = cos( theta2 ); - - const float sinpsi2 = sin( psi2 ); - const float cospsi2 = cos( psi2 ); - - q0 = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; - q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; - q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; - q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; -} - -/* - * normalize quaternion - */ -static inline void norm_quat( void ) { - float mag = q0*q0 + q1*q1 + q2*q2 + q3*q3; - mag = sqrt( mag ); - q0 /= mag; - q1 /= mag; - q2 /= mag; - q3 /= mag; -} - -/* - * Compute the Jacobian of the measurements to the system states. - * You must have already computed the DCM for the quaternion before - * calling this function. - */ -#if 1 -static inline void compute_dphi_dq( void ) { - const float phi_err = 2 / (dcm22*dcm22 + dcm12*dcm12); - - C[0] = (q1 * dcm22) * phi_err; - C[1] = (q0 * dcm22 + 2 * q1 * dcm12) * phi_err; - C[2] = (q3 * dcm22 + 2 * q2 * dcm12) * phi_err; - C[3] = (q2 * dcm22) * phi_err; -} - -static inline void compute_dtheta_dq( void ) { - const float theta_err = -2 / sqrt( 1 - dcm02*dcm02 ); - - C[0] = -q2 * theta_err; - C[1] = q3 * theta_err; - C[2] = -q0 * theta_err; - C[3] = q1 * theta_err; -} - -static inline void compute_dpsi_dq( void ) { - const float psi_err = 2 / (dcm00*dcm00 + dcm01*dcm01); - - C[0] = (q3 * dcm00) * psi_err; - C[1] = (q2 * dcm00) * psi_err; - C[2] = (q1 * dcm00 + 2 * q2 * dcm01) * psi_err; - C[3] = (q0 * dcm00 + 2 * q3 * dcm01) * psi_err; -} -#else -/* paper JSchlep p85 */ -static inline void compute_dphi_dq( void ) { - const float a = (q2+q1)*(q2+q1) + (q3+q0)*(q3+q0); - const float b = (q2-q1)*(q2-q1) + (q3-q0)*(q3-q0); - - C[0] = -(q2+q1)/a - (q2-q1)/b; - C[1] = (q3+q0)/a + (q3-q0)/b; - C[2] = (q3+q0)/a - (q3-q0)/b; - C[3] = -(q2+q1)/a + (q2-q1)/b; -} - -static inline void compute_dtheta_dq( void ) { - const float a = 2 / sqrt( 1 - 4*(q1*q2 + q0*q3)*(q1*q2 + q0*q3)); - - C[0] = q3 * a; - C[1] = q2 * a; - C[2] = q1 * a; - C[3] = q0 * a; -} - -static inline void compute_dpsi_dq( void ) { - const float a = (q2+q1)*(q2+q1) + (q3+q0)*(q3+q0); - const float b = (q2-q1)*(q2-q1) + (q3-q0)*(q3-q0); - - C[0] = -(q2+q1)/a + (q2-q1)/b; - C[1] = (q3+q0)/a - (q3-q0)/b; - C[2] = (q3+q0)/a + (q3-q0)/b; - C[3] = -(q2+q1)/a - (q2-q1)/b; -} -#endif - -static inline float ahrs_roll_of_accel( const float* accel ) { - return atan2(accel[AXIS_Y], accel[AXIS_Z]); -} - -static inline float ahrs_pitch_of_accel( const float* accel) { - float g2 = - accel[AXIS_X]*accel[AXIS_X] + - accel[AXIS_Y]*accel[AXIS_Y] + - accel[AXIS_Z]*accel[AXIS_Z]; - return -asin( accel[AXIS_X] / sqrt( g2 ) ); -} -/* - * The rotation matrix to rotate from NED frame to body frame without - * rotating in the yaw axis is: - * - * [ 1 0 0 ] [ cos(Theta) 0 -sin(Theta) ] - * [ 0 cos(Phi) sin(Phi) ] [ 0 1 0 ] - * [ 0 -sin(Phi) cos(Phi) ] [ sin(Theta) 0 cos(Theta) ] - * - * This expands to: - * - * [ cos(Theta) 0 -sin(Theta) ] - * [ sin(Phi)*sin(Theta) cos(Phi) sin(Phi)*cos(Theta)] - * [ cos(Phi)*sin(Theta) -sin(Phi) cos(Phi)*cos(Theta)] - * - * However, to untilt the compass reading, we need to use the - * transpose of this matrix. - * - * [ cos(Theta) sin(Phi)*sin(Theta) cos(Phi)*sin(Theta) ] - * [ 0 cos(Phi) -sin(Phi) ] - * [ -sin(Theta) sin(Phi)*cos(Theta) cos(Phi)*cos(Theta) ] - * - * Additionally, - * since we already have the DCM computed for our current attitude, - * we can short cut all of the trig. substituting - * in from the definition of euler2quat and quat2euler, we have: - * - * [ cos(Theta) -dcm12*dcm02 -dcm22*dcm02 ] - * [ 0 dcm22 -dcm12 ] - * [ dcm02 dcm12 dcm22 ] - * - */ -static inline float ahrs_yaw_of_mag( const int16_t* mag ) { - const float ctheta = cos( ahrs_theta ); -#if 0 - const float mn = ctheta * mag[0] - - (dcm12 * mag[1] + dcm22 * mag[2]) * dcm02 / ctheta; - - const float me = - (dcm22 * mag[1] - dcm12 * mag[2]) / ctheta; -#else - const float stheta = sin( ahrs_theta ); - const float cphi = cos( ahrs_phi ); - const float sphi = sin( ahrs_phi ); - - const float mn = - ctheta* mag[0]+ - sphi*stheta* mag[1]+ - cphi*stheta* mag[2]; - const float me = - 0* mag[0]+ - cphi* mag[1]+ - -sphi* mag[2]; -#endif - - const float yaw = -atan2( me, mn ); - return yaw; -} - - - - - -#endif /* AHRS_UTILS_H */ diff --git a/sw/logalizer/ahrsview.c b/sw/logalizer/ahrsview.c deleted file mode 100644 index 85a0d37208..0000000000 --- a/sw/logalizer/ahrsview.c +++ /dev/null @@ -1,74 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include - -#include "sliding_plot.h" - -GtkWidget *biases_plot, *eulers_plot; - -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]); - 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) { - - gtk_init(&argc, &argv); - - GtkWidget *window = gtk_window_new (GTK_WINDOW_TOPLEVEL); - gtk_widget_set_size_request (window, 640, 400); - - GtkWidget *vbox1 = gtk_vbox_new (FALSE, 0); - gtk_container_add (GTK_CONTAINER (window), vbox1); - - 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, biases_plot, "^77 AHRS_STATE (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); - - gtk_main(); - return 0; -} diff --git a/sw/logalizer/imuview.c b/sw/logalizer/imuview.c deleted file mode 100644 index 83637137b2..0000000000 --- a/sw/logalizer/imuview.c +++ /dev/null @@ -1,79 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include - -#include "sliding_plot.h" - -GtkWidget *mag_plot; -GtkWidget *gyro_plot; -GtkWidget *accel_plot; - -void on_IMU_MAG(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - float mx = atof(argv[0]); - float my = atof(argv[1]); - float mz = atof(argv[2]); - gfloat mag[] = {mx, my, mz}; - sliding_plot_update(mag_plot, mag); -} - -void on_IMU_ACCEL(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - float ax = atof(argv[0]); - float ay = atof(argv[1]); - float az = atof(argv[2]); - gfloat accel[] = {ax, ay, az}; - sliding_plot_update(accel_plot, accel); -} - -void on_IMU_GYRO(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - float gx = atof(argv[0]); - float gy = atof(argv[1]); - float gz = atof(argv[2]); - gfloat rates[] = {gx, gy, gz}; - sliding_plot_update(gyro_plot, rates); -} - -int main (int argc, char** argv) { - - gtk_init(&argc, &argv); - - GtkWidget *window = gtk_window_new (GTK_WINDOW_TOPLEVEL); - gtk_widget_set_size_request (window, 1280, 480); - - GtkWidget *vbox1 = gtk_vbox_new (FALSE, 0); - gtk_container_add (GTK_CONTAINER (window), vbox1); - - GtkWidget *frame = gtk_frame_new ("Mag"); - gtk_container_set_border_width (GTK_CONTAINER (frame), 10); - gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0); - mag_plot = sliding_plot_new(3); - gtk_container_add (GTK_CONTAINER (frame), mag_plot ); - - frame = gtk_frame_new ("Accel"); - gtk_container_set_border_width (GTK_CONTAINER (frame), 10); - gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0); - accel_plot = sliding_plot_new(3); - gtk_container_add (GTK_CONTAINER (frame), accel_plot ); - - frame = gtk_frame_new ("Gyro"); - gtk_container_set_border_width (GTK_CONTAINER (frame), 10); - gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0); - gyro_plot = sliding_plot_new(3); - gtk_container_add (GTK_CONTAINER (frame), gyro_plot ); - - gtk_widget_show_all(window); - - - IvyInit ("imuview", "imuview READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_IMU_MAG, NULL, "^77 IMU_MAG (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_IMU_ACCEL, NULL, "^77 IMU_ACCEL (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_IMU_GYRO, NULL, "^77 IMU_GYRO (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); - - gtk_main(); - return 0; -} diff --git a/sw/logalizer/ivy_example.c b/sw/logalizer/ivy_example.c deleted file mode 100644 index 6efb50cd34..0000000000 --- a/sw/logalizer/ivy_example.c +++ /dev/null @@ -1,27 +0,0 @@ -#include -#include -#include -#include - -void on_MOTOR_BENCH_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - - //guint time_tick = atoi(argv[0]); - //guint time_sec = atoi(argv[1]); - //guint throttle = atoi(argv[2]); - //guint new_mode = atoi(argv[3]); - -} - - -int main ( int argc, char** argv) { - GMainLoop *ml = g_main_loop_new(NULL, FALSE); - - IvyInit ("IvyExample", "IvyExample READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_MOTOR_BENCH_STATUS, NULL, "^\\S* MOTOR_BENCH_STATUS (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); - - g_main_loop_run(ml); - - - return 0; -} diff --git a/sw/logalizer/motor_bench.c b/sw/logalizer/motor_bench.c deleted file mode 100644 index 259428d240..0000000000 --- a/sw/logalizer/motor_bench.c +++ /dev/null @@ -1,231 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include - -#include "sliding_plot.h" - -GtkWidget *throttle_plot; -GtkWidget *rpm_plot; - -GtkWidget *manual_throttle_scale; -GtkWidget *ramp_period_scale; - -GSList *radiobutton_group = NULL; - -#define PARAM_TIME 0 -#define PARAM_THROTTLE 1 -#define PARAM_RPM 2 -#define PARAM_VOLTAGE 3 -#define PARAM_CURRENT 4 -#define PARAM_THRUST 5 -#define PARAM_TORQUE 6 -#define PARAM_NB 7 - -const gchar* param_names[] = { - "Time", - "Throttle", - "RPM", - "Voltage", - "Current", - "Thrust", - "Torque" -}; - -GtkWidget* param_label[PARAM_NB]; -gfloat param_value[PARAM_NB]; - -#define MODE_HALTED 0 -#define MODE_MANUAL 1 -#define MODE_RAMP 2 -#define MODE_STEP 3 -#define MODE_PRBS 4 -#define MODE_NB 5 - -guint mode; - -GtkWidget* build_gui ( void ); - -gboolean timeout_callback(gpointer data) { - GString* str = g_string_sized_new(64); - g_string_printf(str, "%.1f", param_value[PARAM_TIME]); - gtk_label_set_text(GTK_LABEL(param_label[PARAM_TIME]), str->str); - g_string_printf(str, "%.1f", param_value[PARAM_THROTTLE]); - gtk_label_set_text(GTK_LABEL(param_label[PARAM_THROTTLE]), str->str); - g_string_free(str, TRUE); - return TRUE; -} - -void on_mode_changed (GtkRadioButton *radiobutton, gpointer user_data) { - if (gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton))) { - guint new_mode = (gint)user_data; - IvySendMsg("ME RAW_DATALINK 78 SETTING;0;0;%d", new_mode); - // IvySendMsg("ME DL_SETTINGS 78 0 %d.0", new_mode); - g_message("sending new mode %d" , new_mode); - } -} - -void on_manual_throttle_value_changed (GtkScale *scale, gpointer user_data) { - gfloat new_throttle = gtk_range_get_value(GTK_RANGE(scale)); - g_message("foo %f", new_throttle); - guint foo = (new_throttle/100.*9600.); - IvySendMsg("ME RAW_DATALINK 78 SETTING;3;0;%d", foo); -} - -void on_ramp_period_value_changed (GtkScale *scale, gpointer user_data) { - gfloat new_period = gtk_range_get_value(GTK_RANGE(scale)); - g_message("ramp period %f", new_period); - guint foo = (new_period*1000); - IvySendMsg("ME RAW_DATALINK 78 SETTING;4;0;%d", foo); -} - - - -#define TICK_PER_SEC 15000000. -void on_MOTOR_BENCH_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - guint time_tick = atoi(argv[0]); - guint time_sec = atoi(argv[1]); - guint throttle = atoi(argv[2]); - guint new_mode = atoi(argv[3]); - - float time = (float)time_sec + (float)time_tick / TICK_PER_SEC; - param_value[PARAM_TIME] = time; - param_value[PARAM_THROTTLE] = (float)throttle / 9600. * 100.; - // printf("%f %d %d\n", time, throttle, mode); - gfloat foo[] = {(gfloat)throttle}; - sliding_plot_update(throttle_plot, foo); - - if (new_mode != mode) { - g_message("mode changed %d->%d", mode, new_mode); - GtkRadioButton *radiobutton = g_slist_nth(radiobutton_group, MODE_NB-new_mode-1)->data; - gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(radiobutton), TRUE); - mode = new_mode; - } -} - - -int main ( int argc, char** argv) { - - gtk_init(&argc, &argv); - - g_timeout_add(160, timeout_callback, NULL); - - // GMainLoop *ml = g_main_loop_new(NULL, FALSE); - - IvyInit ("MotorBench", "MotorBench READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_MOTOR_BENCH_STATUS, NULL, "^\\S* MOTOR_BENCH_STATUS (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); - - // g_main_loop_run(ml); - - GtkWidget* window = build_gui(); - gtk_widget_show_all(window); - - gtk_main(); - - return 0; -} - - - -GtkWidget* build_gui ( void ) { - GtkWidget *window; - GtkWidget *vbox1; - GtkWidget *table1; - GtkWidget *label4; - - window = gtk_window_new (GTK_WINDOW_TOPLEVEL); - gtk_window_set_title (GTK_WINDOW (window), "motor bench"); - - vbox1 = gtk_vbox_new (FALSE, 0); - gtk_widget_show (vbox1); - gtk_container_add (GTK_CONTAINER (window), vbox1); - - table1 = gtk_table_new (4, 3, FALSE); - gtk_widget_show (table1); - gtk_box_pack_start (GTK_BOX (vbox1), table1, TRUE, TRUE, 0); - gtk_table_set_col_spacings (GTK_TABLE (table1), 5); - - label4 = gtk_label_new ("mode"); - gtk_widget_show (label4); - gtk_table_attach (GTK_TABLE (table1), label4, 0, 1, 0, 1, - (GtkAttachOptions) (GTK_FILL), - (GtkAttachOptions) (0), 0, 0); - gtk_misc_set_alignment (GTK_MISC (label4), 0, 0.5); - - gchar* mode_names[] = { "Halted", "Manual", "Ramp", "Step", "PRBS" }; - - gint i; - for (i=0; i -#include -#include -#include -#include - -#include -#include - -#include "sliding_plot.h" - - -GtkWidget *plot; - -GtkWidget* build_gui ( void ) { - GtkWidget *window1; - GtkWidget *vbox1; - - window1 = gtk_window_new (GTK_WINDOW_TOPLEVEL); - gtk_window_set_title (GTK_WINDOW (window1), "plot roll loop"); - - vbox1 = gtk_vbox_new (FALSE, 0); - gtk_container_add (GTK_CONTAINER (window1), vbox1); - - GtkWidget *frame = gtk_frame_new ("SaSeur"); - gtk_container_set_border_width (GTK_CONTAINER (frame), 10); - gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0); - plot = sliding_plot_new(2); - gtk_container_add (GTK_CONTAINER (frame), plot ); - - return window1; - - -} - - -void on_DESIRED(IvyClientPtr app, void *user_data, int argc, char *argv[]){ - // float p = atof(argv[0]); - float phi = atof(argv[1]); - float phi_sp = atof(argv[2]); - - gfloat foo[] = {(gfloat)phi, (gfloat)phi_sp}; - sliding_plot_update(plot, foo); - -} - -int main (int argc, char** argv) { - - gtk_init(&argc, &argv); - - GtkWidget* window = build_gui(); - gtk_widget_show_all(window); - - IvyInit ("PlotRollLoop", "PlotRollLoop READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_DESIRED, NULL, "^\\S* TUNE_ROLL (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); - - gtk_main(); - return 0; -} diff --git a/sw/logalizer/test_2.c b/sw/logalizer/test_2.c deleted file mode 100644 index de6d63580c..0000000000 --- a/sw/logalizer/test_2.c +++ /dev/null @@ -1,109 +0,0 @@ -#include - -#include "ahrs_utils.h" - -float C[4]; - -float dcm00; -float dcm01; -float dcm02; -float dcm12; -float dcm22; - -float phi; -float theta; -float psi; - -float q0; -float q1; -float q2; -float q3; - - -/* d_euler / dq */ -/* - phi = atan ( 2(q2q3 + q0q1) / (q0^2 - q1^2 - q2^2 + q3^2)) - -*/ -void test_dphi_dq ( void ) { - float my_dcm22 = q0*q0 - q1*q1 - q2*q2 + q3*q3; - float dcm22_sq = my_dcm22 * my_dcm22; - float my_dcm12 = 2*(q2*q3 + q0*q1); - float dcm12_sq = my_dcm12 * my_dcm12; - C[0] = 2 * q1 * my_dcm22 / (dcm22_sq + dcm12_sq); - C[1] = 2 * q0 * my_dcm22 / (dcm22_sq + dcm12_sq); - C[2] = 2 * q3 * my_dcm22 / (dcm22_sq + dcm12_sq); - C[3] = 2 * q2 * my_dcm22 / (dcm22_sq + dcm12_sq); -} -/* - theta = asin(-2(q1q3 - q0q2)) - - dasin = 1/sqrt(1-x^2) -*/ -void test_dtheta_dq ( void ) { - float my_dcm02 = 2 * (q1*q3 - q0*q2); - float dcm02_sq = my_dcm02 * my_dcm02; - C[0] = 2 * q2 / (sqrt(1 - dcm02_sq)); - C[1] = -2 * q3 / (sqrt(1 - dcm02_sq)); - C[2] = 2 * q0 / (sqrt(1 - dcm02_sq)); - C[3] = -2 * q1 / (sqrt(1 - dcm02_sq)); -} - -/* - psi = atan ( 2(q1q2 + q0q3) / (q0^2 + q1^2 - q2^2 - q3^2)) - - - datan = 1/(1+x^2) -*/ -void test_dpsi_dq ( void ) { - float my_dcm00 = 1 - 2. * q2 * q2 - 2. * q3 * q3; - float my_dcm01 = 2*(q1*q2 +q0*q3); - float dcm00_sq = my_dcm00 * my_dcm00; - float dcm01_sq = my_dcm01 * my_dcm01; - C[0] = 2. * q3 * my_dcm00 / (dcm00_sq + dcm01_sq); - C[1] = 2. * q2 * my_dcm00 / (dcm00_sq + dcm01_sq); - C[2] = 2. * q1 * my_dcm00 / (dcm00_sq + dcm01_sq); - C[3] = 2. * q0 * my_dcm00 / (dcm00_sq + dcm01_sq); -} - - - - - -int main (int argc, char** argv) { - - phi = 0.3; - theta = 0.1; - psi = 0.2; - PrintEuler() - - quat_of_eulers(); - PrintQuat(); - DCM_of_quat(); - PrintDCM(); -#if 1 - printf("phi\n"); - compute_dphi_dq(); - PrintC(); - test_dphi_dq(); - PrintC(); -#endif - -#if 1 - printf("theta\n"); - compute_dtheta_dq(); - PrintC(); - test_dtheta_dq(); - PrintC(); -#endif - -#if 1 - printf("psi\n"); - compute_dpsi_dq(); - PrintC(); - test_dpsi_dq(); - PrintC(); -#endif - - return 0; -}