void stabilization_attitude_run(bool_t enable_integrator) { /* Propagate the second order filter on the gyroscopes */ struct FloatRates *body_rates = stateGetBodyRates_f(); stabilization_indi_second_order_filter(body_rates, &indi.filtered_rate_2deriv, &indi.filtered_rate_deriv, &indi.filtered_rate, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R); /* attitude error */ struct Int32Quat att_err; struct Int32Quat *att_quat = stateGetNedToBodyQuat_i(); // INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_sp_quat); int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat); /* wrap it in the shortest direction */ int32_quat_wrap_shortest(&att_err); int32_quat_normalize(&att_err); /* compute the INDI command */ attitude_run_indi(stabilization_att_indi_cmd, &att_err, enable_integrator); stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL]; stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH]; stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_cmd[COMMAND_YAW]; /* 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_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct Int32Quat att_err; struct Int32Quat *att_quat = stateGetNedToBodyQuat_i(); INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ int32_quat_wrap_shortest(&att_err); int32_quat_normalize(&att_err); /* rate error */ const struct Int32Rates rate_ref_scaled = { OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) }; struct Int32Rates rate_err; struct Int32Rates *body_rate = stateGetBodyRates_i(); RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate)); #define INTEGRATOR_BOUND 100000 /* integrated error */ if (enable_integrator) { stabilization_att_sum_err_quat.qx += att_err.qx / IERROR_SCALE; stabilization_att_sum_err_quat.qy += att_err.qy / IERROR_SCALE; stabilization_att_sum_err_quat.qz += att_err.qz / IERROR_SCALE; Bound(stabilization_att_sum_err_quat.qx, -INTEGRATOR_BOUND, INTEGRATOR_BOUND); Bound(stabilization_att_sum_err_quat.qy, -INTEGRATOR_BOUND, INTEGRATOR_BOUND); Bound(stabilization_att_sum_err_quat.qz, -INTEGRATOR_BOUND, INTEGRATOR_BOUND); } else { /* reset accumulator */ int32_quat_identity(&stabilization_att_sum_err_quat); } /* compute the feed forward command */ attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &stab_att_ref_accel); /* compute the feed back command */ attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat); /* sum feedforward and feedback */ stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]; stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]; stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]; /* 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_indi_run(bool enable_integrator __attribute__((unused)), bool rate_control) { /* attitude error */ struct Int32Quat att_err; struct Int32Quat *att_quat = stateGetNedToBodyQuat_i(); int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat); /* wrap it in the shortest direction */ int32_quat_wrap_shortest(&att_err); int32_quat_normalize(&att_err); /* compute the INDI command */ stabilization_indi_calc_cmd(stabilization_att_indi_cmd, &att_err, rate_control); /* copy the INDI command */ stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL]; stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH]; stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_cmd[COMMAND_YAW]; /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }