void imu_init(void) { /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); /* Compute quaternion and rotation matrix for conversions between body and imu frame */ #if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALISE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #else INT32_QUAT_ZERO(imu.body_to_imu_quat); INT32_RMAT_ZERO(imu.body_to_imu_rmat); #endif imu_impl_init(); }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; /* set ltp_to_body to zero */ INT_EULERS_ZERO(ahrs.ltp_to_body_euler); INT32_QUAT_ZERO(ahrs.ltp_to_body_quat); INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat); INT_RATES_ZERO(ahrs.body_rate); /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat); INT_RATES_ZERO(ahrs.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.high_rez_bias); #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; #else ahrs_impl.correct_gravity = FALSE; #endif #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC ahrs_impl.use_gravity_heuristic = TRUE; #else ahrs_impl.use_gravity_heuristic = FALSE; #endif }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; // FIXME: make ltp_to_imu and ltp_to_body coherent INT_EULERS_ZERO(ahrs.ltp_to_body_euler); INT_EULERS_ZERO(ahrs.ltp_to_imu_euler); INT32_QUAT_ZERO(ahrs.ltp_to_body_quat); INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat); INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat); INT_RATES_ZERO(ahrs.body_rate); INT_RATES_ZERO(ahrs.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.high_rez_bias); }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; /* set ltp_to_body to zero */ INT_EULERS_ZERO(ahrs.ltp_to_body_euler); INT32_QUAT_ZERO(ahrs.ltp_to_body_quat); INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat); INT_RATES_ZERO(ahrs.body_rate); /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat); INT_RATES_ZERO(ahrs.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); ahrs_impl.reinj_1 = FACE_REINJ_1; #ifdef IMU_MAG_OFFSET ahrs_mag_offset = IMU_MAG_OFFSET; #else ahrs_mag_offset = 0.; #endif }