int spi_ap_link_init()
{
  if (spi_link_init()) {
    TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
    return -1;
  }

  // Initialize IMU->Body orientation
  imuFloat.body_to_imu_quat = body_to_imu_quat;
  imuFloat.sample_count = 0;

#ifdef IMU_ALIGN_BENCH
	// This code is for aligning body to imu rotation, turn this on, put the vehicle in hover, pointed north, read BOOZ2_AHRS_REF_QUAT as body to imu (in wing frame)
  struct FloatVect3 x_axis = { 0.0, 1.0, 0.0 };
  FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH);
#endif

  FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat);
	FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat);
  FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat);

  struct FloatRates bias0 = { 0., 0., 0.};
  rdyb_mahrs_init(imuFloat.body_to_imu_quat, bias0);

  return 0;
}
Exemplo n.º 2
0
static void traj_static_sine_update(void) {
  

  aos.imu_rates.p = RadOfDeg(200)*cos(aos.time);
  aos.imu_rates.q = RadOfDeg(200)*cos(0.7*aos.time+2);
  aos.imu_rates.r = RadOfDeg(200)*cos(0.8*aos.time+1);
  FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt);
  FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat);

}
void stabilization_attitude_run(bool_t enable_integrator) {

  /*
   * Update reference
   */
  stabilization_attitude_ref_update();

  /*
   * Compute errors for feedback
   */

  /* attitude error                          */
  struct FloatQuat att_err;
  FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, stab_att_ref_quat);
  /* wrap it in the shortest direction       */
  FLOAT_QUAT_WRAP_SHORTEST(att_err);

  /*  rate error                */
  struct FloatRates rate_err;
  RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate);

  /* integrated error */
  if (enable_integrator) {
    struct FloatQuat new_sum_err, scaled_att_err;
    /* update accumulator */
    scaled_att_err.qi = att_err.qi;
    scaled_att_err.qx = att_err.qx / IERROR_SCALE;
    scaled_att_err.qy = att_err.qy / IERROR_SCALE;
    scaled_att_err.qz = att_err.qz / IERROR_SCALE;
    FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
    FLOAT_QUAT_NORMALIZE(new_sum_err);
    FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
    FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
  } else {
    /* reset accumulator */
    FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
    FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
  }

  attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);

  attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat);

  // FIXME: this is very dangerous! only works if this really includes all commands
  for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) {
    stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i];
  }

  /* bound the result */
  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
Exemplo n.º 4
0
static void traj_step_phi_2nd_order_update(void) {

  if (aos.time > 15) {
    const float omega = RadOfDeg(100);
    const float xi = 0.9;
    struct FloatRates raccel;
    RATES_ASSIGN(raccel, -2.*xi*omega*aos.imu_rates.p - omega*omega*(aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0., 0.);
    FLOAT_RATES_INTEGRATE_FI(aos.imu_rates, raccel, aos.dt);
    FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt);
    FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat);
  }

}
Exemplo n.º 5
0
static void send_att(void) {
  struct FloatEulers ltp_to_imu_euler;
  FLOAT_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat);
  struct Int32Eulers euler_i;
  EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
  struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
  DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
      &euler_i.phi,
      &euler_i.theta,
      &euler_i.psi,
      &(eulers_body->phi),
      &(eulers_body->theta),
      &(eulers_body->psi));
}
void stabilization_attitude_ref_update(void) {

  /* integrate reference attitude            */
#if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
  struct FloatQuat qdot;
  FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
  QUAT_SMUL(qdot, qdot, DT_UPDATE);
  QUAT_ADD(stab_att_ref_quat, qdot);
#else // use finite step (involves trig)
  struct FloatQuat delta_q;
  FLOAT_QUAT_DIFFERENTIAL(delta_q, stab_att_ref_rate, DT_UPDATE);
  /* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */
  struct FloatQuat new_ref_quat;
  FLOAT_QUAT_COMP(new_ref_quat, delta_q, stab_att_ref_quat);
  QUAT_COPY(stab_att_ref_quat, new_ref_quat);
#endif
  FLOAT_QUAT_NORMALIZE(stab_att_ref_quat);

  /* integrate reference rotational speeds   */
  struct FloatRates delta_rate;
  RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE);
  RATES_ADD(stab_att_ref_rate, delta_rate);

  /* compute reference angular accelerations */
  struct FloatQuat err;
  /* compute reference attitude error        */
  FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat);
  /* wrap it in the shortest direction       */
  FLOAT_QUAT_WRAP_SHORTEST(err);
  /* propagate the 2nd order linear model    */
  stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p
    - stab_att_ref_model[ref_idx].omega.p*stab_att_ref_model[ref_idx].omega.p*err.qx;
  stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q
    - stab_att_ref_model[ref_idx].omega.q*stab_att_ref_model[ref_idx].omega.q*err.qy;
  stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r
    - stab_att_ref_model[ref_idx].omega.r*stab_att_ref_model[ref_idx].omega.r*err.qz;

  /*	saturate acceleration */
  const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
  const struct FloatRates MAX_ACCEL = {  REF_ACCEL_MAX_P,  REF_ACCEL_MAX_Q,  REF_ACCEL_MAX_R };
  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);

  /* saturate angular speed and trim accel accordingly */
  SATURATE_SPEED_TRIM_ACCEL();

  /* compute ref_euler */
  FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat);
}
Exemplo n.º 7
0
float test_quat_of_rmat(void) {

    //  struct FloatEulers eul = {-0.280849, 0.613423, -1.850440};
    struct FloatEulers eul = {RadOfDeg(0.131579),  RadOfDeg(-62.397659), RadOfDeg(-110.470299)};
    //  struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)};

    struct FloatMat33 rm;
    FLOAT_RMAT_OF_EULERS(rm, eul);
    DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm);

    struct FloatQuat q;
    FLOAT_QUAT_OF_RMAT(q, rm);
    DISPLAY_FLOAT_QUAT("q_of_rm   ", q);
    DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm   ", q);

    struct FloatQuat qref;
    FLOAT_QUAT_OF_EULERS(qref, eul);
    DISPLAY_FLOAT_QUAT("q_of_euler", qref);
    DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref);

    printf("\n\n\n");

    struct FloatMat33 r_att;
    struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi };
    FLOAT_RMAT_OF_EULERS_312(r_att, e312);
    DISPLAY_FLOAT_RMAT("r_att  ", r_att);
    DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att  ", r_att);

    struct FloatQuat q_att;
    FLOAT_QUAT_OF_RMAT(q_att, r_att);

    struct FloatEulers e_att;
    FLOAT_EULERS_OF_RMAT(e_att, r_att);
    DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att);

    FLOAT_EULERS_OF_QUAT(e_att, q_att);
    DISPLAY_FLOAT_EULERS_DEG("of quat", e_att);

    return 0.;
}
Exemplo n.º 8
0
static void test_4_float(void) {

    printf("euler to quat to euler - float\n");
    /* initial euler angles */
    struct FloatEulers e;
    EULERS_ASSIGN(e, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
    DISPLAY_FLOAT_EULERS_DEG("e", e);

    /* transform to quaternion */
    struct FloatQuat q;
    FLOAT_QUAT_OF_EULERS(q, e);
    //  DISPLAY_FLOAT_QUAT("q", q);
    FLOAT_QUAT_NORMALIZE(q);
    DISPLAY_FLOAT_QUAT("q_n", q);
    DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q);
    /* back to eulers */
    struct FloatEulers e2;
    FLOAT_EULERS_OF_QUAT(e2, q);
    DISPLAY_FLOAT_EULERS_DEG("e2", e2);


}
Exemplo 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;
}
void stabilization_attitude_ref_update() {

  /* integrate reference attitude            */
  struct FloatQuat qdot;
  FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
  QUAT_SMUL(qdot, qdot, DT_UPDATE);
  QUAT_ADD(stab_att_ref_quat, qdot);
  FLOAT_QUAT_NORMALIZE(stab_att_ref_quat);

  /* integrate reference rotational speeds   */
  struct FloatRates delta_rate;
  RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE);
  RATES_ADD(stab_att_ref_rate, delta_rate);

  /* compute reference angular accelerations */
  struct FloatQuat err;
  /* compute reference attitude error        */
  FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat);
  /* wrap it in the shortest direction       */
  FLOAT_QUAT_WRAP_SHORTEST(err);
  /* propagate the 2nd order linear model    */
  stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p
    - stab_att_ref_model[ref_idx].omega.p*stab_att_ref_model[ref_idx].omega.p*err.qx;
  stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q
    - stab_att_ref_model[ref_idx].omega.q*stab_att_ref_model[ref_idx].omega.q*err.qy;
  stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r
    - stab_att_ref_model[ref_idx].omega.r*stab_att_ref_model[ref_idx].omega.r*err.qz;

  /*	saturate acceleration */
  const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
  const struct FloatRates MAX_ACCEL = {  REF_ACCEL_MAX_P,  REF_ACCEL_MAX_Q,  REF_ACCEL_MAX_R };
  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);

  /* compute ref_euler */
  FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat);
}
Exemplo n.º 11
0
void ahrs_propagate(void) {
  struct NedCoor_f accel;
  struct FloatRates body_rates;
  struct FloatEulers eulers;

  // realign all the filter if needed
  // a complete init cycle is required
  if (ins_impl.reset) {
    ins_impl.reset = FALSE;
    ins.status = INS_UNINIT;
    ahrs.status = AHRS_UNINIT;
    init_invariant_state();
  }

  // fill command vector
  struct Int32Rates gyro_meas_body;
  INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro);
  RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
  struct Int32Vect3 accel_meas_body;
  INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
  ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);

  // update correction gains
  error_output(&ins_impl);

  // propagate model
  struct inv_state new_state;
  runge_kutta_4_float((float*)&new_state,
      (float*)&ins_impl.state, INV_STATE_DIM,
      (float*)&ins_impl.cmd, INV_COMMAND_DIM,
      invariant_model, dt);
  ins_impl.state = new_state;

  // normalize quaternion
  FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);

  // set global state
  FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
#if INS_UPDATE_FW_ESTIMATOR
  // Some stupid lines of code for neutrals
  eulers.phi -= ins_roll_neutral;
  eulers.theta -= ins_pitch_neutral;
  stateSetNedToBodyEulers_f(&eulers);
#else
  stateSetNedToBodyQuat_f(&ins_impl.state.quat);
#endif
  RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
  stateSetBodyRates_f(&body_rates);
  stateSetPositionNed_f(&ins_impl.state.pos);
  stateSetSpeedNed_f(&ins_impl.state.speed);
  // untilt accel and remove gravity
  FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel);
  FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
  FLOAT_VECT3_ADD(accel, A);
  stateSetAccelNed_f(&accel);

  //------------------------------------------------------------//

  RunOnceEvery(3,{
      DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice,
        &ins_impl.state.quat.qi,
        &eulers.phi,
        &eulers.theta,
        &eulers.psi,
        &ins_impl.state.speed.x,
        &ins_impl.state.speed.y,
        &ins_impl.state.speed.z,
        &ins_impl.state.pos.x,
        &ins_impl.state.pos.y,
        &ins_impl.state.pos.z,
        &ins_impl.state.bias.p,
        &ins_impl.state.bias.q,
        &ins_impl.state.bias.r,
        &ins_impl.state.as,
        &ins_impl.state.hb,
        &ins_impl.meas.baro_alt,
        &ins_impl.meas.pos_gps.z)
      });

#if LOG_INVARIANT_FILTER
  if (pprzLogFile.fs != NULL) {
    if (!log_started) {
      // log file header
      sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
      log_started = TRUE;
    }
    else {
      sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
          ins_impl.cmd.rates.p,
          ins_impl.cmd.rates.q,
          ins_impl.cmd.rates.r,
          ins_impl.cmd.accel.x,
          ins_impl.cmd.accel.y,
          ins_impl.cmd.accel.z,
          ins_impl.meas.pos_gps.x,
          ins_impl.meas.pos_gps.y,
          ins_impl.meas.pos_gps.z,
          ins_impl.meas.speed_gps.x,
          ins_impl.meas.speed_gps.y,
          ins_impl.meas.speed_gps.z,
          ins_impl.meas.mag.x,
          ins_impl.meas.mag.y,
          ins_impl.meas.mag.z,
          ins_impl.meas.baro_alt,
          ins_impl.state.quat.qi,
          ins_impl.state.quat.qx,
          ins_impl.state.quat.qy,
          ins_impl.state.quat.qz,
          ins_impl.state.bias.p,
          ins_impl.state.bias.q,
          ins_impl.state.bias.r,
          ins_impl.state.speed.x,
          ins_impl.state.speed.y,
          ins_impl.state.speed.z,
          ins_impl.state.pos.x,
          ins_impl.state.pos.y,
          ins_impl.state.pos.z,
          ins_impl.state.hb,
          ins_impl.state.as);
    }
  }
#endif
}
Exemplo n.º 12
0
void parse_ins_msg( void ) {
    if (last_received_packet.ErrID != VN100_Error_None) {
        //TODO send error
        return;
    }

    // parse message (will work only with read and write register)
    switch (last_received_packet.RegID) {
    case VN100_REG_ADOR :
        ins_ador = last_received_packet.Data[0].UInt;
        break;
    case VN100_REG_ADOF :
        ins_adof = last_received_packet.Data[0].UInt;
        break;
    case VN100_REG_SBAUD :
        ins_baud = last_received_packet.Data[0].UInt;
        break;
    case VN100_REG_YPR :
        ins_eulers.phi   = RadOfDeg(last_received_packet.Data[2].Float);
        ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
        ins_eulers.psi   = RadOfDeg(last_received_packet.Data[0].Float);
        break;
    case VN100_REG_QTN :
        ins_quat.qi = last_received_packet.Data[0].Float;
        ins_quat.qx = last_received_packet.Data[1].Float;
        ins_quat.qy = last_received_packet.Data[2].Float;
        ins_quat.qz = last_received_packet.Data[3].Float;
        FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat);
        break;
    case VN100_REG_QTM :
        ins_quat.qi = last_received_packet.Data[0].Float;
        ins_quat.qx = last_received_packet.Data[1].Float;
        ins_quat.qy = last_received_packet.Data[2].Float;
        ins_quat.qz = last_received_packet.Data[3].Float;
        FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat);
        ins_mag.x = last_received_packet.Data[4].Float;
        ins_mag.y = last_received_packet.Data[5].Float;
        ins_mag.z = last_received_packet.Data[6].Float;
        break;
    case VN100_REG_QTA :
        ins_quat.qi = last_received_packet.Data[0].Float;
        ins_quat.qx = last_received_packet.Data[1].Float;
        ins_quat.qy = last_received_packet.Data[2].Float;
        ins_quat.qz = last_received_packet.Data[3].Float;
        FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat);
        ins_accel.x = last_received_packet.Data[4].Float;
        ins_accel.y = last_received_packet.Data[5].Float;
        ins_accel.z = last_received_packet.Data[6].Float;
        break;
    case VN100_REG_QTR :
        ins_quat.qi = last_received_packet.Data[0].Float;
        ins_quat.qx = last_received_packet.Data[1].Float;
        ins_quat.qy = last_received_packet.Data[2].Float;
        ins_quat.qz = last_received_packet.Data[3].Float;
        FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat);
        ins_rates.p = last_received_packet.Data[4].Float;
        ins_rates.q = last_received_packet.Data[5].Float;
        ins_rates.r = last_received_packet.Data[6].Float;
        break;
    case VN100_REG_QMA :
        ins_quat.qi = last_received_packet.Data[0].Float;
        ins_quat.qx = last_received_packet.Data[1].Float;
        ins_quat.qy = last_received_packet.Data[2].Float;
        ins_quat.qz = last_received_packet.Data[3].Float;
        FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat);
        ins_mag.x = last_received_packet.Data[4].Float;
        ins_mag.y = last_received_packet.Data[5].Float;
        ins_mag.z = last_received_packet.Data[6].Float;
        ins_accel.x = last_received_packet.Data[7].Float;
        ins_accel.y = last_received_packet.Data[8].Float;
        ins_accel.z = last_received_packet.Data[9].Float;
        break;
    case VN100_REG_QAR :
        ins_quat.qi = last_received_packet.Data[0].Float;
        ins_quat.qx = last_received_packet.Data[1].Float;
        ins_quat.qy = last_received_packet.Data[2].Float;
        ins_quat.qz = last_received_packet.Data[3].Float;
        FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat);
        ins_accel.x = last_received_packet.Data[4].Float;
        ins_accel.y = last_received_packet.Data[5].Float;
        ins_accel.z = last_received_packet.Data[6].Float;
        ins_rates.p = last_received_packet.Data[7].Float;
        ins_rates.q = last_received_packet.Data[8].Float;
        ins_rates.r = last_received_packet.Data[9].Float;
        break;
    case VN100_REG_QMR :
        ins_quat.qi = last_received_packet.Data[0].Float;
        ins_quat.qx = last_received_packet.Data[1].Float;
        ins_quat.qy = last_received_packet.Data[2].Float;
        ins_quat.qz = last_received_packet.Data[3].Float;
        FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat);
        ins_mag.x = last_received_packet.Data[4].Float;
        ins_mag.y = last_received_packet.Data[5].Float;
        ins_mag.z = last_received_packet.Data[6].Float;
        ins_accel.x = last_received_packet.Data[7].Float;
        ins_accel.y = last_received_packet.Data[8].Float;
        ins_accel.z = last_received_packet.Data[9].Float;
        ins_rates.p = last_received_packet.Data[10].Float;
        ins_rates.q = last_received_packet.Data[11].Float;
        ins_rates.r = last_received_packet.Data[12].Float;
        break;
    case VN100_REG_YMR :
        ins_eulers.phi   = RadOfDeg(last_received_packet.Data[2].Float);
        ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
        ins_eulers.psi   = RadOfDeg(last_received_packet.Data[0].Float);
        ins_mag.x = last_received_packet.Data[3].Float;
        ins_mag.y = last_received_packet.Data[4].Float;
        ins_mag.z = last_received_packet.Data[5].Float;
        ins_accel.x = last_received_packet.Data[6].Float;
        ins_accel.y = last_received_packet.Data[7].Float;
        ins_accel.z = last_received_packet.Data[8].Float;
        ins_rates.p = last_received_packet.Data[9].Float;
        ins_rates.q = last_received_packet.Data[10].Float;
        ins_rates.r = last_received_packet.Data[11].Float;
        break;
    }

}
Exemplo n.º 13
0
static void print_estimator_state(double time) {

#if FILTER_OUTPUT_IN_NED
	
	struct EcefCoor_d pos_ecef,
										cur_pos_ecef,
										cur_vel_ecef;
	struct NedCoor_d	pos_ned,
										vel_ned;
										
	struct DoubleQuat q_ecef2body,
										q_ecef2enu,
										q_enu2body,
										q_ned2enu,
										q_ned2body;
										
	VECTOR_AS_VECT3(pos_ecef,pos_0_ecef);
	VECTOR_AS_VECT3(cur_pos_ecef,ins.avg_state.position);
	VECTOR_AS_VECT3(cur_vel_ecef,ins.avg_state.velocity);
	
	ned_of_ecef_point_d(&pos_ned, &current_ltp, &cur_pos_ecef);
	ned_of_ecef_vect_d(&vel_ned, &current_ltp, &cur_vel_ecef);
	
  int32_t xdd = 0;
  int32_t ydd = 0;
  int32_t zdd = 0;
  
  int32_t xd = vel_ned.x/0.0000019073;
  int32_t yd = vel_ned.y/0.0000019073;
  int32_t zd = vel_ned.z/0.0000019073;
  
  int32_t x = pos_ned.x/0.0039;
  int32_t y = pos_ned.y/0.0039;
  int32_t z = pos_ned.z/0.0039;

  fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
  #if 0
  QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), -ins.avg_state.orientation.x(),
	         -ins.avg_state.orientation.y(), -ins.avg_state.orientation.z());
  QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0);
  
  FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
	FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body);		// q_enu2body = q_ecef2body * (q_ecef2enu)^*
  FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body);					// q_ned2body = q_enu2body * q_ned2enu

  #else /* if 0 */
  QUATERNIOND_AS_DOUBLEQUAT(q_ecef2body, ins.avg_state.orientation);
  DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
  FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body);
  QUAT_ENU_FROM_TO_NED(q_enu2body, q_ned2body);
  
  #endif /* if 0 */
  
  struct FloatEulers e;
  FLOAT_EULERS_OF_QUAT(e, q_ned2body);
  
  
	#if PRINT_EULER_NED
		printf("EULER % 6.1f % 6.1f % 6.1f\n", e.phi*180*M_1_PI, e.theta*180*M_1_PI, e.psi*180*M_1_PI);
	#endif /* PRINT_EULER_NED */
  fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi);
  fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
				sqrt(ins.cov( 0, 0)),  sqrt(ins.cov( 1, 1)),  sqrt(ins.cov( 2, 2)), 
				sqrt(ins.cov( 3, 3)),  sqrt(ins.cov( 4, 4)),  sqrt(ins.cov( 5, 5)), 
				sqrt(ins.cov( 6, 6)),  sqrt(ins.cov( 7, 7)),  sqrt(ins.cov( 8, 8)), 
				sqrt(ins.cov( 9, 9)),  sqrt(ins.cov(10,10)),  sqrt(ins.cov(11,11)));
  fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2));

#else /* FILTER_OUTPUT_IN_ECEF */
  int32_t xdd = 0;
  int32_t ydd = 0;
  int32_t zdd = 0;

  int32_t xd = ins.avg_state.velocity(0)/0.0000019073;
  int32_t yd = ins.avg_state.velocity(1)/0.0000019073;
  int32_t zd = ins.avg_state.velocity(2)/0.0000019073;
  int32_t x = ins.avg_state.position(0)/0.0039;
  int32_t y = ins.avg_state.position(1)/0.0039;
  int32_t z = ins.avg_state.position(2)/0.0039;

  fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
  
  struct FloatQuat q_ecef2body;
  QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(),
	         ins.avg_state.orientation.y(), ins.avg_state.orientation.z());
  struct FloatEulers e_ecef2body;
  FLOAT_EULERS_OF_QUAT(e_ecef2body, q_ecef2body);

  fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e_ecef2body.phi, e_ecef2body.theta, e_ecef2body.psi);
  fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
				sqrt(ins.cov( 0, 0)),  sqrt(ins.cov( 1, 1)),  sqrt(ins.cov( 2, 2)), 
				sqrt(ins.cov( 3, 3)),  sqrt(ins.cov( 4, 4)),  sqrt(ins.cov( 5, 5)), 
				sqrt(ins.cov( 6, 6)),  sqrt(ins.cov( 7, 7)),  sqrt(ins.cov( 8, 8)), 
				sqrt(ins.cov( 9, 9)),  sqrt(ins.cov(10,10)),  sqrt(ins.cov(11,11)));
  fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2));
#endif /* FILTER_OUTPUT_IN_NED / ECEF */
}
Exemplo n.º 14
0
void ahrs_propagate(float dt)
{
  struct FloatVect3 accel;
  struct FloatRates body_rates;

  // realign all the filter if needed
  // a complete init cycle is required
  if (ins_impl.reset) {
    ins_impl.reset = FALSE;
    ins.status = INS_UNINIT;
    ahrs.status = AHRS_UNINIT;
    init_invariant_state();
  }

  // fill command vector
  struct Int32Rates gyro_meas_body;
  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
  int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro);
  RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
  struct Int32Vect3 accel_meas_body;
  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
  ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);

  // update correction gains
  error_output(&ins_impl);

  // propagate model
  struct inv_state new_state;
  runge_kutta_4_float((float *)&new_state,
                      (float *)&ins_impl.state, INV_STATE_DIM,
                      (float *)&ins_impl.cmd, INV_COMMAND_DIM,
                      invariant_model, dt);
  ins_impl.state = new_state;

  // normalize quaternion
  FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);

  // set global state
  stateSetNedToBodyQuat_f(&ins_impl.state.quat);
  RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
  stateSetBodyRates_f(&body_rates);
  stateSetPositionNed_f(&ins_impl.state.pos);
  stateSetSpeedNed_f(&ins_impl.state.speed);
  // untilt accel and remove gravity
  struct FloatQuat q_b2n;
  float_quat_invert(&q_b2n, &ins_impl.state.quat);
  float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel);
  VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
  VECT3_ADD(accel, A);
  stateSetAccelNed_f((struct NedCoor_f *)&accel);

  //------------------------------------------------------------//

#if SEND_INVARIANT_FILTER
  struct FloatEulers eulers;
  FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
  RunOnceEvery(3, {
    pprz_msg_send_INV_FILTER(trans, dev, AC_ID,
    &ins_impl.state.quat.qi,
    &eulers.phi,
    &eulers.theta,
    &eulers.psi,
    &ins_impl.state.speed.x,
    &ins_impl.state.speed.y,
    &ins_impl.state.speed.z,
    &ins_impl.state.pos.x,
    &ins_impl.state.pos.y,
    &ins_impl.state.pos.z,
    &ins_impl.state.bias.p,
    &ins_impl.state.bias.q,
    &ins_impl.state.bias.r,
    &ins_impl.state.as,
    &ins_impl.state.hb,
    &ins_impl.meas.baro_alt,
    &ins_impl.meas.pos_gps.z)
  });
#endif

#if LOG_INVARIANT_FILTER
  if (pprzLogFile.fs != NULL) {
    if (!log_started) {
      // log file header
      sdLogWriteLog(&pprzLogFile,
                    "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
      log_started = TRUE;
    } else {
      sdLogWriteLog(&pprzLogFile,
                    "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
                    ins_impl.cmd.rates.p,
                    ins_impl.cmd.rates.q,
                    ins_impl.cmd.rates.r,
                    ins_impl.cmd.accel.x,
                    ins_impl.cmd.accel.y,
                    ins_impl.cmd.accel.z,
                    ins_impl.meas.pos_gps.x,
                    ins_impl.meas.pos_gps.y,
                    ins_impl.meas.pos_gps.z,
                    ins_impl.meas.speed_gps.x,
                    ins_impl.meas.speed_gps.y,
                    ins_impl.meas.speed_gps.z,
                    ins_impl.meas.mag.x,
                    ins_impl.meas.mag.y,
                    ins_impl.meas.mag.z,
                    ins_impl.meas.baro_alt,
                    ins_impl.state.quat.qi,
                    ins_impl.state.quat.qx,
                    ins_impl.state.quat.qy,
                    ins_impl.state.quat.qz,
                    ins_impl.state.bias.p,
                    ins_impl.state.bias.q,
                    ins_impl.state.bias.r,
                    ins_impl.state.speed.x,
                    ins_impl.state.speed.y,
                    ins_impl.state.speed.z,
                    ins_impl.state.pos.x,
                    ins_impl.state.pos.y,
                    ins_impl.state.pos.z,
                    ins_impl.state.hb,
                    ins_impl.state.as);
    }
  }
#endif
}
void stabilization_attitude_run(bool_t enable_integrator) {

  /*
   * Update reference
   */
  stabilization_attitude_ref_update();

  /*
   * Compute errors for feedback
   */

  /* attitude error                          */
  struct FloatQuat att_err;
  struct FloatQuat* att_quat = stateGetNedToBodyQuat_f();
  FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
  /* wrap it in the shortest direction       */
  FLOAT_QUAT_WRAP_SHORTEST(att_err);

  /*  rate error                */
  struct FloatRates rate_err;
  struct FloatRates* body_rate = stateGetBodyRates_f();
  RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
  /* rate_d error               */
  struct FloatRates body_rate_d;
  RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
  RATES_COPY(last_body_rate, *body_rate);

  /* integrated error */
  if (enable_integrator) {
    struct FloatQuat new_sum_err, scaled_att_err;
    /* update accumulator */
    scaled_att_err.qi = att_err.qi;
    scaled_att_err.qx = att_err.qx / IERROR_SCALE;
    scaled_att_err.qy = att_err.qy / IERROR_SCALE;
    scaled_att_err.qz = att_err.qz / IERROR_SCALE;
    FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
    FLOAT_QUAT_NORMALIZE(new_sum_err);
    FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
    FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
  } else {
    /* reset accumulator */
    FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
    FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
  }

  attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);

  attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, &stabilization_att_sum_err_quat);

  stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
  stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
  stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];

#ifdef HAS_SURFACE_COMMANDS
  stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
  stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
  stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
#endif

  /* bound the result */
  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}