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 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 }