Exemplos de update_fw_estimator em C++ (Cpp)

Exemplo n.º 1

Arquivo: ins_xsens.c Projeto: ERAUBB/paparazzi

void handle_ins_msg(void) {

#if USE_INS
  update_fw_estimator();
#endif

#if USE_IMU
#ifdef XSENS_BACKWARDS
  if (imu_xsens.gyro_available) {
    RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(ins_p), -RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
  }
  if (imu_xsens.accel_available) {
    VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(ins_ax), -ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
  }
  if (imu_xsens.mag_available) {
    VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(ins_mx), -MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
  }
#else
  if (imu_xsens.gyro_available) {
    RATES_ASSIGN(imu.gyro_unscaled, RATE_BFP_OF_REAL(ins_p), RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
  }
  if (imu_xsens.accel_available) {
    VECT3_ASSIGN(imu.accel_unscaled, ACCEL_BFP_OF_REAL(ins_ax), ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
  }
  if (imu_xsens.mag_available) {
    VECT3_ASSIGN(imu.mag_unscaled, MAG_BFP_OF_REAL(ins_mx), MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
  }
#endif /* XSENS_BACKWARDS */
#endif /* USE_IMU */

#if USE_GPS_XSENS
  #ifndef ALT_KALMAN
  #warning NO_VZ
  #endif

  // Horizontal speed
  float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
  if (gps.fix != GPS_FIX_3D) {
    fspeed = 0;
  }
  gps.gspeed = fspeed * 100.;
  gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);

  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
  gps.course = fcourse * 1e7;
#endif // USE_GPS_XSENS
}

Exemplo n.º 2

Arquivo: ins_xsens700.c Projeto: Abhi0204/paparazzi

void handle_ins_msg( void) {

#if USE_INS_MODULE
  update_fw_estimator();
#endif

#if USE_GPS_XSENS
  // Horizontal speed
  float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
  if (gps.fix != GPS_FIX_3D) {
    fspeed = 0;
  }
  gps.gspeed = fspeed * 100.;
  gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);

  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
  gps.course = fcourse * 1e7;
#endif // USE_GPS_XSENS
}