struct DoubleEulers sigma_euler_from_sigma_q(struct DoubleQuat q, struct DoubleQuat sigma_q){ struct DoubleVect3 v_q, v_sigma, temporary_result; struct DoubleEulers sigma_eu; QUAT_IMAGINARY_PART(q, v_q); QUAT_IMAGINARY_PART(sigma_q, v_sigma); if(DOUBLE_VECT3_NORM(v_sigma)>0.5){ EULERS_ASSIGN(sigma_eu, M_PI_2, M_PI_2, M_PI_2); return sigma_eu; } VECT3_CROSS_PRODUCT(temporary_result, v_q, v_sigma); VECT3_SMUL(v_q, v_q, sigma_q.qi); VECT3_SMUL(v_sigma, v_sigma, q.qi); VECT3_ADD(temporary_result, v_sigma); VECT3_SUB(temporary_result, v_q); VECT3_TO_EULERS(temporary_result, sigma_eu); return sigma_eu; }
static void traj_sineX_quad_update(void) { const float om = RadOfDeg(10); if ( aos.time > (M_PI/om) ) { const float a = 20; struct FloatVect3 jerk; VECT3_ASSIGN(jerk , -a*om*om*om*cos(om*aos.time), 0, 0); VECT3_ASSIGN(aos.ltp_accel , -a*om*om *sin(om*aos.time), 0, 0); VECT3_ASSIGN(aos.ltp_vel , a*om *cos(om*aos.time), 0, 0); VECT3_ASSIGN(aos.ltp_pos , a *sin(om*aos.time), 0, 0); // this is based on differential flatness of the quad EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.); FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); const struct FloatEulers e_dot = { 0., 9.81*jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)), 0. }; FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot); } }
static void test_4_float(void) { printf("euler to quat to euler - float\n"); /* initial euler angles */ struct FloatEulers e; EULERS_ASSIGN(e, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.)); DISPLAY_FLOAT_EULERS_DEG("e", e); /* transform to quaternion */ struct FloatQuat q; float_quat_of_eulers(&q, &e); // DISPLAY_FLOAT_QUAT("q", q); float_quat_normalize(&q); DISPLAY_FLOAT_QUAT("q_n", q); DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q); /* back to eulers */ struct FloatEulers e2; float_eulers_of_quat(&e2, &q); DISPLAY_FLOAT_EULERS_DEG("e2", e2); }
static void test_4_int(void) { printf("euler to quat to euler - int\n"); /* initial euler angles */ struct Int32Eulers _e; EULERS_ASSIGN(_e, ANGLE_BFP_OF_REAL(RadOfDeg(-10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("euler orig ", _e); /* transform to quaternion */ struct Int32Quat _q; int32_quat_of_eulers(&_q, &_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q); // int32_quat_normalize(&_q); // DISPLAY_INT32_QUAT_2("_q_n", _q); /* back to eulers */ struct Int32Eulers _e2; int32_eulers_of_quat(&_e2, &_q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("back to euler ", _e2); }
static void traj_stop_stop_x_update(void){ const float t0 = 5.; const float dt_jerk = 0.75; const float dt_nojerk = 10.; const float val_jerk = 5.; FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt); FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt); FLOAT_VECT3_INTEGRATE_FI(aos.ltp_accel, aos.ltp_jerk, aos.dt); if (aos.time < t0) return; else if (aos.time < t0+dt_jerk) { VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); } else if (aos.time < t0 + 2.*dt_jerk) { VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); } else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) { VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); } else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) { VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); } else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) { VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); } else { VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); } // this is based on differential flatness of the quad EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.); FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); const struct FloatEulers e_dot = { 0., 9.81*aos.ltp_jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)), 0. }; FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot); }
static void test_3(void) { /* Compute BODY to IMU eulers */ struct Int32Eulers b2i_e; EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e); /* Compute BODY to IMU quaternion */ struct Int32Quat b2i_q; INT32_QUAT_OF_EULERS(b2i_q, b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); // INT32_QUAT_NORMALIZE(b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ struct Int32RMat b2i_r; INT32_RMAT_OF_EULERS(b2i_r, b2i_e); // DISPLAY_INT32_RMAT("b2i_r", b2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r); /* Compute LTP to IMU eulers */ struct Int32Eulers l2i_e; EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e); /* Compute LTP to IMU quaternion */ struct Int32Quat l2i_q; INT32_QUAT_OF_EULERS(l2i_q, l2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q); /* Compute LTP to IMU rotation matrix */ struct Int32RMat l2i_r; INT32_RMAT_OF_EULERS(l2i_r, l2i_e); // DISPLAY_INT32_RMAT("l2i_r", l2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r); /* again but from quaternion */ struct Int32RMat l2i_r2; INT32_RMAT_OF_QUAT(l2i_r2, l2i_q); // DISPLAY_INT32_RMAT("l2i_r2", l2i_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2); /* Compute LTP to BODY quaternion */ struct Int32Quat l2b_q; INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q); /* Compute LTP to BODY rotation matrix */ struct Int32RMat l2b_r; INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r); // DISPLAY_INT32_RMAT("l2b_r", l2b_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r); /* again but from quaternion */ struct Int32RMat l2b_r2; INT32_RMAT_OF_QUAT(l2b_r2, l2b_q); // DISPLAY_INT32_RMAT("l2b_r2", l2b_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2); /* compute LTP to BODY eulers */ struct Int32Eulers l2b_e; INT32_EULERS_OF_RMAT(l2b_e, l2b_r); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e); /* again but from quaternion */ struct Int32Eulers l2b_e2; INT32_EULERS_OF_QUAT(l2b_e2, l2b_q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2); }
static void test_3(void) { /* Compute BODY to IMU eulers */ struct Int32Eulers b2i_e; EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e); /* Compute BODY to IMU quaternion */ struct Int32Quat b2i_q; int32_quat_of_eulers(&b2i_q, &b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); // int32_quat_normalize(&b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ struct Int32RMat b2i_r; int32_rmat_of_eulers(&b2i_r, &b2i_e); // DISPLAY_INT32_RMAT("b2i_r", b2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r); /* Compute LTP to IMU eulers */ struct Int32Eulers l2i_e; EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e); /* Compute LTP to IMU quaternion */ struct Int32Quat l2i_q; int32_quat_of_eulers(&l2i_q, &l2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q); /* Compute LTP to IMU rotation matrix */ struct Int32RMat l2i_r; int32_rmat_of_eulers(&l2i_r, &l2i_e); // DISPLAY_INT32_RMAT("l2i_r", l2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r); /* again but from quaternion */ struct Int32RMat l2i_r2; int32_rmat_of_quat(&l2i_r2, &l2i_q); // DISPLAY_INT32_RMAT("l2i_r2", l2i_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2); /* Compute LTP to BODY quaternion */ struct Int32Quat l2b_q; int32_quat_comp_inv(&l2b_q, &b2i_q, &l2i_q); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q); /* Compute LTP to BODY rotation matrix */ struct Int32RMat l2b_r; int32_rmat_comp_inv(&l2b_r, &l2i_r, &b2i_r); // DISPLAY_INT32_RMAT("l2b_r", l2b_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r); /* again but from quaternion */ struct Int32RMat l2b_r2; int32_rmat_of_quat(&l2b_r2, &l2b_q); // DISPLAY_INT32_RMAT("l2b_r2", l2b_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2); /* compute LTP to BODY eulers */ struct Int32Eulers l2b_e; int32_eulers_of_rmat(&l2b_e, &l2b_r); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e); /* again but from quaternion */ struct Int32Eulers l2b_e2; int32_eulers_of_quat(&l2b_e2, &l2b_q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2); }
void aos_init(int traj_nb) { aos.traj = &traj[traj_nb]; aos.time = 0; aos.dt = 1. / AHRS_PROPAGATE_FREQUENCY; aos.traj->ts = 0; aos.traj->ts = 1.; // default to one second /* default state */ EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler); RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); FLOAT_VECT3_ZERO(aos.ltp_pos); FLOAT_VECT3_ZERO(aos.ltp_vel); FLOAT_VECT3_ZERO(aos.ltp_accel); FLOAT_VECT3_ZERO(aos.ltp_jerk); aos.traj->init_fun(); imu_init(); ahrs_init(); #ifdef PERFECT_SENSORS RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.); aos.heading_noise = 0.; #else RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.)); RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.)); VECT3_ASSIGN(aos.accel_noise, .5, .5, .5); aos.heading_noise = RadOfDeg(3.); #endif #ifdef FORCE_ALIGNEMENT // DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat); aos_compute_sensors(); // DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias); // DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates); VECT3_COPY(ahrs_aligner.lp_accel, imu.accel); VECT3_COPY(ahrs_aligner.lp_mag, imu.mag); RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro); // DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro); ahrs_align(); // DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias); #endif #ifdef DISABLE_ALIGNEMENT printf("# DISABLE_ALIGNEMENT\n"); #endif #ifdef DISABLE_PROPAGATE printf("# DISABLE_PROPAGATE\n"); #endif #ifdef DISABLE_ACCEL_UPDATE printf("# DISABLE_ACCEL_UPDATE\n"); #endif #ifdef DISABLE_MAG_UPDATE printf("# DISABLE_MAG_UPDATE\n"); #endif printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]); printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n"); #endif #if AHRS_MAG_UPDATE_YAW_ONLY printf("# AHRS_MAG_UPDATE_YAW_ONLY\n"); #endif #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n"); #endif #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n"); #endif #ifdef PERFECT_SENSORS printf("# PERFECT_SENSORS\n"); #endif #if AHRS_USE_GPS_HEADING printf("# AHRS_USE_GPS_HEADING\n"); #endif #if USE_AHRS_GPS_ACCELERATIONS printf("# USE_AHRS_GPS_ACCELERATIONS\n"); #endif printf("# tajectory : %s\n", aos.traj->name); };
static void traj_step_phi_update(void) { if (aos.time > 5) { EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0); FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); } }