C++ (Cpp) FLOAT_RMAT_VECT3_TRANSP_MUL Beispiele

Beispiel #1

Datei: ahrs_float_cmpl_rmat.c Projekt: Ludo6431/paparazzi

void ahrs_update_mag_2d(void) {

  const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y};

  struct FloatVect3 measured_imu;
  MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
  struct FloatVect3 measured_ltp;
  FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_float.ltp_to_imu_rmat, measured_imu);

  const struct FloatVect3 residual_ltp =
    { 0,
      0,
      measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x };

  //  printf("res : %f\n", residual_ltp.z);

  struct FloatVect3 residual_imu;
  FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp);

  const float mag_rate_update_gain = 2.5;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);

  const float mag_bias_update_gain = -2.5e-3;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);

}

Beispiel #2

Datei: ins_ardrone2.c Projekt: 1bitsquared/paparazzi

void ins_propagate() {
  /* untilt accels and speeds */
  FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel);
  FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed);

  //Add g to the accelerations
  ins_impl.ltp_accel.z += 9.81;

  //Save the accelerations and speeds
  stateSetAccelNed_f(&ins_impl.ltp_accel);
  stateSetSpeedNed_f(&ins_impl.ltp_speed);

  //Don't set the height if we use the one from the gps
#if !USE_GPS_HEIGHT
  //Set the height and save the position
  ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
  stateSetPositionNed_i(&ins_impl.ltp_pos);
#endif
}

Beispiel #3

Datei: ahrs_float_cmpl.c Projekt: 1bitsquared/paparazzi

void ahrs_update_mag_2d(void) {

  struct FloatVect2 expected_ltp;
  VECT2_COPY(expected_ltp, ahrs_impl.mag_h);
  // normalize expected ltp in 2D (x,y)
  FLOAT_VECT2_NORMALIZE(expected_ltp);

  struct FloatVect3 measured_imu;
  MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
  struct FloatVect3 measured_ltp;
  FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu);

  struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y};

  // normalize measured ltp in 2D (x,y)
  FLOAT_VECT2_NORMALIZE(measured_ltp_2d);

  const struct FloatVect3 residual_ltp =
    { 0,
      0,
      measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x };

  //  printf("res : %f\n", residual_ltp.z);

  struct FloatVect3 residual_imu;
  FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);


  /* Complementary filter proportional gain.
   * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
   */
  const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
    AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);

  /* Complementary filter integral gain
   * Correct the gyro bias.
   * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
   */
  const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) /
    AHRS_MAG_CORRECT_FREQUENCY;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}