예제 #1
0
void test1234(void) {
    struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)};

    struct FloatVect3 uz = { 0., 0., 1.};
    struct FloatMat33 r_yaw;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);

    struct FloatVect3 uy = { 0., 1., 0.};
    struct FloatMat33 r_pitch;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);

    struct FloatVect3 ux = { 1., 0., 0.};
    struct FloatMat33 r_roll;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);

    struct FloatMat33 r_tmp;
    FLOAT_RMAT_COMP(r_tmp, r_yaw, r_roll);

    struct FloatMat33 r_att;
    FLOAT_RMAT_COMP(r_att, r_tmp, r_pitch);
    DISPLAY_FLOAT_RMAT("r_att_ref  ", r_att);

    FLOAT_RMAT_OF_EULERS_312(r_att, eul);
    DISPLAY_FLOAT_RMAT("r_att312  ", r_att);




}
예제 #2
0
void test_of_axis_angle(void) {

    struct FloatVect3 axis = { 0., 1., 0.};
    FLOAT_VECT3_NORMALIZE(axis);
    DISPLAY_FLOAT_VECT3("axis", axis);
    const float angle = RadOfDeg(30.);
    printf("angle %f\n", DegOfRad(angle));

    struct FloatQuat my_q;
    FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle);
    DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q);

    struct FloatMat33 my_r1;
    FLOAT_RMAT_OF_QUAT(my_r1, my_q);
    DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1);
    DISPLAY_FLOAT_RMAT("rmat1", my_r1);

    struct FloatMat33 my_r;
    FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle);
    DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r);
    DISPLAY_FLOAT_RMAT("rmat", my_r);

    printf("\n");

    struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.};

    struct FloatVect3 uz = { 0., 0., 1.};
    struct FloatMat33 r_yaw;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);

    struct FloatVect3 uy = { 0., 1., 0.};
    struct FloatMat33 r_pitch;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);

    struct FloatVect3 ux = { 1., 0., 0.};
    struct FloatMat33 r_roll;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);

    struct FloatMat33 r_yaw_pitch;
    FLOAT_RMAT_COMP(r_yaw_pitch, r_yaw, r_pitch);

    struct FloatMat33 r_yaw_pitch_roll;
    FLOAT_RMAT_COMP(r_yaw_pitch_roll, r_yaw_pitch, r_roll);

    DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll);
    DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll);

    DISPLAY_FLOAT_EULERS_DEG("eul", eul);
    struct FloatMat33 rmat1;
    FLOAT_RMAT_OF_EULERS(rmat1, eul);
    DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1);
    DISPLAY_FLOAT_RMAT("rmat1", rmat1);

}
예제 #3
0
void ahrs_propagate(void) {

  /* converts gyro to floating point */
  struct FloatRates gyro_float;
  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
  /* unbias measurement */
  RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
  const float dt = 1./512.;
  /* add correction     */
  struct FloatRates omega;
  RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction);
  //  DISPLAY_FLOAT_RATES("omega ", omega);
  /* and zeros it */
  FLOAT_RATES_ZERO(ahrs_impl.rate_correction);

  /* first order integration of rotation matrix */
  struct FloatRMat exp_omega_dt = {
    { 1.        ,  dt*omega.r, -dt*omega.q,
     -dt*omega.r,  1.        ,  dt*omega.p,
      dt*omega.q, -dt*omega.p,  1.                       }};
  struct FloatRMat R_tdt;
  FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt);
  memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt));

  float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat);
  //  struct FloatRMat test;
  //  FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat);
  //  DISPLAY_FLOAT_RMAT("foo", test);

  compute_imu_quat_and_euler_from_rmat();
  compute_body_orientation_and_rates();

}
예제 #4
0
/** in place first order integration of a rotation matrix */
void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
{
  struct FloatRMat exp_omega_dt = {
    {
      1.        ,  dt *omega->r, -dt *omega->q,
      -dt *omega->r,  1.        ,  dt *omega->p,
      dt *omega->q, -dt *omega->p,  1.
    }
  };
  struct FloatRMat R_tdt;
  FLOAT_RMAT_COMP(R_tdt, *rm, exp_omega_dt);
  memcpy(rm, &R_tdt, sizeof(R_tdt));
}
예제 #5
0
float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int display) {

    struct FloatRMat ma2c_f;
    FLOAT_RMAT_COMP(ma2c_f, ma2b_f, mb2c_f);
    struct Int32RMat ma2b_i;
    RMAT_BFP_OF_REAL(ma2b_i, ma2b_f);
    struct Int32RMat mb2c_i;
    RMAT_BFP_OF_REAL(mb2c_i, mb2c_f);
    struct Int32RMat ma2c_i;
    INT32_RMAT_COMP(ma2c_i, ma2b_i, mb2c_i);

    struct FloatRMat err;
    RMAT_DIFF(err, ma2c_f, ma2c_i);
    float norm_err = FLOAT_RMAT_NORM(err);

    if (display) {
        printf("rmap comp\n");
        DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("a2cf", ma2c_f);
        DISPLAY_INT32_RMAT_AS_EULERS_DEG("a2ci", ma2c_i);
    }

    return norm_err;

}
예제 #6
0
void gx3_packet_read_message(void) {
  ahrs_impl.gx3_accel.x     = bef(&ahrs_impl.gx3_packet.msg_buf[1]);
  ahrs_impl.gx3_accel.y     = bef(&ahrs_impl.gx3_packet.msg_buf[5]);
  ahrs_impl.gx3_accel.z     = bef(&ahrs_impl.gx3_packet.msg_buf[9]);
  ahrs_impl.gx3_rate.p      = bef(&ahrs_impl.gx3_packet.msg_buf[13]);
  ahrs_impl.gx3_rate.q      = bef(&ahrs_impl.gx3_packet.msg_buf[17]);
  ahrs_impl.gx3_rate.r      = bef(&ahrs_impl.gx3_packet.msg_buf[21]);
  ahrs_impl.gx3_rmat.m[0]   = bef(&ahrs_impl.gx3_packet.msg_buf[25]);
  ahrs_impl.gx3_rmat.m[1]   = bef(&ahrs_impl.gx3_packet.msg_buf[29]);
  ahrs_impl.gx3_rmat.m[2]   = bef(&ahrs_impl.gx3_packet.msg_buf[33]);
  ahrs_impl.gx3_rmat.m[3]   = bef(&ahrs_impl.gx3_packet.msg_buf[37]);
  ahrs_impl.gx3_rmat.m[4]   = bef(&ahrs_impl.gx3_packet.msg_buf[41]);
  ahrs_impl.gx3_rmat.m[5]   = bef(&ahrs_impl.gx3_packet.msg_buf[45]);
  ahrs_impl.gx3_rmat.m[6]   = bef(&ahrs_impl.gx3_packet.msg_buf[49]);
  ahrs_impl.gx3_rmat.m[7]   = bef(&ahrs_impl.gx3_packet.msg_buf[53]);
  ahrs_impl.gx3_rmat.m[8]   = bef(&ahrs_impl.gx3_packet.msg_buf[57]);
  ahrs_impl.gx3_time    = (uint32_t)(ahrs_impl.gx3_packet.msg_buf[61] << 24 |
                                     ahrs_impl.gx3_packet.msg_buf[62] << 16 | ahrs_impl.gx3_packet.msg_buf[63] << 8 | ahrs_impl.gx3_packet.msg_buf[64]);
  ahrs_impl.gx3_chksm   = GX3_CHKSM(ahrs_impl.gx3_packet.msg_buf);

  ahrs_impl.gx3_freq = 62500.0 / (float)(ahrs_impl.gx3_time - ahrs_impl.gx3_ltime);
  ahrs_impl.gx3_ltime = ahrs_impl.gx3_time;

  // Acceleration
  VECT3_SMUL(ahrs_impl.gx3_accel, ahrs_impl.gx3_accel, 9.80665); // Convert g into m/s2
  ACCELS_BFP_OF_REAL(imu.accel, ahrs_impl.gx3_accel); // for backwards compatibility with fixed point interface
  imuf.accel = ahrs_impl.gx3_accel;

  // Rates
  struct FloatRates body_rate;
  imuf.gyro = ahrs_impl.gx3_rate;
  /* compute body rates */
  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imuf.body_to_imu);
  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, imuf.gyro);
  /* Set state */
  stateSetBodyRates_f(&body_rate);

  // Attitude
  struct FloatRMat ltp_to_body_rmat;
  FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat);

#if AHRS_USE_GPS_HEADING && USE_GPS
  struct FloatEulers ltp_to_body_eulers;
  float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
  float course_f = (float)DegOfRad(gps.course / 1e7);
  if (course_f > 180.0) {
    course_f -= 360.0;
  }
  ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
#else // !AHRS_USE_GPS_HEADING
#ifdef IMU_MAG_OFFSET
  struct FloatEulers ltp_to_body_eulers;
  float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
  ltp_to_body_eulers.psi -= ahrs_impl.mag_offset;
  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
#else
  stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
#endif // IMU_MAG_OFFSET
#endif // !AHRS_USE_GPS_HEADING
}
예제 #7
0
파일: csc_xsens.c 프로젝트: 0lri/paparazzi
// Called after receipt of valid message to
void xsens_parse_msg( uint8_t xsens_id ) {
  uint8_t buf_slot = xsens_msg_buf_ci[xsens_id];

  if (msg_id[xsens_id][buf_slot] == XSENS_ReqOutputModeAck_ID) {
    xsens_output_mode[xsens_id] = XSENS_ReqOutputModeAck_mode(xsens_msg_buf[xsens_id]);
  }
  else if (msg_id[xsens_id][buf_slot] == XSENS_Error_ID) {
    xsens_errorcode[xsens_id] = XSENS_Error_errorcode(xsens_msg_buf[xsens_id]);
  }
  else if (msg_id[xsens_id][buf_slot] == XSENS_MTData_ID) {
    uint8_t offset = 0;
    // test RAW modes else calibrated modes
      if (XSENS_MASK_RAWInertial(xsens_output_mode[xsens_id])){// || (XSENS_MASK_RAWGPS(xsens2_output_mode))) {
	xsens_raw_accel_x[xsens_id] = XSENS_DATA_RAWInertial_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
	xsens_raw_accel_y[xsens_id] = XSENS_DATA_RAWInertial_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
	xsens_raw_accel_z[xsens_id] = XSENS_DATA_RAWInertial_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
	xsens_raw_gyro_x[xsens_id] = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
	xsens_raw_gyro_y[xsens_id] = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
	xsens_raw_gyro_z[xsens_id] = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
        xsens_raw_mag_x[xsens_id]  = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
	xsens_raw_mag_y[xsens_id]  = XSENS_DATA_RAWInertial_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
	xsens_raw_mag_z[xsens_id]  = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
      }
      if (XSENS_MASK_Temp(xsens_output_mode[xsens_id])) {
        offset += XSENS_DATA_Temp_LENGTH;
      }
      if (XSENS_MASK_Calibrated(xsens_output_mode[xsens_id])) {
        if (XSENS_MASK_AccOut(xsens_output_settings[xsens_id])) {
          xsens_accel_x[xsens_id] = XSENS_DATA_Calibrated_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_accel_y[xsens_id] = XSENS_DATA_Calibrated_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_accel_z[xsens_id] = XSENS_DATA_Calibrated_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
        }
        if (XSENS_MASK_GyrOut(xsens_output_settings[xsens_id])) {
          xsens_gyro_x[xsens_id] = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_gyro_y[xsens_id] = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_gyro_z[xsens_id] = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
        }
        if (XSENS_MASK_MagOut(xsens_output_settings[xsens_id])) {
          xsens_mag_x[xsens_id] = XSENS_DATA_Calibrated_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_mag_y[xsens_id] = XSENS_DATA_Calibrated_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_mag_z[xsens_id] = XSENS_DATA_Calibrated_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
          float pitch = xsens_phi[xsens_id];
          float roll = -xsens_theta[xsens_id];
          float tilt_comp_x = xsens_mag_x[xsens_id] * cos(pitch)
                            + xsens_mag_y[xsens_id] * sin(roll) * sin(pitch)
                            - xsens_mag_z[xsens_id] * cos(roll) * sin(pitch);
          float tilt_comp_y = xsens_mag_y[xsens_id] * cos(roll)
                            + xsens_mag_z[xsens_id] * sin(roll);
          xsens_mag_heading[xsens_id] = -atan2( tilt_comp_y, tilt_comp_x);
        }
        offset += XSENS_DATA_Calibrated_LENGTH;
      }
      if (XSENS_MASK_Orientation(xsens_output_mode[xsens_id])) {
        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x0) {
          offset += XSENS_DATA_Quaternion_LENGTH;
        }
        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x1) {
          xsens_phi[xsens_id] = XSENS_DATA_Euler_roll(xsens_msg_buf[xsens_id][buf_slot],offset)  * M_PI/180;
          xsens_theta[xsens_id] =XSENS_DATA_Euler_pitch(xsens_msg_buf[xsens_id][buf_slot],offset) * M_PI/180;
          xsens_psi[xsens_id] =  XSENS_DATA_Euler_yaw(xsens_msg_buf[xsens_id][buf_slot],offset)   * M_PI/180;
          offset += XSENS_DATA_Euler_LENGTH;
        }
        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x2) {
          xsens_rmat[xsens_id].m[0] = XSENS_DATA_Matrix_a(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_rmat[xsens_id].m[1] = XSENS_DATA_Matrix_b(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_rmat[xsens_id].m[2] = XSENS_DATA_Matrix_c(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_rmat[xsens_id].m[3] = XSENS_DATA_Matrix_d(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_rmat[xsens_id].m[4] = XSENS_DATA_Matrix_e(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_rmat[xsens_id].m[5] = XSENS_DATA_Matrix_f(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_rmat[xsens_id].m[6] = XSENS_DATA_Matrix_g(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_rmat[xsens_id].m[7] = XSENS_DATA_Matrix_h(xsens_msg_buf[xsens_id][buf_slot],offset);
          xsens_rmat[xsens_id].m[8] = XSENS_DATA_Matrix_i(xsens_msg_buf[xsens_id][buf_slot],offset);

	  /* //	  FLOAT_RMAT_COMP_INV(xsens_rmat_adj[xsens_id], xsens_rmat_neutral[xsens_id], xsens_rmat[xsens_id]); */
	  struct FloatRMat xsens_rmat_temp[XSENS_COUNT];
	  FLOAT_RMAT_INV(xsens_rmat_temp[xsens_id], xsens_rmat[xsens_id]);

	  FLOAT_RMAT_COMP(xsens_rmat_adj[xsens_id], xsens_rmat_temp[xsens_id], xsens_rmat_neutral[xsens_id]);

	  xsens_phi[xsens_id] = -atan2(xsens_rmat_adj[xsens_id].m[7], xsens_rmat_adj[xsens_id].m[8]);
	  xsens_theta[xsens_id] = asin(xsens_rmat_adj[xsens_id].m[6]);
	  xsens_psi[xsens_id] = atan2(xsens_rmat_adj[xsens_id].m[3], xsens_rmat_adj[xsens_id].m[0]);

	  xsens_psi[xsens_id] -= RadOfDeg(xsens_psi_offset[xsens_id]);

	  /* FLOAT_RMAT_COMP(xsens_rmat_adj[xsens_id], xsens_rmat_neutral[xsens_id], xsens_rmat[xsens_id]); */

          /* // Calculate roll, pitch, yaw from rotation matrix ( p 31-33 MTi-G USer Man and Tech Doc) */
          /* xsens_phi[xsens_id] = -atan2 (xsens_rmat_adj[xsens_id].m[7], xsens_rmat_adj[xsens_id].m[8]); */
          /* xsens_theta[xsens_id] = asin (xsens_rmat_adj[xsens_id].m[6]); */
          /* xsens_psi[xsens_id] = atan2 (xsens_rmat_adj[xsens_id].m[3], xsens_rmat_adj[xsens_id].m[0]); */



          offset += XSENS_DATA_Matrix_LENGTH;
        }
      }
      if (XSENS_MASK_Status(xsens_output_mode[xsens_id])) {
        xsens_msg_status[xsens_id] = XSENS_DATA_Status_status(xsens_msg_buf[xsens_id][buf_slot],offset);
        xsens_mode[xsens_id] = xsens_msg_status[xsens_id];
        offset += XSENS_DATA_Status_LENGTH;
      }
      if (XSENS_MASK_TimeStamp(xsens_output_settings[xsens_id])) {
        uint16_t ts = XSENS_DATA_TimeStamp_ts(xsens_msg_buf[xsens_id][buf_slot],offset);
        if (xsens_time_stamp[xsens_id] + 1 != ts)
          //xsens_err_count[xsens_id]++;
        xsens_time_stamp[xsens_id] = ts;
        offset += XSENS_DATA_TimeStamp_LENGTH;
      }
  }
}