Example #1
0
void ahrs_realign_heading(float heading) {
  FLOAT_ANGLE_NORMALIZE(heading);

  /* quaternion representing only the heading rotation from ltp to body */
  struct FloatQuat q_h_new;
  q_h_new.qx = 0.0;
  q_h_new.qy = 0.0;
  q_h_new.qz = sinf(heading/2.0);
  q_h_new.qi = cosf(heading/2.0);

  struct FloatQuat* ltp_to_body_quat = stateGetNedToBodyQuat_f();
  /* quaternion representing current heading only */
  struct FloatQuat q_h;
  QUAT_COPY(q_h, *ltp_to_body_quat);
  q_h.qx = 0.;
  q_h.qy = 0.;
  FLOAT_QUAT_NORMALIZE(q_h);

  /* quaternion representing rotation from current to new heading */
  struct FloatQuat q_c;
  FLOAT_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);

  /* correct current heading in body frame */
  struct FloatQuat q;
  FLOAT_QUAT_COMP_NORM_SHORTEST(q, q_c, *ltp_to_body_quat);

  /* compute ltp to imu rotation quaternion and matrix */
  FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, ahrs_impl.body_to_imu_quat);
  FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);

  /* set state */
  stateSetNedToBodyQuat_f(&q);

  ahrs_impl.heading_aligned = TRUE;
}
Example #2
0
void ahrs_fc_realign_heading(float heading)
{
  FLOAT_ANGLE_NORMALIZE(heading);

  /* quaternion representing only the heading rotation from ltp to body */
  struct FloatQuat q_h_new;
  q_h_new.qx = 0.0;
  q_h_new.qy = 0.0;
  q_h_new.qz = sinf(heading / 2.0);
  q_h_new.qi = cosf(heading / 2.0);

  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
  struct FloatQuat ltp_to_body_quat;
  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
  /* quaternion representing current heading only */
  struct FloatQuat q_h;
  QUAT_COPY(q_h, ltp_to_body_quat);
  q_h.qx = 0.;
  q_h.qy = 0.;
  float_quat_normalize(&q_h);

  /* quaternion representing rotation from current to new heading */
  struct FloatQuat q_c;
  float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);

  /* correct current heading in body frame */
  struct FloatQuat q;
  float_quat_comp_norm_shortest(&q, &q_c, &ltp_to_body_quat);

  /* compute ltp to imu rotation quaternion and matrix */
  float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
  float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);

  ahrs_fc.heading_aligned = TRUE;
}
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.;

}
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight) {
  sp->phi = (radio_control.values[RADIO_ROLL]  * SP_MAX_PHI / MAX_PPRZ);
  sp->theta = (radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);

  if (in_flight) {
    if (YAW_DEADBAND_EXCEEDED()) {
      sp->psi += (radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
      FLOAT_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
    float delta_psi = sp->psi - stateGetNedToBodyEulers_f()->psi;
    FLOAT_ANGLE_NORMALIZE(delta_psi);
    if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
      sp->psi = stateGetNedToBodyEulers_f()->psi + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
    }
    else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
      sp->psi = stateGetNedToBodyEulers_f()->psi - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
    }
    FLOAT_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.
      float cos_psi;
      float sin_psi;
      float temp_theta;

      float care_free_delta_psi_f = sp->psi - care_free_heading;

      FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);

      sin_psi = sin(care_free_delta_psi_f);
      cos_psi = cos(care_free_delta_psi_f);

      temp_theta = cos_psi*sp->theta - sin_psi*sp->phi;
      sp->phi = cos_psi*sp->phi - sin_psi*sp->theta;

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

#if USE_REF

  /* dumb integrate reference attitude        */
  struct FloatRates delta_rate;
  RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE);
  struct FloatEulers delta_angle;
  EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
  EULERS_ADD(stab_att_ref_euler, delta_angle);
  FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi);

  /* integrate reference rotational speeds   */
  struct FloatRates delta_accel;
  RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE);
  RATES_ADD(stab_att_ref_rate, delta_accel);

  /* compute reference attitude error        */
  struct FloatEulers ref_err;
  EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler);
  /* wrap it in the shortest direction       */
  FLOAT_ANGLE_NORMALIZE(ref_err.psi);

  /* compute reference angular accelerations */
  stab_att_ref_accel.p = -2.*ZETA_P * OMEGA_P * stab_att_ref_rate.p - OMEGA_P * OMEGA_P * ref_err.phi;
  stab_att_ref_accel.q = -2.*ZETA_Q * OMEGA_P * stab_att_ref_rate.q - OMEGA_Q * OMEGA_Q * ref_err.theta;
  stab_att_ref_accel.r = -2.*ZETA_R * OMEGA_P * stab_att_ref_rate.r - OMEGA_R * OMEGA_R * ref_err.psi;

  /*  saturate acceleration */
  const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
  const struct FloatRates MAX_ACCEL = {  REF_ACCEL_MAX_P,  REF_ACCEL_MAX_Q,  REF_ACCEL_MAX_R };
  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);

  /* saturate speed and trim accel accordingly */
  SATURATE_SPEED_TRIM_ACCEL();

#else   /* !USE_REF */
  EULERS_COPY(stab_att_ref_euler, stabilization_att_sp);
  FLOAT_RATES_ZERO(stab_att_ref_rate);
  FLOAT_RATES_ZERO(stab_att_ref_accel);
#endif /* USE_REF */

}
Example #6
0
void ahrs_fc_update_heading(float heading)
{

  FLOAT_ANGLE_NORMALIZE(heading);

  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
  struct FloatQuat ltp_to_body_quat;
  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
  struct FloatRMat ltp_to_body_rmat;
  float_rmat_of_quat(&ltp_to_body_rmat, &ltp_to_body_quat);
  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
  // we only consider x and y
  struct FloatVect2 expected_ltp = {
    RMAT_ELMT(ltp_to_body_rmat, 0, 0),
    RMAT_ELMT(ltp_to_body_rmat, 0, 1)
  };

  // expected_heading cross measured_heading
  struct FloatVect3 residual_ltp = {
    0,
    0,
    expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
  };

  struct FloatVect3 residual_imu;
  float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);

  const float heading_rate_update_gain = 2.5;
  RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain);

  float heading_bias_update_gain;
  /* crude attempt to only update bias if deviation is small
   * e.g. needed when you only have gps providing heading
   * and the inital heading is totally different from
   * the gps course information you get once you have a gps fix.
   * Otherwise the bias will be falsely "corrected".
   */
  if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) {
    heading_bias_update_gain = -2.5e-4;
  } else {
    heading_bias_update_gain = 0.0;
  }
  RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain);
}
Example #7
0
void ahrs_update_heading(float heading) {

    FLOAT_ANGLE_NORMALIZE(heading);

    struct FloatRMat* ltp_to_body_rmat = stateGetNedToBodyRMat_f();
    // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
    // we only consider x and y
    struct FloatVect2 expected_ltp =
    {   RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
        RMAT_ELMT(*ltp_to_body_rmat, 0, 1)
    };

    // expected_heading cross measured_heading
    struct FloatVect3 residual_ltp =
    {   0,
        0,
        expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
    };

    struct FloatVect3 residual_imu;
    FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);

    const float heading_rate_update_gain = 2.5;
    FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain);

    float heading_bias_update_gain;
    /* crude attempt to only update bias if deviation is small
     * e.g. needed when you only have gps providing heading
     * and the inital heading is totally different from
     * the gps course information you get once you have a gps fix.
     * Otherwise the bias will be falsely "corrected".
     */
    if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.)))
        heading_bias_update_gain = -2.5e-4;
    else
        heading_bias_update_gain = 0.0;
    FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain);
}
Example #8
0
void CN_vector_escape_velocity(void)
{

  /////VECTOR METHOD VARIABLES//////
  //Constants
  //Parameters for Butterworth filter
  float A_butter = -0.8541;//-0.7265;
  float B_butter[2] = {0.0730 , 0.0730 };//{0.1367, 0.1367};

  int8_t disp_count = 0;
  //Initalize
  //float fp_angle;
  //float total_vel;
  float Ca;
  float Cv;
  float angle_ver = 0;
  float angle_hor = 0;
  Repulsionforce_Kan.x = 0;
  Repulsionforce_Kan.y = 0;
  Repulsionforce_Kan.z = 0;
  //struct FloatRMat T;
  //struct FloatEulers current_attitude = {0,0,0};
  struct FloatVect3 Total_Kan = {0, 0, 0};
  //Tuning variables
  //float force_max = 200;

  /////ESCAPE METHOD VARIABLES//////
  //Constants
  int8_t min_disparity = 45;
  int8_t number_of_buffer = 20;
  float bias = 2 * M_PI;
  vref_max = 0.1; //CHECK!
  //init
  float available_heading[size_matrix[0] * size_matrix[2]];
  float diff_available_heading[size_matrix[0] * size_matrix[2]];
  float new_heading_diff = 1000;
  float new_heading = 1000;
  int8_t i = 0;
  int8_t i_buffer = 0;
  float Distance_est;
  float V_total = 0;
  ///////////////////////////////////

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

  //Calculate Attractforce_goal
  Attractforce_goal.x = cos(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
  Attractforce_goal.y = sin(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
  Attractforce_goal.z = 0;

  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];
      angle_ver = 0.5 * stereo_fow[1] + stereo_fow[1] / size_matrix[1] - stereo_fow[1] / size_matrix[1] / 2;

      for (int i2 = 0; i2 < 4; i2++) {
        angle_ver = angle_ver - stereo_fow[1] / size_matrix[1];

        Ca = cos((angle_hor - heading_goal_ref) * Cfreq) * erf(1 * sqrt(VECT2_NORM2(pos_diff)));
        if (Ca < 0) {
          Ca = 0;
        }

        //TODO  make dependent on speed: total_vel/vref_max
        Cv = F1 + F2 * Ca;

        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;
          disp_count++;
          Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv / (Distance_est + Dist_offset),
                                 2) * cos(angle_hor) * cos(angle_ver);
          Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv / (Distance_est + Dist_offset),
                                 2) * sin(angle_hor) * cos(angle_ver);
          Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv / (Distance_est + Dist_offset), 2) * sin(angle_ver);


        }

      }
    }
  }

  //Normalize for ammount entries in Matrix
  VECT3_SMUL(Repulsionforce_Kan, Repulsionforce_Kan, 1.0 / (float)disp_count);

  if (repulsionforce_filter_flag == 1) {

    Repulsionforce_Used.x = B_butter[0] * Repulsionforce_Kan.x + B_butter[1] * Repulsionforce_Kan_old.x - A_butter *
                            filter_repforce_old.x;
    Repulsionforce_Used.y = B_butter[0] * Repulsionforce_Kan.y + B_butter[1] * Repulsionforce_Kan_old.y - A_butter *
                            filter_repforce_old.y;
    Repulsionforce_Used.z = B_butter[0] * Repulsionforce_Kan.z + B_butter[1] * Repulsionforce_Kan_old.z - A_butter *
                            filter_repforce_old.z;

    VECT3_COPY(Repulsionforce_Kan_old, Repulsionforce_Kan);
    VECT3_COPY(filter_repforce_old, Repulsionforce_Used);

  }

  else {
    VECT3_COPY(Repulsionforce_Used, Repulsionforce_Kan);
  }

  VECT3_SMUL(Repulsionforce_Used, Repulsionforce_Used, Ko);
  VECT3_SMUL(Attractforce_goal, Attractforce_goal, Kg);

  //Total force
  VECT3_ADD(Total_Kan, Repulsionforce_Used);
  VECT3_ADD(Total_Kan, Attractforce_goal);

  //set variable for stabilization_opticflow
  printf("RepulsionforceX: %f AttractX: %f \n", Repulsionforce_Used.x, Attractforce_goal.x);
  printf("RepulsionforceY: %f AttractY: %f \n", Repulsionforce_Used.y, Attractforce_goal.y);

  //set write values for logger
  //Attractforce_goal_send.x = Attractforce_goal.x;
  //Attractforce_goal_send.y = Attractforce_goal.y;
  //Repulsionforce_Kan_send.x = Repulsionforce_Used.x;
  //Repulsionforce_Kan_send.y = Repulsionforce_Used.y;

  V_total = sqrt(pow(Total_Kan.x, 2) + pow(Total_Kan.y, 2));

  if (V_total < v_min && sqrt(VECT2_NORM2(pos_diff)) > 1) {
    escape_flag = 1;
  }

  else if (V_total > v_max && obstacle_flag == 0) {
    escape_flag = 0;
  }

  printf("V_total: %f", V_total);
  printf("escape_flag: %i, obstacle_flag: %i, set_bias: %i", escape_flag, obstacle_flag, set_bias);
  printf("escape_flag: %i, obstacle_flag: %i, set_bias: %i", escape_flag, obstacle_flag, set_bias);

  if (escape_flag == 0) {
    ref_pitch = Total_Kan.x;
    ref_roll = Total_Kan.y;
  } else if (escape_flag == 1) {
    heading_goal_ref = heading_goal_f - stateGetNedToBodyEulers_f()->psi;

    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];
      for (int i3 = 0; i3 < size_matrix[2]; i3++) {
        angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
        available_heading[i] = angle_hor;
        diff_available_heading[i] = heading_goal_ref - available_heading[i];
        FLOAT_ANGLE_NORMALIZE(diff_available_heading[i]);
        i++;

      }
    }

    i = 0;
    for (int i1 = 0; i1 < size_matrix[0]; i1++) {
      for (int i3 = 0; i3 < size_matrix[2]; i3++) {
        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)READimageBuffer_offset[i1*size_matrix[1]+i2*size_matrix[0]*size_matrix[2] + i3])-18.0)/1000;

            diff_available_heading[i] = 100;
            for (int i_diff = -number_of_buffer; i_diff <= number_of_buffer + 1; i_diff++) {
              i_buffer = i + i_diff;


              if (i_buffer < 1) {
                i_buffer = i_buffer + size_matrix[0] * size_matrix[2];
              }

              if (i_buffer > size_matrix[0]*size_matrix[2]) {
                i_buffer = i_buffer - size_matrix[0] * size_matrix[2];
              }

              diff_available_heading[i_buffer] = 100;
            }
          }
        }
        i++;
      }
    }

    //set bias
    if (set_bias == 1) {
      for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
        if (diff_available_heading[i100] <= 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
          diff_available_heading[i100] = diff_available_heading[i100] - bias;
        }
      }
    } else if (set_bias == 2) {
      for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
        if (diff_available_heading[i100] > 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
          diff_available_heading[i100] = diff_available_heading[i100] + bias;
        }
      }
    }

    for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
      if (fabs(diff_available_heading[i100]) < fabs(new_heading_diff)) {
        new_heading_diff = diff_available_heading[i100];
        new_heading = available_heading[i100];
      }
    }

    if (obstacle_flag == 0 && fabs(new_heading_diff) > (2 * M_PI / 36.0)) {
      obstacle_flag = 1;
      if (new_heading_diff > 0) {
        set_bias = 1;
        direction = -1;
      } else if (new_heading_diff <= 0) {
        set_bias = 2;
        direction = 1;
      }
    }

    if (fabs(new_heading_diff) <= (2 * M_PI / 36.0)) {
      if (waypoint_rot == 0) {
        obstacle_flag = 0;
        set_bias = 0;
      } else {
        waypoint_rot = waypoint_rot - direction * 0.05 * M_PI;
      }
    }

    if (fabs(new_heading_diff) >= 0.5 * M_PI) {
      waypoint_rot = waypoint_rot + direction * 0.25 * M_PI;
    }

    //vref_max should be low
    speed_pot = vref_max * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
    if (speed_pot <= 0) {
      speed_pot = 0;
    }

    new_heading_old = new_heading;

#if PRINT_STUFF
    for (int i100 = 0; i100 < (size_matrix[0] * size_matrix[2]); i100++) {
      printf("%i diff_available_heading: %f available_heading: %f \n", i100, diff_available_heading[i100],
             available_heading[i100]);
    }
    printf("new_heading: %f  current_heading: %f  speed_pot: %f\n", new_heading, stateGetNedToBodyEulers_f()->psi,
           speed_pot);
    printf("set_bias: %i  obstacle_flag: %i", set_bias, obstacle_flag);
#endif

    ref_pitch = cos(new_heading) * speed_pot;
    ref_roll = sin(new_heading) * speed_pot;

  }
}
Example #9
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 #10
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 #11
0
void CN_escape_velocity(void)
{

  //Constants
  int8_t min_disparity = 45;
  int8_t number_of_buffer = 20;
  float bias = 2 * M_PI;
  vref_max = 0.1; //CHECK!

  //init variables
  float angle_hor = 0;
  float available_heading[size_matrix[0]*size_matrix[2]];
  float diff_available_heading[size_matrix[0]*size_matrix[2]];
  float new_heading_diff = 1000;
  float new_heading = 1000;
  int8_t i = 0;
  int8_t i_buffer = 0;
  float distance_est;
  float distance_heading = 0;

  //generate available_headings
  heading_goal_ref = heading_goal_f - stateGetNedToBodyEulers_f()->psi;

  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];
    for (int i3 = 0; i3 < size_matrix[2]; i3++) {
      angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
      available_heading[i] = angle_hor;
      diff_available_heading[i] = heading_goal_ref - available_heading[i];
      FLOAT_ANGLE_NORMALIZE(diff_available_heading[i]);
      i++;

    }
  }

  //set unavailable headings to 100;
  i = 0;
  for (int i1 = 0; i1 < size_matrix[0]; i1++) {
    for (int i3 = 0; i3 < size_matrix[2]; i3++) {
      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 (distance_est < distance_heading) {
            distance_heading = distance_est;
          }

          diff_available_heading[i] = 100;
          for (int i_diff = -number_of_buffer; i_diff <= number_of_buffer + 1; i_diff++) {
            i_buffer = i + i_diff;


            if (i_buffer < 1) {
              i_buffer = i_buffer + size_matrix[0] * size_matrix[2];
            }

            if (i_buffer > size_matrix[0]*size_matrix[2]) {
              i_buffer = i_buffer - size_matrix[0] * size_matrix[2];
            }

            diff_available_heading[i_buffer] = 100;
          }

        }
      }
      i++;
    }
  }

  //set bias
  if (set_bias == 1) {
    for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
      if (diff_available_heading[i100] <= 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
        diff_available_heading[i100] = diff_available_heading[i100] - bias;
      }
    }
  } else if (set_bias == 2) {
    for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
      if (diff_available_heading[i100] > 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
        diff_available_heading[i100] = diff_available_heading[i100] + bias;
      }
    }
  }

  // select minimum available heading
  for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
    if (fabs(diff_available_heading[i100]) < fabs(new_heading_diff)) {
      new_heading_diff = diff_available_heading[i100];
      new_heading = available_heading[i100];
    }
  }

  if (obstacle_flag == 0 && distance_heading > 2.0) {
    new_heading = new_heading_old;
  } else if (obstacle_flag == 0 && fabs(new_heading_diff) > (2 * M_PI / 36.0)) {
    obstacle_flag = 1;
    if (new_heading_diff > 0) {
      set_bias = 1;
      direction = -1;
    } else if (new_heading_diff <= 0) {
      set_bias = 2;
      direction = 1;
    }
  }

  // Rotate waypoint
  if (fabs(new_heading_diff) >= 0.5 * M_PI) {
    waypoint_rot = waypoint_rot + direction * 0.25 * M_PI;
  }

  if (fabs(new_heading_diff) <= (2 * M_PI / 36.0)) {
    if (waypoint_rot == 0) {
      obstacle_flag = 0;
      set_bias = 0;
    } else {
      waypoint_rot = waypoint_rot - direction * 0.05 * M_PI;
    }
  }

  //vref_max should be low
  speed_pot = vref_max * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
  if (speed_pot <= 0) {
    speed_pot = 0;
  }

  ref_pitch = cos(new_heading) * speed_pot;
  ref_roll = sin(new_heading) * speed_pot;
  new_heading_old = new_heading;

#if PRINT_STUFF
  for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
    printf("%i diff_available_heading: %f available_heading: %f \n", i100, diff_available_heading[i100],
           available_heading[i100]);
  }
  printf("new_heading: %f  current_heading: %f  speed_pot: %f\n", new_heading, stateGetNedToBodyEulers_f()->psi,
         speed_pot);
  printf("set_bias: %i  obstacle_flag: %i", set_bias, obstacle_flag);
#endif

}
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
  sp->phi = (radio_control.values[RADIO_ROLL]  * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ);
  sp->theta = (radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_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 += (radio_control.values[RADIO_YAW] * STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
      FLOAT_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
      float omega;
      const float max_phi = RadOfDeg(85.0);
      if(abs(sp->phi) < max_phi)
        omega = 1.3*tanf(sp->phi);
      else //max 60 degrees roll, then take constant omega
        omega = 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
    float heading = stabilization_attitude_get_heading_f();

    float delta_psi = sp->psi - heading;
    FLOAT_ANGLE_NORMALIZE(delta_psi);
    if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
      sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
    }
    else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
      sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
    }
    FLOAT_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.
      float cos_psi;
      float sin_psi;
      float temp_theta;

      float care_free_delta_psi_f = sp->psi - care_free_heading;

      FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);

      sin_psi = sinf(care_free_delta_psi_f);
      cos_psi = cosf(care_free_delta_psi_f);

      temp_theta = cos_psi*sp->theta - sin_psi*sp->phi;
      sp->phi = cos_psi*sp->phi - sin_psi*sp->theta;

      sp->theta = temp_theta;
    }
  }
  else { /* if not flying, use current yaw as setpoint */
    sp->psi = stateGetNedToBodyEulers_f()->psi;
  }
}
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *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_f();
  sp->theta = get_rc_pitch_f();

  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_f() * dt;
      FLOAT_ANGLE_NORMALIZE(sp->psi);
    }
    if (coordinated_turn) {
      //Coordinated turn
      //feedforward estimate angular rotation omega = g*tan(phi)/v
      float omega;
      const float max_phi = RadOfDeg(60.0);
      if (fabsf(sp->phi) < max_phi) {
        omega = 9.81 / COORDINATED_TURN_AIRSPEED * tanf(sp->phi);
      } else { //max 60 degrees roll
        omega = 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
    float heading = stabilization_attitude_get_heading_f();

    float delta_psi = sp->psi - heading;
    FLOAT_ANGLE_NORMALIZE(delta_psi);
    if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
      sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
    } else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
      sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
    }
    FLOAT_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.
      float cos_psi;
      float sin_psi;
      float temp_theta;

      float care_free_delta_psi_f = sp->psi - care_free_heading;

      FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);

      sin_psi = sinf(care_free_delta_psi_f);
      cos_psi = cosf(care_free_delta_psi_f);

      temp_theta = cos_psi * sp->theta - sin_psi * sp->phi;
      sp->phi = cos_psi * sp->phi - sin_psi * sp->theta;

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

  /* update timestamp for dt calculation */
  last_ts = get_sys_time_float();
}