void ahrs_update_accel(void) { // c2 = ltp z-axis in imu-frame struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2), RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2), RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)}; struct Int32Vect3 residual; struct Int32Vect3 pseudo_gravity_measurement; if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame * a_c_body = omega x (omega x r) * (omega x r) = tangential velocity in body frame * a_c_body = omega x vel_tangential_body * assumption: tangential velocity only along body x-axis */ // FIXME: check overflows ! const struct Int32Vect3 vel_tangential_body = {(ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC), 0.0, 0.0}; struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs.body_rate, vel_tangential_body); INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC); /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu); } else {
/** Convert a local ENU position to ECEF. * @param[out] ecef ECEF position in cm * @param[in] def local coordinate system definition * @param[in] enu ENU position in meter << #INT32_POS_FRAC */ void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu) { /* enu_cm = (enu * 100) >> INT32_POS_FRAC * to loose less range: * enu_cm = (enu * 25) >> (INT32_POS_FRAC-2) * which puts max enu input Q23.8 range to 8388km / 25 = 335km */ struct EnuCoor_i enu_cm; VECT3_SMUL(enu_cm, *enu, 25); INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC - 2); ecef_of_enu_vect_i(ecef, def, &enu_cm); VECT3_ADD(*ecef, def->ecef); }
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt) { // check if we had at least one propagation since last update if (ahrs_icq.accel_cnt == 0) { return; } // c2 = ltp z-axis in imu-frame struct Int32RMat ltp_to_imu_rmat; int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat); struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0, 2), RMAT_ELMT(ltp_to_imu_rmat, 1, 2), RMAT_ELMT(ltp_to_imu_rmat, 2, 2) }; struct Int32Vect3 residual; struct Int32Vect3 pseudo_gravity_measurement; if (ahrs_icq.correct_gravity && ahrs_icq.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame * a_c_body = omega x (omega x r) * (omega x r) = tangential velocity in body frame * a_c_body = omega x vel_tangential_body * assumption: tangential velocity only along body x-axis */ // FIXME: check overflows ! #define COMPUTATION_FRAC 16 #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC const struct Int32Vect3 vel_tangential_body = {ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); struct Int32Rates body_rate; int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate); struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body); INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC); /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement * of the gravity vector */ VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_imu); } else {