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)));
  
}
Пример #2
0
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);
  }
}