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 stabilization_rate_init(void) { INT_RATES_ZERO(stabilization_rate_sp); RATES_ASSIGN(stabilization_rate_gain, STABILIZATION_RATE_GAIN_P, STABILIZATION_RATE_GAIN_Q, STABILIZATION_RATE_GAIN_R); RATES_ASSIGN(stabilization_rate_igain, STABILIZATION_RATE_IGAIN_P, STABILIZATION_RATE_IGAIN_Q, STABILIZATION_RATE_IGAIN_R); RATES_ASSIGN(stabilization_rate_ddgain, STABILIZATION_RATE_DDGAIN_P, STABILIZATION_RATE_DDGAIN_Q, STABILIZATION_RATE_DDGAIN_R); INT_RATES_ZERO(stabilization_rate_ref); INT_RATES_ZERO(stabilization_rate_refdot); INT_RATES_ZERO(stabilization_rate_sum_err); #if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "RATE_LOOP", send_rate); #endif }
void ahrs_init(void) { QUAT_ASSIGN(ned_to_body_orientation_quat_i, 1, 0, 0, 0); INT32_QUAT_NORMALIZE(ned_to_body_orientation_quat_i); RATES_ASSIGN(body_rates_i, 0, 0, 0); ahrs.status = AHRS_UNINIT; ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); INT_RATES_ZERO(ahrs_impl.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 VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT;//AHRS状态未初始化 ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE;//未航姿校准 /* set ltp_to_imu so that body is zero */ //设置ltp_to_imu ,imu速度,陀螺仪的偏差,速度矫正, QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); INT_RATES_ZERO(ahrs_impl.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 VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); INT_RATES_ZERO(ahrs_impl.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 VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); //INT32_VECT3_ZERO(imu_accel_local); //This is part of the grav correction hack }
void ahrs_ice_init(void) { ahrs_ice.status = AHRS_ICE_UNINIT; ahrs_ice.is_aligned = false; /* init ltp_to_imu to zero */ INT_EULERS_ZERO(ahrs_ice.ltp_to_imu_euler) INT_RATES_ZERO(ahrs_ice.imu_rate); INT_RATES_ZERO(ahrs_ice.gyro_bias); ahrs_ice.reinj_1 = FACE_REINJ_1; ahrs_ice.mag_offset = AHRS_MAG_OFFSET; }
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 */ 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_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); }
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 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_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_init(void) { ahrs.status = AHRS_UNINIT; ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; /* set ltp_to_imu so that body is zero */ memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_i(&imu.body_to_imu), sizeof(struct Int32Quat)); INT_RATES_ZERO(ahrs_impl.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.high_rez_bias); /* set default filter cut-off frequency and damping */ ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA; ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA; ahrs_set_accel_gains(); ahrs_impl.mag_omega = AHRS_MAG_OMEGA; ahrs_impl.mag_zeta = AHRS_MAG_ZETA; ahrs_set_mag_gains(); /* set default gravity heuristic */ ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; #else ahrs_impl.correct_gravity = FALSE; #endif VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); ahrs_impl.accel_cnt = 0; ahrs_impl.mag_cnt = 0; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat); register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); #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_aligner_init(void) { ahrs_aligner.status = AHRS_ALIGNER_RUNNING;//ahrs_aligner 运行 INT_RATES_ZERO(gyro_sum);//gyro,accel,mag初始化都为0 INT_VECT3_ZERO(accel_sum); INT_VECT3_ZERO(mag_sum); samples_idx = 0; ahrs_aligner.noise = 0; ahrs_aligner.low_noise_cnt = 0; }
void stabilization_rate_init(void) { INT_RATES_ZERO(stabilization_rate_sp); RATES_ASSIGN(stabilization_rate_gain, STABILIZATION_RATE_GAIN_P, STABILIZATION_RATE_GAIN_Q, STABILIZATION_RATE_GAIN_R); RATES_ASSIGN(stabilization_rate_igain, STABILIZATION_RATE_IGAIN_P, STABILIZATION_RATE_IGAIN_Q, STABILIZATION_RATE_IGAIN_R);; INT_RATES_ZERO(stabilization_rate_sum_err); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "RATE_LOOP", send_rate); #endif }
void ahrs_icq_init(void) { ahrs_icq.status = AHRS_ICQ_UNINIT; ahrs_icq.is_aligned = FALSE; ahrs_icq.ltp_vel_norm_valid = FALSE; ahrs_icq.heading_aligned = FALSE; /* init ltp_to_imu quaternion as zero/identity rotation */ int32_quat_identity(&ahrs_icq.ltp_to_imu_quat); INT_RATES_ZERO(ahrs_icq.imu_rate); INT_RATES_ZERO(ahrs_icq.gyro_bias); INT_RATES_ZERO(ahrs_icq.rate_correction); INT_RATES_ZERO(ahrs_icq.high_rez_bias); /* set default filter cut-off frequency and damping */ ahrs_icq.accel_omega = AHRS_ACCEL_OMEGA; ahrs_icq.accel_zeta = AHRS_ACCEL_ZETA; ahrs_icq_set_accel_gains(); ahrs_icq.mag_omega = AHRS_MAG_OMEGA; ahrs_icq.mag_zeta = AHRS_MAG_ZETA; ahrs_icq_set_mag_gains(); /* set default gravity heuristic */ ahrs_icq.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_icq.correct_gravity = TRUE; #else ahrs_icq.correct_gravity = FALSE; #endif VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); ahrs_icq.accel_cnt = 0; ahrs_icq.mag_cnt = 0; }
void stabilization_rate_init(void) { INT_RATES_ZERO(stabilization_rate_sp); RATES_ASSIGN(stabilization_rate_gain, STABILIZATION_RATE_GAIN_P, STABILIZATION_RATE_GAIN_Q, STABILIZATION_RATE_GAIN_R); RATES_ASSIGN(stabilization_rate_igain, STABILIZATION_RATE_IGAIN_P, STABILIZATION_RATE_IGAIN_Q, STABILIZATION_RATE_IGAIN_R); RATES_ASSIGN(stabilization_rate_ddgain, STABILIZATION_RATE_DDGAIN_P, STABILIZATION_RATE_DDGAIN_Q, STABILIZATION_RATE_DDGAIN_R); INT_RATES_ZERO(stabilization_rate_ref); INT_RATES_ZERO(stabilization_rate_refdot); INT_RATES_ZERO(stabilization_rate_sum_err); }
//进行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 ahrs_aligner_init(void) { ahrs_aligner.status = AHRS_ALIGNER_RUNNING; INT_RATES_ZERO(gyro_sum); INT_VECT3_ZERO(accel_sum); INT_VECT3_ZERO(mag_sum); samples_idx = 0; ahrs_aligner.noise = 0; ahrs_aligner.low_noise_cnt = 0; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "FILTER_ALIGNER", send_aligner); #endif }
void ahrs_init(void) { int i, j; for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_T[i][j] = 0.; bafl_P[i][j] = 0.; } /* Insert the diagonal elements of the T-Matrix. These will never change. */ bafl_T[i][i] = 1.0; /* initial covariance values */ if (i<3) { bafl_P[i][i] = 1.0; } else { bafl_P[i][i] = 0.1; } } FLOAT_QUAT_ZERO(bafl_quat); ahrs.status = AHRS_UNINIT; 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); INT_RATES_ZERO(ahrs.body_rate); INT_RATES_ZERO(ahrs.imu_rate); ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL); ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG); bafl_Q_att = BAFL_Q_ATT; bafl_Q_gyro = BAFL_Q_GYRO; FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz); }
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 }
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); }
void stabilization_none_enter(void) { INT_RATES_ZERO(stabilization_none_rc_cmd); }
void stabilization_none_init(void) { INT_RATES_ZERO(stabilization_none_rc_cmd); }
void stabilization_rate_enter(void) { INT_RATES_ZERO(stabilization_rate_sum_err); }
void stabilization_rate_enter(void) { RATES_COPY(stabilization_rate_ref, stabilization_rate_sp); INT_RATES_ZERO(stabilization_rate_sum_err); }
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 }