Пример #1
0
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

}
Пример #2
0
static void handle_ins_msg(void)
{

  update_state_interface();

  if (xsens.new_attitude) {
    new_ins_attitude = true;
    xsens.new_attitude = false;
  }

#if USE_GPS_XSENS
  if (xsens.gps_available) {
    // Horizontal speed
    float fspeed = FLOAT_VECT2_NORM(xsens.vel);
    if (xsens.gps.fix != GPS_FIX_3D) {
      fspeed = 0;
    }
    xsens.gps.gspeed = fspeed * 100.;
    xsens.gps.speed_3d = float_vect3_norm(&xsens.vel) * 100;

    float fcourse = atan2f(xsens.vel.y, xsens.vel.x);
    xsens.gps.course = fcourse * 1e7;
    SetBit(xsens.gps.valid_fields, GPS_VALID_COURSE_BIT);

    gps_xsens_publish();
    xsens.gps_available = false;
  }
#endif // USE_GPS_XSENS
}
Пример #3
0
void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt)
{
  // check if we had at least one propagation since last update
  if (ahrs_fc.accel_cnt == 0) {
    return;
  }

  /* last column of roation matrix = ltp z-axis in imu-frame */
  struct FloatVect3  c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2),
           RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2),
           RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
  };

  struct FloatVect3 imu_accel_float;
  ACCELS_FLOAT_OF_BFP(imu_accel_float, *accel);

  struct FloatVect3 residual;

  struct FloatVect3 pseudo_gravity_measurement;

  if (ahrs_fc.correct_gravity && ahrs_fc.ltp_vel_norm_valid) {
    /*
     * centrifugal acceleration in body frame
     * a_c_body = omega x (omega x r)
     * (omega x r) = tangential velocity in body frame
     * a_c_body = omega x vel_tangential_body
     * assumption: tangential velocity only along body x-axis
     */
    const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0};
    struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
    struct FloatRates body_rate;
    float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
    struct FloatVect3 acc_c_body;
    VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);

    /* convert centrifugal acceleration from body to imu frame */
    struct FloatVect3 acc_c_imu;
    float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);

    /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
    VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);

  } else {
    VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
  }

  VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);

  /* FIR filtered pseudo_gravity_measurement */
#define FIR_FILTER_SIZE 8
  static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1);
  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);

  if (ahrs_fc.gravity_heuristic_factor) {
    /* heuristic on acceleration (gravity estimate) norm */
    /* Factor how strongly to change the weight.
     * e.g. for gravity_heuristic_factor 30:
     * <0.66G = 0, 1G = 1.0, >1.33G = 0
     */

    const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81;
    ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
    Bound(ahrs_fc.weight, 0.15, 1.0);
  } else {
    ahrs_fc.weight = 1.0;
  }

  /* Complementary filter proportional gain.
   * Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt
   * with ahrs_fc.accel_cnt beeing the number of propagations since last update
   */
  const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega *
                                         ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81;
  RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain);

  // reset accel propagation counter
  ahrs_fc.accel_cnt = 0;

  /* Complementary filter integral gain
   * Correct the gyro bias.
   * Ki = (omega*weight)^2 * dt
   */
  const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega *
                                         ahrs_fc.weight * ahrs_fc.weight * dt / 9.81;
  RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain);

  /* FIXME: saturate bias */
}