Esempi in C++ (Cpp) per RATES_AS_VECTOR3D

Esempio n. 1

File: test_libeknav_4.cpp Progetto: Paolo-Maffei/lxyppc-tetrix

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

Esempio n. 2

File: libeknav_from_log.cpp Progetto: 2770862886/paparazzi

static void main_run_ins(uint8_t data_valid) {
  
  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), magnetometer_noise.norm());
	}
  
  #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), accelerometer_noise.norm());
	}
  #endif /* UPDATE_WITH_GRAVITY */
  
   if(BARO_READY(data_valid)){
    ins.obs_baro_report(baro_0_height+imu_baro_height, baro_noise);
  }   // comment out multiple lines */
  
  if(GPS_READY(data_valid)){
		ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, 10*gps_pos_noise, 10*gps_speed_noise);
	}   // comment out multiple lines */
}