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 */ RATES_ADD(stabilization_rate_sum_err, _error); 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), 10); stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q + OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 10); stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r + OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 10); 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_rate_run(bool_t in_flight) { /* reference */ struct Int32Rates _r; RATES_DIFF(_r, stabilization_rate_sp, stabilization_rate_ref); RATES_SDIV(stabilization_rate_refdot, _r, STABILIZATION_RATE_REF_TAU); /* integrate ref */ const struct Int32Rates _delta_ref = { stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), stabilization_rate_refdot.q >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), stabilization_rate_refdot.r >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC)}; RATES_ADD(stabilization_rate_ref, _delta_ref); /* compute feed-forward command */ RATES_EWMULT_RSHIFT(stabilization_rate_ff_cmd, stabilization_rate_ddgain, stabilization_rate_refdot, 9); /* compute feed-back command */ /* error for feedback */ const struct Int32Rates _ref_scaled = { OFFSET_AND_ROUND(stabilization_rate_ref.p, (REF_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) }; struct Int32Rates _error; struct Int32Rates* body_rate = stateGetBodyRates_i(); RATES_DIFF(_error, _ref_scaled, (*body_rate)); if (in_flight) { /* update integrator */ RATES_ADD(stabilization_rate_sum_err, _error); 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), 10); stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q + OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 10); stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r + OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 10); stabilization_rate_fb_cmd.p = stabilization_rate_fb_cmd.p >> 11; stabilization_rate_fb_cmd.q = stabilization_rate_fb_cmd.q >> 11; stabilization_rate_fb_cmd.r = stabilization_rate_fb_cmd.r >> 11; /* sum to final command */ stabilization_cmd[COMMAND_ROLL] = stabilization_rate_ff_cmd.p + stabilization_rate_fb_cmd.p; stabilization_cmd[COMMAND_PITCH] = stabilization_rate_ff_cmd.q + stabilization_rate_fb_cmd.q; stabilization_cmd[COMMAND_YAW] = stabilization_rate_ff_cmd.r + stabilization_rate_fb_cmd.r; /* 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_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_run(bool_t in_flight) { /* update reference */ stabilization_attitude_ref_update(); /* compute feedforward command */ stabilization_att_ff_cmd[COMMAND_ROLL] = OFFSET_AND_ROUND(stabilization_gains.dd.x * stab_att_ref_accel.p, 5); stabilization_att_ff_cmd[COMMAND_PITCH] = OFFSET_AND_ROUND(stabilization_gains.dd.y * stab_att_ref_accel.q, 5); stabilization_att_ff_cmd[COMMAND_YAW] = OFFSET_AND_ROUND(stabilization_gains.dd.z * stab_att_ref_accel.r, 5); /* compute feedback command */ /* attitude error */ const struct Int32Eulers att_ref_scaled = { OFFSET_AND_ROUND(stab_att_ref_euler.phi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)) }; struct Int32Eulers att_err; struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i(); EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler)); INT32_ANGLE_NORMALIZE(att_err.psi); if (in_flight) { /* update integrator */ EULERS_ADD(stabilization_att_sum_err, att_err); EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR); } else { INT_EULERS_ZERO(stabilization_att_sum_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)); /* PID */ stabilization_att_fb_cmd[COMMAND_ROLL] = stabilization_gains.p.x * att_err.phi + stabilization_gains.d.x * rate_err.p + OFFSET_AND_ROUND2((stabilization_gains.i.x * stabilization_att_sum_err.phi), 10); stabilization_att_fb_cmd[COMMAND_PITCH] = stabilization_gains.p.y * att_err.theta + stabilization_gains.d.y * rate_err.q + OFFSET_AND_ROUND2((stabilization_gains.i.y * stabilization_att_sum_err.theta), 10); stabilization_att_fb_cmd[COMMAND_YAW] = stabilization_gains.p.z * att_err.psi + stabilization_gains.d.z * rate_err.r + OFFSET_AND_ROUND2((stabilization_gains.i.z * stabilization_att_sum_err.psi), 10); /* with P gain of 100, att_err of 180deg (3.14 rad) * fb cmd: 100 * 3.14 * 2^12 / 2^CMD_SHIFT = 628 * max possible command is 9600 */ #define CMD_SHIFT 11 /* sum feedforward and feedback */ stabilization_cmd[COMMAND_ROLL] = OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]), CMD_SHIFT); stabilization_cmd[COMMAND_PITCH] = OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]), CMD_SHIFT); stabilization_cmd[COMMAND_YAW] = OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]), CMD_SHIFT); /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }