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_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 */ struct Int32Rates rate_err; struct Int32Rates* body_rate = stateGetBodyRates_i(); RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate); /* integrated error */ if (enable_integrator) { struct Int32Quat new_sum_err, scaled_att_err; /* update accumulator */ scaled_att_err.qi = att_err.qi; scaled_att_err.qx = att_err.qx / IERROR_SCALE; scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; INT32_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); INT32_QUAT_NORMALIZE(new_sum_err); QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat); } else { /* reset accumulator */ INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); INT_EULERS_ZERO( stabilization_att_sum_err ); } /* 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_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct FloatQuat att_err; FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(att_err); /* rate error */ struct FloatRates rate_err; RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate); /* integrated error */ if (enable_integrator) { struct FloatQuat new_sum_err, scaled_att_err; /* update accumulator */ scaled_att_err.qi = att_err.qi; scaled_att_err.qx = att_err.qx / IERROR_SCALE; scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { /* reset accumulator */ FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat); // FIXME: this is very dangerous! only works if this really includes all commands for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) { stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; } /* 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; INT32_QUAT_INV_COMP(att_err, ahrs.ltp_to_body_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ INT32_QUAT_WRAP_SHORTEST(att_err); INT32_QUAT_NORMALIZE(att_err); /* rate error */ struct Int32Rates rate_err; RATES_DIFF(rate_err, ahrs.body_rate, stab_att_ref_rate); /* integrated error */ if (enable_integrator) { struct Int32Quat new_sum_err, scaled_att_err; /* update accumulator */ scaled_att_err.qi = att_err.qi; scaled_att_err.qx = att_err.qx / IERROR_SCALE; scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; INT32_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); INT32_QUAT_NORMALIZE(new_sum_err); QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat); } else { /* reset accumulator */ INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); INT_EULERS_ZERO( stabilization_att_sum_err ); } attitude_run_ff(stabilization_att_ff_cmd, current_stabilization_gains, &stab_att_ref_accel); attitude_run_fb(stabilization_att_fb_cmd, current_stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat); for (int i = COMMAND_ROLL; i <= COMMAND_YAW; i++) { stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; Bound(stabilization_cmd[i], -200, 200); } }
void stabilization_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct FloatQuat att_err; struct FloatQuat* att_quat = stateGetNedToBodyQuat_f(); FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(att_err); /* rate error */ struct FloatRates rate_err; struct FloatRates* body_rate = stateGetBodyRates_f(); RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate); /* rate_d error */ struct FloatRates body_rate_d; RATES_DIFF(body_rate_d, *body_rate, last_body_rate); RATES_COPY(last_body_rate, *body_rate); /* integrated error */ if (enable_integrator) { struct FloatQuat new_sum_err, scaled_att_err; /* update accumulator */ scaled_att_err.qi = att_err.qi; scaled_att_err.qx = att_err.qx / IERROR_SCALE; scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { /* reset accumulator */ FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, &stabilization_att_sum_err_quat); 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]; #ifdef HAS_SURFACE_COMMANDS stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE]; stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE]; stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE]; #endif /* 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 enable_integrator) { /* * Update reference */ static const float dt = (1./PERIODIC_FREQUENCY); attitude_ref_quat_float_update(&att_ref_quat_f, &stab_att_sp_quat, dt); /* * Compute errors for feedback */ /* attitude error */ struct FloatQuat att_err; struct FloatQuat *att_quat = stateGetNedToBodyQuat_f(); float_quat_inv_comp(&att_err, att_quat, &att_ref_quat_f.quat); /* wrap it in the shortest direction */ float_quat_wrap_shortest(&att_err); /* rate error */ struct FloatRates rate_err; struct FloatRates *body_rate = stateGetBodyRates_f(); RATES_DIFF(rate_err, att_ref_quat_f.rate, *body_rate); /* rate_d error */ RATES_DIFF(body_rate_d, *body_rate, last_body_rate); RATES_COPY(last_body_rate, *body_rate); #define INTEGRATOR_BOUND 1.0 /* 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 */ float_quat_identity(&stabilization_att_sum_err_quat); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &att_ref_quat_f.accel); attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, &stabilization_att_sum_err_quat); 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]; #ifdef HAS_SURFACE_COMMANDS stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE]; stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE]; stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE]; #endif /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }