void ahrs_propagate(void) { /* unbias gyro */ struct Int32Rates uf_rate; RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias); #if USE_NOISE_CUT static struct Int32Rates last_uf_rate = { 0, 0, 0 }; if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) { #endif /* low pass rate */ #if USE_NOISE_FILTER RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN); RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1); #else RATES_ADD(ahrs.imu_rate, uf_rate); RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2); #endif #if USE_NOISE_CUT } RATES_COPY(last_uf_rate, uf_rate); #endif /* integrate eulers */ struct Int32Eulers euler_dot; INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate); EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot); /* low pass measurement */ EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement); EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2); /* compute residual */ EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.hi_res_euler); INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi); struct Int32Eulers correction; /* compute a correction */ EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1); /* correct estimation */ EULERS_ADD(ahrs_impl.hi_res_euler, correction); INTEG_EULER_NORMALIZE(ahrs_impl.hi_res_euler.psi); /* Compute LTP to IMU eulers */ EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); compute_imu_quat_and_rmat_from_euler(); compute_body_orientation(); }
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt) { int32_t freq = (int32_t)(1. / dt); /* unbias gyro */ struct Int32Rates omega; RATES_DIFF(omega, *gyro, ahrs_icq.gyro_bias); /* low pass rate */ #ifdef AHRS_PROPAGATE_LOW_PASS_RATES RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 2); RATES_ADD(ahrs_icq.imu_rate, omega); RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 3); #else RATES_COPY(ahrs_icq.imu_rate, omega); #endif /* add correction */ RATES_ADD(omega, ahrs_icq.rate_correction); /* and zeros it */ INT_RATES_ZERO(ahrs_icq.rate_correction); /* integrate quaternion */ int32_quat_integrate_fi(&ahrs_icq.ltp_to_imu_quat, &ahrs_icq.high_rez_quat, &omega, freq); int32_quat_normalize(&ahrs_icq.ltp_to_imu_quat); // increase accel and mag propagation counters ahrs_icq.accel_cnt++; ahrs_icq.mag_cnt++; }
void ahrs_propagate(void) { /* unbias gyro */ struct Int32Rates omega; RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias); /* low pass rate */ #ifdef AHRS_PROPAGATE_LOW_PASS_RATES RATES_SMUL(ahrs.imu_rate, ahrs.imu_rate,2); RATES_ADD(ahrs.imu_rate, omega); RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 3); #else RATES_COPY(ahrs.imu_rate, omega); #endif /* add correction */ RATES_ADD(omega, ahrs_impl.rate_correction); /* and zeros it */ INT_RATES_ZERO(ahrs_impl.rate_correction); /* integrate quaternion */ INT32_QUAT_INTEGRATE_FI(ahrs.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY); INT32_QUAT_NORMALIZE(ahrs.ltp_to_imu_quat); compute_imu_euler_and_rmat_from_quat(); compute_body_orientation(); }
void ahrs_propagate(void) { /* unbias gyro */ struct Int32Rates omega; RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias); /* low pass rate */ //#ifdef AHRS_PROPAGATE_LOW_PASS_RATES if (gyro_lowpass_filter > 1) { RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter-1); RATES_ADD(ahrs_impl.imu_rate, omega); RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter); //#else } else { RATES_COPY(ahrs_impl.imu_rate, omega); //#endif } /* add correction */ RATES_ADD(omega, ahrs_impl.rate_correction); /* and zeros it */ INT_RATES_ZERO(ahrs_impl.rate_correction); /* integrate quaternion */ INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY); INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat); set_body_state_from_quat(); }
void stabilization_rate_run(bool_t in_flight) { /* reference */ struct Int32Rates _r; RATES_DIFF(_r, stabilization_rate_sp, stabilization_rate_ref); RATES_SDIV(stabilization_rate_refdot, _r, STABILIZATION_RATE_REF_TAU); /* integrate ref */ const struct Int32Rates _delta_ref = { stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), stabilization_rate_refdot.q >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), stabilization_rate_refdot.r >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC)}; RATES_ADD(stabilization_rate_ref, _delta_ref); /* compute feed-forward command */ RATES_EWMULT_RSHIFT(stabilization_rate_ff_cmd, stabilization_rate_ddgain, stabilization_rate_refdot, 9); /* compute feed-back command */ /* error for feedback */ const struct Int32Rates _ref_scaled = { OFFSET_AND_ROUND(stabilization_rate_ref.p, (REF_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) }; struct Int32Rates _error; struct Int32Rates* body_rate = stateGetBodyRates_i(); RATES_DIFF(_error, _ref_scaled, (*body_rate)); if (in_flight) { /* update integrator */ RATES_ADD(stabilization_rate_sum_err, _error); RATES_BOUND_CUBE(stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR); } else { INT_RATES_ZERO(stabilization_rate_sum_err); } /* PI */ stabilization_rate_fb_cmd.p = stabilization_rate_gain.p * _error.p + OFFSET_AND_ROUND2((stabilization_rate_igain.p * stabilization_rate_sum_err.p), 10); stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q + OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 10); stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r + OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 10); stabilization_rate_fb_cmd.p = stabilization_rate_fb_cmd.p >> 11; stabilization_rate_fb_cmd.q = stabilization_rate_fb_cmd.q >> 11; stabilization_rate_fb_cmd.r = stabilization_rate_fb_cmd.r >> 11; /* sum to final command */ stabilization_cmd[COMMAND_ROLL] = stabilization_rate_ff_cmd.p + stabilization_rate_fb_cmd.p; stabilization_cmd[COMMAND_PITCH] = stabilization_rate_ff_cmd.q + stabilization_rate_fb_cmd.q; stabilization_cmd[COMMAND_YAW] = stabilization_rate_ff_cmd.r + stabilization_rate_fb_cmd.r; /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }
void ahrs_ice_propagate(struct Int32Rates *gyro) { /* unbias gyro */ struct Int32Rates uf_rate; RATES_DIFF(uf_rate, *gyro, ahrs_ice.gyro_bias); #if USE_NOISE_CUT static struct Int32Rates last_uf_rate = { 0, 0, 0 }; if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) { #endif /* low pass rate */ #if USE_NOISE_FILTER RATES_SUM_SCALED(ahrs_ice.imu_rate, ahrs_ice.imu_rate, uf_rate, NOISE_FILTER_GAIN); RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, NOISE_FILTER_GAIN + 1); #else RATES_ADD(ahrs_ice.imu_rate, uf_rate); RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, 2); #endif #if USE_NOISE_CUT } RATES_COPY(last_uf_rate, uf_rate); #endif /* integrate eulers */ struct Int32Eulers euler_dot; int32_eulers_dot_of_rates(&euler_dot, &ahrs_ice.ltp_to_imu_euler, &ahrs_ice.imu_rate); EULERS_ADD(ahrs_ice.hi_res_euler, euler_dot); /* low pass measurement */ EULERS_ADD(ahrs_ice.measure, ahrs_ice.measurement); EULERS_SDIV(ahrs_ice.measure, ahrs_ice.measure, 2); /* compute residual */ EULERS_DIFF(ahrs_ice.residual, ahrs_ice.measure, ahrs_ice.hi_res_euler); INTEG_EULER_NORMALIZE(ahrs_ice.residual.psi); struct Int32Eulers correction; /* compute a correction */ EULERS_SDIV(correction, ahrs_ice.residual, ahrs_ice.reinj_1); /* correct estimation */ EULERS_ADD(ahrs_ice.hi_res_euler, correction); INTEG_EULER_NORMALIZE(ahrs_ice.hi_res_euler.psi); /* Compute LTP to IMU eulers */ EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE); }
//进行ahrs校准器的运行, void ahrs_aligner_run(void) { RATES_ADD(gyro_sum, imu.gyro); VECT3_ADD(accel_sum, imu.accel); VECT3_ADD(mag_sum, imu.mag); ref_sensor_samples[samples_idx] = imu.accel.z;//该数组大小为60(PERIDIC FREQUENCY) samples_idx++;//samples_idx从0开始 #ifdef AHRS_ALIGNER_LED RunOnceEvery(50, {LED_TOGGLE(AHRS_ALIGNER_LED)});//如果定义了ahrs校准器的指示灯时会让该灯以固定频率闪烁 #endif if (samples_idx >= SAMPLES_NB) { int32_t avg_ref_sensor = accel_sum.z; if ( avg_ref_sensor >= 0) avg_ref_sensor += SAMPLES_NB / 2; else avg_ref_sensor -= SAMPLES_NB / 2; avg_ref_sensor /= SAMPLES_NB; //噪声的误差计算 ahrs_aligner.noise = 0; int i; for (i=0; i<SAMPLES_NB; i++) { int32_t diff = ref_sensor_samples[i] - avg_ref_sensor; ahrs_aligner.noise += abs(diff); } //存储平均值(60次)到ahrs校准的lp_xxx RATES_SDIV(ahrs_aligner.lp_gyro, gyro_sum, SAMPLES_NB); VECT3_SDIV(ahrs_aligner.lp_accel, accel_sum, SAMPLES_NB); VECT3_SDIV(ahrs_aligner.lp_mag, mag_sum, SAMPLES_NB); //清零 INT_RATES_ZERO(gyro_sum); INT_VECT3_ZERO(accel_sum); INT_VECT3_ZERO(mag_sum); samples_idx = 0; if (ahrs_aligner.noise < LOW_NOISE_THRESHOLD) ahrs_aligner.low_noise_cnt++; else if ( ahrs_aligner.low_noise_cnt > 0) ahrs_aligner.low_noise_cnt--; if (ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) { ahrs_aligner.status = AHRS_ALIGNER_LOCKED;//如果ahrs校准器的噪声(noise)值低于阈值的次数为5次,那么ahrs校准器将关闭 #ifdef AHRS_ALIGNER_LED LED_ON(AHRS_ALIGNER_LED);//ahrs校准器关闭的话,对应的led灯就会关闭 #endif } } }
void imu_periodic( void ) { // Start reading the latest gyroscope data if (!imu_krooz.mpu.config.initialized) mpu60x0_i2c_start_configure(&imu_krooz.mpu); if (!imu_krooz.hmc.initialized) hmc58xx_start_configure(&imu_krooz.hmc); if (imu_krooz.meas_nb) { RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb); #if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled); #endif VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb); #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); #endif RATES_SMUL(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, IMU_KROOZ_GYRO_AVG_FILTER); RATES_ADD(imu_krooz.gyro_filtered, imu.gyro_unscaled); RATES_SDIV(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, (IMU_KROOZ_GYRO_AVG_FILTER + 1)); RATES_COPY(imu.gyro_unscaled, imu_krooz.gyro_filtered); VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER); VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled); VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1)); VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered); RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0); imu_krooz.meas_nb = 0; imu_krooz.gyr_valid = TRUE; imu_krooz.acc_valid = TRUE; } //RunOnceEvery(10,imu_krooz_downlink_raw()); }
void stabilization_rate_run(bool_t in_flight) { /* compute feed-back command */ struct Int32Rates _error; struct Int32Rates *body_rate = stateGetBodyRates_i(); RATES_DIFF(_error, stabilization_rate_sp, (*body_rate)); if (in_flight) { /* update integrator */ //divide the sum_err_increment to make sure it doesn't accumulate to the max too fast struct Int32Rates sum_err_increment; RATES_SDIV(sum_err_increment, _error, 5); RATES_ADD(stabilization_rate_sum_err, sum_err_increment); RATES_BOUND_CUBE(stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR); } else { INT_RATES_ZERO(stabilization_rate_sum_err); } /* PI */ stabilization_rate_fb_cmd.p = stabilization_rate_gain.p * _error.p + OFFSET_AND_ROUND2((stabilization_rate_igain.p * stabilization_rate_sum_err.p), 6); stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q + OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 6); stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r + OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 6); stabilization_cmd[COMMAND_ROLL] = stabilization_rate_fb_cmd.p >> 11; stabilization_cmd[COMMAND_PITCH] = stabilization_rate_fb_cmd.q >> 11; stabilization_cmd[COMMAND_YAW] = stabilization_rate_fb_cmd.r >> 11; /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }
static inline void on_gyro_accel_event( void ) { #ifdef AHRS_CPU_LED LED_ON(AHRS_CPU_LED); #endif // Run aligner on raw data as it also makes averages. if (ahrs.status == AHRS_UNINIT) { ImuScaleGyro(imu); ImuScaleAccel(imu); ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) ahrs_align(); return; } #if PERIODIC_FREQUENCY == 60 ImuScaleGyro(imu); ImuScaleAccel(imu); ahrs_propagate(); ahrs_update_accel(); ahrs_update_fw_estimator(); #else //PERIODIC_FREQUENCY static uint8_t _reduced_propagation_rate = 0; static uint8_t _reduced_correction_rate = 0; static struct Int32Vect3 acc_avg; static struct Int32Rates gyr_avg; RATES_ADD(gyr_avg, imu.gyro_unscaled); VECT3_ADD(acc_avg, imu.accel_unscaled); _reduced_propagation_rate++; if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY)) { } else { _reduced_propagation_rate = 0; RATES_SDIV(imu.gyro_unscaled, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) ); INT_RATES_ZERO(gyr_avg); ImuScaleGyro(imu); ahrs_propagate(); _reduced_correction_rate++; if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY)) { _reduced_correction_rate = 0; VECT3_SDIV(imu.accel_unscaled, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) ); INT_VECT3_ZERO(acc_avg); ImuScaleAccel(imu); ahrs_update_accel(); ahrs_update_fw_estimator(); } } #endif //PERIODIC_FREQUENCY #ifdef AHRS_CPU_LED LED_OFF(AHRS_CPU_LED); #endif #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP new_ins_attitude = 1; #endif }