static void main_run_ins(uint8_t data_valid) { struct timespec now; clock_gettime(TIMER, &now); double dt_imu_freq = 0.001953125; // 1/512; // doesn't work? ins.predict(RATES_AS_VECTOR3D(imu_float.gyro), VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq); if(MAG_READY(data_valid)){ ins.obs_vector(reference_direction, VECT3_AS_VECTOR3D(imu_float.mag), mag_noise); } #if UPDATE_WITH_GRAVITY if(CLOSE_TO_GRAVITY(imu_float.accel)){ // use the gravity as reference ins.obs_vector(ins.avg_state.position.normalized(), VECT3_AS_VECTOR3D(imu_float.accel), 1.0392e-3); } #endif if(GPS_READY(data_valid)){ ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, gps_pos_noise, gps_speed_noise); } print_estimator_state(absTime(time_diff(now, start))); }
static void main_run_from_file(int file_descriptor, struct raw_log_entry first_entry){ struct raw_log_entry e = first_entry; uint8_t read_ok = 1; int t = 10*(int)(1+e.time*0.1); while (read_ok) { if(e.time>t){ printf("%6.2fs %6i\n", e.time, entry_counter); t += 10; } if ((e.time<68.48)||(e.time>68.51)){ print_estimator_state(e.time); main_run_ins(e.message.valid_sensors); } e = read_raw_log_entry(file_descriptor, &read_ok); } }