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_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_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);
}
// simple rate control without reference model nor attitude
void ctrl_module_run(bool_t in_flight)
{
if (!in_flight) {
// Reset integrators
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 0;
} else {
stabilization_cmd[COMMAND_ROLL] = ctrl_module_demo.rc_x * ctrl_module_demo_pr_ff_gain -
stateGetBodyRates_i()->p * ctrl_module_demo_pr_d_gain;
stabilization_cmd[COMMAND_PITCH] = ctrl_module_demo.rc_y * ctrl_module_demo_pr_ff_gain -
stateGetBodyRates_i()->q * ctrl_module_demo_pr_d_gain;
stabilization_cmd[COMMAND_YAW] = ctrl_module_demo.rc_z * ctrl_module_demo_y_ff_gain -
stateGetBodyRates_i()->r * ctrl_module_demo_y_d_gain;
stabilization_cmd[COMMAND_THRUST] = ctrl_module_demo.rc_t;
}
}
void ahrs_update_accel(float dt) {
// check if we had at least one propagation since last update
if (ahrs_impl.accel_cnt == 0)
return;
// c2 = ltp z-axis in imu-frame
struct Int32RMat ltp_to_imu_rmat;
int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat);
struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ltp_to_imu_rmat, 2,2)};
struct Int32Vect3 residual;
struct Int32Vect3 pseudo_gravity_measurement;
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
// FIXME: check overflows !
#define COMPUTATION_FRAC 16
#define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC
const struct Int32Vect3 vel_tangential_body =
{ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
struct Int32Vect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body);
INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC);
/* convert centrifucal acceleration from body to imu frame */
struct Int32Vect3 acc_c_imu;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement
* of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
} else {
static void send_att(void) { //FIXME really use this message here ?
struct Int32Rates* body_rate = stateGetBodyRates_i();
struct Int32Eulers* att = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err.phi,
&stabilization_att_sum_err.theta,
&stabilization_att_sum_err.psi,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW]);
}
static void send_att(struct transport_tx *trans, struct link_device *dev) //FIXME really use this message here ?
{
struct Int32Rates *body_rate = stateGetBodyRates_i();
struct Int32Eulers *att = stateGetNedToBodyEulers_i();
pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err_quat.qx,
&stabilization_att_sum_err_quat.qy,
&stabilization_att_sum_err_quat.qz,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW]);
}
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 ahrs_update_accel(void) {
// c2 = ltp z-axis in imu-frame
struct Int32RMat ltp_to_imu_rmat;
INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ltp_to_imu_rmat, 2,2)};
struct Int32Vect3 residual;
struct Int32Vect3 pseudo_gravity_measurement;
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
// FIXME: check overflows !
#define COMPUTATION_FRAC 16
const struct Int32Vect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
struct Int32Vect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body);
INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-COMPUTATION_FRAC);
/* convert centrifucal acceleration from body to imu frame */
struct Int32Vect3 acc_c_imu;
INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
} else {
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);
}