// This is a simple second order low pass filter static void stabilization_indi_second_order_filter(struct IndiFilter *filter, struct FloatRates *input) { float_rates_integrate_fi(&filter->x, &filter->dx, 1.0 / PERIODIC_FREQUENCY); float_rates_integrate_fi(&filter->dx, &filter->ddx, 1.0 / PERIODIC_FREQUENCY); filter->ddx.p = -filter->dx.p * 2 * filter->zeta * filter->omega + (input->p - filter->x.p) * filter->omega2; filter->ddx.q = -filter->dx.q * 2 * filter->zeta * filter->omega + (input->q - filter->x.q) * filter->omega2; filter->ddx.r = -filter->dx.r * 2 * filter->zeta * filter->omega_r + (input->r - filter->x.r) * filter->omega2_r; }
// This is a simple second order low pass filter void stabilization_indi_second_order_filter(struct FloatRates *input, struct FloatRates *filter_ddx, struct FloatRates *filter_dx, struct FloatRates *filter_x, float omega, float zeta, float omega_r) { float_rates_integrate_fi(filter_x, filter_dx, 1.0 / PERIODIC_FREQUENCY); float_rates_integrate_fi(filter_dx, filter_ddx, 1.0 / PERIODIC_FREQUENCY); float omega2 = omega * omega; float omega2_r = omega_r * omega_r; filter_ddx->p = -filter_dx->p * 2 * zeta * omega + (input->p - filter_x->p) * omega2; \ filter_ddx->q = -filter_dx->q * 2 * zeta * omega + (input->q - filter_x->q) * omega2; \ filter_ddx->r = -filter_dx->r * 2 * zeta * omega_r + (input->r - filter_x->r) * omega2_r; }
static void traj_step_phi_2nd_order_update(void) { if (aos.time > 15) { const float omega = RadOfDeg(100); const float xi = 0.9; struct FloatRates raccel; RATES_ASSIGN(raccel, -2.*xi * omega * aos.imu_rates.p - omega * omega * (aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0., 0.); float_rates_integrate_fi(&aos.imu_rates, &raccel, aos.dt); float_quat_integrate(&aos.ltp_to_imu_quat, &aos.imu_rates, aos.dt); float_eulers_of_quat(&aos.ltp_to_imu_euler, &aos.ltp_to_imu_quat); } }