static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_body_quat);
RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.body_rate);
#else
QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_imu_quat);
RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.imu_rate);
#endif /* OUTPUT_IN_BODY_FRAME */
RATES_ASSIGN(output[i].bias_est, 0., 0., 0.);
// memset(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
}
static void store_filter_output(int i) {
#ifdef OUTPUT_IN_BODY_FRAME
QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.body_rate);
#else
struct FloatEulers eul_f;
EULERS_FLOAT_OF_BFP(eul_f, ahrs_impl.ltp_to_imu_euler);
FLOAT_QUAT_OF_EULERS(output[i].quat_est, eul_f);
RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.imu_rate);
#endif /* OUTPUT_IN_BODY_FRAME */
RATES_ASSIGN(output[i].bias_est, 0., 0., 0.);
// memset(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
}
void ahrs_propagate(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
const float dt = 1./512.;
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction);
// DISPLAY_FLOAT_RATES("omega ", omega);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_impl.rate_correction);
/* first order integration of rotation matrix */
struct FloatRMat exp_omega_dt = {
{ 1. , dt*omega.r, -dt*omega.q,
-dt*omega.r, 1. , dt*omega.p,
dt*omega.q, -dt*omega.p, 1. }};
struct FloatRMat R_tdt;
FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt);
memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt));
float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat);
// struct FloatRMat test;
// FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat);
// DISPLAY_FLOAT_RMAT("foo", test);
compute_imu_quat_and_euler_from_rmat();
compute_body_orientation_and_rates();
}
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
{
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
&gyro_float.p, &gyro_float.q, &gyro_float.r);
}
bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_imu_quat, lp_accel, lp_mag);
ahrs_fc.heading_aligned = TRUE;
#else
/* Compute an initial orientation from accel and just set heading to zero */
ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_imu_quat, lp_accel);
ahrs_fc.heading_aligned = FALSE;
#endif
/* Convert initial orientation from quat to rotation matrix representations. */
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, *lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_fc.gyro_bias, bias0);
ahrs_fc.status = AHRS_FC_RUNNING;
ahrs_fc.is_aligned = TRUE;
return TRUE;
}
void ahrs_propagate(void)
{
/* convert imu data to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
/* unbias rate measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
/* Uncouple Motions */
#ifdef IMU_GYRO_P_Q
float dp=0,dq=0,dr=0;
dp += ahrs_float.imu_rate.q * IMU_GYRO_P_Q;
dp += ahrs_float.imu_rate.r * IMU_GYRO_P_R;
dq += ahrs_float.imu_rate.p * IMU_GYRO_Q_P;
dq += ahrs_float.imu_rate.r * IMU_GYRO_Q_R;
dr += ahrs_float.imu_rate.p * IMU_GYRO_R_P;
dr += ahrs_float.imu_rate.q * IMU_GYRO_R_Q;
ahrs_float.imu_rate.p += dp;
ahrs_float.imu_rate.q += dq;
ahrs_float.imu_rate.r += dr;
#endif
Matrix_update();
// INFO, ahrs struct only updated in ahrs_update_fw_estimator
Normalize();
}
static void dialog_with_io_proc() {
struct AutopilotMessageCRCFrame msg_in;
struct AutopilotMessageCRCFrame msg_out;
uint8_t crc_valid;
for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i];
// for (uint8_t i=0; i<4; i++) msg_out.payload.msg_down.csc_servo_cmd[i] = otp.csc_servo_outputs[i];
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
struct AutopilotMessagePTUp *in = &msg_in.payload.msg_up;
RATES_FLOAT_OF_BFP(otp.imu.gyro, in->gyro);
ACCELS_FLOAT_OF_BFP(otp.imu.accel, in->accel);
MAGS_FLOAT_OF_BFP(otp.imu.mag, in->mag);
otp.io_proc_msg_cnt = in->stm_msg_cnt;
otp.io_proc_err_cnt = in->stm_crc_err_cnt;
otp.rc_status = in->rc_status;
otp.baro_abs = in->pressure_absolute;
otp.baro_diff = in->pressure_differential;
}
void ahrs_align(void) {
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
ahrs_impl.heading_aligned = TRUE;
#else
/* Compute an initial orientation from accel and just set heading to zero */
ahrs_float_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel);
ahrs_impl.heading_aligned = FALSE;
#endif
/* Convert initial orientation from quat to rotation matrix representations. */
FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
/* Compute initial body orientation */
compute_body_orientation_and_rates();
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
}
void ahrs_propagate(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_impl.imu_rate,gyro_float);
#endif
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_impl.rate_correction);
const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
#if AHRS_PROPAGATE_RMAT
FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt);
float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat);
FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt);
FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
#endif
compute_body_orientation_and_rates();
}
void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt)
{
/* convert imu data to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, *gyro);
/* unbias rate measurement */
RATES_DIFF(ahrs_dcm.imu_rate, gyro_float, ahrs_dcm.gyro_bias);
/* Uncouple Motions */
#ifdef IMU_GYRO_P_Q
float dp = 0, dq = 0, dr = 0;
dp += ahrs_dcm.imu_rate.q * IMU_GYRO_P_Q;
dp += ahrs_dcm.imu_rate.r * IMU_GYRO_P_R;
dq += ahrs_dcm.imu_rate.p * IMU_GYRO_Q_P;
dq += ahrs_dcm.imu_rate.r * IMU_GYRO_Q_R;
dr += ahrs_dcm.imu_rate.p * IMU_GYRO_R_P;
dr += ahrs_dcm.imu_rate.q * IMU_GYRO_R_Q;
ahrs_dcm.imu_rate.p += dp;
ahrs_dcm.imu_rate.q += dq;
ahrs_dcm.imu_rate.r += dr;
#endif
Matrix_update(dt);
Normalize();
compute_ahrs_representations();
}
void stateCalcBodyRates_f(void) {
if (bit_is_set(state.rate_status, RATE_F))
return;
if (bit_is_set(state.rate_status, RATE_I)) {
RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_i);
}
/* set bit to indicate this representation is computed */
SetBit(state.rate_status, RATE_F);
}
void ahrs_align(void)
{
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* use average gyro as initial value for bias */
struct FloatRates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0);
// ins and ahrs are now running
ahrs.status = AHRS_RUNNING;
ins.status = INS_RUNNING;
}
void ahrs_align(void) {
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* set initial body orientation */
set_body_state_from_quat();
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
}
void print_raw_log_entry(struct raw_log_entry* e){
struct DoubleVect3 tempvect;
struct DoubleRates temprates;
printf("%f\t", e->time);
printf("%i\t", e->message.valid_sensors);
RATES_FLOAT_OF_BFP(temprates, e->message.gyro);
printf("% f % f % f\t", temprates.p, temprates.q, temprates.r);
ACCELS_FLOAT_OF_BFP(tempvect, e->message.accel);
printf("% f % f % f\t", tempvect.x, tempvect.y, tempvect.z);
MAGS_FLOAT_OF_BFP(tempvect, e->message.mag);
printf("% f % f % f\t", tempvect.x, tempvect.y, tempvect.z);
printf("% i % i % i\t", e->message.ecef_pos.x, e->message.ecef_pos.y, e->message.ecef_pos.z);
printf("% i % i % i\t", e->message.ecef_vel.x, e->message.ecef_vel.y, e->message.ecef_vel.z);
double baro_scaled = (double)(e->message.pressure_absolute)/256;
printf("%f", baro_scaled);
}
void ahrs_update_fw_estimator(void)
{
struct FloatEulers att;
// export results to estimator
EULERS_FLOAT_OF_BFP(att, ahrs.ltp_to_body_euler);
estimator_phi = att.phi - ins_roll_neutral;
estimator_theta = att.theta - ins_pitch_neutral;
estimator_psi = att.psi;
struct FloatRates rates;
RATES_FLOAT_OF_BFP(rates, ahrs.body_rate);
estimator_p = rates.p;
estimator_q = rates.q;
estimator_r = rates.r;
}
void ahrs_align(void) {
/* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
/* Compute initial body orientation */
compute_body_orientation_and_rates();
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
}
void ahrs_propagate(void) {
/* compute unbiased rates */
RATES_FLOAT_OF_BFP(bafe_rates, imu.gyro);
RATES_SUB(bafe_rates, bafe_bias);
/* compute F
F is only needed later on to update the state covariance P.
However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
compute the time derivative of the quaternion, so we compute F now
*/
/* Fill in Wxq(pqr) into F */
bafe_F[0][0] = bafe_F[1][1] = bafe_F[2][2] = bafe_F[3][3] = 0;
bafe_F[1][0] = bafe_F[2][3] = bafe_rates.p * 0.5;
bafe_F[2][0] = bafe_F[3][1] = bafe_rates.q * 0.5;
bafe_F[3][0] = bafe_F[1][2] = bafe_rates.r * 0.5;
bafe_F[0][1] = bafe_F[3][2] = -bafe_F[1][0];
bafe_F[0][2] = bafe_F[1][3] = -bafe_F[2][0];
bafe_F[0][3] = bafe_F[2][1] = -bafe_F[3][0];
/* Fill in [4:6][0:3] region */
bafe_F[0][4] = bafe_F[2][6] = bafe_quat.qx * 0.5;
bafe_F[0][5] = bafe_F[3][4] = bafe_quat.qy * 0.5;
bafe_F[0][6] = bafe_F[1][5] = bafe_quat.qz * 0.5;
bafe_F[1][4] = bafe_F[2][5] = bafe_F[3][6] = bafe_quat.qi * -0.5;
bafe_F[3][5] = -bafe_F[0][4];
bafe_F[1][6] = -bafe_F[0][5];
bafe_F[2][4] = -bafe_F[0][6];
/* quat_dot = Wxq(pqr) * quat */
bafe_qdot.qi= bafe_F[0][1]*bafe_quat.qx+bafe_F[0][2]*bafe_quat.qy+bafe_F[0][3] * bafe_quat.qz;
bafe_qdot.qx= bafe_F[1][0]*bafe_quat.qi +bafe_F[1][2]*bafe_quat.qy+bafe_F[1][3] * bafe_quat.qz;
bafe_qdot.qy= bafe_F[2][0]*bafe_quat.qi+bafe_F[2][1]*bafe_quat.qx +bafe_F[2][3] * bafe_quat.qz;
bafe_qdot.qz= bafe_F[3][0]*bafe_quat.qi+bafe_F[3][1]*bafe_quat.qx+bafe_F[3][2]*bafe_quat.qy ;
/* propagate quaternion */
bafe_quat.qi += bafe_qdot.qi * BAFE_DT;
bafe_quat.qx += bafe_qdot.qx * BAFE_DT;
bafe_quat.qy += bafe_qdot.qy * BAFE_DT;
bafe_quat.qz += bafe_qdot.qz * BAFE_DT;
}
static inline void propagate_ref(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
/* lowpass angular rates */
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate,
(1.-alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_impl.imu_rate, gyro_float);
#endif
/* propagate reference quaternion only if rate is non null */
const float no = FLOAT_RATES_NORM(ahrs_impl.imu_rate);
if (no > FLT_MIN) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
const float a = 0.5*no*dt;
const float ca = cosf(a);
const float sa_ov_no = sinf(a)/no;
const float dp = sa_ov_no*ahrs_impl.imu_rate.p;
const float dq = sa_ov_no*ahrs_impl.imu_rate.q;
const float dr = sa_ov_no*ahrs_impl.imu_rate.r;
const float qi = ahrs_impl.ltp_to_imu_quat.qi;
const float qx = ahrs_impl.ltp_to_imu_quat.qx;
const float qy = ahrs_impl.ltp_to_imu_quat.qy;
const float qz = ahrs_impl.ltp_to_imu_quat.qz;
ahrs_impl.ltp_to_imu_quat.qi = ca*qi - dp*qx - dq*qy - dr*qz;
ahrs_impl.ltp_to_imu_quat.qx = dp*qi + ca*qx + dr*qy - dq*qz;
ahrs_impl.ltp_to_imu_quat.qy = dq*qi - dr*qx + ca*qy + dp*qz;
ahrs_impl.ltp_to_imu_quat.qz = dr*qi + dq*qx - dp*qy + ca*qz;
// printf("%f\n", ahrs_impl.ltp_to_imu_quat.qi);
}
}
static inline void propagate_ref(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
/* lowpass angular rates */
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate,
(1.-alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_impl.imu_rate, gyro_float);
#endif
/* propagate reference quaternion */
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, ahrs_impl.imu_rate, dt);
}
void ins_reset_altitude_ref(void)
{
#if INS_UPDATE_FW_ESTIMATOR
struct UtmCoor_f utm = state.utm_origin_f;
utm.alt = gps.hmsl / 1000.0f;
stateSetLocalUtmOrigin_f(&utm);
#else
struct LlaCoor_i lla = {
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
struct LtpDef_i ltp_def;
ltp_def_from_lla_i(<p_def, &lla);
ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(<p_def);
#endif
}
void ahrs_init(void)
{
ahrs.status = AHRS_UNINIT;
}
void ahrs_align(void)
{
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* use average gyro as initial value for bias */
struct FloatRates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0);
// ins and ahrs are now running
ahrs.status = AHRS_RUNNING;
ins.status = INS_RUNNING;
}
bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
/* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_dcm.ltp_to_imu_euler, lp_accel, lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
struct FloatRMat ltp_to_imu_rmat;
float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
/* set filter dcm */
set_dcm_matrix_from_rmat(<p_to_imu_rmat);
/* use averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, *lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_dcm.gyro_bias, bias0);
ahrs_dcm.status = AHRS_DCM_RUNNING;
ahrs_dcm.is_aligned = TRUE;
return TRUE;
}
/**
* copy imu.gyro into gyro_float
* compute imu_rate without bias
* scale values to [rad/s]
* Update Matrix
* Normalize
*/
void ahrs_propagate(void)
{
struct FloatRates gyro_float;
/* convert bfp imu data to floating point */
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); // olri div by (1<<12)
/* unbias rate measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
// scale gyro adc raw to [rad/s] FIXME // olri
ahrs_float.imu_rate.p /= 61.35f;
ahrs_float.imu_rate.q /= 57.96f;
ahrs_float.imu_rate.r /= 60.10f;
/* Update Matrix */
Matrix_update();
/* Normalize */
Normalize();
// INFO, ahrs struct only updated in ahrs_update_fw_estimator
}
void ahrs_fc_propagate(struct Int32Rates *gyro, float dt)
{
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, *gyro);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_fc.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_fc.imu_rate, gyro_float);
#endif
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, gyro_float, ahrs_fc.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_fc.rate_correction);
#if AHRS_PROPAGATE_RMAT
float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt);
float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat);
float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt);
float_quat_normalize(&ahrs_fc.ltp_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
#endif
// increase accel and mag propagation counters
ahrs_fc.accel_cnt++;
ahrs_fc.mag_cnt++;
}
/*
* Propagate our dynamic system in time
*
* Run the strapdown computation and the predict step of the filter
*
*
* quat_dot = Wxq(pqr) * quat
* bias_dot = 0
*
* Wxq is the quaternion omega matrix:
*
* [ 0, -p, -q, -r ]
* 1/2 * [ p, 0, r, -q ]
* [ q, -r, 0, p ]
* [ r, q, -p, 0 ]
*
*/
void ahrs_propagate(void) {
int i, j, k;
ahrs_lowpass_accel();
/* compute unbiased rates */
RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
RATES_SUB(bafl_rates, bafl_bias);
/* run strapdown computation
*
*/
/* multiplicative quaternion update */
/* compute correction quaternion */
QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q * BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2);
/* normalize it. maybe not necessary?? */
float q_sq;
q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4;
if (q_sq > 1) { /* this should actually never happen */
FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
} else {
bafl_qr.qi = sqrtf(1 - q_sq);
}
/* propagate correction quaternion */
FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr);
FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
//TODO check if broot force normalization is good, use lagrange normalization?
FLOAT_QUAT_NORMALISE(bafl_quat);
/*
* compute all representations
*/
/* maintain rotation matrix representation */
FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
/* maintain euler representation */
FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
AHRS_TO_BFP();
AHRS_LTP_TO_BODY();
/* error KF-Filter
* propagate only the error covariance matrix since error is corrected after each measurement
*
* F = [ 0 0 0 ]
* [ 0 0 0 -Cbn ]
* [ 0 0 0 ]
* [ 0 0 0 0 0 0 ]
* [ 0 0 0 0 0 0 ]
* [ 0 0 0 0 0 0 ]
*
* T = e^(dt * F)
*
* T = [ 1 0 0 ]
* [ 0 1 0 -Cbn*dt ]
* [ 0 0 1 ]
* [ 0 0 0 1 0 0 ]
* [ 0 0 0 0 1 0 ]
* [ 0 0 0 0 0 1 ]
*
* P_prio = T * P * T_T + Q
*
*/
/*
* compute state transition matrix T
*
* diagonal elements of T are always one
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
}
}
/*
* estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q
*/
/* Temporary multiplication temp(6x6) = T(6x6) * P(6x6) */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_tempP[i][j] = 0.;
for (k = 0; k < BAFL_SSIZE; k++) {
bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
}
}
}
/* P_k(6x6) = temp(6x6) * T_T(6x6) + Q */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_P[i][j] = 0.;
for (k = 0; k < BAFL_SSIZE; k++) {
bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j]
}
}
if (i<3) {
bafl_P[i][i] += bafl_Q_att;
} else {
bafl_P[i][i] += bafl_Q_gyro;
}
}
#ifdef LKF_PRINT_P
printf("Pp =");
for (i = 0; i < 6; i++) {
printf("[");
for (j = 0; j < 6; j++) {
printf("%f\t", bafl_P[i][j]);
}
printf("]\n ");
}
printf("\n");
#endif
}
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
}
static void report(void) {
int output_sensors = FALSE;
int output_pos = FALSE;
printf("%f ", aos.time);
printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi),
DegOfRad(aos.ltp_to_imu_euler.theta),
DegOfRad(aos.ltp_to_imu_euler.psi));
printf("%f %f %f ", DegOfRad(aos.imu_rates.p),
DegOfRad(aos.imu_rates.q),
DegOfRad(aos.imu_rates.r));
printf("%f %f %f ", DegOfRad(aos.gyro_bias.p),
DegOfRad(aos.gyro_bias.q),
DegOfRad(aos.gyro_bias.r));
#if AHRS_TYPE == AHRS_TYPE_ICQ
struct Int32Eulers ltp_to_imu_euler_i;
int32_eulers_of_quat(<p_to_imu_euler_i, &ahrs_impl.ltp_to_imu_quat);
struct FloatEulers ltp_to_imu_euler_f;
EULERS_FLOAT_OF_BFP(ltp_to_imu_euler_f, ltp_to_imu_euler_i);
printf("%f %f %f ", DegOfRad(ltp_to_imu_euler_f.phi),
DegOfRad(ltp_to_imu_euler_f.theta),
DegOfRad(ltp_to_imu_euler_f.psi));
struct FloatRates imu_rate_f;
RATES_FLOAT_OF_BFP(imu_rate_f, ahrs_impl.imu_rate);
printf("%f %f %f ", DegOfRad(imu_rate_f.p),
DegOfRad(imu_rate_f.q),
DegOfRad(imu_rate_f.r));
struct FloatRates bias;
RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias);
printf("%f %f %f ", DegOfRad(bias.p),
DegOfRad(bias.q),
DegOfRad(bias.r));
#elif AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR
printf("%f %f %f ", DegOfRad(ahrs_impl.ltp_to_imu_euler.phi),
DegOfRad(ahrs_impl.ltp_to_imu_euler.theta),
DegOfRad(ahrs_impl.ltp_to_imu_euler.psi));
printf("%f %f %f ", DegOfRad(ahrs_impl.imu_rate.p),
DegOfRad(ahrs_impl.imu_rate.q),
DegOfRad(ahrs_impl.imu_rate.r));
printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p),
DegOfRad(ahrs_impl.gyro_bias.q),
DegOfRad(ahrs_impl.gyro_bias.r));
#endif
if (output_pos) {
printf("%f %f %f ", aos.ltp_pos.x,
aos.ltp_pos.y,
aos.ltp_pos.z);
}
if (output_sensors) {
}
printf("\n");
}
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 ahrs_align(void) {
RATES_FLOAT_OF_BFP(bafl_bias, ahrs_aligner.lp_gyro);
ACCELS_FLOAT_OF_BFP(bafl_accel_measure, ahrs_aligner.lp_accel);
ahrs.status = AHRS_RUNNING;
}
static void send_gyro(void) {
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice,
&gyro_float.p, &gyro_float.q, &gyro_float.r);
}