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 {
