Example #1
0
void imu_init(void) {

    /* initialises neutrals */
    RATES_ASSIGN(imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);

    VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);

#if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
    VECT3_ASSIGN(imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL,   IMU_MAG_Z_NEUTRAL);
#else
#if USE_MAGNETOMETER
    INFO("Magnetometer neutrals are set to zero, you should calibrate!")
#endif
    INT_VECT3_ZERO(imu.mag_neutral);
#endif

    /*
      Compute quaternion and rotation matrix
      for conversions between body and imu frame
    */
    struct Int32Eulers body_to_imu_eulers =
    {   ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
        ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
        ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI)
    };
    INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
    INT32_QUAT_NORMALIZE(imu.body_to_imu_quat);
    INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);

    imu_impl_init();
}
Example #2
0
void imu_navgo_event( void )
{

  // If the itg3200 I2C transaction has succeeded: convert the data
  itg3200_event();
  if (itg3200_data_available) {
    RATES_COPY(imu.gyro_unscaled, itg3200_data);
    itg3200_data_available = FALSE;
    gyr_valid = TRUE;
  }

  // If the adxl345 I2C transaction has succeeded: convert the data
  adxl345_event();
  if (adxl345_data_available) {
    VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z);
    adxl345_data_available = FALSE;
    acc_valid = TRUE;
  }

  // HMC58XX event task
  hmc58xx_event();
  if (hmc58xx_data_available) {
    VECT3_ASSIGN(imu.mag_unscaled, -hmc58xx_data.x, -hmc58xx_data.y, hmc58xx_data.z);
    hmc58xx_data_available = FALSE;
    mag_valid = TRUE;
  }

}
void stabilization_attitude_init(void) {

    stabilization_attitude_ref_init();


    VECT3_ASSIGN(stabilization_gains.p,
                 STABILIZATION_ATTITUDE_PHI_PGAIN,
                 STABILIZATION_ATTITUDE_THETA_PGAIN,
                 STABILIZATION_ATTITUDE_PSI_PGAIN);

    VECT3_ASSIGN(stabilization_gains.d,
                 STABILIZATION_ATTITUDE_PHI_DGAIN,
                 STABILIZATION_ATTITUDE_THETA_DGAIN,
                 STABILIZATION_ATTITUDE_PSI_DGAIN);

    VECT3_ASSIGN(stabilization_gains.i,
                 STABILIZATION_ATTITUDE_PHI_IGAIN,
                 STABILIZATION_ATTITUDE_THETA_IGAIN,
                 STABILIZATION_ATTITUDE_PSI_IGAIN);

    VECT3_ASSIGN(stabilization_gains.dd,
                 STABILIZATION_ATTITUDE_PHI_DDGAIN,
                 STABILIZATION_ATTITUDE_THETA_DDGAIN,
                 STABILIZATION_ATTITUDE_PSI_DDGAIN);


    INT_EULERS_ZERO( stabilization_att_sum_err );

}
Example #4
0
void imu_init(void) {

#ifdef IMU_POWER_GPIO
  gpio_setup_output(IMU_POWER_GPIO);
  IMU_POWER_GPIO_ON(IMU_POWER_GPIO);
#endif

  /* initialises neutrals */
  RATES_ASSIGN(imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);

  VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);

#if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
  VECT3_ASSIGN(imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL,   IMU_MAG_Z_NEUTRAL);
#else
#if USE_MAGNETOMETER
INFO("Magnetometer neutrals are set to zero, you should calibrate!")
#endif
  INT_VECT3_ZERO(imu.mag_neutral);
#endif

  /*
    Compute quaternion and rotation matrix
    for conversions between body and imu frame
  */
  struct Int32Eulers body_to_imu_eulers =
    { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) };
  INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
  INT32_QUAT_NORMALIZE(imu.body_to_imu_quat);
  INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);

#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
#if USE_IMU_FLOAT
#else // !USE_IMU_FLOAT
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw);
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled);
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw);
  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled);
  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag);
#endif // !USE_IMU_FLOAT
#endif // DOWNLINK

  imu_impl_init();
}
Example #5
0
void imu_init(void) {

#ifdef IMU_POWER_GPIO
  gpio_setup_output(IMU_POWER_GPIO);
  IMU_POWER_GPIO_ON(IMU_POWER_GPIO);
#endif

  /* initialises neutrals */
  RATES_ASSIGN(imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);

  VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);

#if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
  VECT3_ASSIGN(imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL,   IMU_MAG_Z_NEUTRAL);
#else
#if USE_MAGNETOMETER
INFO("Magnetometer neutrals are set to zero, you should calibrate!")
#endif
  INT_VECT3_ZERO(imu.mag_neutral);
#endif

  struct FloatEulers body_to_imu_eulers =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
  orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif

#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
#if !USE_IMU_FLOAT
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw);
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled);
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw);
  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled);
  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag);
#endif // !USE_IMU_FLOAT
#endif // DOWNLINK

  imu_impl_init();
}
Example #6
0
void  nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) {
    FLOAT_VECT3_ZERO(gyro->value);
    gyro->resolution = NPS_GYRO_RESOLUTION;
    FLOAT_MAT33_DIAG(gyro->sensitivity,
                     NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR);
    VECT3_ASSIGN(gyro->neutral,
                 NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R);
    VECT3_ASSIGN(gyro->noise_std_dev,
                 NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R);
    VECT3_ASSIGN(gyro->bias_initial,
                 NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R);
    VECT3_ASSIGN(gyro->bias_random_walk_std_dev,
                 NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P,
                 NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q,
                 NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R);
    FLOAT_VECT3_ZERO(gyro->bias_random_walk_value);
    gyro->next_update = time;
    gyro->data_available = FALSE;
}
void ahrs_icq_init(void)
{

  ahrs_icq.status = AHRS_ICQ_UNINIT;
  ahrs_icq.is_aligned = FALSE;

  ahrs_icq.ltp_vel_norm_valid = FALSE;
  ahrs_icq.heading_aligned = FALSE;

  /* init ltp_to_imu quaternion as zero/identity rotation */
  int32_quat_identity(&ahrs_icq.ltp_to_imu_quat);

  INT_RATES_ZERO(ahrs_icq.imu_rate);

  INT_RATES_ZERO(ahrs_icq.gyro_bias);
  INT_RATES_ZERO(ahrs_icq.rate_correction);
  INT_RATES_ZERO(ahrs_icq.high_rez_bias);

  /* set default filter cut-off frequency and damping */
  ahrs_icq.accel_omega = AHRS_ACCEL_OMEGA;
  ahrs_icq.accel_zeta = AHRS_ACCEL_ZETA;
  ahrs_icq_set_accel_gains();
  ahrs_icq.mag_omega = AHRS_MAG_OMEGA;
  ahrs_icq.mag_zeta = AHRS_MAG_ZETA;
  ahrs_icq_set_mag_gains();

  /* set default gravity heuristic */
  ahrs_icq.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;

#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  ahrs_icq.correct_gravity = TRUE;
#else
  ahrs_icq.correct_gravity = FALSE;
#endif

  VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(AHRS_H_X),
               MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));

  ahrs_icq.accel_cnt = 0;
  ahrs_icq.mag_cnt = 0;
}
Example #8
0
void imu_umarim_event( void )
{

  // If the itg3200 I2C transaction has succeeded: convert the data
  itg3200_event();
  if (itg3200_data_available) {
    RATES_ASSIGN(imu.gyro_unscaled, itg3200_data.p, itg3200_data.q, itg3200_data.r);
    itg3200_data_available = FALSE;
    gyr_valid = TRUE;
  }

  // If the adxl345 I2C transaction has succeeded: convert the data
  adxl345_event();
  if (adxl345_data_available) {
    // Be careful with orientation of the ADXL (ITG axes are taken as default reference)
    VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z);
    adxl345_data_available = FALSE;
    acc_valid = TRUE;
  }

}
Example #9
0
void geo_mag_event(void)
{

  if (geo_mag_calc_flag) {
    double gha[MAXCOEFF]; // Geomag global variables
    int32_t nmax;

    /* Current date in decimal year, for example 2012.68 */
    double sdate = GPS_EPOCH_BEGIN +
                   (double)gps.week / WEEKS_IN_YEAR +
                   (double)gps.tow / 1000 / SECS_IN_YEAR;

    /* LLA Position in decimal degrees and altitude in km */
    double latitude = (double)gps.lla_pos.lat / 1e7;
    double longitude = (double)gps.lla_pos.lon / 1e7;
    double alt = (double)gps.lla_pos.alt / 1e6;

    // Calculates additional coeffs
    nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
    // Calculates absolute magnet fields
    mag_calc(1, latitude, longitude, alt, nmax, gha,
             &geo_mag.vect.x, &geo_mag.vect.y, &geo_mag.vect.z,
             IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
    double_vect3_normalize(&geo_mag.vect);

    // copy to ahrs
#ifdef AHRS_FLOAT
    VECT3_COPY(DefaultAhrsImpl.mag_h, geo_mag.vect);
#else
    // convert to MAG_BFP and copy to ahrs
    VECT3_ASSIGN(DefaultAhrsImpl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x),
                 MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z));
#endif

    geo_mag.ready = TRUE;
  }
  geo_mag_calc_flag = FALSE;
}
Example #10
0
void ahrs_init(void) {
  ahrs.status = AHRS_UNINIT;
  ahrs_impl.ltp_vel_norm_valid = FALSE;
  ahrs_impl.heading_aligned = FALSE;

  /* Initialises IMU alignement */
  struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
  FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
  FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

  /* Set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
  RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);

  FLOAT_RATES_ZERO(ahrs_impl.imu_rate);

  /* set default filter cut-off frequency and damping */
  ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
  ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
  ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
  ahrs_impl.mag_zeta = AHRS_MAG_ZETA;

#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  ahrs_impl.correct_gravity = TRUE;
#else
  ahrs_impl.correct_gravity = FALSE;
#endif

  ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;

  VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);

#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
  register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
static struct raw_log_entry first_entry_after_initialisation(int file_descriptor){
  int        imu_measurements = 0,      // => Gyro + Accel
    magnetometer_measurements = 0,
            baro_measurements = 0,
             gps_measurements = 0;      // only the position
  
  struct DoubleMat33 attitude_profile_matrix, sigmaB;   // the attitude profile matrix is often called "B"
  struct Orientation_Measurement  gravity,
                                  magneto,
                                  fake;  
  struct DoubleQuat q_ned2body, sigma_q;
  
  /* Prepare the attitude profile matrix */
  FLOAT_MAT33_ZERO(attitude_profile_matrix);
  FLOAT_MAT33_ZERO(sigmaB);
  
  // for faster converging, but probably more rounding error
  #define MEASUREMENT_WEIGHT_SCALE 10
  
  /* set the gravity measurement */
  VECT3_ASSIGN(gravity.reference_direction, 0,0,-1);
  gravity.weight_of_the_measurement = MEASUREMENT_WEIGHT_SCALE/(double)(imu_frequency);    // originally 1/(imu_frequency*gravity.norm()
  //gravity.weight_of_the_measurement = 1;
  
  /* set the magneto - measurement */
  EARTHS_GEOMAGNETIC_FIELD_NORMED(magneto.reference_direction);
  magneto.weight_of_the_measurement = MEASUREMENT_WEIGHT_SCALE/(double)(mag_frequency);    // originally 1/(mag_frequency*reference_direction.norm()
  //magneto.weight_of_the_measurement = 1;
  
  uint8_t read_ok;
  #if WITH_GPS
  struct raw_log_entry e = next_GPS(file_descriptor);
  #else /* WITH_GPS */
  struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok);
  pos_0_ecef = Vector3d(4627578.56, 119659.25, 4373248.00);
  pos_cov_0 = Vector3d::Ones()*100;
  speed_0_ecef    = Vector3d::Zero();
  speed_cov_0 = Vector3d::Ones();
  #endif /* WITH_GPS */
  
  #ifdef EKNAV_FROM_LOG_DEBUG
    int imu_ready = 0, 
        mag_ready = 0,
        baro_ready = 0,
        gps_ready = 0;
  #endif /* EKNAV_FROM_LOG_DEBUG */
  
  for(read_ok = 1; (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements); e = read_raw_log_entry(file_descriptor, &read_ok)){
    if(IMU_READY(e.message.valid_sensors)){
      imu_measurements++;
      
      // update the estimated bias
      bias_0 = NEW_MEAN(bias_0, RATES_BFP_AS_VECTOR3D(e.message.gyro), imu_measurements);
      
      // update the attitude profile matrix
      ACCELS_FLOAT_OF_BFP(gravity.measured_direction,e.message.accel);
      add_orientation_measurement(&attitude_profile_matrix, gravity);
    }
    if(MAG_READY(e.message.valid_sensors)){
      magnetometer_measurements++;
      // update the attitude profile matrix
      MAGS_FLOAT_OF_BFP(magneto.measured_direction,e.message.mag);
      add_orientation_measurement(&attitude_profile_matrix, magneto);
      
      // now, generate fake measurement with the last gravity measurement
      fake = fake_orientation_measurement(gravity, magneto);
      add_orientation_measurement(&attitude_profile_matrix, fake);
    }
    if(BARO_READY(e.message.valid_sensors)){
      baro_measurements++;
      // TODO: Fix it!
      //NEW_MEAN(baro_0_height, BARO_FLOAT_OF_BFP(e.message.pressure_absolute), baro_measurements);
      baro_0_height = (baro_0_height*(baro_measurements-1)+BARO_FLOAT_OF_BFP(e.message.pressure_absolute))/baro_measurements;
    }
    if(GPS_READY(e.message.valid_sensors)){
      gps_measurements++;
      // update the estimated bias
      pos_0_ecef = NEW_MEAN(pos_0_ecef, VECT3_AS_VECTOR3D(e.message.ecef_pos)/100, gps_measurements);
      speed_0_ecef = NEW_MEAN(speed_0_ecef, VECT3_AS_VECTOR3D(e.message.ecef_vel)/100, gps_measurements);
    }
    
    #ifdef EKNAV_FROM_LOG_DEBUG
    if(imu_ready==0){
      if(!NOT_ENOUGH_IMU_MEASUREMENTS(imu_measurements)){
        printf("IMU READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
        imu_ready = 1;
      }
    }
    if(mag_ready==0){
      if(!NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(magnetometer_measurements)){
        printf("MAG READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
        mag_ready = 1;
      }
    }
    if(baro_ready==0){
      if(!NOT_ENOUGH_BARO_MEASUREMENTS(baro_measurements)){
        printf("BARO READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
        baro_ready = 1;
      }
    }
    if(gps_ready==0){
      if(!NOT_ENOUGH_GPS_MEASUREMENTS(gps_measurements)){
        printf("GPS READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
        gps_ready = 1;
      }
    }
    #endif /* EKNAV_FROM_LOG_DEBUG */
  }
  // setting the covariance
  gravity.weight_of_the_measurement *= imu_measurements;
  VECTOR_AS_VECT3(gravity.measured_direction, accelerometer_noise);
  magneto.weight_of_the_measurement *= magnetometer_measurements;
  VECTOR_AS_VECT3(magneto.measured_direction, magnetometer_noise);
  add_set_of_three_measurements(&sigmaB, gravity, magneto);
  
  #ifdef EKNAV_FROM_LOG_DEBUG
  DISPLAY_FLOAT_RMAT("     B", attitude_profile_matrix);
  DISPLAY_FLOAT_RMAT("sigmaB", sigmaB);
  #endif /* EKNAV_FROM_LOG_DEBUG */
  
  //  setting the initial orientation
  q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6, sigmaB, &sigma_q);
	orientation_0 = ecef2body_from_pprz_ned2body(pos_0_ecef,q_ned2body);
  
  baro_0_height += pos_0_ecef.norm();
  
  struct DoubleEulers sigma_eu = sigma_euler_from_sigma_q(q_ned2body, sigma_q);
  orientation_cov_0 = EULER_AS_VECTOR3D(sigma_eu);
  #if WITH_GPS
  pos_cov_0 = 10*gps_pos_noise / gps_measurements;
  speed_cov_0 = 10*gps_speed_noise / gps_measurements;
  #endif  // WITH_GPS
  
  return e;
}
Example #12
0
void aspirin2_subsystem_event( void )
{
    int32_t x, y, z;

    // If the itg3200 I2C transaction has succeeded: convert the data
    if (aspirin2_mpu60x0.status == I2CTransSuccess)
    {
#define MPU_OFFSET_GYRO 9
        x = (int16_t) ((aspirin2_mpu60x0.buf[0+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[1+MPU_OFFSET_GYRO]);
        y = (int16_t) ((aspirin2_mpu60x0.buf[2+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[3+MPU_OFFSET_GYRO]);
        z = (int16_t) ((aspirin2_mpu60x0.buf[4+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[5+MPU_OFFSET_GYRO]);

        RATES_ASSIGN(imu.gyro_unscaled, x, y, z);

#define MPU_OFFSET_ACC 1
        x = (int16_t) ((aspirin2_mpu60x0.buf[0+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[1+MPU_OFFSET_ACC]);
        y = (int16_t) ((aspirin2_mpu60x0.buf[2+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[3+MPU_OFFSET_ACC]);
        z = (int16_t) ((aspirin2_mpu60x0.buf[4+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[5+MPU_OFFSET_ACC]);

        VECT3_ASSIGN(imu.accel_unscaled, x, y, z);

        // Is this is new data
        if (aspirin2_mpu60x0.buf[0] & 0x01)
        {
            gyr_valid = TRUE;
            acc_valid = TRUE;
        }
        else
        {
        }

        aspirin2_mpu60x0.status = I2CTransDone;  // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
    }
    /*
      // If the adxl345 I2C transaction has succeeded: convert the data
      if (ppzuavimu_adxl345.status == I2CTransSuccess)
      {
        x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
        y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
        z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);

    #ifdef ASPIRIN_IMU
        VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
    #else // PPZIMU
        VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
    #endif

        acc_valid = TRUE;
        ppzuavimu_adxl345.status = I2CTransDone;
      }

      // If the hmc5843 I2C transaction has succeeded: convert the data
      if (ppzuavimu_hmc5843.status == I2CTransSuccess)
      {
        x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
        y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
        z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);

    #ifdef ASPIRIN_IMU
        VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
    #else // PPZIMU
        VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
    #endif

        mag_valid = TRUE;
        ppzuavimu_hmc5843.status = I2CTransDone;
      }
    */
}