diff --git a/conf/messages.xml b/conf/messages.xml index 82375b4e22..41db426566 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1224,6 +1224,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index 9990bc4f9c..9ff78c0cd5 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -4,7 +4,7 @@ - + diff --git a/conf/telemetry/telemetry_booz2_flixr.xml b/conf/telemetry/telemetry_booz2_flixr.xml index ff7efffb38..ede7aaaf76 100644 --- a/conf/telemetry/telemetry_booz2_flixr.xml +++ b/conf/telemetry/telemetry_booz2_flixr.xml @@ -3,30 +3,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - @@ -34,14 +10,19 @@ + + + + - - + + + - + @@ -49,6 +30,7 @@ + @@ -60,12 +42,24 @@ - + - + + + + + + + + + + + + + @@ -80,14 +74,18 @@ - + + + + + @@ -95,6 +93,9 @@ + + + @@ -106,10 +107,19 @@ - + + + + + + + + - diff --git a/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c b/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c index 0c5f289e6d..583ed08b65 100644 --- a/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c +++ b/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c @@ -33,24 +33,29 @@ struct BoozAhrs booz_ahrs; -/* our estimated attitude */ +/* our estimated attitude (ltp <-> imu) */ struct FloatQuat bafl_quat; /* our estimated gyro biases */ struct FloatRates bafl_bias; -/* our estimated attitude errors */ -struct FloatQuat bafl_quat_err; -/* our estimated gyro bias errors */ -struct FloatRates bafl_bias_err; /* we get unbiased body rates as byproduct */ struct FloatRates bafl_rates; /* maintain a euler angles representation */ struct FloatEulers bafl_eulers; /* maintains a rotation matrix representation */ struct FloatRMat bafl_dcm; + +/* our estimated attitude errors */ +struct FloatQuat bafl_quat_err; +/* our estimated gyro bias errors */ +struct FloatRates bafl_bias_err; /* time derivative of our quaternion */ struct FloatQuat bafl_qdot; +/* correction quaternion of strapdown computatino */ +struct FloatQuat bafl_qr; +#ifndef BAFL_SSIZE #define BAFL_SSIZE 6 +#endif /* error covariance matrix */ float bafl_P[BAFL_SSIZE][BAFL_SSIZE]; /* filter state */ @@ -121,10 +126,6 @@ struct FloatVect3 bafl_h; * We only have an expected noise in the pitch and roll accelerometers * and in the compass. */ -#define BAFL_R_PHI 1.3 * 1.3 -#define BAFL_R_THETA 1.3 * 1.3 -#define BAFL_R_PSI 2.5 * 2.5 - #define BAFL_SIGMA_ACCEL 5.0 #define BAFL_SIGMA_MAG 300. float bafl_sigma_accel; @@ -201,14 +202,47 @@ void booz_ahrs_propagate(void) { */ /* compute qdot and normalize it */ - FLOAT_QUAT_DERIVATIVE_LAGRANGE(bafl_qdot, bafl_rates, bafl_quat); + //FLOAT_QUAT_DERIVATIVE(bafl_qdot, bafl_rates, bafl_quat); /* multiply qdot with dt */ - QUAT_SMUL(bafl_qdot, bafl_qdot, BAFL_DT); + //QUAT_SMUL(bafl_qdot, bafl_qdot, BAFL_DT); /* propagate quaternion */ - QUAT_ADD(bafl_quat, bafl_qdot); + //QUAT_ADD(bafl_quat, bafl_qdot); + + /* multiplicative quaternion update */ + QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT/2, bafl_rates.q * BAFL_DT/2, bafl_rates.r * BAFL_DT/2); + FLOAT_QUAT_COMP(bafl_quat, bafl_qr, bafl_quat); + + + + FLOAT_QUAT_NORMALISE(bafl_quat); + + /* maintain rotation matrix representation */ FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat); + /* maintain euler representation */ + FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm); + + /* + * convert to binary floating point + */ + /* IMU rate */ + RATES_BFP_OF_REAL(booz_ahrs.imu_rate, bafl_rates); + /* LTP to IMU eulers */ + EULERS_BFP_OF_REAL(booz_ahrs.ltp_to_imu_euler, bafl_eulers); + /* LTP to IMU quaternion */ + QUAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_quat, bafl_quat); + /* LTP to IMU rotation matrix */ + RMAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_rmat, bafl_dcm); + + /* Compute LTP to BODY quaternion */ + INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_imu.body_to_imu_quat, booz_ahrs.ltp_to_imu_quat); + /* Compute LTP to BODY rotation matrix */ + INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, booz_imu.body_to_imu_rmat); + /* compute LTP to BODY eulers */ + INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat); + /* compute body rates */ + INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, booz_imu.body_to_imu_rmat, booz_ahrs.imu_rate); //TODO check if normalization is good @@ -479,7 +513,7 @@ void booz_ahrs_update_accel(void) { /* correct attitude */ - FLOAT_QUAT_COMP(bafl_quat, bafl_quat, bafl_quat_err); + FLOAT_QUAT_COMP(bafl_quat, bafl_quat_err, bafl_quat); /* correct gyro bias */ @@ -719,7 +753,7 @@ void booz_ahrs_update_mag(void) { /* correct attitude */ - FLOAT_QUAT_COMP(bafl_quat, bafl_quat, bafl_quat_err); + FLOAT_QUAT_COMP(bafl_quat, bafl_quat_err, bafl_quat); /* correct gyro bias */ diff --git a/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.h b/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.h index ffe6328b1b..bf5b601e35 100644 --- a/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.h +++ b/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.h @@ -29,6 +29,19 @@ #include "std.h" #include "math/pprz_algebra_int.h" +extern struct FloatQuat bafl_quat; +extern struct FloatRates bafl_bias; +extern struct FloatRates bafl_rates; +extern struct FloatEulers bafl_eulers; +extern struct FloatRMat bafl_dcm; + +extern struct FloatQuat bafl_quat_err; +extern struct FloatRates bafl_bias_err; + +#define BAFL_SSIZE 6 +extern float bafl_P[BAFL_SSIZE][BAFL_SSIZE]; +extern float bafl_X[BAFL_SSIZE]; + extern float bafl_sigma_accel; extern float bafl_sigma_mag; extern float bafl_R_accel; diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c index 0ffb5dd04b..15427d334d 100644 --- a/sw/airborne/booz/booz2_main.c +++ b/sw/airborne/booz/booz2_main.c @@ -183,7 +183,7 @@ static inline void on_gyro_accel_event( void ) { else { booz_ahrs_propagate(); #ifdef USE_AHRS_LKF - booz_ahrs_update_accel(); + //booz_ahrs_update_accel(); #endif // booz2_filter_attitude_update(); booz_ins_propagate(); @@ -202,6 +202,6 @@ static inline void on_gps_event(void) { static inline void on_mag_event(void) { BoozImuScaleMag(); #ifdef USE_AHRS_LKF - booz_ahrs_update_mag(); + //booz_ahrs_update_mag(); #endif } diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index ee17fbe59d..f43dab5b64 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -288,6 +288,43 @@ #define PERIODIC_SEND_BOOZ2_FILTER() {} #endif +#ifdef USE_AHRS_LKF +#include "booz_ahrs.h" +#include "ahrs/booz_ahrs_float_lkf.h" +#define PERIODIC_SEND_BOOZ_AHRS_LKF() { \ + DOWNLINK_SEND_BOOZ_AHRS_LKF(&bafl_eulers.phi, \ + &bafl_eulers.theta, \ + &bafl_eulers.psi, \ + &bafl_quat.qi, \ + &bafl_quat.qx, \ + &bafl_quat.qy, \ + &bafl_quat.qz, \ + &bafl_rates.p, \ + &bafl_rates.q, \ + &bafl_rates.r, \ + &bafl_bias.p, \ + &bafl_bias.q, \ + &bafl_bias.r); \ + } +#define PERIODIC_SEND_BOOZ_AHRS_LKF_DEBUG() { \ + DOWNLINK_SEND_BOOZ_AHRS_LKF_DEBUG(&bafl_X[0], \ + &bafl_X[1], \ + &bafl_X[2], \ + &bafl_bias_err.p, \ + &bafl_bias_err.q, \ + &bafl_bias_err.r, \ + &bafl_P[0][0], \ + &bafl_P[1][1], \ + &bafl_P[2][2], \ + &bafl_P[3][3], \ + &bafl_P[4][4], \ + &bafl_P[5][5]); \ + } +#else +#define PERIODIC_SEND_BOOZ_AHRS_LKF() {} +#define PERIODIC_SEND_BOOZ_AHRS_LKF_DEBUG() {} +#endif + #define PERIODIC_SEND_BOOZ2_AHRS_QUAT() { \ DOWNLINK_SEND_BOOZ2_AHRS_QUAT(&booz_ahrs.ltp_to_imu_quat.qi, \ diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h index 1431cf8fa7..9ae39f4da6 100644 --- a/sw/airborne/math/pprz_algebra.h +++ b/sw/airborne/math/pprz_algebra.h @@ -376,6 +376,14 @@ // Quaternions // // +#define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \ + (_q).qi = (_i); \ + (_q).qx = (_x); \ + (_q).qy = (_y); \ + (_q).qz = (_z); \ + } + + #define QUAT_DIFF(_qc, _qa, _qb) { \ (_qc).qi = (_qa).qi - (_qb).qi; \ diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index a8c15b283e..a4004770ce 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -299,12 +299,7 @@ struct FloatRates { * Quaternions */ -#define FLOAT_QUAT_ZERO(_q) { \ - (_q).qi = 1.; \ - (_q).qx = 0.; \ - (_q).qy = 0.; \ - (_q).qz = 0.; \ - } +#define FLOAT_QUAT_ZERO(_q) QUAT_ASSIGN(_q, 1., 0., 0., 0.) /* _q += _qa */ #define FLOAT_QUAT_ADD(_qo, _qi) QUAT_ADD(_qo, _qi) diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c index f14f9b7616..b908ee2991 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy.c @@ -9,8 +9,8 @@ #include "nps_autopilot.h" #include "nps_fdm.h" -static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), +static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]); void nps_ivy_init(void) { @@ -26,8 +26,8 @@ void nps_ivy_init(void) { #include "settings.h" #include "dl_protocol.h" #include "downlink.h" -static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), +static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { uint8_t index = atoi(argv[2]); float value = atof(argv[3]); @@ -40,27 +40,34 @@ void nps_ivy_display(void) { /* - IvySendMsg("%d COMMANDS %f %f %f %f", + IvySendMsg("%d COMMANDS %f %f %f %f", AC_ID, autopilot.commands[SERVO_FRONT], - autopilot.commands[SERVO_BACK], + autopilot.commands[SERVO_BACK], autopilot.commands[SERVO_RIGHT], - autopilot.commands[SERVO_LEFT]); + autopilot.commands[SERVO_LEFT]); */ - IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", + IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", AC_ID, - DegOfRad(fdm.body_ecef_rotvel.p), - DegOfRad(fdm.body_ecef_rotvel.q), + DegOfRad(fdm.body_ecef_rotvel.p), + DegOfRad(fdm.body_ecef_rotvel.q), DegOfRad(fdm.body_ecef_rotvel.r), - DegOfRad(fdm.ltp_to_body_eulers.phi), - DegOfRad(fdm.ltp_to_body_eulers.theta), + DegOfRad(fdm.ltp_to_body_eulers.phi), + DegOfRad(fdm.ltp_to_body_eulers.theta), DegOfRad(fdm.ltp_to_body_eulers.psi)); - IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f", + IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f", AC_ID, - (fdm.ltp_ecef_vel.x), - (fdm.ltp_ecef_vel.y), + (fdm.ltp_ecef_vel.x), + (fdm.ltp_ecef_vel.y), (fdm.ltp_ecef_vel.z), - (fdm.ltpprz_pos.x), - (fdm.ltpprz_pos.y), + (fdm.ltpprz_pos.x), + (fdm.ltpprz_pos.y), (fdm.ltpprz_pos.z)); +#if 0 + IvySendMsg("%d BOOZ_SIM_GYRO_BIAS %f %f %f", + AC_ID, + (sensors.gyro.bias_random_walk_value.p), + (sensors.gyro.bias_random_walk_value.q), + (sensors.gyro.bias_random_walk_value.r)); +#endif }