コード例 #1
0
void stabilization_attitude_init(void) {

  stabilization_attitude_ref_init();

  INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
  INT_EULERS_ZERO( stabilization_att_sum_err );
}
コード例 #2
0
ファイル: guidance_h.c プロジェクト: lethargi/paparazzi
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

}
コード例 #3
0
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 );

}
コード例 #4
0
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 );

}
コード例 #5
0
ファイル: ahrs_int_cmpl_quat.c プロジェクト: ERAUBB/paparazzi
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

}
コード例 #6
0
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
}
コード例 #7
0
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 );
}
コード例 #8
0
ファイル: guidance_h.c プロジェクト: lethargi/paparazzi
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;
  }

}
コード例 #9
0
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);
}
コード例 #10
0
ファイル: ahrs_int_cmpl.c プロジェクト: cbeighley/paparazzi
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);

}
コード例 #11
0
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);
}
コード例 #12
0
ファイル: booz2_fms.c プロジェクト: robotang/wasp
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();
}
コード例 #13
0
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);
}
コード例 #14
0
ファイル: booz2_fms.c プロジェクト: robotang/wasp
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();
}
コード例 #15
0
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
}
コード例 #16
0
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;
}
コード例 #17
0
ファイル: vi.c プロジェクト: AE4317group07/paparazzi
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();

}
コード例 #18
0
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);

}
コード例 #19
0
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);
  }
}
コード例 #20
0
ファイル: vi.c プロジェクト: AE4317group07/paparazzi
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();
}
コード例 #21
0
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 );
}
コード例 #22
0
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
}
コード例 #23
0
void stabilization_attitude_init(void) {
  INT_EULERS_ZERO(stab_att_sp_euler);
}
コード例 #24
0
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);

}