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