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);
}
/*
* 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 ahrs_align(void) {
get_phi_theta_measurement_fom_accel(&ahrs_impl.hi_res_euler.phi, &ahrs_impl.hi_res_euler.theta, ahrs_aligner.lp_accel);
get_psi_measurement_from_mag(&ahrs_impl.hi_res_euler.psi,
ahrs_impl.hi_res_euler.phi/F_UPDATE, ahrs_impl.hi_res_euler.theta/F_UPDATE, ahrs_aligner.lp_mag);
EULERS_COPY(ahrs_impl.measure, ahrs_impl.hi_res_euler);
EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler);
/* Compute LTP to IMU eulers */
EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);
compute_imu_quat_and_rmat_from_euler();
compute_body_orientation();
RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
ahrs.status = AHRS_RUNNING;
}
bool ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
get_phi_theta_measurement_fom_accel(&ahrs_ice.hi_res_euler.phi,
&ahrs_ice.hi_res_euler.theta, lp_accel);
get_psi_measurement_from_mag(&ahrs_ice.hi_res_euler.psi,
ahrs_ice.hi_res_euler.phi / F_UPDATE,
ahrs_ice.hi_res_euler.theta / F_UPDATE, lp_mag);
EULERS_COPY(ahrs_ice.measure, ahrs_ice.hi_res_euler);
EULERS_COPY(ahrs_ice.measurement, ahrs_ice.hi_res_euler);
/* Compute LTP to IMU eulers */
EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE);
RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro);
ahrs_ice.status = AHRS_ICE_RUNNING;
ahrs_ice.is_aligned = true;
return true;
}
void stabilization_attitude_ref_update()
{
#if USE_REF
/* dumb integrate reference attitude */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE);
struct FloatEulers delta_angle;
EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
EULERS_ADD(stab_att_ref_euler, delta_angle);
FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi);
/* integrate reference rotational speeds */
struct FloatRates delta_accel;
RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE);
RATES_ADD(stab_att_ref_rate, delta_accel);
/* compute reference attitude error */
struct FloatEulers ref_err;
EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler);
/* wrap it in the shortest direction */
FLOAT_ANGLE_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */
stab_att_ref_accel.p = -2.*ZETA_P * OMEGA_P * stab_att_ref_rate.p - OMEGA_P * OMEGA_P * ref_err.phi;
stab_att_ref_accel.q = -2.*ZETA_Q * OMEGA_P * stab_att_ref_rate.q - OMEGA_Q * OMEGA_Q * ref_err.theta;
stab_att_ref_accel.r = -2.*ZETA_R * OMEGA_P * stab_att_ref_rate.r - OMEGA_R * OMEGA_R * ref_err.psi;
/* 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 speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */
EULERS_COPY(stab_att_ref_euler, stabilization_att_sp);
FLOAT_RATES_ZERO(stab_att_ref_rate);
FLOAT_RATES_ZERO(stab_att_ref_accel);
#endif /* USE_REF */
}
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);
}
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;
/* 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);
}
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_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
EULERS_COPY(ahrs_impl.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat);
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;
}