float float_rmat_reorthogonalize(struct FloatRMat *rm) { const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0, 0), RMAT_ELMT(*rm, 0, 1), RMAT_ELMT(*rm, 0, 2) }; const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1, 0), RMAT_ELMT(*rm, 1, 1), RMAT_ELMT(*rm, 1, 2) }; float _err = -0.5 * VECT3_DOT_PRODUCT(r0, r1); struct FloatVect3 r0_t; VECT3_SUM_SCALED(r0_t, r0, r1, _err); struct FloatVect3 r1_t; VECT3_SUM_SCALED(r1_t, r1, r0, _err); struct FloatVect3 r2_t; VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t); float s = renorm_factor(VECT3_NORM2(r0_t)); MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s); s = renorm_factor(VECT3_NORM2(r1_t)); MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s); s = renorm_factor(VECT3_NORM2(r2_t)); MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s); return _err; }
void ahrs_update_accel(void) { #if USE_NOISE_CUT || USE_NOISE_FILTER static struct Int32Vect3 last_accel = { 0, 0, 0 }; #endif #if USE_NOISE_CUT if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) { #endif #if USE_NOISE_FILTER VECT3_SUM_SCALED(imu.accel, imu.accel, last_accel, NOISE_FILTER_GAIN); VECT3_SDIV(imu.accel, imu.accel, NOISE_FILTER_GAIN+1); #endif get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel); #if USE_NOISE_CUT } VECT3_COPY(last_accel, imu.accel); #endif }