From 338f2e3740d0e4f0af5cec7f30b2b4f8b3f664c8 Mon Sep 17 00:00:00 2001 From: Martin Dieblich Date: Wed, 6 Oct 2010 17:28:28 +0000 Subject: [PATCH] Added a lot of logs, changed the way or raw-logging and cleaned test_libeknav_4 --- conf/airframes/Poine/test_libeknav.xml | 2 +- doc/pprz_geodetic/ned_enu.tex | 4 +- sw/airborne/fms/libeknav/Makefile | 2 +- sw/airborne/fms/libeknav/ins_qkf.hpp | 1 + sw/airborne/fms/libeknav/ins_qkf_predict.cpp | 18 +- sw/airborne/fms/libeknav/raw_log.h | 3 +- sw/airborne/fms/libeknav/raw_log_to_ascii.c | 1 + sw/airborne/fms/libeknav/test_libeknav_4.cpp | 202 ++++++++++--------- sw/airborne/fms/libeknav/test_libeknav_4.hpp | 168 +++++++++------ 9 files changed, 235 insertions(+), 166 deletions(-) diff --git a/conf/airframes/Poine/test_libeknav.xml b/conf/airframes/Poine/test_libeknav.xml index 01fb15df5a..225c68da0f 100644 --- a/conf/airframes/Poine/test_libeknav.xml +++ b/conf/airframes/Poine/test_libeknav.xml @@ -4,7 +4,7 @@ HOST=auto1 - + USER=root PAPARAZZI_INC = -I$(PAPARAZZI_HOME)/var/$(AIRCRAFT) \ -I$(PAPARAZZI_SRC)/sw/airborne \ -I$(PAPARAZZI_SRC)/sw/include diff --git a/doc/pprz_geodetic/ned_enu.tex b/doc/pprz_geodetic/ned_enu.tex index 4dfcc42e7e..7e30fc5ad7 100644 --- a/doc/pprz_geodetic/ned_enu.tex +++ b/doc/pprz_geodetic/ned_enu.tex @@ -30,7 +30,7 @@ p_{ENU} \qquad p_{ENU} = \begin{pmatrix} or directly switching the values \begin{equation} \begin{pmatrix}x \\ y \\ z \end{pmatrix}_{NED} = -\begin{pmatrix}y \\ x \\ -z \end{pmatrix}_{NED} +\begin{pmatrix}y \\ x \\ -z \end{pmatrix}_{ENU} \end{equation} \inHfile{ENU\_OF\_TO\_NED(po, pi)}{pprz\_geodetic} \inHfile{INT32\_VECT2\_ENU\_OF\_NED(o, i)}{pprz\_geodetic\_int} @@ -50,4 +50,4 @@ or directly switching the values \input{transformations/ecef2ned_enu} \subsubsection*{from LLA} -\input{transformations/lla2ned_enu} \ No newline at end of file +\input{transformations/lla2ned_enu} diff --git a/sw/airborne/fms/libeknav/Makefile b/sw/airborne/fms/libeknav/Makefile index 5776796e9a..ecb15d70b0 100644 --- a/sw/airborne/fms/libeknav/Makefile +++ b/sw/airborne/fms/libeknav/Makefile @@ -4,9 +4,9 @@ raw_log_to_ascii: raw_log_to_ascii.c fetch_log: - scp @auto1:/tmp/log_ins_test3.data . scp @auto1:/tmp/log_test3.bin . ./raw_log_to_ascii > bla.dat + scp @auto1:/tmp/log_ins_test3.data . clean: -rm -f *.o *~ *.d diff --git a/sw/airborne/fms/libeknav/ins_qkf.hpp b/sw/airborne/fms/libeknav/ins_qkf.hpp index 09c5e866d3..05e6ba9a8b 100644 --- a/sw/airborne/fms/libeknav/ins_qkf.hpp +++ b/sw/airborne/fms/libeknav/ins_qkf.hpp @@ -164,6 +164,7 @@ struct basic_ins_qkf Quaterniond initial_orientation = Quaterniond::Identity(), const Vector3d& vel_estimate = Vector3d::Zero()); + /* //Old one without orientation_init() basic_ins_qkf(const Vector3d& pos_estimate, double pos_error, double bias_error, double v_error, diff --git a/sw/airborne/fms/libeknav/ins_qkf_predict.cpp b/sw/airborne/fms/libeknav/ins_qkf_predict.cpp index 0a55cfafe4..3390b382ef 100644 --- a/sw/airborne/fms/libeknav/ins_qkf_predict.cpp +++ b/sw/airborne/fms/libeknav/ins_qkf_predict.cpp @@ -28,10 +28,9 @@ #include "ins_qkf.hpp" #include "assertions.hpp" - #ifdef TIME_OPS -#include "timer.hpp" -# include +//#include "timer.hpp" +#include #endif using namespace Eigen; @@ -59,12 +58,25 @@ linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d& // accel_cov is the relationship between error vectors in the tangent space // of the vehicle orientation and the translational reference frame. + Matrix3d rot = _this.avg_state.orientation.conjugate().toRotationMatrix(); Vector3d accel_ecef = rot*accel_meas; // a_e = (q_e2b)^* x a_b = q_b2e x a_b Vector3d accel_gravity = _this.avg_state.position.normalized()*(-9.81); Vector3d accel_resid = accel_ecef + accel_gravity; // a = (xdd-g)+g = xdd; + #if 0 + printf("==================================================\n"); + printf("Quaternion: % 1.4f % 1.4f % 1.4f % 1.4f\n", + _this.avg_state.orientation.w(), + _this.avg_state.orientation.x(), + _this.avg_state.orientation.y(), + _this.avg_state.orientation.z()); + printf("Accel_meas: % 1.4f % 1.4f % 1.4f\n", accel_meas(0), accel_meas(1), accel_meas(2)); + printf("Accel_ecef: % 1.4f % 1.4f % 1.4f\n", accel_ecef(0), accel_ecef(1), accel_ecef(2)); + printf("Accel_grav: % 1.4f % 1.4f % 1.4f\n", accel_gravity(0), accel_gravity(1), accel_gravity(2)); + #endif + #if 0 // This form works well with zero static acceleration. Matrix accel_cov = diff --git a/sw/airborne/fms/libeknav/raw_log.h b/sw/airborne/fms/libeknav/raw_log.h index cf9ea94f30..598f0e9f2f 100644 --- a/sw/airborne/fms/libeknav/raw_log.h +++ b/sw/airborne/fms/libeknav/raw_log.h @@ -3,13 +3,14 @@ #include "math/pprz_algebra_float.h" -struct raw_log_entry { +struct __attribute__ ((packed)) raw_log_entry{ float time; struct FloatRates gyro; struct FloatVect3 accel; struct FloatVect3 mag; struct FloatVect3 ecef_pos; struct FloatVect3 ecef_vel; + uint8_t data_valid; }; #endif /* LIBEKNAV_RAW_LOG_H */ diff --git a/sw/airborne/fms/libeknav/raw_log_to_ascii.c b/sw/airborne/fms/libeknav/raw_log_to_ascii.c index e0cadf60d8..4d58697f74 100644 --- a/sw/airborne/fms/libeknav/raw_log_to_ascii.c +++ b/sw/airborne/fms/libeknav/raw_log_to_ascii.c @@ -47,6 +47,7 @@ int main(int argc, char** argv) { void print_raw_log_entry(struct raw_log_entry* entry){ printf("%f\t", entry->time); + printf("%d\t", entry->data_valid); printf("%+f %+f %+f\t", entry->gyro.p, entry->gyro.q, entry->gyro.r); printf("%+f %+f %+f\t", entry->accel.x, entry->accel.y, entry->accel.z); printf("%+f %+f %+f\t", entry->mag.x, entry->mag.y, entry->mag.z); diff --git a/sw/airborne/fms/libeknav/test_libeknav_4.cpp b/sw/airborne/fms/libeknav/test_libeknav_4.cpp index b77b2f206d..94cc35623a 100644 --- a/sw/airborne/fms/libeknav/test_libeknav_4.cpp +++ b/sw/airborne/fms/libeknav/test_libeknav_4.cpp @@ -3,17 +3,18 @@ #include - + struct timespec start, prev; FILE* ins_logfile; // note: initilaized in init_ins_state unsigned int counter; - +#if RUN_FILTER //useless initialization (I hate C++) static basic_ins_qkf ins = basic_ins_qkf(Vector3d::Zero(), 0, 0, 0, Vector3d::Zero(), Vector3d::Zero(), Vector3d::Zero()); // import most common Eigen types +#endif USING_PART_OF_NAMESPACE_EIGEN int main(int, char *[]) { @@ -40,12 +41,20 @@ int main(int, char *[]) { static void main_init(void) { - printf("Filter output will be in "); - #if FILTER_OUTPUT_IN_NED - printf("NED\n"); + #if RUN_FILTER + printf("FILTER output will be in "); + #if FILTER_OUTPUT_IN_NED + printf("NED\n"); + #else + printf("ECEF\n"); + #endif #else - printf("ECEF\n"); + printf("Filter wont run\n"); #endif + + #if UPDATE_WITH_GRAVITY + printf("the orientation becomes UPDATED with the GRAVITY\n"); + #endif TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); @@ -63,9 +72,10 @@ static void main_init(void) { TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); return; } - + #if RUN_FILTER init_ins_state(); set_reference_direction(); + #endif main_rawlog_init(IMU_LOG_FILE); @@ -80,44 +90,52 @@ static void main_periodic(int my_sig_num __attribute__ ((unused))) { } uint8_t data_valid = main_dialog_with_io_proc(); + #if RUN_FILTER main_run_ins(data_valid); - main_rawlog_dump(); + #endif + main_rawlog_dump(data_valid); } static uint8_t main_dialog_with_io_proc() { - - struct AutopilotMessageCRCFrame msg_in; - struct AutopilotMessageCRCFrame msg_out; - uint8_t crc_valid; + + DEFINE_AutopilotMessageCRCFrame_IN_and_OUT(message); + uint8_t crc_valid; // for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i]; - spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); + spi_link_send(&message_out, sizeof(struct AutopilotMessageCRCFrame), &message_in, &crc_valid); - struct AutopilotMessageVIUp *in = &msg_in.payload.msg_up; + struct AutopilotMessageVIUp *in = &message_in.payload.msg_up; - if(in->valid_sensors & (1<< VI_IMU_DATA_VALID)){ - RATES_FLOAT_OF_BFP(imu_float.gyro, in->gyro); - ACCELS_FLOAT_OF_BFP(imu_float.accel, in->accel); + if(IMU_READY(in->valid_sensors)){ + COPY_RATES_ACCEL_TO_IMU_FLOAT(in); } - if(in->valid_sensors & (1<< VI_MAG_DATA_VALID)){ - MAGS_FLOAT_OF_BFP(imu_float.mag, in->mag); - printf("MAG: %f %f %f\n", imu_float.mag.x, imu_float.mag.y, imu_float.mag.z); + if(MAG_READY(in->valid_sensors)){ + #if SYNTHETIC_MAG_MODE + EARTHS_GEOMAGNETIC_FIELD_NORMED(imu_float.mag); + #else + COPY_MAG_TO_IMU_FLOAT(in); + #endif + #if PRINT_MAG + printmag(); + #endif } - if(in->valid_sensors & (1<ecef_pos); - printf("GPS: %d %d %d\n", imu_ecef_pos.x, imu_ecef_pos.y, imu_ecef_pos.z); - VECT3_COPY(imu_ecef_vel, in->ecef_vel); + if(GPS_READY(in->valid_sensors)){ + COPY_GPS_TO_IMU(in); + #if PRINT_GPS + printgps(); + #endif } return in->valid_sensors; } +#if RUN_FILTER static void main_run_ins(uint8_t data_valid) { struct timespec now; @@ -125,30 +143,28 @@ static void main_run_ins(uint8_t data_valid) { double dt_imu_freq = 0.001953125; // 1/512; // doesn't work? - ins.predict(RATES_AS_VECTOR(imu_float.gyro), COORDS_AS_VECTOR(imu_float.accel), dt_imu_freq); + ins.predict(RATES_AS_VECTOR3D(imu_float.gyro), VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq); - if(data_valid & (1< diag_cov; - diag_cov << Vector3d::Ones()*bias_cov_0*bias_cov_0, - Vector3d::Ones()*M_PI*M_PI*0.5, - Vector3d::Ones()*pos_cov_0*pos_cov_0, - Vector3d::Ones()*speed_cov_0*speed_cov_0; + diag_cov << Vector3d::Ones() * bias_cov_0 * bias_cov_0 , + Vector3d::Ones() * M_PI*0.5 * M_PI*0.5 , + Vector3d::Ones() * pos_cov_0 * pos_cov_0 , + Vector3d::Ones() * speed_cov_0 * speed_cov_0; ins.cov = diag_cov.asDiagonal(); } static void set_reference_direction(void){ - struct DoubleVect3 ref_dir_ned, - ref_dir_ecef; - EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned); // the "true" magnetic field - //LAB_REFERENCE(ref_dir_ned); // measured in the LAB + struct NedCoor_d ref_dir_ned; + struct EcefCoor_d pos_0_ecef_pprz, + ref_dir_ecef; + EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned); - struct DoubleRMat ned2ecef; - /* copied and modified form pprz_geodetic */ - const double sin_lat = sin(pos_0_lla.lat); - const double cos_lat = cos(pos_0_lla.lat); - const double sin_lon = sin(pos_0_lla.lon); - const double cos_lon = cos(pos_0_lla.lon); - ned2ecef.m[0] = -sin_lat*cos_lon; - ned2ecef.m[1] = -sin_lon; - ned2ecef.m[2] = -cos_lat*cos_lon; - ned2ecef.m[3] = sin_lat*sin_lon; - ned2ecef.m[4] = cos_lon; - ned2ecef.m[5] = -cos_lat*sin_lon; - ned2ecef.m[6] = cos_lat; - ned2ecef.m[7] = 0.; - ned2ecef.m[8] = -sin_lat; + struct LtpDef_d current_ltp; + VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef); + ltp_def_from_ecef_d(¤t_ltp, &pos_0_ecef_pprz); + ecef_of_ned_vect_d(&ref_dir_ecef, ¤t_ltp, &ref_dir_ned); - RMAT_VECT3_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned); - //MAT33_VECT3_TRANSP_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned); - reference_direction = COORDS_AS_VECTOR(ref_dir_ecef); + // old transformation: + //struct DoubleRMat ned2ecef; + //NED_TO_ECEF_MAT(pos_0_lla, ned2ecef.m); + //RMAT_VECT3_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned); + + reference_direction = VECT3_AS_VECTOR3D(ref_dir_ecef); //reference_direction = Vector3d(1, 0, 0); - std::cout <<"reference direction: " << reference_direction.transpose() << std::endl; + std::cout <<"reference direction NED : " << VECT3_AS_VECTOR3D(ref_dir_ned).transpose() << std::endl; + std::cout <<"reference direction ECEF: " << reference_direction.transpose() << std::endl; +} +#endif + + +/* helpstuff */ +/** tiny little functions **/ +void printmag(void){ + printf("MAG: %f %f %f\n", imu_float.mag.x, imu_float.mag.y, imu_float.mag.z); +} + +void printgps(void){ + printf("GPS: %d %d %d\n", imu_ecef_pos.x, imu_ecef_pos.y, imu_ecef_pos.z); } - -/* helpstuff - * - * - * - */ -/* time measurement */ +/** time measurement **/ double absTime(struct timespec T){ return (double)(T.tv_sec + T.tv_nsec*1e-9); @@ -251,11 +262,7 @@ struct timespec time_diff(struct timespec end, struct timespec start){ return dT; } -/* Logging - * - * - * - */ +/** Logging **/ static void main_rawlog_init(const char* filename) { @@ -266,7 +273,7 @@ static void main_rawlog_init(const char* filename) { } } -static void main_rawlog_dump(void) { +static void main_rawlog_dump(uint8_t data_valid) { struct timespec now; clock_gettime(TIMER, &now); struct raw_log_entry e; @@ -277,10 +284,12 @@ static void main_rawlog_dump(void) { VECT3_COPY(e.mag, imu_float.mag); VECT3_COPY(e.ecef_pos, imu_ecef_pos); VECT3_COPY(e.ecef_vel, imu_ecef_vel); + e.data_valid = data_valid; write(raw_log_fd, &e, sizeof(e)); } +#if RUN_FILTER static void print_estimator_state(double time) { #if FILTER_OUTPUT_IN_NED @@ -298,9 +307,9 @@ static void print_estimator_state(double time) { q_ned2enu, q_ned2body; - VECTOR_AS_COORDS(pos_ecef,pos_0_ecef); - VECTOR_AS_COORDS(cur_pos_ecef,ins.avg_state.position); - VECTOR_AS_COORDS(cur_vel_ecef,ins.avg_state.velocity); + VECTOR_AS_VECT3(pos_ecef,pos_0_ecef); + VECTOR_AS_VECT3(cur_pos_ecef,ins.avg_state.position); + VECTOR_AS_VECT3(cur_vel_ecef,ins.avg_state.velocity); ltp_def_from_ecef_d(¤t_ltp, &pos_ecef); @@ -321,8 +330,8 @@ static void print_estimator_state(double time) { fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z); - QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(), - ins.avg_state.orientation.y(), ins.avg_state.orientation.z()); + QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), -ins.avg_state.orientation.x(), + -ins.avg_state.orientation.y(), -ins.avg_state.orientation.z()); QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0); FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef); @@ -332,7 +341,10 @@ static void print_estimator_state(double time) { struct FloatEulers e; FLOAT_EULERS_OF_QUAT(e, q_ned2body); - + + #if PRINT_EULER_NED + printf("EULER % 6.1f % 6.1f % 6.1f\n", e.phi*180*M_1_PI, e.theta*180*M_1_PI, e.psi*180*M_1_PI); + #endif fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi); fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID, sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)), @@ -355,13 +367,13 @@ static void print_estimator_state(double time) { fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z); - struct FloatQuat q; - QUAT_ASSIGN(q, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(), + struct FloatQuat q_ecef2body; + QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(), ins.avg_state.orientation.y(), ins.avg_state.orientation.z()); - struct FloatEulers e; - FLOAT_EULERS_OF_QUAT(e, q); + struct FloatEulers e_ecef2body; + FLOAT_EULERS_OF_QUAT(e_ecef2body, q_ecef2body); - fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi); + fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e_ecef2body.phi, e_ecef2body.theta, e_ecef2body.psi); fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID, sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)), sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)), @@ -370,4 +382,4 @@ static void print_estimator_state(double time) { fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2)); #endif } - +#endif diff --git a/sw/airborne/fms/libeknav/test_libeknav_4.hpp b/sw/airborne/fms/libeknav/test_libeknav_4.hpp index 8019d98f45..f4ad3d4dc0 100644 --- a/sw/airborne/fms/libeknav/test_libeknav_4.hpp +++ b/sw/airborne/fms/libeknav/test_libeknav_4.hpp @@ -4,6 +4,7 @@ #include #include "ins_qkf.hpp" +#include "paparazzi_eigen_conversion.h" #include #include @@ -32,106 +33,147 @@ extern "C" { #include "math/pprz_geodetic_double.c" #include -#define FILTER_OUTPUT_IN_NED 1 + +/* constants */ +/** Compilation-control **/ +#define RUN_FILTER 1 #define UPDATE_WITH_GRAVITY 0 +#define SYNTHETIC_MAG_MODE 1 +#define FILTER_OUTPUT_IN_NED 1 +#define PRINT_MAG 0 +#define PRINT_GPS 1 +#define PRINT_EULER_NED 0 -/* - * - * Initialization - * - */ - -/* initial state */ - +/** geodetic **/ //Toulouse Lat: 43° 35' 24'' Lon: 1° 25' 48'' -#define TOULOUSE_LATTITUDE UGLY_ANGLE_IN_RADIANS(43,35,24) -#define TOULOUSE_LONGITUDE UGLY_ANGLE_IN_RADIANS(1,25,48) +#define TOULOUSE_LATTITUDE ARCSEC_ACRMIN_ANGLE_IN_RADIANS(43,35,24) +#define TOULOUSE_LONGITUDE ARCSEC_ACRMIN_ANGLE_IN_RADIANS(1,25,48) #define TOULOUSE_HEIGHT 0 //Toulouse Declination is 22' West and Inclination 59° 8' Down -#define TOULOUSE_DECLINATION -UGLY_ANGLE_IN_RADIANS(0,22,0) -#define TOULOUSE_INCLINATION -UGLY_ANGLE_IN_RADIANS(59,8,0) +#define TOULOUSE_DECLINATION -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(0,22,0) +#define TOULOUSE_INCLINATION -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(59,8,0) -struct LlaCoor_f pos_0_lla; -Vector3d pos_0_ecef; -Vector3d speed_0_ecef; -// Vector3d orientation(0., 0., 0.); -Vector3d bias_0(0., 0., 0.); - -/** - * how to compute the magnetic field: - * http://gsc.nrcan.gc.ca/geomag/field/comp_e.php - * - * online-calculator: - * http://geomag.nrcan.gc.ca/apps/mfcal-eng.php - */ +/** magnetic field + ** how to compute the magnetic field: + ** http://gsc.nrcan.gc.ca/geomag/field/comp_e.php + ** + ** online-calculator: + ** http://geomag.nrcan.gc.ca/apps/mfcal-eng.php + **/ #if 0 #define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) { \ - ref.z = sin(TOULOUSE_INCLINATION); \ - double h = sqrt(1-ref.z*ref.z); \ - ref.x = h*cos(TOULOUSE_DECLINATION); \ - ref.y = h*sin(TOULOUSE_DECLINATION); \ + ref.z = sin(TOULOUSE_INCLINATION); \ + double h = sqrt(1-ref.z*ref.z); \ + ref.x = h*cos(TOULOUSE_DECLINATION); \ + ref.y = h*sin(TOULOUSE_DECLINATION); \ } #else -#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 0.51292422348174, -0.00331095113378, 0.85842750338526) +//#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 0.51292422348174, -0.00331095113378, 0.85842750338526) +#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 0.51562740288882, -0.05707735220832, 0.85490967783446) +#endif +Vector3d reference_direction; +#if RUN_FILTER +static void set_reference_direction(void); #endif -// mean of the measurment data -#define LAB_REFERENCE(ref) VECT3_ASSIGN(ref, -0.22496030821134, 0.70578892222179, 0.67175505729281) +/** other **/ +#define GRAVITY 9.81 +#define MAX_DISTANCE_FROM_GRAVITY_FOR_UPDATE 0.03 -Vector3d reference_direction; +/* Initialisation */ +static void main_init(void); -/* initial covariance */ +/** initial state **/ +struct LlaCoor_f pos_0_lla; +Vector3d pos_0_ecef; +Vector3d speed_0_ecef = Vector3d::Zero(); +Vector3d bias_0(0., 0., 0.); +#if RUN_FILTER +static void init_ins_state(void); +#endif + +/** initial covariance **/ const double pos_cov_0 = 1e4; const double speed_cov_0 = 3.; // const double orientation_cov_0 = RadOfDeg(5.)*RadOfDeg(5.); const double bias_cov_0 = 0.447; const double mag_noise = std::pow(5 / 180.0 * M_PI, 2); -/* system noise */ -const double mag_error = 2.536e-3; -const Vector3d gyro_white_noise(1.1328*1.1328e-4, 0.9192*0.9192e-4, 1.2291*1.2291e-4); -const Vector3d gyro_stability_noise(-1.7605*1.7605e-4, 0.5592*0.5592e-4, 1.1486*1.1486e-4 ); -const Vector3d accel_white_noise(2.3707*2.3707e-4, 2.4575*2.4575e-4, 2.5139*2.5139e-4); - - -/* - * - * HEADERS - * - */ + +/* system noise */ +const double mag_error = 2.536e-3; +const Vector3d gyro_white_noise ( 1.1328*1.1328e-4, 0.9192*0.9192e-4, 1.2291*1.2291e-4); +const Vector3d gyro_stability_noise ( -1.7605*1.7605e-4, 0.5592*0.5592e-4, 1.1486*1.1486e-4); +const Vector3d accel_white_noise ( 2.3707*2.3707e-4, 2.4575*2.4575e-4, 2.5139*2.5139e-4); +const Vector3d gps_pos_noise = Vector3d::Ones() *10 *10 ; +const Vector3d gps_speed_noise = Vector3d::Ones() * 0.1* 0.1; +/* STM32 Communication */ +static void main_periodic(int my_sig_num); static void main_trick_libevent(void); static void on_foo_event(int fd, short event __attribute__((unused)), void *arg); static struct event foo_event; - - -static void main_rawlog_init(const char* filename); -static void main_rawlog_dump(void); - -static void main_init(void); -static void init_ins_state(void); -static void set_reference_direction(void); -static void main_periodic(int my_sig_num); static uint8_t main_dialog_with_io_proc(void); -static void main_run_ins(uint8_t); +/* libeknav */ +#if RUN_FILTER +static void main_run_ins(uint8_t); +#endif + /* Logging */ #define IMU_LOG_FILE "/tmp/log_test3.bin" -#define INS_LOG_FILE "/tmp/log_ins_test3.data" +static void main_rawlog_init(const char* filename); +static void main_rawlog_dump(uint8_t); + +#if RUN_FILTER static void print_estimator_state(double); +#define INS_LOG_FILE "/tmp/log_ins_test3.data" +#endif + /* time measurement */ #define TIMER CLOCK_MONOTONIC double absTime(struct timespec); struct timespec time_diff(struct timespec, struct timespec); + /* Other */ -#define UGLY_ANGLE_IN_RADIANS(degree, arcmin, arcsec) ((degree+arcmin/60+arcsec/3600)*M_PI/180) -#define COORDS_AS_VECTOR(coords) Vector3d(coords.x, coords.y, coords.z) -#define RATES_AS_VECTOR(rates) Vector3d(rates.p,rates.q,rates.r) -#define ABS(a) ((a<0)?-a:a) -#define VECTOR_AS_COORDS(coords, vector) { coords.x = vector(0); coords.y = vector(1); coords.z = vector(2);} + /** tiny little functions **/ +#define DEFINE_AutopilotMessageCRCFrame_IN_and_OUT(name) \ + struct AutopilotMessageCRCFrame name##_in; \ + struct AutopilotMessageCRCFrame name##_out +void printmag(void); +void printgps(void); + + /** Sensors **/ +#define COPY_RATES_ACCEL_TO_IMU_FLOAT(pointer){ \ + RATES_FLOAT_OF_BFP(imu_float.gyro, pointer->gyro); \ + ACCELS_FLOAT_OF_BFP(imu_float.accel, pointer->accel); \ +} +#define COPY_MAG_TO_IMU_FLOAT(pointer) MAGS_FLOAT_OF_BFP(imu_float.mag, pointer->mag) +#define COPY_GPS_TO_IMU(pointer){ \ + VECT3_COPY(imu_ecef_pos, pointer->ecef_pos); \ + VECT3_COPY(imu_ecef_vel, pointer->ecef_vel); \ +} + +#define IMU_READY(data_valid) (data_valid & (1<