void ahrs_update_mag_2d(void) { const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y}; struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); struct FloatVect3 measured_ltp; FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_float.ltp_to_imu_rmat, measured_imu); const struct FloatVect3 residual_ltp = { 0, 0, measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x }; // printf("res : %f\n", residual_ltp.z); struct FloatVect3 residual_imu; FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp); const float mag_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); const float mag_bias_update_gain = -2.5e-3; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); }
void ins_propagate() { /* untilt accels and speeds */ FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel); FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed); //Add g to the accelerations ins_impl.ltp_accel.z += 9.81; //Save the accelerations and speeds stateSetAccelNed_f(&ins_impl.ltp_accel); stateSetSpeedNed_f(&ins_impl.ltp_speed); //Don't set the height if we use the one from the gps #if !USE_GPS_HEIGHT //Set the height and save the position ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; stateSetPositionNed_i(&ins_impl.ltp_pos); #endif }
void ahrs_update_mag_2d(void) { struct FloatVect2 expected_ltp; VECT2_COPY(expected_ltp, ahrs_impl.mag_h); // normalize expected ltp in 2D (x,y) FLOAT_VECT2_NORMALIZE(expected_ltp); struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); struct FloatVect3 measured_ltp; FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu); struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y}; // normalize measured ltp in 2D (x,y) FLOAT_VECT2_NORMALIZE(measured_ltp_2d); const struct FloatVect3 residual_ltp = { 0, 0, measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x }; // printf("res : %f\n", residual_ltp.z); struct FloatVect3 residual_imu; FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp); /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY */ const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY */ const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); }