void stabilization_attitude_init(void)
{
/* setpoints */
FLOAT_EULERS_ZERO(stab_att_sp_euler);
float_quat_identity(&stab_att_sp_quat);
/* reference */
attitude_ref_quat_float_init(&att_ref_quat_f);
attitude_ref_quat_float_schedule(&att_ref_quat_f, STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT);
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]);
#ifdef HAS_SURFACE_COMMANDS
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]);
#endif
}
float_quat_identity(&stabilization_att_sum_err_quat);
FLOAT_RATES_ZERO(last_body_rate);
FLOAT_RATES_ZERO(body_rate_d);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_FLOAT, send_att);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_FLOAT, send_att_ref);
#endif
}
void ahrs_fc_init(void)
{
ahrs_fc.status = AHRS_FC_UNINIT;
ahrs_fc.is_aligned = FALSE;
ahrs_fc.ltp_vel_norm_valid = FALSE;
ahrs_fc.heading_aligned = FALSE;
/* init ltp_to_imu quaternion as zero/identity rotation */
float_quat_identity(&ahrs_fc.ltp_to_imu_quat);
FLOAT_RATES_ZERO(ahrs_fc.imu_rate);
/* set default filter cut-off frequency and damping */
ahrs_fc.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_fc.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_fc.mag_omega = AHRS_MAG_OMEGA;
ahrs_fc.mag_zeta = AHRS_MAG_ZETA;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_fc.correct_gravity = TRUE;
#else
ahrs_fc.correct_gravity = FALSE;
#endif
ahrs_fc.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
VECT3_ASSIGN(ahrs_fc.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
ahrs_fc.accel_cnt = 0;
ahrs_fc.mag_cnt = 0;
}
void stabilization_attitude_enter(void)
{
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_f();
attitude_ref_quat_float_enter(&att_ref_quat_f, stab_att_sp_euler.psi);
float_quat_identity(&stabilization_att_sum_err_quat);
}
/* init state and measurements */
static inline void init_invariant_state(void)
{
// init state
float_quat_identity(&ahrs_float_inv.state.quat);
FLOAT_RATES_ZERO(ahrs_float_inv.state.bias);
ahrs_float_inv.state.as = 1.0f;
ahrs_float_inv.state.cs = 1.0f;
// init measures
FLOAT_VECT3_ZERO(ahrs_float_inv.meas.accel);
FLOAT_VECT3_ZERO(ahrs_float_inv.meas.mag);
}
/* init state and measurements */
static inline void init_invariant_state(void)
{
// init state
float_quat_identity(&ins_impl.state.quat);
FLOAT_RATES_ZERO(ins_impl.state.bias);
FLOAT_VECT3_ZERO(ins_impl.state.pos);
FLOAT_VECT3_ZERO(ins_impl.state.speed);
ins_impl.state.as = 1.0f;
ins_impl.state.hb = 0.0f;
// init measures
FLOAT_VECT3_ZERO(ins_impl.meas.pos_gps);
FLOAT_VECT3_ZERO(ins_impl.meas.speed_gps);
ins_impl.meas.baro_alt = 0.0f;
// init baro
ins_baro_initialized = FALSE;
ins_gps_fix_once = FALSE;
}
void v_ctl_init(void)
{
/* mode */
v_ctl_mode = V_CTL_MODE_MANUAL;
v_ctl_speed_mode = V_CTL_SPEED_THROTTLE; //There is only one, added here to be universal in flightplan for different control modes
/* outer loop */
v_ctl_altitude_setpoint = 0.;
v_ctl_altitude_pre_climb = 0.;
v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
#ifdef V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH
v_ctl_auto_throttle_nominal_cruise_pitch = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH;
#else
v_ctl_auto_throttle_nominal_cruise_pitch = 0.;
#endif
v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
v_ctl_auto_airspeed_setpoint_slew = v_ctl_auto_airspeed_setpoint;
v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
v_ctl_max_acceleration = V_CTL_MAX_ACCELERATION;
/* inner loops */
v_ctl_climb_setpoint = 0.;
/* "auto throttle" inner loop parameters */
v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
v_ctl_auto_throttle_of_airspeed_pgain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN;
v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
#ifdef V_CTL_ENERGY_TOT_PGAIN
v_ctl_energy_total_pgain = V_CTL_ENERGY_TOT_PGAIN;
v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
#else
v_ctl_energy_total_pgain = 0.;
v_ctl_energy_total_igain = 0.;
v_ctl_energy_diff_pgain = 0.;
v_ctl_energy_diff_igain = 0.;
#warning "V_CTL_ENERGY_TOT GAINS are not defined and set to 0"
#endif
#ifdef V_CTL_ALTITUDE_MAX_CLIMB
v_ctl_max_climb = V_CTL_ALTITUDE_MAX_CLIMB;
#else
v_ctl_max_climb = 2;
#warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
#endif
#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
v_ctl_auto_groundspeed_sum_err = 0.;
#endif
v_ctl_throttle_setpoint = 0;
float_quat_identity(&imu_to_body_quat);
AbiBindMsgIMU_ACCEL_INT32(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgBODY_TO_IMU_QUAT(V_CTL_ENERGY_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
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);
}
