static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
  RATES_COPY(output[i].rate_est, ahrs_impl.body_rate);
#else
  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_imu_quat);
  RATES_COPY(output[i].rate_est, ahrs_impl.imu_rate);
#endif /* OUTPUT_IN_BODY_FRAME */
  RATES_COPY(output[i].bias_est, ahrs_impl.gyro_bias);
  //  memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
}
// 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);
}
Exemple #3
0
void ahrs_init(void) {
  ahrs_float.status = AHRS_UNINIT;

  /*
   * Initialises our IMU alignement variables
   * This should probably done in the IMU code instead
   */
  struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
  FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
  FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

  /* set ltp_to_body to zero */
  FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
  FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
  FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
  FLOAT_RATES_ZERO(ahrs_float.body_rate);

  /* set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
  RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
  EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
  FLOAT_RATES_ZERO(ahrs_float.imu_rate);

  /* set inital filter dcm */
  set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
}
void ahrs_init(void) {

  QUAT_ASSIGN(ned_to_body_orientation_quat_i, 1, 0, 0, 0);
  INT32_QUAT_NORMALIZE(ned_to_body_orientation_quat_i);
  RATES_ASSIGN(body_rates_i, 0, 0, 0);


  ahrs.status = AHRS_UNINIT;
  ahrs_impl.ltp_vel_norm_valid = FALSE;
  ahrs_impl.heading_aligned = FALSE;

  /* set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
  INT_RATES_ZERO(ahrs_impl.imu_rate);

  INT_RATES_ZERO(ahrs_impl.gyro_bias);
  INT_RATES_ZERO(ahrs_impl.rate_correction);
  INT_RATES_ZERO(ahrs_impl.high_rez_bias);

#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  ahrs_impl.correct_gravity = TRUE;
#else
  ahrs_impl.correct_gravity = FALSE;
#endif

#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
  ahrs_impl.use_gravity_heuristic = TRUE;
#else
  ahrs_impl.use_gravity_heuristic = FALSE;
#endif

  VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));

}
void ahrs_init(void) {

  ahrs.status = AHRS_UNINIT;//AHRS状态未初始化
  ahrs_impl.ltp_vel_norm_valid = FALSE;
  ahrs_impl.heading_aligned = FALSE;//未航姿校准

  /* set ltp_to_imu so that body is zero */
  //设置ltp_to_imu ,imu速度,陀螺仪的偏差,速度矫正,
  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
  INT_RATES_ZERO(ahrs_impl.imu_rate);

  INT_RATES_ZERO(ahrs_impl.gyro_bias);
  INT_RATES_ZERO(ahrs_impl.rate_correction);
  INT_RATES_ZERO(ahrs_impl.high_rez_bias);

#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  ahrs_impl.correct_gravity = TRUE;
#else
  ahrs_impl.correct_gravity = FALSE;
#endif

#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
  ahrs_impl.use_gravity_heuristic = TRUE;
#else
  ahrs_impl.use_gravity_heuristic = FALSE;
#endif

  VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));

}
void ahrs_init(void) {

  ahrs.status = AHRS_UNINIT;
  ahrs_impl.ltp_vel_norm_valid = FALSE;
  ahrs_impl.heading_aligned = FALSE;

  /* set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
  INT_RATES_ZERO(ahrs_impl.imu_rate);

  INT_RATES_ZERO(ahrs_impl.gyro_bias);
  INT_RATES_ZERO(ahrs_impl.rate_correction);
  INT_RATES_ZERO(ahrs_impl.high_rez_bias);

#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  ahrs_impl.correct_gravity = TRUE;
#else
  ahrs_impl.correct_gravity = FALSE;
#endif

#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
  ahrs_impl.use_gravity_heuristic = TRUE;
#else
  ahrs_impl.use_gravity_heuristic = FALSE;
#endif

  VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));

  //INT32_VECT3_ZERO(imu_accel_local); //This is part of the grav correction hack
}
void ahrs_realign_heading(float heading) {
  FLOAT_ANGLE_NORMALIZE(heading);

  /* quaternion representing only the heading rotation from ltp to body */
  struct FloatQuat q_h_new;
  q_h_new.qx = 0.0;
  q_h_new.qy = 0.0;
  q_h_new.qz = sinf(heading/2.0);
  q_h_new.qi = cosf(heading/2.0);

  struct FloatQuat* ltp_to_body_quat = stateGetNedToBodyQuat_f();
  /* quaternion representing current heading only */
  struct FloatQuat q_h;
  QUAT_COPY(q_h, *ltp_to_body_quat);
  q_h.qx = 0.;
  q_h.qy = 0.;
  FLOAT_QUAT_NORMALIZE(q_h);

  /* quaternion representing rotation from current to new heading */
  struct FloatQuat q_c;
  FLOAT_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);

  /* correct current heading in body frame */
  struct FloatQuat q;
  FLOAT_QUAT_COMP_NORM_SHORTEST(q, q_c, *ltp_to_body_quat);

  /* compute ltp to imu rotation quaternion and matrix */
  FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, ahrs_impl.body_to_imu_quat);
  FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);

  /* set state */
  stateSetNedToBodyQuat_f(&q);

  ahrs_impl.heading_aligned = TRUE;
}
void ahrs_init(void) {

  ahrs.status = AHRS_UNINIT;
  ahrs_impl.ltp_vel_norm_valid = FALSE;
  ahrs_impl.heading_aligned = FALSE;

  /* set ltp_to_body to zero */
  INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
  INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
  INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
  INT_RATES_ZERO(ahrs.body_rate);

  /* set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
  RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
  INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
  INT_RATES_ZERO(ahrs.imu_rate);

  INT_RATES_ZERO(ahrs_impl.gyro_bias);
  INT_RATES_ZERO(ahrs_impl.rate_correction);
  INT_RATES_ZERO(ahrs_impl.high_rez_bias);

#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  ahrs_impl.correct_gravity = TRUE;
#else
  ahrs_impl.correct_gravity = FALSE;
#endif

#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
  ahrs_impl.use_gravity_heuristic = TRUE;
#else
  ahrs_impl.use_gravity_heuristic = FALSE;
#endif

}
void ahrs_fc_realign_heading(float heading)
{
  FLOAT_ANGLE_NORMALIZE(heading);

  /* quaternion representing only the heading rotation from ltp to body */
  struct FloatQuat q_h_new;
  q_h_new.qx = 0.0;
  q_h_new.qy = 0.0;
  q_h_new.qz = sinf(heading / 2.0);
  q_h_new.qi = cosf(heading / 2.0);

  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
  struct FloatQuat ltp_to_body_quat;
  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
  /* quaternion representing current heading only */
  struct FloatQuat q_h;
  QUAT_COPY(q_h, ltp_to_body_quat);
  q_h.qx = 0.;
  q_h.qy = 0.;
  float_quat_normalize(&q_h);

  /* quaternion representing rotation from current to new heading */
  struct FloatQuat q_c;
  float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);

  /* correct current heading in body frame */
  struct FloatQuat q;
  float_quat_comp_norm_shortest(&q, &q_c, &ltp_to_body_quat);

  /* compute ltp to imu rotation quaternion and matrix */
  float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
  float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);

  ahrs_fc.heading_aligned = TRUE;
}
Exemple #10
0
void ahrs_init(void) {

  ahrs.status = AHRS_UNINIT;

  /*
   * Initialises our IMU alignement variables
   * This should probably done in the IMU code instead
   */
  struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
  FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
  FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

  /* Set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);

  FLOAT_RATES_ZERO(ahrs_impl.imu_rate);

  VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);

  /*
   * Initialises our state
   */
  FLOAT_RATES_ZERO(ahrs_impl.gyro_bias);
  const float P0_a = 1.;
  const float P0_b = 1e-4;
  float P0[6][6] = {{ P0_a, 0.,   0.,   0.,   0.,   0.  },
                    { 0.,   P0_a, 0.,   0.,   0.,   0.  },
                    { 0.,   0.,   P0_a, 0.,   0.,   0.  },
                    { 0.,   0.,   0.,   P0_b, 0.,   0.  },
                    { 0.,   0.,   0.,   0.,   P0_b, 0.  },
                    { 0.,   0.,   0.,   0.,   0.,   P0_b}};
  memcpy(ahrs_impl.P, P0, sizeof(P0));

}
Exemple #11
0
void ahrs_init(void) {
    ahrs.status = AHRS_UNINIT;
    ahrs_impl.ltp_vel_norm_valid = FALSE;
    ahrs_impl.heading_aligned = FALSE;

    /* Initialises IMU alignement */
    struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
    FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
    FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

    /* Set ltp_to_imu so that body is zero */
    QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
    RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);

    FLOAT_RATES_ZERO(ahrs_impl.imu_rate);

#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
    ahrs_impl.correct_gravity = TRUE;
#else
    ahrs_impl.correct_gravity = FALSE;
#endif

    VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
}
void stabilization_attitude_run(bool_t enable_integrator) {

  /*
   * Update reference
   */
  stabilization_attitude_ref_update();

  /*
   * Compute errors for feedback
   */

  /* attitude error                          */
  struct Int32Quat att_err;
  struct Int32Quat* att_quat = stateGetNedToBodyQuat_i();
  INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
  /* wrap it in the shortest direction       */
  INT32_QUAT_WRAP_SHORTEST(att_err);
  INT32_QUAT_NORMALIZE(att_err);

  /*  rate error                */
  struct Int32Rates rate_err;
  struct Int32Rates* body_rate = stateGetBodyRates_i();
  RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);

  /* integrated error */
  if (enable_integrator) {
    struct Int32Quat 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;
    INT32_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
    INT32_QUAT_NORMALIZE(new_sum_err);
    QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
    INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat);
  } else {
    /* reset accumulator */
    INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
    INT_EULERS_ZERO( stabilization_att_sum_err );
  }

  /* compute the feed forward command */
  attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &stab_att_ref_accel);

  /* compute the feed back command */
  attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);

  /* sum feedforward and feedback */
  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];

  /* bound the result */
  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
Exemple #13
0
/*
 * Compute body orientation and rates from imu orientation and rates
 */
void compute_body_orientation_and_rates(void) {

  /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */

  QUAT_COPY(ahrs_float.ltp_to_body_quat, ahrs_float.ltp_to_imu_quat);
  EULERS_COPY(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_imu_euler);
  RMAT_COPY(ahrs_float.ltp_to_body_rmat, ahrs_float.ltp_to_imu_rmat);
  RATES_COPY(ahrs_float.body_rate, ahrs_float.imu_rate);
}
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight) {

  // FIXME: remove me, do in quaternion directly
  // is currently still needed, since the yaw setpoint integration is done in eulers
#if defined STABILIZATION_ATTITUDE_TYPE_INT
  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
#else
  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight);
#endif

  struct FloatQuat q_rp_cmd;
#if USE_EARTH_BOUND_RC_SETPOINT
  stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd);
#else
  stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd);
#endif

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

  //Care Free mode
  if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) {
    //care_free_heading has been set to current psi when entering care free mode.
    FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, care_free_heading);
  }
  else {
    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);

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

    /* 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);
  }
}
void sim_overwrite_ahrs(void) {

  struct FloatQuat quat_f;
  QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
  stateSetNedToBodyQuat_f(&quat_f);

  struct FloatRates rates_f;
  RATES_COPY(rates_f, fdm.body_ecef_rotvel);
  stateSetBodyRates_f(&rates_f);

}
Exemple #16
0
void ahrs_init(void) {
  ahrs.status = AHRS_UNINIT;
  /* set ltp_to_imu so that body is zero */
  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imuf.body_to_imu);
  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat);
#ifdef IMU_MAG_OFFSET
  ahrs_impl.mag_offset = IMU_MAG_OFFSET;
#else
  ahrs_impl.mag_offset = 0.0;
#endif
  ahrs_aligner.status = AHRS_ALIGNER_LOCKED;
}
/** Read attitude setpoint from RC as quaternion
 * Interprets the stick positions as axes.
 * @param[in]  coordinated_turn  true if in horizontal mode forward
 * @param[in]  in_carefree       true if in carefree mode
 * @param[in]  in_flight         true if in flight
 * @param[out] q_sp              attitude setpoint as quaternion
 */
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree,
    bool_t coordinated_turn)
{

  // FIXME: remove me, do in quaternion directly
  // is currently still needed, since the yaw setpoint integration is done in eulers
#if defined STABILIZATION_ATTITUDE_TYPE_INT
  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#else
  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#endif

  struct FloatQuat q_rp_cmd;
  stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd);

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

  //Care Free mode
  if (in_carefree) {
    //care_free_heading has been set to current psi when entering care free mode.
    float_quat_of_axis_angle(&q_yaw, &zaxis, care_free_heading);
  } else {
    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);

  if (in_flight) {
    /* get current heading setpoint */
    struct FloatQuat q_yaw_sp;
#if defined STABILIZATION_ATTITUDE_TYPE_INT
    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
#else
    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
#endif

    /* 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);
  }
}
void stabilization_attitude_run(bool_t enable_integrator) {

  /*
   * Update reference
   */
  stabilization_attitude_ref_update();

  /*
   * Compute errors for feedback
   */

  /* attitude error                          */
  struct Int32Quat att_err;
  INT32_QUAT_INV_COMP(att_err, ahrs.ltp_to_body_quat, stab_att_ref_quat);
  /* wrap it in the shortest direction       */
  INT32_QUAT_WRAP_SHORTEST(att_err);
  INT32_QUAT_NORMALIZE(att_err);

  /*  rate error                */
  struct Int32Rates rate_err;
  RATES_DIFF(rate_err, ahrs.body_rate, stab_att_ref_rate);

  /* integrated error */
  if (enable_integrator) {
    struct Int32Quat 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;
    INT32_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
    INT32_QUAT_NORMALIZE(new_sum_err);
    QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
    INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat);
  } else {
    /* reset accumulator */
    INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
    INT_EULERS_ZERO( stabilization_att_sum_err );
  }

  attitude_run_ff(stabilization_att_ff_cmd, current_stabilization_gains, &stab_att_ref_accel);

  attitude_run_fb(stabilization_att_fb_cmd, current_stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);

  for (int i = COMMAND_ROLL; i <= COMMAND_YAW; i++) {
    stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i];
     Bound(stabilization_cmd[i], -200, 200);
  }
}
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);
}
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
}
Exemple #21
0
void ahrs_init(void) {
  //ahrs_float.status = AHRS_UNINIT;
  // set to running for now and only use ahrs.status, not ahrs_float.status
  ahrs.status = AHRS_RUNNING;

  ahrs_sim_available = FALSE;

  /* set ltp_to_body to zero */
  FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
  FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
  FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
  FLOAT_RATES_ZERO(ahrs_float.body_rate);

  /* set ltp_to_imu to same as ltp_to_body, currently no difference simulated */
  QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_body_quat);
  EULERS_COPY(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_body_euler);
  RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_body_rmat);
  RATES_COPY(ahrs_float.imu_rate, ahrs_float.body_rate);
}
//Function that reads the rc setpoint in an earth bound frame
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool_t in_flight,
    bool_t in_carefree, bool_t coordinated_turn)
{
  // FIXME: remove me, do in quaternion directly
  // is currently still needed, since the yaw setpoint integration is done in eulers
#if defined STABILIZATION_ATTITUDE_TYPE_INT
  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#else
  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#endif

  const struct FloatVect3 zaxis = {0., 0., 1.};

  struct FloatQuat q_rp_cmd;
  stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd);

  if (in_flight) {
    /* get current heading setpoint */
    struct FloatQuat q_yaw_sp;

#if defined STABILIZATION_ATTITUDE_TYPE_INT
    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
#else
    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
#endif

    float_quat_comp(q_sp, &q_yaw_sp, &q_rp_cmd);
  } else {
    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);

    QUAT_COPY(*q_sp, q_rp_sp);
  }
}
Exemple #23
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);
}
Exemple #24
0
void ahrs_init(void) {
    ahrs.status = AHRS_UNINIT;

    /*
     * Initialises our IMU alignement variables
     * This should probably done in the IMU code instead
     */
    struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
    FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
    FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

    /* set ltp_to_body to zero */
    FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
    FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
    FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
    FLOAT_RATES_ZERO(ahrs_float.body_rate);

    /* set ltp_to_imu so that body is zero */
    QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
    RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
    EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
    FLOAT_RATES_ZERO(ahrs_float.imu_rate);

    /* set inital filter dcm */
    set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);

#if USE_HIGH_ACCEL_FLAG
    high_accel_done = FALSE;
    high_accel_flag = FALSE;
#endif

    ahrs_impl.gps_speed = 0;
    ahrs_impl.gps_acceleration = 0;
    ahrs_impl.gps_course = 0;
    ahrs_impl.gps_course_valid = FALSE;
    ahrs_impl.gps_age = 100;
}
void ahrs_init(void) {
  ahrs.status = AHRS_UNINIT;
  ahrs_impl.ltp_vel_norm_valid = FALSE;
  ahrs_impl.heading_aligned = FALSE;

  /* Initialises IMU alignement */
  struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
  FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
  FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

  /* Set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
  RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);

  FLOAT_RATES_ZERO(ahrs_impl.imu_rate);

  /* set default filter cut-off frequency and damping */
  ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
  ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
  ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
  ahrs_impl.mag_zeta = AHRS_MAG_ZETA;

#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  ahrs_impl.correct_gravity = TRUE;
#else
  ahrs_impl.correct_gravity = FALSE;
#endif

  ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;

  VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);

#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
  register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
//Function that reads the rc setpoint in an earth bound frame
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat* q_sp, bool_t in_flight) {
  // FIXME: remove me, do in quaternion directly
  // is currently still needed, since the yaw setpoint integration is done in eulers
  #if defined STABILIZATION_ATTITUDE_TYPE_INT
  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
  #else
  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight);
  #endif

  const struct FloatVect3 zaxis = {0., 0., 1.};

  struct FloatQuat q_rp_cmd;
  stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd);

  if (in_flight) {
    /* get current heading setpoint */
    struct FloatQuat q_yaw_sp;

    #if defined STABILIZATION_ATTITUDE_TYPE_INT
    FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
    #else
    FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, stab_att_sp_euler.psi);
    #endif

    FLOAT_QUAT_COMP(*q_sp, q_yaw_sp, q_rp_cmd);
  }
  else {
    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);

    QUAT_COPY(*q_sp, q_rp_sp);
  }
}
void ahrs_init(void) {
  ahrs.status = AHRS_UNINIT;

  /* Initialises IMU alignement */
  struct FloatEulers body_to_imu_euler =
    {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
  FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
  FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);

  /* Set ltp_to_body to zero */
  FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
  FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
  FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
  FLOAT_RATES_ZERO(ahrs_float.body_rate);

  /* Set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
  RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
  EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);

  FLOAT_RATES_ZERO(ahrs_float.imu_rate);

}
Exemple #28
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 ahrs_init(void) {
  ahrs.status = AHRS_UNINIT;

  /* set ltp_to_body to zero */
  INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
  INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
  INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
  INT_RATES_ZERO(ahrs.body_rate);

  /* set ltp_to_imu so that body is zero */
  QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
  RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
  INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
  INT_RATES_ZERO(ahrs.imu_rate);

  INT_RATES_ZERO(ahrs_impl.gyro_bias);
  ahrs_impl.reinj_1 = FACE_REINJ_1;

#ifdef IMU_MAG_OFFSET
  ahrs_mag_offset = IMU_MAG_OFFSET;
#else
  ahrs_mag_offset = 0.;
#endif
}