static inline void on_gyro_accel_event( void ) {
ImuScaleGyro(imu);
ImuScaleAccel(imu);
if (ahrs.status == AHRS_UNINIT) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
}
else {
ahrs_propagate();
ahrs_update_accel();
ahrs_update_fw_estimator();
}
}
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
}
