void ahrs_init(void) { ahrs_float.status = AHRS_UNINIT; /* * Initialises our IMU alignement variables * This should probably done in the IMU code instead */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); /* set ltp_to_body to zero */ FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); FLOAT_RATES_ZERO(ahrs_float.body_rate); /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler); FLOAT_RATES_ZERO(ahrs_float.imu_rate); /* set inital filter dcm */ set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; /* Set ltp_to_imu so that body is zero */ memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); FLOAT_RATES_ZERO(ahrs_impl.imu_rate); /* set inital filter dcm */ set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu)); ahrs_impl.gps_speed = 0; ahrs_impl.gps_acceleration = 0; ahrs_impl.gps_course = 0; ahrs_impl.gps_course_valid = FALSE; ahrs_impl.gps_age = 100; }
void ahrs_dcm_init(void) { ahrs_dcm.status = AHRS_DCM_UNINIT; ahrs_dcm.is_aligned = FALSE; /* init ltp_to_imu euler with zero */ FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_imu_euler); FLOAT_RATES_ZERO(ahrs_dcm.imu_rate); /* set inital filter dcm */ set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu)); ahrs_dcm.gps_speed = 0; ahrs_dcm.gps_acceleration = 0; ahrs_dcm.gps_course = 0; ahrs_dcm.gps_course_valid = FALSE; ahrs_dcm.gps_age = 100; }
bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *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 */ ahrs_dcm.gyro_bias = *lp_gyro; ahrs_dcm.status = AHRS_DCM_RUNNING; ahrs_dcm.is_aligned = true; return true; }
void ahrs_align(void) { /* Compute an initial orientation using euler angles */ ahrs_float_get_euler_from_accel_mag(&ahrs_impl.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* Convert initial orientation in quaternion and rotation matrice representations. */ struct FloatRMat ltp_to_imu_rmat; FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler); /* set filter dcm */ set_dcm_matrix_from_rmat(<p_to_imu_rmat); /* Set initial body orientation */ set_body_orientation_and_rates(); /* use 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_init(void) { ahrs.status = AHRS_UNINIT; /* * Initialises our IMU alignement variables * This should probably done in the IMU code instead */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); /* set ltp_to_body to zero */ FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); FLOAT_RATES_ZERO(ahrs_float.body_rate); /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler); FLOAT_RATES_ZERO(ahrs_float.imu_rate); /* set inital filter dcm */ set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); #if USE_HIGH_ACCEL_FLAG high_accel_done = FALSE; high_accel_flag = FALSE; #endif ahrs_impl.gps_speed = 0; ahrs_impl.gps_acceleration = 0; ahrs_impl.gps_course = 0; ahrs_impl.gps_course_valid = FALSE; ahrs_impl.gps_age = 100; }
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; }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; /* * Initialises our IMU alignement variables * This should probably done in the IMU code instead */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); EULERS_COPY(ahrs_impl.ltp_to_imu_euler, body_to_imu_euler); FLOAT_RATES_ZERO(ahrs_impl.imu_rate); /* set inital filter dcm */ set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat); ahrs_impl.gps_speed = 0; ahrs_impl.gps_acceleration = 0; ahrs_impl.gps_course = 0; ahrs_impl.gps_course_valid = FALSE; ahrs_impl.gps_age = 100; }
void Normalize(void) { float error = 0; float temporary[3][3]; float renorm = 0; uint8_t problem = FALSE; // Find the non-orthogonality of X wrt Y error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19 // Add half the XY error to X, and half to Y Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19 // The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z) Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20 // Normalize lenght of X renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]); // a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT // b) if the norm is further from 1, use a real sqrt // c) norm is huge: disaster! reset! mayday! if (renorm < 1.5625f && renorm > 0.64f) { renorm = .5 * (3 - renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { renorm = 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif } else { problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; #endif } Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); // Normalize lenght of Y renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm = .5 * (3 - renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { renorm = 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif } else { problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; #endif } Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); // Normalize lenght of Z renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm = .5 * (3 - renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { renorm = 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif } else { problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; #endif } Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); // Reset on trouble if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu)); problem = FALSE; } }