static void feed_imu(int i) { if (i>0) { RATES_COPY(imu.gyro_prev, imu.gyro); } else { RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro); } RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro); ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel); MAGS_BFP_OF_REAL(imu.mag, samples[i].mag); }
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 }
void UM6_packet_read_message(void) { if ((UM6_status == UM6Running) && PacketLength > 11) { switch (PacketAddr) { case IMU_UM6_GYRO_PROC: UM6_rate.p = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352; UM6_rate.q = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352; UM6_rate.r = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352; RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec RATES_BFP_OF_REAL(imu.gyro, UM6_rate); break; case IMU_UM6_ACCEL_PROC: UM6_accel.x = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105; UM6_accel.y = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105; UM6_accel.z = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105; VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2 ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int break; case IMU_UM6_MAG_PROC: UM6_mag.x = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176; UM6_mag.y = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176; UM6_mag.z = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176; // Assume the same units for magnetic field // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss). MAGS_BFP_OF_REAL(imu.mag, UM6_mag); break; case IMU_UM6_EULER: UM6_eulers.phi = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863; UM6_eulers.theta = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863; UM6_eulers.psi = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863; EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers); break; case IMU_UM6_QUAT: UM6_quat.qi = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693; UM6_quat.qx = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693; UM6_quat.qy = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693; UM6_quat.qz = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693; QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat); break; default: break; } } }