//进行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);
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
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());
}
static inline void ahrs_lowpass_accel(void) {
// get latest measurement
ACCELS_FLOAT_OF_BFP(bafl_accel_last, imu.accel);
// low pass it
VECT3_ADD(bafl_accel_measure, bafl_accel_last);
VECT3_SDIV(bafl_accel_measure, bafl_accel_measure, 2);
#ifdef BAFL_DEBUG
bafl_phi_accel = atan2f( -bafl_accel_measure.y, -bafl_accel_measure.z);
bafl_theta_accel = atan2f(bafl_accel_measure.x, sqrtf(bafl_accel_measure.y*bafl_accel_measure.y + bafl_accel_measure.z*bafl_accel_measure.z));
#endif
}
/** Convert a ECEF position to local ENU.
* @param[out] enu ENU position in meter << #INT32_POS_FRAC
* @param[in] def local coordinate system definition
* @param[in] ecef ECEF position in cm
*/
void enu_of_ecef_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
{
struct EnuCoor_i enu_cm;
enu_of_ecef_point_i(&enu_cm, def, ecef);
/* enu = (enu_cm / 100) << INT32_POS_FRAC
* to loose less range:
* enu_cm = enu << (INT32_POS_FRAC-2) / 25
* which puts max enu output Q23.8 range to 8388km / 25 = 335km
*/
INT32_VECT3_LSHIFT(*enu, enu_cm, INT32_POS_FRAC - 2);
VECT3_SDIV(*enu, *enu, 25);
}
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);
}
uint32_t now_ts = get_sys_time_usec();
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);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
}
if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x;
imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y;
imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z;
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
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);
INT_VECT3_ZERO(imu_krooz.accel_sum);
INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
RunOnceEvery(128, {axis_nb = 5;});
void ahrs_update_accel(void) {
#if USE_NOISE_CUT || USE_NOISE_FILTER
static struct Int32Vect3 last_accel = { 0, 0, 0 };
#endif
#if USE_NOISE_CUT
if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) {
#endif
#if USE_NOISE_FILTER
VECT3_SUM_SCALED(imu.accel, imu.accel, last_accel, NOISE_FILTER_GAIN);
VECT3_SDIV(imu.accel, imu.accel, NOISE_FILTER_GAIN+1);
#endif
get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel);
#if USE_NOISE_CUT
}
VECT3_COPY(last_accel, imu.accel);
#endif
}
/* compute the mean of the last n accel measurements */
static inline void b2_hff_compute_accel_body_mean(uint8_t n) {
struct Int32Vect3 sum;
int i, j;
INT_VECT3_ZERO(sum);
if (n > 1) {
if (n > acc_body.n) {
n = acc_body.n;
}
for (i = 1; i <= n; i++) {
j = (acc_body.w - i) > 0 ? (acc_body.w - i) : (acc_body.w - i + acc_body.size);
VECT3_ADD(sum, acc_body.buf[j]);
}
VECT3_SDIV(acc_body_mean, sum, n);
} else {
VECT3_COPY(acc_body_mean, sum);
}
}
void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt)
{
// check if we had at least one propagation since last update
if (ahrs_fc.accel_cnt == 0) {
return;
}
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, *accel);
struct FloatVect3 residual;
struct FloatVect3 pseudo_gravity_measurement;
if (ahrs_fc.correct_gravity && ahrs_fc.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0};
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
}
VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
/* FIR filtered pseudo_gravity_measurement */
#define FIR_FILTER_SIZE 8
static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1);
VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_fc.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81;
ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
Bound(ahrs_fc.weight, 0.15, 1.0);
} else {
ahrs_fc.weight = 1.0;
}
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt
* with ahrs_fc.accel_cnt beeing the number of propagations since last update
*/
const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega *
ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain);
// reset accel propagation counter
ahrs_fc.accel_cnt = 0;
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2 * dt
*/
const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega *
ahrs_fc.weight * ahrs_fc.weight * dt / 9.81;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain);
/* FIXME: saturate bias */
}
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
}
void ahrs_update_accel(void) {
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
struct FloatVect3 residual;
struct FloatVect3 pseudo_gravity_measurement;
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
}
FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
/* FIR filtered pseudo_gravity_measurement */
#define FIR_FILTER_SIZE 8
static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1);
VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_impl.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81;
ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
Bound(ahrs_impl.weight, 0.15, 1.0);
}
else {
ahrs_impl.weight = 1.0;
}
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
*/
const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega *
ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
*/
const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega *
ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);
/* FIXME: saturate bias */
}
