mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 14:18:00 +08:00
[logalizer] remove cruft and fix compilation of disp3d
maybe just remove disp3d and some of the other remaining stuff as well...
This commit is contained in:
Felix Ruess
+2
-4
@@ -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
|
||||
|
||||
+15
-80
@@ -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
|
||||
|
||||
@@ -1,230 +0,0 @@
|
||||
#ifndef AHRS_UTILS_H
|
||||
#define AHRS_UTILS_H
|
||||
|
||||
#include "frames.h"
|
||||
|
||||
#ifdef DEBUG
|
||||
#include <stdio.h>
|
||||
|
||||
#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 */
|
||||
@@ -1,74 +0,0 @@
|
||||
#include <gtk/gtk.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
@@ -1,79 +0,0 @@
|
||||
#include <gtk/gtk.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
@@ -1,27 +0,0 @@
|
||||
#include <glib.h>
|
||||
#include <stdlib.h>
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -1,231 +0,0 @@
|
||||
#include <glib.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <gtk/gtk.h>
|
||||
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#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<MODE_NB; i++) {
|
||||
GtkWidget* radiobutton = gtk_radio_button_new_with_mnemonic (NULL, mode_names[i]);
|
||||
gtk_widget_show (radiobutton);
|
||||
gtk_table_attach (GTK_TABLE (table1), radiobutton, 0, 1, 1+i, 2+i,
|
||||
(GtkAttachOptions) (GTK_FILL),
|
||||
(GtkAttachOptions) (0), 0, 0);
|
||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (radiobutton), radiobutton_group);
|
||||
radiobutton_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (radiobutton));
|
||||
g_signal_connect ((gpointer) radiobutton, "toggled",
|
||||
G_CALLBACK (on_mode_changed),
|
||||
((gpointer)i));
|
||||
}
|
||||
|
||||
manual_throttle_scale = gtk_hscale_new (GTK_ADJUSTMENT (gtk_adjustment_new (0., 0, 100, 1, 1, 1)));
|
||||
gtk_table_attach (GTK_TABLE (table1), manual_throttle_scale, 1, 1+PARAM_NB, 1+MODE_MANUAL, 2+MODE_MANUAL,
|
||||
(GtkAttachOptions) (GTK_EXPAND | GTK_FILL),
|
||||
(GtkAttachOptions) (GTK_FILL), 0, 0);
|
||||
gtk_range_set_update_policy (GTK_RANGE (manual_throttle_scale), GTK_UPDATE_DELAYED);
|
||||
g_signal_connect ((gpointer) manual_throttle_scale, "value_changed",
|
||||
G_CALLBACK (on_manual_throttle_value_changed),
|
||||
NULL);
|
||||
|
||||
ramp_period_scale = gtk_hscale_new (GTK_ADJUSTMENT (gtk_adjustment_new (0., 1, 100, 1, 1, 1)));
|
||||
gtk_table_attach (GTK_TABLE (table1), ramp_period_scale, 1, 1+PARAM_NB, 2+MODE_MANUAL, 3+MODE_MANUAL,
|
||||
(GtkAttachOptions) (GTK_EXPAND | GTK_FILL),
|
||||
(GtkAttachOptions) (GTK_FILL), 0, 0);
|
||||
gtk_range_set_update_policy (GTK_RANGE (ramp_period_scale), GTK_UPDATE_DELAYED);
|
||||
g_signal_connect ((gpointer) ramp_period_scale, "value_changed",
|
||||
G_CALLBACK (on_ramp_period_value_changed),
|
||||
NULL);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
for (i=0; i<PARAM_NB; i++) {
|
||||
GtkWidget* label = gtk_label_new (param_names[i]);
|
||||
gtk_table_attach (GTK_TABLE (table1), label, i, i+1, PARAM_NB+1, PARAM_NB + 2,
|
||||
(GtkAttachOptions) (GTK_FILL),
|
||||
(GtkAttachOptions) (0), 0, 0);
|
||||
param_label[i] = gtk_label_new ("unknown");
|
||||
gtk_table_attach (GTK_TABLE (table1), param_label[i], i, i+1, PARAM_NB+2, PARAM_NB + 3,
|
||||
(GtkAttachOptions) (GTK_FILL),
|
||||
(GtkAttachOptions) (0), 0, 0);
|
||||
}
|
||||
|
||||
GtkWidget *frame = gtk_frame_new ("Throttle");
|
||||
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
throttle_plot = sliding_plot_new(1);
|
||||
gtk_container_add (GTK_CONTAINER (frame), throttle_plot );
|
||||
|
||||
|
||||
frame = gtk_frame_new ("RPM");
|
||||
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
// rpm_plot = sliding_plot_new(1);
|
||||
// gtk_container_add (GTK_CONTAINER (frame), rpm_plot );
|
||||
|
||||
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
|
||||
return window;
|
||||
}
|
||||
@@ -1,60 +0,0 @@
|
||||
#include <gtk/gtk.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
@@ -1,109 +0,0 @@
|
||||
#include <math.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
Reference in New Issue
Block a user