예제 #1
0
/* Parse the InterMCU message */
static inline void mag_pitot_parse_msg(void)
{
  uint32_t now_ts = get_sys_time_usec();

  /* Parse the mag-pitot message */
  uint8_t msg_id = pprzlink_get_msg_id(mp_msg_buf);

#if PPRZLINK_DEFAULT_VER == 2
  // Check that the message is really a intermcu message
  if (pprzlink_get_msg_class_id(mp_msg_buf) == DL_intermcu_CLASS_ID) {
#endif
  switch (msg_id) {
  /* Got a magneto message */
  case DL_IMCU_REMOTE_MAG: {
    struct Int32Vect3 raw_mag;

    // Read the raw magneto information
    raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf);
    raw_mag.y = DL_IMCU_REMOTE_MAG_mag_y(mp_msg_buf);
    raw_mag.z = DL_IMCU_REMOTE_MAG_mag_z(mp_msg_buf);

    // Rotate the magneto
    struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag);
    int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &raw_mag);

    // Send and set the magneto IMU data
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_MAG_PITOT_ID, now_ts, &imu.mag);
    break;
  }

  /* Got a barometer message */
  case DL_IMCU_REMOTE_BARO: {
    float pitot_stat = DL_IMCU_REMOTE_BARO_pitot_stat(mp_msg_buf);
    float pitot_temp = DL_IMCU_REMOTE_BARO_pitot_temp(mp_msg_buf);

    AbiSendMsgBARO_ABS(IMU_MAG_PITOT_ID, pitot_stat);
    AbiSendMsgTEMPERATURE(IMU_MAG_PITOT_ID, pitot_temp);
    break;
  }

  /* Got an airspeed message */
  case DL_IMCU_REMOTE_AIRSPEED: {
    // Should be updated to differential pressure
    float pitot_ias = DL_IMCU_REMOTE_AIRSPEED_pitot_IAS(mp_msg_buf);
    AbiSendMsgAIRSPEED(IMU_MAG_PITOT_ID, pitot_ias);
    break;
  }

    default:
      break;
  }
#if PPRZLINK_DEFAULT_VER == 2
  }
#endif
}
예제 #2
0
inline static void h_ctl_cl_loop(void)
{

#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
#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 nz = -ACCEL_FLOAT_OF_BFP(accel_meas_body.z) / 9.81f;
  // max load factor to be taken into acount
  // to prevent negative flap movement du to negative acceleration
  Bound(nz, 1.f, 2.f);
#else
  float nz = 1.f;
#endif
#endif

  // Compute a corrected airspeed corresponding to the current load factor nz
  // with Cz the lift coef at 1g, Czn the lift coef at n g, both at the same speed V,
  // the corrected airspeed Vn is so that nz = Czn/Cz = V^2 / Vn^2,
  // thus Vn = V / sqrt(nz)
#if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT
  float corrected_airspeed = v_ctl_auto_airspeed_setpoint;
#else
  float corrected_airspeed = stateGetAirspeed_f();
#endif
#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
  corrected_airspeed /= sqrtf(nz);
#endif
  Bound(corrected_airspeed, STALL_AIRSPEED, RACE_AIRSPEED);

  float cmd = 0.f;
  // deadband around NOMINAL_AIRSPEED, rest linear
  if (corrected_airspeed > NOMINAL_AIRSPEED + H_CTL_CL_DEADBAND) {
    cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_RACE - H_CTL_CL_FLAPS_NOMINAL) / (RACE_AIRSPEED - NOMINAL_AIRSPEED);
  }
  else if (corrected_airspeed < NOMINAL_AIRSPEED - H_CTL_CL_DEADBAND) {
    cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_STALL - H_CTL_CL_FLAPS_NOMINAL) / (STALL_AIRSPEED - NOMINAL_AIRSPEED);
  }

  // no control in manual mode
  if (pprz_mode == PPRZ_MODE_MANUAL) {
    cmd = 0.f;
  }
  // bound max flap angle
  Bound(cmd, H_CTL_CL_FLAPS_RACE, H_CTL_CL_FLAPS_STALL);
  // from percent to pprz
  cmd = cmd * MAX_PPRZ;
  h_ctl_flaps_setpoint = TRIM_PPRZ(cmd);
}
예제 #3
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);
}
예제 #4
0
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
{
  // check if we had at least one propagation since last update
  if (ahrs_icq.accel_cnt == 0) {
    return;
  }

  // c2 = ltp z-axis in imu-frame
  struct Int32RMat ltp_to_imu_rmat;
  int32_rmat_of_quat(&ltp_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
  struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0, 2),
           RMAT_ELMT(ltp_to_imu_rmat, 1, 2),
           RMAT_ELMT(ltp_to_imu_rmat, 2, 2)
  };
  struct Int32Vect3 residual;

  struct Int32Vect3 pseudo_gravity_measurement;

  if (ahrs_icq.correct_gravity && ahrs_icq.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
     */

    // FIXME: check overflows !
#define COMPUTATION_FRAC 16
#define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC

    const struct Int32Vect3 vel_tangential_body =
      {ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
    struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu);
    struct Int32Rates body_rate;
    int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate);
    struct Int32Vect3 acc_c_body;
    VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
    INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC);

    /* convert centrifucal acceleration from body to imu frame */
    struct Int32Vect3 acc_c_imu;
    int32_rmat_vmult(&acc_c_imu, 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, *accel, acc_c_imu);
  } else {
예제 #5
0
/**
 * Handle all the events of the Navstik IMU components.
 * When there is data available convert it to the correct axis and save it in the imu structure.
 */
void imu_bebop_event(void)
{
  uint32_t now_ts = get_sys_time_usec();

  /* MPU-60x0 event taks */
  mpu60x0_i2c_event(&imu_bebop.mpu);

  if (imu_bebop.mpu.data_available) {
    /* default orientation of the MPU is upside down sor corrigate this here */
    RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q,
                 -imu_bebop.mpu.data_rates.rates.r);
    VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y,
                 -imu_bebop.mpu.data_accel.vect.z);

    imu_bebop.mpu.data_available = false;
    imu_scale_gyro(&imu);
    imu_scale_accel(&imu);
    AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
    AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
  }

  /* AKM8963 event task */
  ak8963_event(&imu_bebop.ak);

  if (imu_bebop.ak.data_available) {
#if BEBOP_VERSION2
    struct Int32Vect3 mag_temp;
    // In the second bebop version the magneto is turned 90 degrees
    VECT3_ASSIGN(mag_temp, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z);

    // Rotate the magneto
    struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&imu_to_mag_bebop);
    int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &mag_temp);
#else //BEBOP regular first verion
    VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z);
#endif

    imu_bebop.ak.data_available = false;
    imu_scale_mag(&imu);
    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
  }
}