Esempi in C++ (Cpp) per float_rates_add_gaussian_noise

Esempio n. 1

File: ahrs_on_synth.c Progetto: EwoudSmeur/paparazzi

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

}

Esempio n. 2

File: ahrs_on_synth.c Progetto: AshuLara/paparazzi

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


}