From 71b4392ea76bdc2f2e0a85fb4c1a9d1daebe1717 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Mon, 4 Dec 2006 03:07:34 +0000 Subject: [PATCH] *** empty log message *** --- sw/airborne/ahrs_new.c | 128 ++++++++++++-------------------------- sw/airborne/ahrs_new.h | 5 +- sw/airborne/imu_v3.c | 2 +- sw/airborne/imu_v3.h | 13 ++-- sw/airborne/main_imu.c | 37 ++++++++--- sw/logalizer/Makefile | 5 ++ sw/logalizer/ahrs_utils.h | 114 +++++++++++++++++++++++++++++++++ sw/logalizer/test_2.c | 109 ++++++++++++++++++++++++++++++++ 8 files changed, 307 insertions(+), 106 deletions(-) create mode 100644 sw/logalizer/ahrs_utils.h create mode 100644 sw/logalizer/test_2.c diff --git a/sw/airborne/ahrs_new.c b/sw/airborne/ahrs_new.c index 6552779db7..f4353a977f 100644 --- a/sw/airborne/ahrs_new.c +++ b/sw/airborne/ahrs_new.c @@ -15,9 +15,6 @@ * frequency with which ahrs_state_update() is called with the body * angular rates. */ -//#define dt 0.04 -//#define dt 0.0625 -//#define dt 0.0117185 #define dt 0.015625 //#define CONFIG_SPLIT_COVARIANCE @@ -75,11 +72,8 @@ static real_t dcm22; * every other state step. This is because the covariance should change * at a rate somewhat slower than the dynamics of the system. */ -static real_t P[7][7]; +real_t P[7][7]; static real_t Pdot[7][7]; -#ifdef CONFIG_SPLIT_COVARIANCE -static index_t covariance_state; -#endif /* * A represents the Jacobian of the derivative of the system with respect @@ -89,8 +83,7 @@ static index_t covariance_state; real_t A[4][7]; /* - * These Kalman filter variables share space with A when the filter - * is being updated. + * Kalman filter variables. */ real_t PCt[7]; real_t K[7]; @@ -104,7 +97,7 @@ real_t E; * * Since we compute each axis independently, we only allocate one * column out of the C matrix at a time. This allows us to reuse the - * matrix. Additionally, this space is shared by Qdot. + * matrix. */ real_t C[4]; real_t Qdot[4]; @@ -140,28 +133,17 @@ real_t Qdot[4]; /* - * Simple helper to normalize our quaternion attitude estimate. We could - * probably do this at a lower time step to save on calls to sqrt(). + * Simple helper to normalize our quaternion attitude estimate. */ void norm_quat( void ) { - // index_t i; - real_t mag = 0; - - // for( i=0 ; i<4 ; i++ ) - // mag += quat[i] * quat[i]; - mag = q0*q0 + q1*q1 + q2*q2 + q3*q3; - + real_t mag = q0*q0 + q1*q1 + q2*q2 + q3*q3; mag = sqrt( mag ); - - // for( i=0 ; i<4 ; i++ ) - // quat[i] /= mag; q0 /= mag; q1 /= mag; q2 /= mag; q3 /= mag; - } /* @@ -231,16 +213,17 @@ compute_A_bias( void ) * * P_dot = A*P + P*A_transpose + Q * - * However, this takes a very long time to compute. So we split - * the multiplications and additions into two separate routines. - * * The first of these zeros P_dot, computes part of (A*P + P*A_tranpose) * and adds in the parts of Q that are non-zero. Note that we know that * A has three zero rows at the bottom, so we do not include those in our * math. + * + * The second part of the covariance update computes the inner portion + * of Pdot, the 4x4 region that corresponds to the quaternion. It also + * updates the covariance matrix P. */ static void -covariance_update_0( void ) +covariance_update( void ) { index_t i; index_t j; @@ -270,20 +253,6 @@ covariance_update_0( void ) */ for( i=4 ; i<7 ; i++ ) Pdot[i][i] += Q_gyro; -} - - -/* - * The second part of the covariance update computes the inner portion - * of Pdot, the 4x4 region that corresponds to the quaternion. It also - * updates the covariance matrix P. - */ -static void -covariance_update_1( void ) -{ - index_t i; - index_t j; - index_t k; /* * Compute A*P + P*At for the region [0..3][0..3] @@ -373,20 +342,7 @@ ahrs_state_update( void ) */ //norm_quat(); -#ifdef CONFIG_SPLIT_COVARIANCE - /* Compute our split covariance update */ - if( covariance_state == 0 ) { - covariance_state = 1; - covariance_update_0(); - } - else { - covariance_state = 0; - covariance_update_1(); - } -#else - covariance_update_0(); - covariance_update_1(); -#endif + covariance_update(); } /* @@ -413,17 +369,14 @@ compute_DCM( void ) static inline void compute_dphi_dq( void ) { - index_t i; - const real_t phi_err = 2 / (dcm22*dcm22 + dcm12*dcm12); - C[0] = (q1 * dcm22); - C[1] = (q0 * dcm22 + 2 * q1 * dcm12); - C[2] = (q3 * dcm22 + 2 * q2 * dcm12); - C[3] = (q2 * dcm22); - - for( i=0 ; i<4 ; i++ ) - C[i] *= phi_err; + C[0] = (q1 * dcm22) * phi_err; + C[1] = (q0 * dcm22 + 2 * q1 * dcm12) * phi_err; + //C[1] = (q0 * dcm22 ) * phi_err; + C[2] = (q3 * dcm22 + 2 * q2 * dcm12) * phi_err; + //C[2] = (q3 * dcm22) * phi_err; + C[3] = (q2 * dcm22) * phi_err; } static inline void @@ -440,17 +393,14 @@ compute_dtheta_dq( void ) static inline void compute_dpsi_dq( void ) { - index_t i; + const float psi_err = 2 / (dcm00*dcm00 + dcm01*dcm01); - const real_t psi_err = 2 / (dcm00*dcm00 + dcm01*dcm01); - - C[0] = (q3 * dcm00); - C[1] = (q2 * dcm00); - C[2] = (q1 * dcm00 + 2 * q2 * dcm01); - C[3] = (q0 * dcm00 + 2 * q3 * dcm01); - - for( i=0 ; i<4 ; i++ ) - C[i] *= psi_err; + C[0] = (q3 * dcm00) * psi_err; + C[1] = (q2 * dcm00) * psi_err; + C[2] = (q1 * dcm00 + 2 * q2 * dcm01) * psi_err; + //C[2] = (q1 * dcm00) * psi_err; + C[3] = (q0 * dcm00 + 2 * q3 * dcm01) * psi_err; + //C[3] = (q0 * dcm00) * psi_err; } @@ -711,9 +661,11 @@ real_t ahrs_pitch_of_accel( real_t* accel_cal) { */ void ahrs_compass_update( - real_t heading + const int16_t* mag ) { + + real_t heading = ahrs_heading_of_mag(mag); // Update the DCM since this will require compute_DCM(); compute_euler_heading(); @@ -738,23 +690,23 @@ ahrs_compass_update( static void euler2quat( void ) { - const real_t phi = ahrs_euler[0] / 2.0; - const real_t theta = ahrs_euler[1] / 2.0; - const real_t psi = ahrs_euler[2] / 2.0; + const real_t phi0 = ahrs_euler[0] / 2.0; + const real_t theta0 = ahrs_euler[1] / 2.0; + const real_t psi0 = ahrs_euler[2] / 2.0; - const real_t shphi0 = sin( phi ); - const real_t chphi0 = cos( phi ); + const real_t sinphi0 = sin( phi0 ); + const real_t cosphi0 = cos( phi0 ); - const real_t shtheta0 = sin( theta ); - const real_t chtheta0 = cos( theta ); + const real_t sintheta0 = sin( theta0 ); + const real_t costheta0 = cos( theta0 ); - const real_t shpsi0 = sin( psi ); - const real_t chpsi0 = cos( psi ); + const real_t sinpsi0 = sin( psi0 ); + const real_t cospsi0 = cos( psi0 ); - q0 = chphi0 * chtheta0 * chpsi0 + shphi0 * shtheta0 * shpsi0; - q1 = -chphi0 * shtheta0 * shpsi0 + shphi0 * chtheta0 * chpsi0; - q2 = chphi0 * shtheta0 * chpsi0 + shphi0 * chtheta0 * shpsi0; - q3 = chphi0 * chtheta0 * shpsi0 - shphi0 * shtheta0 * chpsi0; + q0 = cosphi0 * costheta0 * cospsi0 + sinphi0 * sintheta0 * sinpsi0; + q1 = -cosphi0 * sintheta0 * sinpsi0 + sinphi0 * costheta0 * cospsi0; + q2 = cosphi0 * sintheta0 * cospsi0 + sinphi0 * costheta0 * sinpsi0; + q3 = cosphi0 * costheta0 * sinpsi0 - sinphi0 * sintheta0 * cospsi0; } /* diff --git a/sw/airborne/ahrs_new.h b/sw/airborne/ahrs_new.h index 67081cb4b1..dc600e1acf 100644 --- a/sw/airborne/ahrs_new.h +++ b/sw/airborne/ahrs_new.h @@ -11,13 +11,14 @@ extern real_t ahrs_pqr[3]; extern real_t q0, q1, q2, q3; extern real_t ahrs_euler[3]; extern real_t bias_p, bias_q, bias_r; - +extern real_t P[7][7]; /* covariance */ +extern real_t A[4][7]; /* jacobian */ extern void ahrs_init( const int16_t *mag ); extern void ahrs_state_update( void ); extern void ahrs_pitch_update( real_t pitch); extern void ahrs_roll_update( real_t roll); -extern void ahrs_compass_update( real_t heading ); +extern void ahrs_compass_update( const int16_t* mag); extern real_t ahrs_roll_of_accel( real_t* accel_cal ); extern real_t ahrs_pitch_of_accel( real_t* accel_cal); diff --git a/sw/airborne/imu_v3.c b/sw/airborne/imu_v3.c index dbcd67ec4a..cdba9db58d 100644 --- a/sw/airborne/imu_v3.c +++ b/sw/airborne/imu_v3.c @@ -13,7 +13,7 @@ int16_t imu_mag[AXIS_NB]; float imu_bat; struct adc_buf buf_ax; -/* struct adc_buf buf_ay; */ +struct adc_buf buf_ay; struct adc_buf buf_az; struct adc_buf buf_bat; diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h index b18753ee41..8d379800b0 100644 --- a/sw/airborne/imu_v3.h +++ b/sw/airborne/imu_v3.h @@ -8,7 +8,7 @@ /* accelerometers in arbitrary unit */ extern float imu_accel[AXIS_NB]; /* gyros in rad/s */ -extern float imu_gyro[AXIS_NB]; +extern float imu_gyro[AXIS_NB]; /* magnetometer in arbitrary unit */ extern int16_t imu_mag[AXIS_NB]; /* battery in volts */ @@ -16,7 +16,6 @@ extern float imu_bat; extern void imu_init(void); - extern struct adc_buf buf_ax; extern struct adc_buf buf_ay; extern struct adc_buf buf_az; @@ -34,14 +33,14 @@ extern struct adc_buf buf_bat; imu_accel[AXIS_Z]= buf_az.sum; \ int_enable(); \ imu_accel[AXIS_X] = -((imu_accel[AXIS_X] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_X_NEUTRAL); \ - imu_accel[AXIS_Y] = (imu_accel[AXIS_Y] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \ - imu_accel[AXIS_Z] = (imu_accel[AXIS_Z] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \ + imu_accel[AXIS_Y] = (imu_accel[AXIS_Y] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \ + imu_accel[AXIS_Z] = (imu_accel[AXIS_Z] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \ } #else -#define ImuUpdateAccels() { \ +#define ImuUpdateAccels() { \ imu_accel[AXIS_X]= -((buf_ax.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_X_NEUTRAL); \ - imu_accel[AXIS_Y]= (buf_ay.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \ - imu_accel[AXIS_Z]= (buf_az.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \ + imu_accel[AXIS_Y]= (buf_ay.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \ + imu_accel[AXIS_Z]= (buf_az.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \ } #endif diff --git a/sw/airborne/main_imu.c b/sw/airborne/main_imu.c index ee3e211a32..5ac86406ca 100644 --- a/sw/airborne/main_imu.c +++ b/sw/airborne/main_imu.c @@ -17,13 +17,18 @@ #include "messages.h" #include "downlink.h" +#define SEND_ACCEL 1 +#define SEND_MAG 1 +#define SEND_GYRO 1 +#define SEND_AHRS_STATE 1 +#define SEND_AHRS_COV 1 static inline void main_init( void ); static inline void main_periodic_task( void ); static inline void main_event_task( void); //struct adc_buf buf_ax; -struct adc_buf buf_ay; +//struct adc_buf buf_ay; //struct adc_buf buf_az; //struct adc_buf buf_bat; @@ -66,7 +71,6 @@ static inline void main_init( void ) { main_init_spi1(); max1167_init(); micromag_init(); - // spi0_init(); link_imu_init(); int_enable(); } @@ -79,7 +83,9 @@ static inline void main_event_task( void ) { micromag_data_available = FALSE; spi_cur_slave = SPI_SLAVE_NONE; ImuUpdateMag(); - DOWNLINK_SEND_AHRS_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]); +#ifdef SEND_MAG + DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]); +#endif spi_cur_slave = SPI_SLAVE_MAX; max1167_read(); } @@ -88,11 +94,17 @@ static inline void main_event_task( void ) { max1167_data_available = FALSE; spi_cur_slave = SPI_SLAVE_NONE; ImuUpdateGyros(); +#ifdef SEND_GYRO + DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_X], &imu_gyro[AXIS_Y], &imu_gyro[AXIS_Z]); +#endif ImuUpdateAccels(); +#ifdef SEND_ACCEL + DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z]); +#endif ahrs_task(); t1=T0TC; - uint32_t dt = t1 - t0; + // uint32_t dt = t1 - t0; // DOWNLINK_SEND_TIME(&dt); link_imu_send(); @@ -137,17 +149,26 @@ static inline void ahrs_task( void ) { case AHRS_STEP_ROLL: ahrs_state_update(); ahrs_roll_update(ahrs_roll_of_accel(imu_accel)); +#ifdef SEND_AHRS_STATE + DOWNLINK_SEND_AHRS_STATE(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r); +#endif break; case AHRS_STEP_PITCH: ahrs_state_update(); ahrs_pitch_update(ahrs_pitch_of_accel(imu_accel)); +#ifdef SEND_AHRS_STATE + DOWNLINK_SEND_AHRS_STATE(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r); +#endif break; case AHRS_STEP_YAW: ahrs_state_update(); - ahrs_compass_update(ahrs_heading_of_mag(imu_mag)); - DOWNLINK_SEND_AHRS(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r); - // DOWNLINK_SEND_AHRS2(&ahrs_euler[AXIS_X], &ahrs_euler[AXIS_Y], &ahrs_euler[AXIS_Z]); - // DOWNLINK_SEND_AHRS2(&imu_gyro[AXIS_X], &imu_gyro[AXIS_Y], &imu_gyro[AXIS_Z]); + ahrs_compass_update(imu_mag); +#ifdef SEND_AHRS_STATE + DOWNLINK_SEND_AHRS_STATE(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r); +#endif +#ifdef SEND_AHRS_COV + DOWNLINK_SEND_AHRS_COV(&P[0][0], &P[1][1]); +#endif break; } ahrs_step++; diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile index cbc1e502b3..882f74e4c7 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -29,6 +29,11 @@ all: play play : play.ml $(OCAMLC) $(INCLUDES) -custom -o $@ unix.cma str.cma xml-light.cma glibivy-ocaml.cma -I +lablgtk2 -I ../lib/ocaml lablgtk.cma lib-pprz.cma gtkInit.cmo $^ + + + +pt : ahrsview imuview ahrs2fg + 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 diff --git a/sw/logalizer/ahrs_utils.h b/sw/logalizer/ahrs_utils.h new file mode 100644 index 0000000000..89d6470dd1 --- /dev/null +++ b/sw/logalizer/ahrs_utils.h @@ -0,0 +1,114 @@ +#ifndef AHRS_UTILS_H +#define AHRS_UTILS_H + +#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]); \ + } +extern float C[4]; + +extern float dcm00; +extern float dcm01; +extern float dcm02; +extern float dcm12; +extern float dcm22; + +extern float phi; +extern float theta; +extern float psi; + +extern float q0; +extern float q1; +extern float q2; +extern float q3; + +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 ) { + phi = atan2( dcm12, dcm22 ); + theta = -asin( dcm02 ); + psi = atan2( dcm01, dcm00 ); +} + +static inline void quat_of_eulers ( void ) { + const float phi2 = phi / 2.0; + const float theta2 = theta / 2.0; + const float psi2 = 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; +} + +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; +} + +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; +} + + + + +#endif /* AHRS_UTILS_H */ diff --git a/sw/logalizer/test_2.c b/sw/logalizer/test_2.c new file mode 100644 index 0000000000..5860ec09c4 --- /dev/null +++ b/sw/logalizer/test_2.c @@ -0,0 +1,109 @@ +#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 * dcm22 / (dcm22_sq + dcm12_sq); + C[1] = 2 * q0 * dcm22 / (dcm22_sq + dcm12_sq); + C[2] = 2 * q3 * dcm22 / (dcm22_sq + dcm12_sq); + C[3] = 2 * q2 * 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 * dcm00 / (dcm00_sq + dcm01_sq); + C[1] = 2. * q2 * dcm00 / (dcm00_sq + dcm01_sq); + C[2] = 2. * q1 * dcm00 / (dcm00_sq + dcm01_sq); + C[3] = 2. * q0 * dcm00 / (dcm00_sq + dcm01_sq); +} + + + + + +int main (int argc, char** argv) { + + phi = 0.3; + theta = 0.5; + psi = 0.5; + 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; +}