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;
}
Exemple #6
0
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);
}