void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
void guidance_h_init(void)
{
guidance_h.mode = GUIDANCE_H_MODE_KILL;
guidance_h.use_ref = GUIDANCE_H_USE_REF;
guidance_h.approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST;
INT_VECT2_ZERO(guidance_h.sp.pos);
INT_VECT2_ZERO(guidance_h_trim_att_integrator);
INT_EULERS_ZERO(guidance_h.rc_sp);
guidance_h.sp.heading = 0;
guidance_h.sp.heading_rate = 0;
guidance_h.gains.p = GUIDANCE_H_PGAIN;
guidance_h.gains.i = GUIDANCE_H_IGAIN;
guidance_h.gains.d = GUIDANCE_H_DGAIN;
guidance_h.gains.a = GUIDANCE_H_AGAIN;
guidance_h.gains.v = GUIDANCE_H_VGAIN;
transition_percentage = 0;
transition_theta_offset = 0;
gh_ref_init();
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
guidance_h_module_init();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_INT, send_gh);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HOVER_LOOP, send_hover_loop);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_REF_INT, send_href);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER, send_tune_hover);
#endif
}
void stabilization_attitude_enter(void) {
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
reset_psi_ref_from_body();
INT_EULERS_ZERO( stabilization_att_sum_err );
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
VECT3_ASSIGN(stabilization_gains.p,
STABILIZATION_ATTITUDE_PHI_PGAIN,
STABILIZATION_ATTITUDE_THETA_PGAIN,
STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(stabilization_gains.d,
STABILIZATION_ATTITUDE_PHI_DGAIN,
STABILIZATION_ATTITUDE_THETA_DGAIN,
STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN(stabilization_gains.i,
STABILIZATION_ATTITUDE_PHI_IGAIN,
STABILIZATION_ATTITUDE_THETA_IGAIN,
STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN(stabilization_gains.dd,
STABILIZATION_ATTITUDE_PHI_DDGAIN,
STABILIZATION_ATTITUDE_THETA_DDGAIN,
STABILIZATION_ATTITUDE_PSI_DDGAIN);
INT_EULERS_ZERO( stabilization_att_sum_err );
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* set ltp_to_body to zero */
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
INT_RATES_ZERO(ahrs.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
ahrs_impl.use_gravity_heuristic = TRUE;
#else
ahrs_impl.use_gravity_heuristic = FALSE;
#endif
}
void stabilization_attitude_init(void)
{
stabilization_attitude_ref_init();
VECT3_ASSIGN(stabilization_gains.p,
STABILIZATION_ATTITUDE_PHI_PGAIN,
STABILIZATION_ATTITUDE_THETA_PGAIN,
STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(stabilization_gains.d,
STABILIZATION_ATTITUDE_PHI_DGAIN,
STABILIZATION_ATTITUDE_THETA_DGAIN,
STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN(stabilization_gains.i,
STABILIZATION_ATTITUDE_PHI_IGAIN,
STABILIZATION_ATTITUDE_THETA_IGAIN,
STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN(stabilization_gains.dd,
STABILIZATION_ATTITUDE_PHI_DDGAIN,
STABILIZATION_ATTITUDE_THETA_DDGAIN,
STABILIZATION_ATTITUDE_PSI_DDGAIN);
INT_EULERS_ZERO(stabilization_att_sum_err);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
#endif
}
void stabilization_attitude_enter(void) {
stabilization_attitude_ref_enter();
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
//FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
void guidance_h_read_rc(bool in_flight)
{
switch (guidance_h.mode) {
case GUIDANCE_H_MODE_RC_DIRECT:
stabilization_none_read_rc();
break;
#if USE_STABILIZATION_RATE
case GUIDANCE_H_MODE_RATE:
#if SWITCH_STICKS_FOR_RATE_CONTROL
stabilization_rate_read_rc_switched_sticks();
#else
stabilization_rate_read_rc();
#endif
break;
#endif
case GUIDANCE_H_MODE_CARE_FREE:
stabilization_attitude_read_rc(in_flight, TRUE, FALSE);
break;
case GUIDANCE_H_MODE_FORWARD:
stabilization_attitude_read_rc(in_flight, FALSE, TRUE);
break;
case GUIDANCE_H_MODE_ATTITUDE:
stabilization_attitude_read_rc(in_flight, FALSE, FALSE);
break;
case GUIDANCE_H_MODE_HOVER:
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h.rc_sp, in_flight, FALSE, FALSE);
#if GUIDANCE_H_USE_SPEED_REF
read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight);
/* enable x,y velocity setpoints */
SetBit(guidance_h.sp.mask, 5);
#endif
break;
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
case GUIDANCE_H_MODE_MODULE:
guidance_h_module_read_rc();
break;
#endif
case GUIDANCE_H_MODE_NAV:
if (radio_control.status == RC_OK) {
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h.rc_sp, in_flight, FALSE, FALSE);
} else {
INT_EULERS_ZERO(guidance_h.rc_sp);
}
break;
case GUIDANCE_H_MODE_FLIP:
stabilization_attitude_read_rc(in_flight, FALSE, FALSE);
break;
default:
break;
}
}
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 ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
// FIXME: make ltp_to_imu and ltp_to_body coherent
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
}
void stabilization_attitude_enter(void) {
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
stabilization_attitude_ref_enter();
INT32_QUAT_ZERO(stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err);
}
void booz_fms_init(void) {
booz_fms_on = FALSE;
booz_fms_timeout = TRUE;
booz_fms_last_msg = BOOZ_FMS_TIMEOUT;
booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
INT_EULERS_ZERO(booz_fms_input.h_sp.attitude);
booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB;
booz_fms_input.v_sp.climb = 0;
booz_fms_impl_init();
}
void stabilization_attitude_enter(void) {
#if !USE_SETPOINTS_WITH_TRANSITIONS
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
#endif
stabilization_attitude_ref_enter();
INT32_QUAT_ZERO(stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err);
}
void booz_fms_periodic(void) {
if (booz_fms_last_msg < BOOZ_FMS_TIMEOUT)
booz_fms_last_msg++;
else {
booz_fms_timeout = TRUE;
booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
INT_EULERS_ZERO(booz_fms_input.h_sp.attitude);
booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB;
booz_fms_input.v_sp.climb = 0;
}
booz_fms_impl_periodic();
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
register_periodic_telemetry(DefaultPeriodic, "AHRS_REF_QUAT", send_ahrs_ref_quat);
#endif
}
void ahrs_ice_init(void)
{
ahrs_ice.status = AHRS_ICE_UNINIT;
ahrs_ice.is_aligned = false;
/* init ltp_to_imu to zero */
INT_EULERS_ZERO(ahrs_ice.ltp_to_imu_euler)
INT_RATES_ZERO(ahrs_ice.imu_rate);
INT_RATES_ZERO(ahrs_ice.gyro_bias);
ahrs_ice.reinj_1 = FACE_REINJ_1;
ahrs_ice.mag_offset = AHRS_MAG_OFFSET;
}
void vi_init(void)
{
vi.enabled = FALSE;
vi.timeouted = TRUE;
vi.last_msg = VI_TIMEOUT;
vi.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
INT_EULERS_ZERO(vi.input.h_sp.attitude);
vi.input.v_mode = GUIDANCE_V_MODE_CLIMB;
vi.input.v_sp.climb = 0;
vi_impl_init();
}
void ahrs_init(void) {
int i, j;
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_T[i][j] = 0.;
bafl_P[i][j] = 0.;
}
/* Insert the diagonal elements of the T-Matrix. These will never change. */
bafl_T[i][i] = 1.0;
/* initial covariance values */
if (i<3) {
bafl_P[i][i] = 1.0;
} else {
bafl_P[i][i] = 0.1;
}
}
FLOAT_QUAT_ZERO(bafl_quat);
ahrs.status = AHRS_UNINIT;
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate);
ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL);
ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG);
bafl_Q_att = BAFL_Q_ATT;
bafl_Q_gyro = BAFL_Q_GYRO;
FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz);
}
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 vi_periodic(void)
{
#if (VI_TIMEOUT != 0)
if (vi.last_msg < VI_TIMEOUT) {
vi.last_msg++;
} else {
vi.timeouted = TRUE;
vi.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
INT_EULERS_ZERO(vi.input.h_sp.attitude);
vi.input.v_mode = GUIDANCE_V_MODE_CLIMB;
vi.input.v_sp.climb = 0;
}
#endif
vi_impl_periodic();
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
/*
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
}
*/
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* set ltp_to_body to zero */
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
INT_RATES_ZERO(ahrs.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
RMAT_COPY(ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
INT32_EULERS_OF_RMAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_rmat);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
ahrs_impl.reinj_1 = FACE_REINJ_1;
#ifdef IMU_MAG_OFFSET
ahrs_mag_offset = IMU_MAG_OFFSET;
#else
ahrs_mag_offset = 0.;
#endif
}
void stabilization_attitude_init(void) {
INT_EULERS_ZERO(stab_att_sp_euler);
}
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);
}