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_attitude_ref_update() { /* integrate reference attitude */ struct FloatQuat qdot; FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat); QUAT_SMUL(qdot, qdot, DT_UPDATE); QUAT_ADD(stab_att_ref_quat, qdot); FLOAT_QUAT_NORMALIZE(stab_att_ref_quat); /* integrate reference rotational speeds */ struct FloatRates delta_rate; RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE); RATES_ADD(stab_att_ref_rate, delta_rate); /* compute reference angular accelerations */ struct FloatQuat err; /* compute reference attitude error */ FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(err); /* propagate the 2nd order linear model */ stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p - stab_att_ref_model[ref_idx].omega.p*stab_att_ref_model[ref_idx].omega.p*err.qx; stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q - stab_att_ref_model[ref_idx].omega.q*stab_att_ref_model[ref_idx].omega.q*err.qy; stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r - stab_att_ref_model[ref_idx].omega.r*stab_att_ref_model[ref_idx].omega.r*err.qz; /* saturate acceleration */ const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); /* compute ref_euler */ FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat); }
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 }