Example #1
0
void parse_mf_daq_msg(void)
{
    mf_daq.nb = dl_buffer[2];
    if (mf_daq.nb > 0) {
        if (mf_daq.nb > MF_DAQ_SIZE) {
            mf_daq.nb = MF_DAQ_SIZE;
        }
        // Store data struct directly from dl_buffer
        float *buf = (float*)(dl_buffer+3);
        memcpy(mf_daq.values, buf, mf_daq.nb * sizeof(float));
        // Log on SD card
        if (log_started) {
            DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values);
            DOWNLINK_SEND_MF_DAQ_STATE(pprzlog_tp, chibios_sdlog,
                                       &autopilot_flight_time,
                                       &stateGetBodyRates_f()->p,
                                       &stateGetBodyRates_f()->q,
                                       &stateGetBodyRates_f()->r,
                                       &stateGetNedToBodyEulers_f()->phi,
                                       &stateGetNedToBodyEulers_f()->theta,
                                       &stateGetNedToBodyEulers_f()->psi,
                                       &stateGetAccelNed_f()->x,
                                       &stateGetAccelNed_f()->y,
                                       &stateGetAccelNed_f()->z,
                                       &stateGetSpeedEnu_f()->x,
                                       &stateGetSpeedEnu_f()->y,
                                       &stateGetSpeedEnu_f()->z,
                                       &stateGetPositionLla_f()->lat,
                                       &stateGetPositionLla_f()->lon,
                                       &stateGetPositionLla_f()->alt,
                                       &stateGetHorizontalWindspeed_f()->y,
                                       &stateGetHorizontalWindspeed_f()->x);
        }
    }
}
Example #2
0
/**
 * Send the attitude
 */
static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev)
{
  mavlink_msg_attitude_send(MAVLINK_COMM_0,
                            get_sys_time_msec(),
                            stateGetNedToBodyEulers_f()->phi,     // Phi
                            stateGetNedToBodyEulers_f()->theta,   // Theta
                            stateGetNedToBodyEulers_f()->psi,     // Psi
                            stateGetBodyRates_f()->p,             // p
                            stateGetBodyRates_f()->q,             // q
                            stateGetBodyRates_f()->r);            // r
  MAVLinkSendMessage();
}
Example #3
0
/**
 * Send the attitude
 */
static inline void mavlink_send_attitude(void)
{
  mavlink_msg_attitude_send(MAVLINK_COMM_0,
                            get_sys_time_msec(),
                            stateGetNedToBodyEulers_f()->phi,     // Phi
                            stateGetNedToBodyEulers_f()->theta,   // Theta
                            stateGetNedToBodyEulers_f()->psi,     // Psi
                            stateGetBodyRates_f()->p,             // p
                            stateGetBodyRates_f()->q,             // q
                            stateGetBodyRates_f()->r);            // r
  MAVLinkSendMessage();
}
Example #4
0
static inline void mavlink_send_attitude_quaternion(void)
{
  mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
                                       get_sys_time_msec(),
                                       stateGetNedToBodyQuat_f()->qi,
                                       stateGetNedToBodyQuat_f()->qx,
                                       stateGetNedToBodyQuat_f()->qy,
                                       stateGetNedToBodyQuat_f()->qz,
                                       stateGetBodyRates_f()->p,
                                       stateGetBodyRates_f()->q,
                                       stateGetBodyRates_f()->r);
  MAVLinkSendMessage();
}
Example #5
0
static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev)
{
  mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
                                       get_sys_time_msec(),
                                       stateGetNedToBodyQuat_f()->qi,
                                       stateGetNedToBodyQuat_f()->qx,
                                       stateGetNedToBodyQuat_f()->qy,
                                       stateGetNedToBodyQuat_f()->qz,
                                       stateGetBodyRates_f()->p,
                                       stateGetBodyRates_f()->q,
                                       stateGetBodyRates_f()->r);
  MAVLinkSendMessage();
}
// This is a Least Mean Squares adaptive filter
// It estiamtes the actuator effectiveness online by comparing the expected angular acceleration based on the inputs with the measured angular acceleration
void lms_estimation(void)
{

  // Only pass really low frequencies so you don't adapt to noise
  float omega = 10.0;
  float zeta = 0.8;
  stabilization_indi_second_order_filter(&indi.u_act_dyn, &udotdot_estimation, &udot_estimation, &indi_u_estimation,
                                         omega, zeta, omega);
  struct FloatRates *body_rates = stateGetBodyRates_f();
  stabilization_indi_second_order_filter(body_rates, &filtered_rate_2deriv_estimation, &filtered_rate_deriv_estimation,
                                         &filtered_rate_estimation, omega, zeta, omega);

  // The inputs are scaled in order to avoid overflows
  float du = udot_estimation.p * SCALE;
  g_est.p = g_est.p - (g_est.p * du - filtered_rate_2deriv_estimation.p) * du * mu;
  du = udot_estimation.q * SCALE;
  g_est.q = g_est.q - (g_est.q * du - filtered_rate_2deriv_estimation.q) * du * mu;
  du = udot_estimation.r * SCALE;
  float ddu = udotdot_estimation.r * SCALE / PERIODIC_FREQUENCY;
  float error = (g_est.r * du + g2_est * ddu - filtered_rate_2deriv_estimation.r);
  g_est.r = g_est.r - error * du * mu / 3;
  g2_est = g2_est - error * 1000 * ddu * mu / 3;

  //the g values should be larger than zero, otherwise there is positive feedback, the command will go to max and there is nothing to learn anymore...
  if (g_est.p < 0.01) { g_est.p = 0.01; }
  if (g_est.q < 0.01) { g_est.q = 0.01; }
  if (g_est.r < 0.01) { g_est.r = 0.01; }
  if (g2_est < 0.01) { g2_est = 0.01; }

  //Commit the estimated G values and apply the scaling
  g1.p = g_est.p * SCALE;
  g1.q = g_est.q * SCALE;
  g1.r = g_est.r * SCALE;
  g2 = g2_est * SCALE;
}
void stabilization_attitude_run(bool_t enable_integrator)
{

    /* Propagate the second order filter on the gyroscopes */
    struct FloatRates *body_rates = stateGetBodyRates_f();
    stabilization_indi_second_order_filter(body_rates, &indi.filtered_rate_2deriv, &indi.filtered_rate_deriv,
                                           &indi.filtered_rate, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);

    /* attitude error                          */
    struct Int32Quat att_err;
    struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
//   INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_sp_quat);
    int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
    /* wrap it in the shortest direction       */
    int32_quat_wrap_shortest(&att_err);
    int32_quat_normalize(&att_err);

    /* compute the INDI command */
    attitude_run_indi(stabilization_att_indi_cmd, &att_err, enable_integrator);

    stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL];
    stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH];
    stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_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);
}
static inline void h_ctl_roll_rate_loop()
{
  float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint;

  /* I term calculation */
  static float roll_rate_sum_err = 0.;
  static uint8_t roll_rate_sum_idx = 0;
  static float roll_rate_sum_values[H_CTL_ROLL_RATE_SUM_NB_SAMPLES];

  roll_rate_sum_err -= roll_rate_sum_values[roll_rate_sum_idx];
  roll_rate_sum_values[roll_rate_sum_idx] = err;
  roll_rate_sum_err += err;
  roll_rate_sum_idx++;
  if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) { roll_rate_sum_idx = 0; }

  /* D term calculations */
  static float last_err = 0;
  float d_err = err - last_err;
  last_err = err;

  float throttle_dep_pgain =
    Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain,
          v_ctl_throttle_setpoint / ((float)MAX_PPRZ));
  float cmd = throttle_dep_pgain * (err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES +
                                    h_ctl_roll_rate_dgain * d_err);

  h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
Example #9
0
// This is a Least Mean Squares adaptive filter
// It estiamtes the actuator effectiveness online by comparing the expected angular acceleration based on the inputs with the measured angular acceleration
static inline void lms_estimation(void)
{
  static struct IndiEstimation *est = &indi.est;
  // Only pass really low frequencies so you don't adapt to noise
  stabilization_indi_second_order_filter(&est->u, &indi.u_act_dyn);
  struct FloatRates *body_rates = stateGetBodyRates_f();
  stabilization_indi_second_order_filter(&est->rate, body_rates);

  // The inputs are scaled in order to avoid overflows
  float du = est->u.dx.p * INDI_EST_SCALE;
  est->g1.p = est->g1.p - (est->g1.p * du - est->rate.ddx.p) * du * est->mu;
  du = est->u.dx.q * INDI_EST_SCALE;
  est->g1.q = est->g1.q - (est->g1.q * du - est->rate.ddx.q) * du * est->mu;
  du = est->u.dx.r * INDI_EST_SCALE;
  float ddu = est->u.ddx.r * INDI_EST_SCALE / PERIODIC_FREQUENCY;
  float error = (est->g1.r * du + est->g2 * ddu - est->rate.ddx.r);
  est->g1.r = est->g1.r - error * du * est->mu / 3;
  est->g2 = est->g2 - error * 1000 * ddu * est->mu / 3;

  //the g values should be larger than zero, otherwise there is positive feedback, the command will go to max and there is nothing to learn anymore...
  if (est->g1.p < 0.01) { est->g1.p = 0.01; }
  if (est->g1.q < 0.01) { est->g1.q = 0.01; }
  if (est->g1.r < 0.01) { est->g1.r = 0.01; }
  if (est->g2   < 0.01) { est->g2 = 0.01; }

  if (indi.adaptive) {
    //Commit the estimated G values and apply the scaling
    indi.g1.p = est->g1.p * INDI_EST_SCALE;
    indi.g1.q = est->g1.q * INDI_EST_SCALE;
    indi.g1.r = est->g1.r * INDI_EST_SCALE;
    indi.g2   = est->g2 * INDI_EST_SCALE;
  }
}
void stabilization_attitude_run(bool_t  in_flight) {

  stabilization_attitude_ref_update();

  /* Compute feedforward */
  stabilization_att_ff_cmd[COMMAND_ROLL] =
    stabilization_gains.dd.x * stab_att_ref_accel.p / 32.;
  stabilization_att_ff_cmd[COMMAND_PITCH] =
    stabilization_gains.dd.y * stab_att_ref_accel.q / 32.;
  stabilization_att_ff_cmd[COMMAND_YAW] =
    stabilization_gains.dd.z * stab_att_ref_accel.r / 32.;

  /* Compute feedback                  */
  /* attitude error            */
  struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
  struct FloatEulers att_err;
  EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);
  FLOAT_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 {
    FLOAT_EULERS_ZERO(stabilization_att_sum_err);
  }

  /*  rate error                */
  struct FloatRates* rate_float = stateGetBodyRates_f();
  struct FloatRates rate_err;
  RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float);

  /*  PID                  */

  stabilization_att_fb_cmd[COMMAND_ROLL] =
    stabilization_gains.p.x  * att_err.phi +
    stabilization_gains.d.x  * rate_err.p +
    stabilization_gains.i.x  * stabilization_att_sum_err.phi / 1024.;

  stabilization_att_fb_cmd[COMMAND_PITCH] =
    stabilization_gains.p.y  * att_err.theta +
    stabilization_gains.d.y  * rate_err.q +
    stabilization_gains.i.y  * stabilization_att_sum_err.theta / 1024.;

  stabilization_att_fb_cmd[COMMAND_YAW] =
    stabilization_gains.p.z  * att_err.psi +
    stabilization_gains.d.z  * rate_err.r +
    stabilization_gains.i.z  * stabilization_att_sum_err.psi / 1024.;


  stabilization_cmd[COMMAND_ROLL] =
    (stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
  stabilization_cmd[COMMAND_PITCH] =
    (stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
  stabilization_cmd[COMMAND_YAW] =
    (stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW])/16.;

}
static void attitude_run_indi(int32_t indi_commands[], struct Int32Quat *att_err, bool_t in_flight)
{
  //Calculate required angular acceleration
  struct FloatRates *body_rate = stateGetBodyRates_f();
  indi.angular_accel_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
                             - reference_acceleration.rate_p * body_rate->p;
  indi.angular_accel_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
                             - reference_acceleration.rate_q * body_rate->q;
  indi.angular_accel_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
                             - reference_acceleration.rate_r * body_rate->r;

  //Incremented in angular acceleration requires increment in control input
  //G1 is the actuator effectiveness. In the yaw axis, we need something additional: G2.
  //It takes care of the angular acceleration caused by the change in rotation rate of the propellers
  //(they have significant inertia, see the paper mentioned in the header for more explanation)
  indi.du.p = 1.0 / g1.p * (indi.angular_accel_ref.p - indi.filtered_rate_deriv.p);
  indi.du.q = 1.0 / g1.q * (indi.angular_accel_ref.q - indi.filtered_rate_deriv.q);
  indi.du.r = 1.0 / (g1.r + g2) * (indi.angular_accel_ref.r - indi.filtered_rate_deriv.r + g2 * indi.du.r);

  //add the increment to the total control input
  indi.u_in.p = indi.u.p + indi.du.p;
  indi.u_in.q = indi.u.q + indi.du.q;
  indi.u_in.r = indi.u.r + indi.du.r;

  //bound the total control input
  Bound(indi.u_in.p, -4500, 4500);
  Bound(indi.u_in.q, -4500, 4500);
  Bound(indi.u_in.r, -4500, 4500);

  //Propagate input filters
  //first order actuator dynamics
  indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
  indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
  indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);

  //sensor filter
  stabilization_indi_second_order_filter(&indi.u_act_dyn, &indi.udotdot, &indi.udot, &indi.u,
                                         STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);

  //Don't increment if thrust is off
  if (stabilization_cmd[COMMAND_THRUST] < 300) {
    FLOAT_RATES_ZERO(indi.u);
    FLOAT_RATES_ZERO(indi.du);
    FLOAT_RATES_ZERO(indi.u_act_dyn);
    FLOAT_RATES_ZERO(indi.u_in);
    FLOAT_RATES_ZERO(indi.udot);
    FLOAT_RATES_ZERO(indi.udotdot);
  } else {
#if STABILIZATION_INDI_USE_ADAPTIVE
#warning "Use caution with adaptive indi. See the wiki for more info"
    lms_estimation();
#endif
  }

  /*  INDI feedback */
  indi_commands[COMMAND_ROLL] = indi.u_in.p;
  indi_commands[COMMAND_PITCH] = indi.u_in.q;
  indi_commands[COMMAND_YAW] = indi.u_in.r;
}
Example #12
0
void ahrs_update_accel(void) {

    /* last column of roation matrix = ltp z-axis in imu-frame */
    struct FloatVect3  c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
               RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
               RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)
    };

    struct FloatVect3 imu_accel_float;
    ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);

    struct FloatVect3 residual;

    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
         */
        const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
        struct FloatVect3 acc_c_body;
        VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);

        /* convert centrifugal acceleration from body to imu frame */
        struct FloatVect3 acc_c_imu;
        FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);

        /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
        struct FloatVect3 corrected_gravity;
        VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu);

        /* compute the residual of gravity vector in imu frame */
        FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
    } else {
        FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2);
    }

#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
    /* heuristic on acceleration norm */
    const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float);
    const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
#else
    const float weight = 1.;
#endif

    /* compute correction */
    const float gravity_rate_update_gain = -5e-2; // -5e-2
    FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);

    const float gravity_bias_update_gain = 1e-5; // -5e-6
    FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);

    /* FIXME: saturate bias */

}
Example #13
0
inline static void h_ctl_yaw_loop(void)
{

#if H_CTL_YAW_TRIM_NY
  // Actual Acceleration from IMU:
#if (!defined SITL || defined USE_NPS)
  struct Int32Vect3 accel_meas_body, accel_ned;
  struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
  struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
  VECT3_COPY(accel_ned, (*accel_tmp));
  accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f);
  int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned);
  float ny = -ACCEL_FLOAT_OF_BFP(accel_meas_body.y) / 9.81f; // Lateral load factor (in g)
#else
  float ny = 0.f;
#endif

  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
    h_ctl_yaw_ny_sum_err = 0.;
  } else {
    if (h_ctl_yaw_ny_igain > 0.) {
      // only update when: phi<60degrees and ny<2g
      if (fabsf(stateGetNedToBodyEulers_f()->phi) < 1.05 && fabsf(ny) < 2.) {
        h_ctl_yaw_ny_sum_err += ny * H_CTL_REF_DT;
        // max half rudder deflection for trim
        BoundAbs(h_ctl_yaw_ny_sum_err, MAX_PPRZ / (2. * h_ctl_yaw_ny_igain));
      }
    } else {
      h_ctl_yaw_ny_sum_err = 0.;
    }
  }
#endif

#ifdef USE_AIRSPEED
  float Vo = stateGetAirspeed_f();
  Bound(Vo, STALL_AIRSPEED, RACE_AIRSPEED);
#else
  float Vo = NOMINAL_AIRSPEED;
#endif

  h_ctl_ref.yaw_rate = h_ctl_yaw_rate_setpoint // set by RC
                       + 9.81f / Vo * sinf(h_ctl_roll_setpoint); // for turns
  float d_err = h_ctl_ref.yaw_rate - stateGetBodyRates_f()->r;

  float cmd = + h_ctl_yaw_dgain * d_err
#if H_CTL_YAW_TRIM_NY
              + h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err
#endif
              ;
  cmd /= airspeed_ratio2;
  h_ctl_rudder_setpoint = TRIM_PPRZ(cmd);
}
Example #14
0
void mf_daq_send_state(void)
{
    // Send aircraft state to DAQ board
    DOWNLINK_SEND_MF_DAQ_STATE(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
                               &autopilot_flight_time,
                               &stateGetBodyRates_f()->p,
                               &stateGetBodyRates_f()->q,
                               &stateGetBodyRates_f()->r,
                               &stateGetNedToBodyEulers_f()->phi,
                               &stateGetNedToBodyEulers_f()->theta,
                               &stateGetNedToBodyEulers_f()->psi,
                               &stateGetAccelNed_f()->x,
                               &stateGetAccelNed_f()->y,
                               &stateGetAccelNed_f()->z,
                               &stateGetSpeedEnu_f()->x,
                               &stateGetSpeedEnu_f()->y,
                               &stateGetSpeedEnu_f()->z,
                               &stateGetPositionLla_f()->lat,
                               &stateGetPositionLla_f()->lon,
                               &stateGetPositionLla_f()->alt,
                               &stateGetHorizontalWindspeed_f()->y,
                               &stateGetHorizontalWindspeed_f()->x);
}
inline static void h_ctl_roll_loop( void ) {
  float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint;
  struct FloatRates* body_rate = stateGetBodyRates_f();
#ifdef SITL
  static float last_err = 0;
  body_rate->p = (err - last_err)/(1/60.);
  last_err = err;
#endif
  float cmd = h_ctl_roll_attitude_gain * err
    + h_ctl_roll_rate_gain * body_rate->p
    + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;

  h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
static void send_att(void) {
  struct FloatRates* body_rate = stateGetBodyRates_f();
  struct FloatEulers* att = stateGetNedToBodyEulers_f();
  DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(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],
                                    &body_rate_d.p, &body_rate_d.q, &body_rate_d.r);
}
static void send_att(struct transport_tx *trans, struct link_device *dev) {
  struct FloatRates* body_rate = stateGetBodyRates_f();
  struct FloatEulers* att = stateGetNedToBodyEulers_f();
  float foo = 0.0;
  pprz_msg_send_STAB_ATTITUDE_FLOAT(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.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],
      &foo, &foo, &foo);
}
Example #18
0
void CN_potential_velocity(void)
{
  float OF_Result_Vy = 0;
  float OF_Result_Vx = 0;

  //Constants
  float new_heading;
  float alpha_fil = 0.2;

  //Initialize
  float potential_obst = 0;  //define potential field variables
  float potential_obst_integrated = 0;
  float Distance_est;
  float fp_angle;
  float diff_angle;
  float total_vel;
  float current_heading;
  float angle_hor = - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2] /
                    2; //calculate position angle of stereo

  //Tune
  //float dt = 0.5;  //define delta t for integration! check response under 0.1 and 1
  int8_t min_disparity = 40;

  //Current state of system;
  float r_old = stateGetBodyRates_f()->r;
  current_heading = stateGetNedToBodyEulers_f()->psi; //check maybe cause of errors
  //current_heading = 0;

  total_vel = pow((OF_Result_Vy * OF_Result_Vy + OF_Result_Vx * OF_Result_Vx), 0.5);

  if (total_vel > vmin) {
    fp_angle = atan2(OF_Result_Vx, OF_Result_Vy);
  } else {
    fp_angle = 0;
  }

  heading_goal_ref = (fp_angle + current_heading) - heading_goal_f;
  FLOAT_ANGLE_NORMALIZE(heading_goal_ref);


  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
    angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
                / 2; //Check if bodyframe is correct with current_heading correction
    for (int i3 = 0; i3 < size_matrix[2]; i3++) {
      angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
      FLOAT_ANGLE_NORMALIZE(angle_hor);

      for (int i2 = 0; i2 < size_matrix[1]; i2++) {

        if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
          Distance_est = ((baseline * (float)focal / (float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
                           size_matrix[2] + i3] - 18.0)) / 1000;
        } else {
          Distance_est = 2000;
        }
        diff_angle = fp_angle - angle_hor;
        FLOAT_ANGLE_NORMALIZE(diff_angle);
        potential_obst = potential_obst + K_obst * (diff_angle) * exp(-c3_oa * fabs(diff_angle)) * exp(
                           -c4_oa * Distance_est); //(tan(obst_width[i]+c5)-tan(c5));
        potential_obst_integrated = potential_obst_integrated + K_obst * c3_oa * (fabs(diff_angle) + 1) / (c3_oa * c3_oa) * exp(
                                      -c3_oa * fabs(diff_angle)) * exp(-c4_oa * Distance_est); // (tan(obst_width[i]+c5)-tan(c5));

      }
    }
  }

  //calculate angular accelaration from potential field
  r_dot_new = -b_damp * r_old - K_goal * (heading_goal_ref) * (exp(-c1_oa * sqrt(VECT2_NORM2(
                pos_diff))) + c2_oa) + potential_obst;

  // Calculate velocity from potential
  speed_pot = vref_max * exp(-kv * potential_obst_integrated) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff))) - epsilon;
  if (speed_pot <= 0) {
    speed_pot = 0;
  }

  //calculate new_ref_speeds
  new_heading = fp_angle + alpha_fil * r_dot_new;
  FLOAT_ANGLE_NORMALIZE(new_heading);

  ref_pitch = new_heading;
}
Example #19
0
void CN_potential_heading(void)
{
  //float OF_Result_Vy = 0;
  //float OF_Result_Vx = 0

  //Initialize
  float potential_obst = 0;  //define potential field variables
  float potential_obst_temp = 0;
  float potential_obst_integrated = 0;
  float Distance_est;
  float angle_hor = - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2] /
                    2; //calculate position angle of stereo
  float angle_hor_abs;
  float obst_count = 0.0;
  //float total_vel;
  //float current_heading;
  //float fp_angle;

  //Tune
  //float dt = 0.5;  //define delta t for integration! check response under 0.1 and 1
  int8_t min_disparity = 40;

  //Current state of system;
  float r_old = stateGetBodyRates_f()->r;
  //current_heading = stateGetNedToBodyEulers_f()->psi; //check maybe cause of errors
  //current_heading = 0;

  //check if side slip is zero
  //total_vel = pow((OF_Result_Vy*OF_Result_Vy + OF_Result_Vx*OF_Result_Vx),0.5);

  //if (total_vel>vmin){
  //    fp_angle = atan2(OF_Result_Vx,OF_Result_Vy);
  //}
  //else{
  //    fp_angle = 0;
  //}
  //fp_angle = 0;

  heading_goal_ref = stateGetNedToBodyEulers_f()->psi - heading_goal_f;//
  FLOAT_ANGLE_NORMALIZE(heading_goal_ref);


  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
    angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
                / 2; //Check if bodyframe is correct with current_heading correction
    for (int i3 = 0; i3 < size_matrix[2]; i3++) {

      angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
      FLOAT_ANGLE_NORMALIZE(angle_hor);
      potential_obst_temp = 0.0;
      obst_count = 0;

      for (int i2 = 0; i2 < 4; i2++) {

        if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
          Distance_est = ((baseline * (float)focal / (float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
                           size_matrix[2] + i3] - 18.0)) / 1000;

          if (angle_hor < 0) {
            angle_hor_abs = -1 * angle_hor;
          } else {
            angle_hor_abs = angle_hor;
          }
          potential_obst_temp = potential_obst_temp - K_obst * (angle_hor) * exp(-c3_oa * angle_hor_abs) * exp(
                                  -c4_oa * Distance_est);//(tan(obst_width[i]+c5)-tan(c5));
          potential_obst_integrated = potential_obst_integrated + K_obst * c3_oa * (fabs(angle_hor) + 1) / (c3_oa * c3_oa) * exp(
                                        -c3_oa * fabs(angle_hor)) * exp(-c4_oa * Distance_est); // (tan(obst_width[i]+c5)-tan(c5));
          //printf("angle_hor: %f, angle_hor_abs: %f, c3: %f, potential_obst: %f", angle_hor, angle_hor_abs,c3_oa,potential_obst);
          obst_count++;

        } else {
          Distance_est = 2000;
        }

        //potential_obst = potential_obst + K_obst*(angle_hor);//*exp(-c3*abs(angle_hor))*exp(-c4*Distance_est);//(tan(obst_width[i]+c5)-tan(c5));
        //potential_obst_write = potential_obst;
        //potential_obst_integrated = potential_obst_integrated + K_obst*c3*(abs(angle_hor)+1)/(c3*c3)*exp(-c3*abs(angle_hor))*exp(-c4*Distance_est);// (tan(obst_width[i]+c5)-tan(c5));

      }
      if (obst_count != 0) {
        potential_obst = potential_obst + potential_obst_temp / ((float)obst_count);
      }
    }
  }

  //printf("current_heading, %f heading_goal_f: %f",current_heading, heading_goal_ref);
  //calculate angular accelaration from potential field

  r_dot_new = -b_damp * r_old - K_goal * (heading_goal_ref) * (exp(-c1_oa * sqrt(VECT2_NORM2(
                pos_diff))) + c2_oa) + potential_obst;

  // Calculate velocity from potential
  speed_pot = vref_max * exp(-kv * potential_obst_integrated) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff))) - epsilon;
  if (speed_pot <= 0) {
    speed_pot = 0;
  }

  //calculate new_heading(ref_pitch)
  //ref_pitch = (current_state(3)+sideslip) + alpha_fil*r_dot_new;)

}
Example #20
0
static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct Int32Quat *att_err, bool_t rate_control)
{
  /* Propagate the second order filter on the gyroscopes */
  struct FloatRates *body_rates = stateGetBodyRates_f();
  stabilization_indi_second_order_filter(&indi.rate, body_rates);

#if STABILIZATION_INDI_FILTER_ROLL_RATE
  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
                             - indi.reference_acceleration.rate_p * indi.rate.x.p;
#else
  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
                             - indi.reference_acceleration.rate_p * body_rates->p;
#endif
#if STABILIZATION_INDI_FILTER_PITCH_RATE
  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
                             - indi.reference_acceleration.rate_q * indi.rate.x.q;
#else
  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
                             - indi.reference_acceleration.rate_q * body_rates->q;
#endif
#if STABILIZATION_INDI_FILTER_YAW_RATE
  indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
                             - indi.reference_acceleration.rate_r * indi.rate.x.r;
#else
  indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
                             - indi.reference_acceleration.rate_r * body_rates->r;
#endif

  /* Check if we are running the rate controller and overwrite */
  if(rate_control) {
    indi.angular_accel_ref.p =  indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL]  / MAX_PPRZ * indi.max_rate - body_rates->p);
    indi.angular_accel_ref.q =  indi.reference_acceleration.rate_q * ((float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * indi.max_rate - body_rates->q);
    indi.angular_accel_ref.r =  indi.reference_acceleration.rate_r * ((float)radio_control.values[RADIO_YAW]   / MAX_PPRZ * indi.max_rate - body_rates->r);
  }

  //Incremented in angular acceleration requires increment in control input
  //G1 is the actuator effectiveness. In the yaw axis, we need something additional: G2.
  //It takes care of the angular acceleration caused by the change in rotation rate of the propellers
  //(they have significant inertia, see the paper mentioned in the header for more explanation)
  indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate.dx.p);
  indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate.dx.q);
  indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate.dx.r + indi.g2 * indi.du.r);

  //add the increment to the total control input
  indi.u_in.p = indi.u.x.p + indi.du.p;
  indi.u_in.q = indi.u.x.q + indi.du.q;
  indi.u_in.r = indi.u.x.r + indi.du.r;

  //bound the total control input
  Bound(indi.u_in.p, -4500, 4500);
  Bound(indi.u_in.q, -4500, 4500);
  Bound(indi.u_in.r, -4500, 4500);

  //Propagate input filters
  //first order actuator dynamics
  indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
  indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
  indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);

  //sensor filter
  stabilization_indi_second_order_filter(&indi.u, &indi.u_act_dyn);

  //Don't increment if thrust is off
  if (stabilization_cmd[COMMAND_THRUST] < 300) {
    FLOAT_RATES_ZERO(indi.du);
    FLOAT_RATES_ZERO(indi.u_act_dyn);
    FLOAT_RATES_ZERO(indi.u_in);
    FLOAT_RATES_ZERO(indi.u.x);
    FLOAT_RATES_ZERO(indi.u.dx);
    FLOAT_RATES_ZERO(indi.u.ddx);
  } else {
    lms_estimation();
  }

  /*  INDI feedback */
  indi_commands[COMMAND_ROLL] = indi.u_in.p;
  indi_commands[COMMAND_PITCH] = indi.u_in.q;
  indi_commands[COMMAND_YAW] = indi.u_in.r;
}
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);
}
static void send_tune_roll(void) {
  DOWNLINK_SEND_TUNE_ROLL(DefaultChannel, DefaultDevice,
      &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
}
inline static void h_ctl_pitch_loop( void ) {
#if !USE_GYRO_PITCH_RATE
  static float last_err;
#endif

  /* sanity check */
  if (h_ctl_pitch_of_roll <0.)
    h_ctl_pitch_of_roll = 0.;

  h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi);
#if USE_PITCH_TRIM
  loiter();
#endif

#if USE_ANGLE_REF
  // Update reference setpoints for pitch
  h_ctl_ref.pitch_angle += h_ctl_ref.pitch_rate * H_CTL_REF_DT;
  h_ctl_ref.pitch_rate += h_ctl_ref.pitch_accel * H_CTL_REF_DT;
  h_ctl_ref.pitch_accel = H_CTL_REF_W_Q*H_CTL_REF_W_Q * (h_ctl_pitch_loop_setpoint - h_ctl_ref.pitch_angle) - 2*H_CTL_REF_XI_Q*H_CTL_REF_W_Q * h_ctl_ref.pitch_rate;
  // Saturation on references
  BoundAbs(h_ctl_ref.pitch_accel, h_ctl_ref.max_q_dot);
  if (h_ctl_ref.pitch_rate > h_ctl_ref.max_q) {
    h_ctl_ref.pitch_rate = h_ctl_ref.max_q;
    if (h_ctl_ref.pitch_accel > 0.) h_ctl_ref.pitch_accel = 0.;
  }
  else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) {
    h_ctl_ref.pitch_rate = - h_ctl_ref.max_q;
    if (h_ctl_ref.pitch_accel < 0.) h_ctl_ref.pitch_accel = 0.;
  }
#else
  h_ctl_ref.pitch_angle = h_ctl_pitch_loop_setpoint;
  h_ctl_ref.pitch_rate = 0.;
  h_ctl_ref.pitch_accel = 0.;
#endif

  // Compute errors
  float err =  h_ctl_ref.pitch_angle - stateGetNedToBodyEulers_f()->theta;
#if USE_GYRO_PITCH_RATE
  float d_err = h_ctl_ref.pitch_rate - stateGetBodyRates_f()->q;
#else // soft derivation
  float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref.pitch_rate;
  last_err = err;
#endif

  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
    h_ctl_pitch_sum_err = 0.;
  }
  else {
    if (h_ctl_pitch_igain > 0.) {
      h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
      BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain);
    } else {
      h_ctl_pitch_sum_err = 0.;
    }
  }

  float cmd = - h_ctl_pitch_Kffa * h_ctl_ref.pitch_accel
    - h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate
    + h_ctl_pitch_pgain * err
    + h_ctl_pitch_dgain * d_err
    + h_ctl_pitch_igain * h_ctl_pitch_sum_err;

  cmd /= airspeed_ratio2;

  h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
inline static void h_ctl_roll_loop( void ) {

  static float cmd_fb = 0.;

#if USE_ANGLE_REF
  // Update reference setpoints for roll
  h_ctl_ref.roll_angle += h_ctl_ref.roll_rate * H_CTL_REF_DT;
  h_ctl_ref.roll_rate += h_ctl_ref.roll_accel * H_CTL_REF_DT;
  h_ctl_ref.roll_accel = H_CTL_REF_W_P*H_CTL_REF_W_P * (h_ctl_roll_setpoint - h_ctl_ref.roll_angle) - 2*H_CTL_REF_XI_P*H_CTL_REF_W_P * h_ctl_ref.roll_rate;
  // Saturation on references
  BoundAbs(h_ctl_ref.roll_accel, h_ctl_ref.max_p_dot);
  if (h_ctl_ref.roll_rate > h_ctl_ref.max_p) {
    h_ctl_ref.roll_rate = h_ctl_ref.max_p;
    if (h_ctl_ref.roll_accel > 0.) h_ctl_ref.roll_accel = 0.;
  }
  else if (h_ctl_ref.roll_rate < - h_ctl_ref.max_p) {
    h_ctl_ref.roll_rate = - h_ctl_ref.max_p;
    if (h_ctl_ref.roll_accel < 0.) h_ctl_ref.roll_accel = 0.;
  }
#else
  h_ctl_ref.roll_angle = h_ctl_roll_setpoint;
  h_ctl_ref.roll_rate = 0.;
  h_ctl_ref.roll_accel = 0.;
#endif

#ifdef USE_KFF_UPDATE
  // update Kff gains
  h_ctl_roll_Kffa += KFFA_UPDATE * h_ctl_ref.roll_accel * cmd_fb / (h_ctl_ref.max_p_dot*h_ctl_ref.max_p_dot);
  h_ctl_roll_Kffd += KFFD_UPDATE * h_ctl_ref.roll_rate  * cmd_fb / (h_ctl_ref.max_p*h_ctl_ref.max_p);
#ifdef SITL
  printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
#endif
  h_ctl_roll_Kffa = Max(h_ctl_roll_Kffa, 0);
  h_ctl_roll_Kffd = Max(h_ctl_roll_Kffd, 0);
#endif

  // Compute errors
  float err = h_ctl_ref.roll_angle - stateGetNedToBodyEulers_f()->phi;
  struct FloatRates* body_rate = stateGetBodyRates_f();
  float d_err = h_ctl_ref.roll_rate - body_rate->p;

  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
    h_ctl_roll_sum_err = 0.;
  }
  else {
    if (h_ctl_roll_igain > 0.) {
      h_ctl_roll_sum_err += err * H_CTL_REF_DT;
      BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain);
    } else {
      h_ctl_roll_sum_err = 0.;
    }
  }

  cmd_fb = h_ctl_roll_attitude_gain * err;
  float cmd = - h_ctl_roll_Kffa * h_ctl_ref.roll_accel
    - h_ctl_roll_Kffd * h_ctl_ref.roll_rate
    - cmd_fb
    - h_ctl_roll_rate_gain * d_err
    - h_ctl_roll_igain * h_ctl_roll_sum_err
    + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;

  cmd /= airspeed_ratio2;

  // Set aileron commands
  h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
static void send_tune_roll(struct transport_tx *trans, struct link_device *dev) {
  pprz_msg_send_TUNE_ROLL(trans, dev, AC_ID,
      &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
}
void stabilization_attitude_run(bool_t enable_integrator) {

  /*
   * Update reference
   */
  stabilization_attitude_ref_update();

  /*
   * Compute errors for feedback
   */

  /* attitude error                          */
  struct FloatQuat att_err;
  struct FloatQuat* att_quat = stateGetNedToBodyQuat_f();
  FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_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, stab_att_ref_rate, *body_rate);
  /* rate_d error               */
  struct FloatRates body_rate_d;
  RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
  RATES_COPY(last_body_rate, *body_rate);

  /* integrated error */
  if (enable_integrator) {
    struct FloatQuat 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;
    FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
    FLOAT_QUAT_NORMALIZE(new_sum_err);
    FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
    FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
  } else {
    /* reset accumulator */
    FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
    FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
  }

  attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_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);
}
Example #27
0
void ahrs_update_accel(void) {

  /* last column of roation matrix = ltp z-axis in imu-frame */
  struct FloatVect3  c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
                            RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
                            RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)};

  struct FloatVect3 imu_accel_float;
  ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);

  struct FloatVect3 residual;

  struct FloatVect3 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
     */
    const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
    struct FloatVect3 acc_c_body;
    VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);

    /* convert centrifugal acceleration from body to imu frame */
    struct FloatVect3 acc_c_imu;
    FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.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_float, acc_c_imu);

  } else {
    VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
  }

  FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);

  /* FIR filtered pseudo_gravity_measurement */
  #define FIR_FILTER_SIZE 8
  static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1);
  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);

  if (ahrs_impl.gravity_heuristic_factor) {
    /* heuristic on acceleration (gravity estimate) norm */
    /* Factor how strongly to change the weight.
     * e.g. for gravity_heuristic_factor 30:
     * <0.66G = 0, 1G = 1.0, >1.33G = 0
     */

    const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81;
    ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
    Bound(ahrs_impl.weight, 0.15, 1.0);
  }
  else {
    ahrs_impl.weight = 1.0;
  }

  /* Complementary filter proportional gain.
   * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
   */
  const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega *
    ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81);
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);

  /* Complementary filter integral gain
   * Correct the gyro bias.
   * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
   */
  const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega *
    ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81);
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);

  /* FIXME: saturate bias */

}
Example #28
0
static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct Int32Quat *att_err, bool rate_control)
{
  /* Propagate the second order filter on the gyroscopes */
  struct FloatRates *body_rates = stateGetBodyRates_f();
  stabilization_indi_second_order_filter(&indi.rate, body_rates);

  //The rates used for feedback are by default the measured rates. If needed they can be filtered (see below)
  struct FloatRates rates_for_feedback;
  RATES_COPY(rates_for_feedback, (*body_rates));

  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
  //Note that due to the delay, the PD controller can not be as aggressive.
#if STABILIZATION_INDI_FILTER_ROLL_RATE
  rates_for_feedback.p = indi.rate.x.p;
#endif
#if STABILIZATION_INDI_FILTER_PITCH_RATE
  rates_for_feedback.q = indi.rate.x.q;
#endif
#if STABILIZATION_INDI_FILTER_YAW_RATE
  rates_for_feedback.r = indi.rate.x.r;
#endif

  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
                             - indi.reference_acceleration.rate_p * rates_for_feedback.p;

  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
                             - indi.reference_acceleration.rate_q * rates_for_feedback.q;

  //This separates the P and D controller and lets you impose a maximum yaw rate.
  float rate_ref_r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)/indi.reference_acceleration.rate_r;
  BoundAbs(rate_ref_r, indi.attitude_max_yaw_rate);
  indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref_r - rates_for_feedback.r);

  /* Check if we are running the rate controller and overwrite */
  if(rate_control) {
    indi.angular_accel_ref.p =  indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL]  / MAX_PPRZ * indi.max_rate - body_rates->p);
    indi.angular_accel_ref.q =  indi.reference_acceleration.rate_q * ((float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * indi.max_rate - body_rates->q);
    indi.angular_accel_ref.r =  indi.reference_acceleration.rate_r * ((float)radio_control.values[RADIO_YAW]   / MAX_PPRZ * indi.max_rate - body_rates->r);
  }

  //Increment in angular acceleration requires increment in control input
  //G1 is the control effectiveness. In the yaw axis, we need something additional: G2.
  //It takes care of the angular acceleration caused by the change in rotation rate of the propellers
  //(they have significant inertia, see the paper mentioned in the header for more explanation)
  indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate.dx.p);
  indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate.dx.q);
  indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate.dx.r + indi.g2 * indi.du.r);

  //add the increment to the total control input
  indi.u_in.p = indi.u.x.p + indi.du.p;
  indi.u_in.q = indi.u.x.q + indi.du.q;
  indi.u_in.r = indi.u.x.r + indi.du.r;

  //bound the total control input
  Bound(indi.u_in.p, -4500, 4500);
  Bound(indi.u_in.q, -4500, 4500);
  Bound(indi.u_in.r, -4500, 4500);

  //Propagate input filters
  //first order actuator dynamics
  indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
  indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
  indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);

  //sensor filter
  stabilization_indi_second_order_filter(&indi.u, &indi.u_act_dyn);

  //Don't increment if thrust is off
  //TODO: this should be something more elegant, but without this the inputs will increment to the maximum before
  //even getting in the air.
  if (stabilization_cmd[COMMAND_THRUST] < 300) {
    FLOAT_RATES_ZERO(indi.du);
    FLOAT_RATES_ZERO(indi.u_act_dyn);
    FLOAT_RATES_ZERO(indi.u_in);
    FLOAT_RATES_ZERO(indi.u.x);
    FLOAT_RATES_ZERO(indi.u.dx);
    FLOAT_RATES_ZERO(indi.u.ddx);
  } else {
    // only run the estimation if the commands are not zero.
    lms_estimation();
  }

  /*  INDI feedback */
  indi_commands[COMMAND_ROLL] = indi.u_in.p;
  indi_commands[COMMAND_PITCH] = indi.u_in.q;
  indi_commands[COMMAND_YAW] = indi.u_in.r;
}