Esempio n. 1
0
float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display)
{

  struct FloatQuat qa2b_f;
  float_quat_comp_inv(&qa2b_f, &qa2c_f, &qb2c_f);
  struct Int32Quat qa2c_i;
  QUAT_BFP_OF_REAL(qa2c_i, qa2c_f);
  struct Int32Quat qb2c_i;
  QUAT_BFP_OF_REAL(qb2c_i, qb2c_f);
  struct Int32Quat qa2b_i;
  int32_quat_comp_inv(&qa2b_i, &qa2c_i, &qb2c_i);

  struct FloatQuat err;
  QUAT_DIFF(err, qa2b_f, qa2b_i);
  float norm_err = FLOAT_QUAT_NORM(err);

  if (display) {
    printf("quat comp_inv\n");
    DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("a2bf", qa2b_f);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("a2bi", qa2b_i);
  }

  return norm_err;

}
void orientationCalcRMat_i(struct OrientationReps* orientation) {
  if (bit_is_set(orientation->status, ORREP_RMAT_I))
    return;

  if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
    RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
  }
  else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
    INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i);
  }
  else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
    INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
  }
  else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
    QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
    SetBit(orientation->status, ORREP_QUAT_I);
    INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_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_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
  }
  /* set bit to indicate this representation is computed */
  SetBit(orientation->status, ORREP_RMAT_I);
}
void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) {
  struct FloatEulers rpy_f;
  EULERS_FLOAT_OF_BFP(rpy_f, *rpy);
  struct FloatQuat quat_f;
  quat_from_rpy_cmd_f(&quat_f, &rpy_f);
  QUAT_BFP_OF_REAL(*quat, quat_f);
}
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
  struct FloatQuat q_sp;
#if USE_EARTH_BOUND_RC_SETPOINT
  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
#else
  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
#endif
  QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
}
void sim_overwrite_ahrs(void) {

  EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, fdm.ltp_to_body_eulers);

  QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, fdm.ltp_to_body_quat);

  RATES_BFP_OF_REAL(ahrs.body_rate, fdm.body_ecef_rotvel);

  INT32_RMAT_OF_QUAT(ahrs.ltp_to_body_rmat, ahrs.ltp_to_body_quat);

}
void sim_overwrite_ahrs(void) {

  struct Int32Quat quat;
  QUAT_BFP_OF_REAL(quat, fdm.ltp_to_body_quat);
  stateSetNedToBodyQuat_i(&quat);

  struct Int32Rates rates;
  RATES_BFP_OF_REAL(rates, fdm.body_ecef_rotvel);
  stateSetBodyRates_i(&rates);

}
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading) {
  // use float conversion for now...
  struct FloatVect2 cmd_f;
  cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x);
  cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y);
  float heading_f = ANGLE_FLOAT_OF_BFP(heading);

  struct FloatQuat quat_f;
  quat_from_earth_cmd_f(&quat_f, &cmd_f, heading_f);

  // convert back to fixed point
  QUAT_BFP_OF_REAL(*quat, quat_f);
}
void stabilization_attitude_read_rc(bool_t in_flight) {

#if USE_SETPOINTS_WITH_TRANSITIONS
  stabilization_attitude_read_rc_absolute(in_flight);
#else

  //FIXME: remove me, do in quaternion directly
  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);

  struct FloatQuat q_rp_cmd;
#if USE_EARTH_BOUND_RC_SETPOINT
  stabilization_attitude_read_rc_roll_pitch_earth_quat(&q_rp_cmd);
#else
  stabilization_attitude_read_rc_roll_pitch_quat(&q_rp_cmd);
#endif

  /* get current heading */
  const struct FloatVect3 zaxis = {0., 0., 1.};
  struct FloatQuat q_yaw;
  FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(stateGetNedToBodyEulers_i()->psi));

  /* apply roll and pitch commands with respect to current heading */
  struct FloatQuat q_sp;
  FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp_cmd);
  FLOAT_QUAT_NORMALIZE(q_sp);

  if (in_flight)
  {
    /* get current heading setpoint */
    struct FloatQuat q_yaw_sp;
    FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));

    /* rotation between current yaw and yaw setpoint */
    struct FloatQuat q_yaw_diff;
    FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw);

    /* temporary copy with roll/pitch command and current heading */
    struct FloatQuat q_rp_sp;
    QUAT_COPY(q_rp_sp, q_sp);

    /* compute final setpoint with yaw */
    FLOAT_QUAT_COMP_NORM_SHORTEST(q_sp, q_rp_sp, q_yaw_diff);
  }

  QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
#endif
}
Esempio n. 9
0
float test_eulers_of_quat(struct FloatQuat fq, int display) {

    struct FloatEulers fe;
    FLOAT_EULERS_OF_QUAT(fe, fq);
    struct Int32Quat iq;
    QUAT_BFP_OF_REAL(iq, fq);
    struct Int32Eulers ie;
    INT32_EULERS_OF_QUAT(ie, iq);
    struct FloatEulers fe2;
    EULERS_FLOAT_OF_BFP(fe2, ie);
    EULERS_SUB(fe2, ie);
    float norm_err = FLOAT_EULERS_NORM(fe2);

    if (display) {
        printf("euler of quat\n");
        DISPLAY_FLOAT_QUAT("fq", fq);
        DISPLAY_FLOAT_EULERS_DEG("fe", fe);
        DISPLAY_INT32_EULERS("ie", ie);
        DISPLAY_INT32_EULERS_AS_FLOAT_DEG("ieaf", ie);
    }

    return norm_err;
}
Esempio n. 10
0
void stabilization_attitude_set_setpoint_rp_quat_f(bool in_flight, int32_t heading)
{
  struct FloatQuat q_rp_cmd;
  float_quat_of_eulers(&q_rp_cmd, &guidance_euler_cmd); //TODO this is a quaternion without yaw! add the desired yaw before you use it!

  /* get current heading */
  const struct FloatVect3 zaxis = {0., 0., 1.};
  struct FloatQuat q_yaw;

  float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);

  /* roll/pitch commands applied to to current heading */
  struct FloatQuat q_rp_sp;
  float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
  float_quat_normalize(&q_rp_sp);

  struct FloatQuat q_sp;

  if (in_flight) {
    /* get current heading setpoint */
    struct FloatQuat q_yaw_sp;
    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(heading));


    /* rotation between current yaw and yaw setpoint */
    struct FloatQuat q_yaw_diff;
    float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw);

    /* compute final setpoint with yaw */
    float_quat_comp_norm_shortest(&q_sp, &q_rp_sp, &q_yaw_diff);
  } else {
    QUAT_COPY(q_sp, q_rp_sp);
  }

  QUAT_BFP_OF_REAL(stab_att_sp_quat,q_sp);
}
void orientationCalcEulers_i(struct OrientationReps* orientation)
{
  if (bit_is_set(orientation->status, ORREP_EULER_I)) {
    return;
  }

  if (bit_is_set(orientation->status, ORREP_EULER_F)) {
    EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
  } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
    int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i));
  } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
    int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_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_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i));
  } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
    QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
    SetBit(orientation->status, ORREP_QUAT_I);
    int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i));
  }
  /* set bit to indicate this representation is computed */
  SetBit(orientation->status, ORREP_EULER_I);
}
Esempio n. 12
0
void UM6_packet_read_message(void)
{
  if ((UM6_status == UM6Running) && PacketLength > 11) {
    switch (PacketAddr) {
      case IMU_UM6_GYRO_PROC:
        UM6_rate.p =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352;
        UM6_rate.q =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352;
        UM6_rate.r =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352;
        RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec
        RATES_BFP_OF_REAL(imu.gyro, UM6_rate);
        break;
      case IMU_UM6_ACCEL_PROC:
        UM6_accel.x =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105;
        UM6_accel.y =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105;
        UM6_accel.z =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105;
        VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2
        ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int
        break;
      case IMU_UM6_MAG_PROC:
        UM6_mag.x =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176;
        UM6_mag.y =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176;
        UM6_mag.z =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176;
        // Assume the same units for magnetic field
        // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss).
        MAGS_BFP_OF_REAL(imu.mag, UM6_mag);
        break;
      case IMU_UM6_EULER:
        UM6_eulers.phi =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863;
        UM6_eulers.theta =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863;
        UM6_eulers.psi =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863;
        EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad
        EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers);
        break;
      case IMU_UM6_QUAT:
        UM6_quat.qi =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693;
        UM6_quat.qx =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693;
        UM6_quat.qy =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693;
        UM6_quat.qz =
          ((float)((int16_t)
                   ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693;
        QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat);
        break;
      default:
        break;
    }
  }
}