void orientationCalcQuat_i(struct OrientationReps* orientation) {
  if (bit_is_set(orientation->status, ORREP_QUAT_I))
    return;

  if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
    QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
  }
  else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
    INT32_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i);
  }
  else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
    INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_i);
  }
  else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
    RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
    SetBit(orientation->status, ORREP_RMAT_I);
    INT32_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i);
  }
  else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
    EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
    SetBit(orientation->status, ORREP_EULER_I);
    INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_i);
  }
  /* set bit to indicate this representation is computed */
  SetBit(orientation->status, ORREP_QUAT_I);
}
예제 #2
0
파일: imu.c 프로젝트: FW-M/paparazzi
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);
  VECT3_ASSIGN(imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL,   IMU_MAG_Z_NEUTRAL);

  /*
    Compute quaternion and rotation matrix
    for conversions between body and imu frame
  */
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined  IMU_BODY_TO_IMU_PSI
  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_NORMALISE(imu.body_to_imu_quat);
  INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
#else
  INT32_QUAT_ZERO(imu.body_to_imu_quat);
  INT32_RMAT_ZERO(imu.body_to_imu_rmat);
#endif

  imu_impl_init();
}
예제 #3
0
static void test_10(void) {

    struct FloatEulers euler;
    EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
    DISPLAY_FLOAT_EULERS_DEG("euler", euler);
    struct FloatQuat quat;
    FLOAT_QUAT_OF_EULERS(quat, euler);
    DISPLAY_FLOAT_QUAT("####quat", quat);

    struct Int32Eulers euleri;
    EULERS_BFP_OF_REAL(euleri, euler);
    DISPLAY_INT32_EULERS("euleri", euleri);
    struct Int32Quat quati;
    INT32_QUAT_OF_EULERS(quati, euleri);
    DISPLAY_INT32_QUAT("####quat", quati);
    struct Int32RMat rmati;
    INT32_RMAT_OF_EULERS(rmati, euleri);
    DISPLAY_INT32_RMAT("####rmat", rmati);

    struct Int32Quat quat_ltp_to_body;
    struct Int32Quat body_to_imu_quat;
    INT32_QUAT_ZERO( body_to_imu_quat);


    INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati);
    DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);

}
예제 #4
0
파일: imu.c 프로젝트: glason/paparazzi
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
#pragma message "Info: Magnetomter neutrals are set to zero!"
#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();
}
예제 #5
0
// reset to "hover" setpoint
static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initial)
{
  int32_t pitch_rotation_angle;
  struct Int32Quat pitch_axis_quat;

  struct Int32Quat pitch_rotated_quat, pitch_rotated_quat2;

  struct Int32Vect3 y_axis = { 0, 1, 0 };

  struct Int32Eulers rotated_eulers;

  // compose rotation about Y axis (pitch axis) from hover
  pitch_rotation_angle = ANGLE_BFP_OF_REAL(-QUAT_SETPOINT_HOVER_PITCH);
  INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle);
  INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat, *initial, pitch_axis_quat);

  INT32_EULERS_OF_QUAT(rotated_eulers, pitch_rotated_quat);

  // reset euler angles
  rotated_eulers.theta = _theta;
  rotated_eulers.phi = _psi;

  INT32_QUAT_OF_EULERS(pitch_rotated_quat, rotated_eulers);

  // compose rotation about Y axis (pitch axis) to hover
  pitch_rotation_angle = ANGLE_BFP_OF_REAL(QUAT_SETPOINT_HOVER_PITCH);
  INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle);
  INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat2, pitch_rotated_quat, pitch_axis_quat);

  // store result into setpoint
  QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2);
}
예제 #6
0
/* Compute ltp to imu rotation in quaternion and rotation matrice representation
   from the euler angle representation */
__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) {

  /* Compute LTP to IMU quaternion */
  INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
  /* Compute LTP to IMU rotation matrix */
  INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler);

}
예제 #7
0
파일: imu.c 프로젝트: EricPoppe/paparazzi
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();
}
예제 #8
0
static void test_2(void) {

    struct Int32Vect3 v1 = { 5000, 5000, 5000 };
    DISPLAY_INT32_VECT3("v1", v1);

    struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
    DISPLAY_FLOAT_EULERS("euler_f", euler_f);

    struct Int32Eulers euler_i;
    EULERS_BFP_OF_REAL(euler_i, euler_f);
    DISPLAY_INT32_EULERS("euler_i", euler_i);

    struct Int32Quat quat_i;
    INT32_QUAT_OF_EULERS(quat_i, euler_i);
    DISPLAY_INT32_QUAT("quat_i", quat_i);
    INT32_QUAT_NORMALIZE(quat_i);
    DISPLAY_INT32_QUAT("quat_i_n", quat_i);

    struct Int32Vect3 v2;
    INT32_QUAT_VMULT(v2, quat_i, v1);
    DISPLAY_INT32_VECT3("v2", v2);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_QUAT(rmat_i, quat_i);
    DISPLAY_INT32_RMAT("rmat_i", rmat_i);

    struct Int32Vect3 v3;
    INT32_RMAT_VMULT(v3, rmat_i, v1);
    DISPLAY_INT32_VECT3("v3", v3);

    struct Int32RMat rmat_i2;
    INT32_RMAT_OF_EULERS(rmat_i2, euler_i);
    DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);

    struct Int32Vect3 v4;
    INT32_RMAT_VMULT(v4, rmat_i2, v1);
    DISPLAY_INT32_VECT3("v4", v4);

    struct FloatQuat quat_f;
    FLOAT_QUAT_OF_EULERS(quat_f, euler_f);
    DISPLAY_FLOAT_QUAT("quat_f", quat_f);

    struct FloatVect3 v5;
    VECT3_COPY(v5, v1);
    DISPLAY_FLOAT_VECT3("v5", v5);
    struct FloatVect3 v6;
    FLOAT_QUAT_VMULT(v6, quat_f, v5);
    DISPLAY_FLOAT_VECT3("v6", v6);

}
예제 #9
0
static void test_1(void) {

    struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
    DISPLAY_FLOAT_EULERS("euler_f", euler_f);

    struct Int32Eulers euler_i;
    EULERS_BFP_OF_REAL(euler_i, euler_f);
    DISPLAY_INT32_EULERS("euler_i", euler_i);

    struct FloatQuat quat_f;
    FLOAT_QUAT_OF_EULERS(quat_f, euler_f);
    DISPLAY_FLOAT_QUAT("quat_f", quat_f);

    struct Int32Quat quat_i;
    INT32_QUAT_OF_EULERS(quat_i, euler_i);
    DISPLAY_INT32_QUAT("quat_i", quat_i);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_QUAT(rmat_i, quat_i);
    DISPLAY_INT32_RMAT("rmat_i", rmat_i);

}
예제 #10
0
static void test_4_int(void) {

    printf("euler to quat to euler - int\n");
    /* initial euler angles */
    struct Int32Eulers _e;
    EULERS_ASSIGN(_e, ANGLE_BFP_OF_REAL(RadOfDeg(-10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("euler orig ", _e);

    /* transform to quaternion */
    struct Int32Quat _q;
    INT32_QUAT_OF_EULERS(_q, _e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q);
    //  INT32_QUAT_NORMALIZE(_q);
    //  DISPLAY_INT32_QUAT_2("_q_n", _q);

    /* back to eulers */
    struct Int32Eulers _e2;
    INT32_EULERS_OF_QUAT(_e2, _q);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("back to euler ", _e2);


}
예제 #11
0
void stabilization_attitude_read_rc_absolute(bool_t in_flight) {

  // FIXME: wtf???
#ifdef AIRPLANE_STICKS
  pprz_t roll = radio_control.values[RADIO_ROLL];
  pprz_t pitch = radio_control.values[RADIO_PITCH];
  pprz_t yaw = radio_control.values[RADIO_YAW];
#else // QUAD STICKS
  pprz_t roll = radio_control.values[RADIO_YAW];
  pprz_t pitch = radio_control.values[RADIO_PITCH];
  pprz_t yaw = -radio_control.values[RADIO_ROLL];
#endif
  struct Int32Eulers sticks_eulers;
  struct Int32Quat sticks_quat, prev_sp_quat;

  // heading hold?
  if (in_flight) {
    // compose setpoint based on previous setpoint + pitch/roll sticks
    reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), &stab_att_sp_quat);

    // get commanded yaw rate from sticks
    sticks_eulers.phi = RATE_BFP_OF_REAL(APPLY_DEADBAND(roll, STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF / RC_UPDATE_FREQ);
    sticks_eulers.theta = 0;
    sticks_eulers.psi = 0;

    // convert yaw rate * dt into quaternion
    INT32_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
    QUAT_COPY(prev_sp_quat, stab_att_sp_quat)

    // update setpoint by rotating by incremental yaw command
    INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, sticks_quat);
  } else { /* if not flying, use current body position + pitch/yaw from sticks to compose setpoint */
    reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), stateGetNedToBodyQuat_i());
  }

  // update euler setpoints for telemetry
  INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat);
}
예제 #12
0
static void test_3(void) {

    /* Compute BODY to IMU eulers */
    struct Int32Eulers b2i_e;
    EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e);

    /* Compute BODY to IMU quaternion */
    struct Int32Quat b2i_q;
    INT32_QUAT_OF_EULERS(b2i_q, b2i_e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
    //  INT32_QUAT_NORMALIZE(b2i_q);
    //  DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);

    /* Compute BODY to IMU rotation matrix */
    struct Int32RMat b2i_r;
    INT32_RMAT_OF_EULERS(b2i_r, b2i_e);
    //  DISPLAY_INT32_RMAT("b2i_r", b2i_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r);

    /* Compute LTP to IMU eulers */
    struct Int32Eulers l2i_e;
    EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e);

    /* Compute LTP to IMU quaternion */
    struct Int32Quat l2i_q;
    INT32_QUAT_OF_EULERS(l2i_q, l2i_e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q);

    /* Compute LTP to IMU rotation matrix */
    struct Int32RMat l2i_r;
    INT32_RMAT_OF_EULERS(l2i_r, l2i_e);
    //  DISPLAY_INT32_RMAT("l2i_r", l2i_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r);


    /* again but from quaternion */
    struct Int32RMat l2i_r2;
    INT32_RMAT_OF_QUAT(l2i_r2, l2i_q);
    //  DISPLAY_INT32_RMAT("l2i_r2", l2i_r2);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2);

    /* Compute LTP to BODY quaternion */
    struct Int32Quat l2b_q;
    INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);

    /* Compute LTP to BODY rotation matrix */
    struct Int32RMat l2b_r;
    INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r);
    //  DISPLAY_INT32_RMAT("l2b_r", l2b_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r);

    /* again but from quaternion */
    struct Int32RMat l2b_r2;
    INT32_RMAT_OF_QUAT(l2b_r2, l2b_q);
    //  DISPLAY_INT32_RMAT("l2b_r2", l2b_r2);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2);


    /* compute LTP to BODY eulers */
    struct Int32Eulers l2b_e;
    INT32_EULERS_OF_RMAT(l2b_e, l2b_r);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e);

    /* again but from quaternion */
    struct Int32Eulers l2b_e2;
    INT32_EULERS_OF_QUAT(l2b_e2, l2b_q);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2);

}
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
  // copy euler setpoint for debugging
  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
  INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler);
  INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
}