void ahrs_propagate(void) { /* converts gyro to floating point */ struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev); /* unbias measurement */ RATES_SUB(gyro_float, ahrs_impl.gyro_bias); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES const float alpha = 0.1; FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha); #else RATES_COPY(ahrs_impl.imu_rate,gyro_float); #endif /* add correction */ struct FloatRates omega; RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction); /* and zeros it */ FLOAT_RATES_ZERO(ahrs_impl.rate_correction); const float dt = 1./AHRS_PROPAGATE_FREQUENCY; #if AHRS_PROPAGATE_RMAT FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt); float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat); FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat); #endif #if AHRS_PROPAGATE_QUAT FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt); FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat); FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); #endif compute_body_orientation_and_rates(); }
void ahrs_propagate(void) { /* converts gyro to floating point */ struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev); /* unbias measurement */ RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias); const float dt = 1./512.; /* add correction */ struct FloatRates omega; RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction); // DISPLAY_FLOAT_RATES("omega ", omega); /* and zeros it */ FLOAT_RATES_ZERO(ahrs_impl.rate_correction); /* first order integration of rotation matrix */ struct FloatRMat exp_omega_dt = { { 1. , dt*omega.r, -dt*omega.q, -dt*omega.r, 1. , dt*omega.p, dt*omega.q, -dt*omega.p, 1. }}; struct FloatRMat R_tdt; FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt); memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt)); float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat); // struct FloatRMat test; // FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat); // DISPLAY_FLOAT_RMAT("foo", test); compute_imu_quat_and_euler_from_rmat(); compute_body_orientation_and_rates(); }
void aos_compute_sensors(void) { struct FloatRates gyro; RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias); // printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r)); float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise); RATES_BFP_OF_REAL(imu.gyro, gyro); RATES_BFP_OF_REAL(imu.gyro_prev, gyro); struct FloatVect3 g_ltp = {0., 0., 9.81}; struct FloatVect3 accelero_ltp; VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp); struct FloatVect3 accelero_imu; float_quat_vmult(&accelero_imu, &aos.ltp_to_imu_quat, &accelero_ltp); float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise); ACCELS_BFP_OF_REAL(imu.accel, accelero_imu); #ifndef DISABLE_MAG_UPDATE struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; struct FloatVect3 h_imu; float_quat_vmult(&h_imu, &aos.ltp_to_imu_quat, &h_earth); MAGS_BFP_OF_REAL(imu.mag, h_imu); #endif aos.heading_meas = aos.ltp_to_imu_euler.psi + get_gaussian_noise() * aos.heading_noise; #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel); ahrs_impl.ltp_vel_norm_valid = true; #endif #if AHRS_TYPE == AHRS_TYPE_FCR2 ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel); ahrs_impl.ltp_vel_norm_valid = true; #endif #if AHRS_TYPE == AHRS_TYPE_FCR ahrs_impl.gps_speed = float_vect3_norm(&aos.ltp_vel); ahrs_impl.gps_age = 0; ahrs_update_gps(); //RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration)); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(float_vect3_norm(&aos.ltp_vel)); ahrs_impl.ltp_vel_norm_valid = true; #endif #endif }
void ahrs_fc_propagate(struct Int32Rates *gyro, float dt) { /* converts gyro to floating point */ struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, *gyro); /* unbias measurement */ RATES_SUB(gyro_float, ahrs_fc.gyro_bias); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES const float alpha = 0.1; FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), gyro_float, alpha); #else RATES_COPY(ahrs_fc.imu_rate, gyro_float); #endif /* add correction */ struct FloatRates omega; RATES_SUM(omega, gyro_float, ahrs_fc.rate_correction); /* and zeros it */ FLOAT_RATES_ZERO(ahrs_fc.rate_correction); #if AHRS_PROPAGATE_RMAT float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt); float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat); float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat); #endif #if AHRS_PROPAGATE_QUAT float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt); float_quat_normalize(&ahrs_fc.ltp_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); #endif // increase accel and mag propagation counters ahrs_fc.accel_cnt++; ahrs_fc.mag_cnt++; }
void aos_compute_sensors(void) { struct FloatRates gyro; RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias); // printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r)); float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise); RATES_BFP_OF_REAL(imu.gyro, gyro); RATES_BFP_OF_REAL(imu.gyro_prev, gyro); struct FloatVect3 g_ltp = {0., 0., 9.81}; struct FloatVect3 accelero_ltp; VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp); struct FloatVect3 accelero_imu; FLOAT_QUAT_VMULT(accelero_imu, aos.ltp_to_imu_quat, accelero_ltp); float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise); ACCELS_BFP_OF_REAL(imu.accel, accelero_imu); struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; struct FloatVect3 h_imu; FLOAT_QUAT_VMULT(h_imu, aos.ltp_to_imu_quat, h_earth); MAGS_BFP_OF_REAL(imu.mag, h_imu); #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel)); #endif #endif }