Exemple #1
0
void imu_init(void) {

  /* initialises neutrals */
  RATES_ASSIGN(imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);
  VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
  VECT3_ASSIGN(imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL,   IMU_MAG_Z_NEUTRAL);

  /*
    Compute quaternion and rotation matrix
    for conversions between body and imu frame
  */
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined  IMU_BODY_TO_IMU_PSI
  struct Int32Eulers body_to_imu_eulers =
    { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) };
  INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
  INT32_QUAT_NORMALISE(imu.body_to_imu_quat);
  INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
#else
  INT32_QUAT_ZERO(imu.body_to_imu_quat);
  INT32_RMAT_ZERO(imu.body_to_imu_rmat);
#endif

  imu_impl_init();
}
// reset to "hover" setpoint
static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initial)
{
  int32_t pitch_rotation_angle;
  struct Int32Quat pitch_axis_quat;

  struct Int32Quat pitch_rotated_quat, pitch_rotated_quat2;

  struct Int32Vect3 y_axis = { 0, 1, 0 };

  struct Int32Eulers rotated_eulers;

  // compose rotation about Y axis (pitch axis) from hover
  pitch_rotation_angle = ANGLE_BFP_OF_REAL(-QUAT_SETPOINT_HOVER_PITCH);
  INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle);
  INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat, *initial, pitch_axis_quat);

  INT32_EULERS_OF_QUAT(rotated_eulers, pitch_rotated_quat);

  // reset euler angles
  rotated_eulers.theta = _theta;
  rotated_eulers.phi = _psi;

  INT32_QUAT_OF_EULERS(pitch_rotated_quat, rotated_eulers);

  // compose rotation about Y axis (pitch axis) to hover
  pitch_rotation_angle = ANGLE_BFP_OF_REAL(QUAT_SETPOINT_HOVER_PITCH);
  INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle);
  INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat2, pitch_rotated_quat, pitch_axis_quat);

  // store result into setpoint
  QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2);
}
Exemple #3
0
void imu_init(void) {

  /* initialises neutrals */
  RATES_ASSIGN(imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);

  VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);

#if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
  VECT3_ASSIGN(imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL,   IMU_MAG_Z_NEUTRAL);
#else
#if USE_MAGNETOMETER
#pragma message "Info: Magnetomter neutrals are set to zero!"
#endif
  INT_VECT3_ZERO(imu.mag_neutral);
#endif

  /*
    Compute quaternion and rotation matrix
    for conversions between body and imu frame
  */
  struct Int32Eulers body_to_imu_eulers =
    { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) };
  INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
  INT32_QUAT_NORMALIZE(imu.body_to_imu_quat);
  INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);

  imu_impl_init();
}
Exemple #4
0
void imu_init(void) {

#ifdef IMU_POWER_GPIO
  gpio_setup_output(IMU_POWER_GPIO);
  IMU_POWER_GPIO_ON(IMU_POWER_GPIO);
#endif

  /* initialises neutrals */
  RATES_ASSIGN(imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);

  VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);

#if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
  VECT3_ASSIGN(imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL,   IMU_MAG_Z_NEUTRAL);
#else
#if USE_MAGNETOMETER
INFO("Magnetometer neutrals are set to zero, you should calibrate!")
#endif
  INT_VECT3_ZERO(imu.mag_neutral);
#endif

  /*
    Compute quaternion and rotation matrix
    for conversions between body and imu frame
  */
  struct Int32Eulers body_to_imu_eulers =
    { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
      ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) };
  INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
  INT32_QUAT_NORMALIZE(imu.body_to_imu_quat);
  INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);

#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
#if USE_IMU_FLOAT
#else // !USE_IMU_FLOAT
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw);
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled);
  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled);
  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw);
  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled);
  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag);
#endif // !USE_IMU_FLOAT
#endif // DOWNLINK

  imu_impl_init();
}
/** Read attitude setpoint from RC as euler angles.
 * @param[in]  in_flight  true if in flight
 * @param[out] sp         attitude setpoint as euler angles
 */
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) {

  sp->phi = ((int32_t) radio_control.values[RADIO_ROLL]  * SP_MAX_PHI / MAX_PPRZ);
  sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);

  if (in_flight) {
    if (YAW_DEADBAND_EXCEEDED()) {
      sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
      INT32_ANGLE_NORMALIZE(sp->psi);
    }
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
    // Make sure the yaw setpoint does not differ too much from the real yaw
    // to prevent a sudden switch at 180 deg
    int32_t delta_psi = sp->psi - stateGetNedToBodyEulers_i()->psi;
    int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
    INT32_ANGLE_NORMALIZE(delta_psi);
    if (delta_psi > delta_limit){
      sp->psi = stateGetNedToBodyEulers_i()->psi + delta_limit;
    }
    else if (delta_psi < -delta_limit){
      sp->psi = stateGetNedToBodyEulers_i()->psi - delta_limit;
    }
    INT32_ANGLE_NORMALIZE(sp->psi);
#endif
    //Care Free mode
    if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) {
      //care_free_heading has been set to current psi when entering care free mode.
      int32_t cos_psi;
      int32_t sin_psi;
      int32_t temp_theta;
      int32_t care_free_delta_psi_i;

      care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);

      INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);

      PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
      PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);

      temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
      sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);

      sp->theta = temp_theta;
    }
  }
  else { /* if not flying, use current yaw as setpoint */
    sp->psi = stateGetNedToBodyEulers_i()->psi;
  }
}
static int32_t get_rc_yaw(void)
{
  const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
  int32_t yaw = radio_control.values[RADIO_YAW];
  DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
  return yaw * max_rc_r / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
}
static int32_t get_rc_pitch(void)
{
  const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
  int32_t pitch = radio_control.values[RADIO_PITCH];
#if STABILIZATION_ATTITUDE_DEADBAND_E
  DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E);
  return pitch * max_rc_theta / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E);
#else
  return pitch * max_rc_theta / MAX_PPRZ;
#endif
}
static int32_t get_rc_roll(void)
{
  const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
  int32_t roll = radio_control.values[RADIO_ROLL];
#if STABILIZATION_ATTITUDE_DEADBAND_A
  DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A);
  return roll * max_rc_phi / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A);
#else
  return roll * max_rc_phi / MAX_PPRZ;
#endif
}
Exemple #9
0
static void test_4_int(void) {

    printf("euler to quat to euler - int\n");
    /* initial euler angles */
    struct Int32Eulers _e;
    EULERS_ASSIGN(_e, ANGLE_BFP_OF_REAL(RadOfDeg(-10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("euler orig ", _e);

    /* transform to quaternion */
    struct Int32Quat _q;
    INT32_QUAT_OF_EULERS(_q, _e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q);
    //  INT32_QUAT_NORMALIZE(_q);
    //  DISPLAY_INT32_QUAT_2("_q_n", _q);

    /* back to eulers */
    struct Int32Eulers _e2;
    INT32_EULERS_OF_QUAT(_e2, _q);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("back to euler ", _e2);


}
Exemple #10
0
void aos_run(void)
{

  aos_compute_state();
  aos_compute_sensors();
#ifndef DISABLE_ALIGNEMENT
  if (ahrs.status == AHRS_UNINIT) {
    ahrs_aligner_run();
    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
      ahrs_align();
    }
  } else {
#endif /* DISABLE_ALIGNEMENT */
    ahrs_propagate(aos.dt);
    ahrs_update_accel();

#ifndef DISABLE_MAG_UPDATE
    ahrs_update_mag();
#endif


#if AHRS_USE_GPS_HEADING

#if AHRS_TYPE == AHRS_TYPE_ICQ
    int32_t heading = ANGLE_BFP_OF_REAL(aos.heading_meas);
#endif
#if AHRS_TYPE == AHRS_TYPE_FCQ
    float heading = aos.heading_meas;
#endif

#if AHRS_TYPE == AHRS_TYPE_FCR
    ahrs_impl.gps_course = aos.heading_meas;
    ahrs_impl.gps_course_valid = true;
#else
    if (aos.time > 10) {
      if (!ahrs_impl.heading_aligned) {
        ahrs_realign_heading(heading);
      } else {
        RunOnceEvery(100, ahrs_update_heading(heading));
      }
    }
#endif

#endif // AHRS_USE_GPS_HEADING

#ifndef DISABLE_ALIGNEMENT
  }
#endif

}
Exemple #11
0
void camera_mount_run(void) {
  // floating point version of DCM00
  const float dcm22 = TRIG_FLOAT_OF_BFP(ahrs.ltp_to_body_rmat.m[8]);

  // hover "pitch", derived from ltp to body rmat, rotated by +90 about Y axis
  int32_t hover_pitch = ANGLE_BFP_OF_REAL(-asinf(dcm22));

  commands[COMMAND_CAMERA] = CAMERA_MOUNT_HOVER + (hover_pitch * CAMERA_MOUNT_GAIN_NUM / CAMERA_MOUNT_GAIN_DEN);

  switch (guidance_h_mode) {
    case GUIDANCE_H_MODE_TOYTRONICS_FORWARD:
    case GUIDANCE_H_MODE_TOYTRONICS_AEROBATIC:
      commands[COMMAND_CAMERA] = CAMERA_MOUNT_FORWARD;
      break;

    default:
      break;
      // other modes do nothing, leave previous command in place
  }

  // add in user setpoint from transmitter
  commands[COMMAND_CAMERA] += radio_control.values[CAMERA_RADIO_CHANNEL];
  Bound(commands[COMMAND_CAMERA], MIN_PPRZ, MAX_PPRZ);
}
void guidance_flip_run(void)
{
  uint32_t timer;
  int32_t phi;
  static uint32_t timer_save = 0;

  timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY;
  phi = stateGetNedToBodyEulers_i()->phi;

  switch (flip_state) {
    case 0:
      flip_cmd_earth.x = 0;
      flip_cmd_earth.y = 0;
      stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
                                             heading_save);
      stabilization_attitude_run(autopilot_in_flight);
      stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
      timer_save = 0;

      if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) {
        flip_state++;
      }
      break;

    case 1:
      stabilization_cmd[COMMAND_ROLL]   = 9000; // Rolling command
      stabilization_cmd[COMMAND_PITCH]  = 0;
      stabilization_cmd[COMMAND_YAW]    = 0;
      stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?

      if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
        flip_state++;
      }
      break;

    case 2:
      stabilization_cmd[COMMAND_ROLL]   = 0;
      stabilization_cmd[COMMAND_PITCH]  = 0;
      stabilization_cmd[COMMAND_YAW]    = 0;
      stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?

      if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
        timer_save = timer;
        flip_state++;
      }
      break;

    case 3:
      flip_cmd_earth.x = 0;
      flip_cmd_earth.y = 0;
      stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
                                             heading_save);
      stabilization_attitude_run(autopilot_in_flight);

      stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling

      if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) {
        flip_state++;
      }
      break;

    default:
      autopilot_mode_auto2 = autopilot_mode_old;
      autopilot_set_mode(autopilot_mode_old);
      stab_att_sp_euler.psi = heading_save;
      flip_rollout = false;
      flip_counter = 0;
      timer_save = 0;
      flip_state = 0;

      stabilization_cmd[COMMAND_ROLL]   = 0;
      stabilization_cmd[COMMAND_PITCH]  = 0;
      stabilization_cmd[COMMAND_YAW]    = 0;
      stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
      break;
  }
}
/** Read attitude setpoint from RC as euler angles
 * @param[in]  coordinated_turn  true if in horizontal mode forward
 * @param[in]  in_carefree       true if in carefree mode
 * @param[in]  in_flight         true if in flight
 * @param[out] sp                attitude setpoint as euler angles
 */
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
  const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
  const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
  const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);

  sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) /  MAX_PPRZ);
  sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) /  MAX_PPRZ);

  if (in_flight) {
    /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
    if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
      sp->psi += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) /  MAX_PPRZ / RC_UPDATE_FREQ);
      INT32_ANGLE_NORMALIZE(sp->psi);
    }
    if (coordinated_turn) {
      //Coordinated turn
      //feedforward estimate angular rotation omega = g*tan(phi)/v
      //Take v = 9.81/1.3 m/s
      int32_t omega;
      const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0));
      if(abs(sp->phi) < max_phi)
        omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
      else //max 60 degrees roll, then take constant omega
        omega = ANGLE_BFP_OF_REAL(1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0)));

      sp->psi += omega/RC_UPDATE_FREQ;
    }
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
    // Make sure the yaw setpoint does not differ too much from the real yaw
    // to prevent a sudden switch at 180 deg
    const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);

    int32_t heading = stabilization_attitude_get_heading_i();

    int32_t delta_psi = sp->psi - heading;
    INT32_ANGLE_NORMALIZE(delta_psi);
    if (delta_psi > delta_limit){
      sp->psi = heading + delta_limit;
    }
    else if (delta_psi < -delta_limit){
      sp->psi = heading - delta_limit;
    }
    INT32_ANGLE_NORMALIZE(sp->psi);
#endif
    //Care Free mode
    if (in_carefree) {
      //care_free_heading has been set to current psi when entering care free mode.
      int32_t cos_psi;
      int32_t sin_psi;
      int32_t temp_theta;
      int32_t care_free_delta_psi_i;

      care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);

      INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);

      PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
      PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);

      temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
      sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);

      sp->theta = temp_theta;
    }
  }
  else { /* if not flying, use current yaw as setpoint */
    sp->psi = stateGetNedToBodyEulers_i()->psi;
  }
}
Exemple #14
0
/**
 * Update the controls based on a vision result
 * @param[in] *result The opticflow calculation result used for control
 */
void OA_update()
{
  float v_x = 0;
  float v_y = 0;

  if (opti_speed_flag == 1) {
    //rotation_vector.psi = stateGetNedToBodyEulers_f()->psi;
    //opti_speed = stateGetSpeedNed_f();

    //Transform to body frame.
    //float_rmat_of_eulers_312(&T, &rotation_vector)Attractforce_goal_send;
    //MAT33_VECT3_MUL(*opti_speed, T, *opti_speed);

    // Calculate the speed in body frame
    struct FloatVect2 speed_cur;
    float psi = stateGetNedToBodyEulers_f()->psi;
    float s_psi = sin(psi);
    float c_psi = cos(psi);
    speed_cur.x = c_psi * stateGetSpeedNed_f()->x + s_psi * stateGetSpeedNed_f()->y;
    speed_cur.y = -s_psi * stateGetSpeedNed_f()->x + c_psi * stateGetSpeedNed_f()->y;

    opti_speed_read.x = speed_cur.x * 100;
    opti_speed_read.y = speed_cur.y * 100;

    //set result_vel
    v_x = speed_cur.y * 100;
    v_y = speed_cur.x * 100;
  } else {
  }

  if (OA_method_flag == NO_OBSTACLE_AVOIDANCE) {
    /* Calculate the error if we have enough flow */
    opticflow_stab.desired_vx = 0;
    opticflow_stab.desired_vy = 0;

    err_vx = opticflow_stab.desired_vx - v_x;
    err_vy = opticflow_stab.desired_vy - v_y;

    /* Calculate the integrated errors (TODO: bound??) */
    opticflow_stab.err_vx_int += err_vx / 100;
    opticflow_stab.err_vy_int += err_vy / 100;

    /* Calculate the commands */
    opticflow_stab.cmd.phi   = opticflow_stab.phi_pgain * err_vx / 100
                               + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
    opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
                                 + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);

    /* Bound the roll and pitch commands */
    BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
    BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
  }

  if (OA_method_flag == PINGPONG) {
    opticflow_stab.cmd.phi = ANGLE_BFP_OF_REAL(ref_roll);
    opticflow_stab.cmd.theta = ANGLE_BFP_OF_REAL(ref_pitch);
  }

  if (OA_method_flag == 2) {
    Total_Kan_x = r_dot_new;
    Total_Kan_y = speed_pot;

    alpha_fil = 0.1;

    yaw_rate = (int32_t)(alpha_fil * ANGLE_BFP_OF_REAL(r_dot_new));
    opticflow_stab.cmd.psi  = stateGetNedToBodyEulers_i()->psi + yaw_rate;

    INT32_ANGLE_NORMALIZE(opticflow_stab.cmd.psi);

    opticflow_stab.desired_vx = 0;
    opticflow_stab.desired_vy = speed_pot;

    /* Calculate the error if we have enough flow */

    err_vx = opticflow_stab.desired_vx - v_x;
    err_vy = opticflow_stab.desired_vy - v_y;

    /* Calculate the integrated errors (TODO: bound??) */
    opticflow_stab.err_vx_int += err_vx / 100;
    opticflow_stab.err_vy_int += err_vy / 100;

    /* Calculate the commands */
    opticflow_stab.cmd.phi   = opticflow_stab.phi_pgain * err_vx / 100
                               + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;

    opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
                                 + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);

    /* Bound the roll and pitch commands */
    BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
    BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);

  }
  if (OA_method_flag == POT_HEADING) {
    new_heading = ref_pitch;

    opticflow_stab.desired_vx = sin(new_heading) * speed_pot * 100;
    opticflow_stab.desired_vy = cos(new_heading) * speed_pot * 100;

    /* Calculate the error if we have enough flow */
    err_vx = opticflow_stab.desired_vx - v_x;
    err_vy = opticflow_stab.desired_vy - v_y;


    /* Calculate the integrated errors (TODO: bound??) */
    opticflow_stab.err_vx_int += err_vx / 100;
    opticflow_stab.err_vy_int += err_vy / 100;

    /* Calculate the commands */
    opticflow_stab.cmd.phi   = opticflow_stab.phi_pgain * err_vx / 100
                               + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
    opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
                                 + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);

    /* Bound the roll and pitch commands */
    BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
    BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT)

  }
Exemple #15
0
static void test_3(void) {

    /* Compute BODY to IMU eulers */
    struct Int32Eulers b2i_e;
    EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e);

    /* Compute BODY to IMU quaternion */
    struct Int32Quat b2i_q;
    INT32_QUAT_OF_EULERS(b2i_q, b2i_e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
    //  INT32_QUAT_NORMALIZE(b2i_q);
    //  DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);

    /* Compute BODY to IMU rotation matrix */
    struct Int32RMat b2i_r;
    INT32_RMAT_OF_EULERS(b2i_r, b2i_e);
    //  DISPLAY_INT32_RMAT("b2i_r", b2i_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r);

    /* Compute LTP to IMU eulers */
    struct Int32Eulers l2i_e;
    EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e);

    /* Compute LTP to IMU quaternion */
    struct Int32Quat l2i_q;
    INT32_QUAT_OF_EULERS(l2i_q, l2i_e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q);

    /* Compute LTP to IMU rotation matrix */
    struct Int32RMat l2i_r;
    INT32_RMAT_OF_EULERS(l2i_r, l2i_e);
    //  DISPLAY_INT32_RMAT("l2i_r", l2i_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r);


    /* again but from quaternion */
    struct Int32RMat l2i_r2;
    INT32_RMAT_OF_QUAT(l2i_r2, l2i_q);
    //  DISPLAY_INT32_RMAT("l2i_r2", l2i_r2);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2);

    /* Compute LTP to BODY quaternion */
    struct Int32Quat l2b_q;
    INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);

    /* Compute LTP to BODY rotation matrix */
    struct Int32RMat l2b_r;
    INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r);
    //  DISPLAY_INT32_RMAT("l2b_r", l2b_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r);

    /* again but from quaternion */
    struct Int32RMat l2b_r2;
    INT32_RMAT_OF_QUAT(l2b_r2, l2b_q);
    //  DISPLAY_INT32_RMAT("l2b_r2", l2b_r2);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2);


    /* compute LTP to BODY eulers */
    struct Int32Eulers l2b_e;
    INT32_EULERS_OF_RMAT(l2b_e, l2b_r);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e);

    /* again but from quaternion */
    struct Int32Eulers l2b_e2;
    INT32_EULERS_OF_QUAT(l2b_e2, l2b_q);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2);

}
/** Read attitude setpoint from RC as euler angles
 * @param[in]  coordinated_turn  true if in horizontal mode forward
 * @param[in]  in_carefree       true if in carefree mode
 * @param[in]  in_flight         true if in flight
 * @param[out] sp                attitude setpoint as euler angles
 */
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
    bool coordinated_turn)
{
  /* last time this function was called, used to calculate yaw setpoint update */
  static float last_ts = 0.f;

  sp->phi = get_rc_roll();
  sp->theta = get_rc_pitch();

  if (in_flight) {
    /* calculate dt for yaw integration */
    float dt = get_sys_time_float() - last_ts;
    /* make sure nothing drastically weird happens, bound dt to 0.5sec */
    Bound(dt, 0, 0.5);

    /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
    if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
      sp->psi += get_rc_yaw() * dt;
      INT32_ANGLE_NORMALIZE(sp->psi);
    }
    if (coordinated_turn) {
      //Coordinated turn
      //feedforward estimate angular rotation omega = g*tan(phi)/v
      int32_t omega;
      const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(60.0));
      if (abs(sp->phi) < max_phi) {
        omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
      } else { //max 60 degrees roll
        omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)));
      }

      sp->psi += omega * dt;
    }
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
    // Make sure the yaw setpoint does not differ too much from the real yaw
    // to prevent a sudden switch at 180 deg
    const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);

    int32_t heading = stabilization_attitude_get_heading_i();

    int32_t delta_psi = sp->psi - heading;
    INT32_ANGLE_NORMALIZE(delta_psi);
    if (delta_psi > delta_limit) {
      sp->psi = heading + delta_limit;
    } else if (delta_psi < -delta_limit) {
      sp->psi = heading - delta_limit;
    }
    INT32_ANGLE_NORMALIZE(sp->psi);
#endif
    //Care Free mode
    if (in_carefree) {
      //care_free_heading has been set to current psi when entering care free mode.
      int32_t cos_psi;
      int32_t sin_psi;
      int32_t temp_theta;
      int32_t care_free_delta_psi_i;

      care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);

      INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);

      PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
      PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);

      temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi,
                   INT32_ANGLE_FRAC);
      sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);

      sp->theta = temp_theta;
    }
  } else { /* if not flying, use current yaw as setpoint */
    sp->psi = stateGetNedToBodyEulers_i()->psi;
  }

  /* update timestamp for dt calculation */
  last_ts = get_sys_time_float();
}
Exemple #17
0
static void test_3(void)
{

  /* Compute BODY to IMU eulers */
  struct Int32Eulers b2i_e;
  EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)),
                ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e);

  /* Compute BODY to IMU quaternion */
  struct Int32Quat b2i_q;
  int32_quat_of_eulers(&b2i_q, &b2i_e);
  DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
  //  int32_quat_normalize(&b2i_q);
  //  DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);

  /* Compute BODY to IMU rotation matrix */
  struct Int32RMat b2i_r;
  int32_rmat_of_eulers(&b2i_r, &b2i_e);
  //  DISPLAY_INT32_RMAT("b2i_r", b2i_r);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r);

  /* Compute LTP to IMU eulers */
  struct Int32Eulers l2i_e;
  EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)),
                ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e);

  /* Compute LTP to IMU quaternion */
  struct Int32Quat l2i_q;
  int32_quat_of_eulers(&l2i_q, &l2i_e);
  DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q);

  /* Compute LTP to IMU rotation matrix */
  struct Int32RMat l2i_r;
  int32_rmat_of_eulers(&l2i_r, &l2i_e);
  //  DISPLAY_INT32_RMAT("l2i_r", l2i_r);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r);


  /* again but from quaternion */
  struct Int32RMat l2i_r2;
  int32_rmat_of_quat(&l2i_r2, &l2i_q);
  //  DISPLAY_INT32_RMAT("l2i_r2", l2i_r2);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2);

  /* Compute LTP to BODY quaternion */
  struct Int32Quat l2b_q;
  int32_quat_comp_inv(&l2b_q, &b2i_q, &l2i_q);
  DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);

  /* Compute LTP to BODY rotation matrix */
  struct Int32RMat l2b_r;
  int32_rmat_comp_inv(&l2b_r, &l2i_r, &b2i_r);
  //  DISPLAY_INT32_RMAT("l2b_r", l2b_r);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r);

  /* again but from quaternion */
  struct Int32RMat l2b_r2;
  int32_rmat_of_quat(&l2b_r2, &l2b_q);
  //  DISPLAY_INT32_RMAT("l2b_r2", l2b_r2);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2);


  /* compute LTP to BODY eulers */
  struct Int32Eulers l2b_e;
  int32_eulers_of_rmat(&l2b_e, &l2b_r);
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e);

  /* again but from quaternion */
  struct Int32Eulers l2b_e2;
  int32_eulers_of_quat(&l2b_e2, &l2b_q);
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2);

}