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
}
