inline static void h_ctl_pitch_loop( void ) {
  static float last_err;
  struct FloatEulers* att = stateGetNedToBodyEulers_f();
  /* sanity check */
  if (h_ctl_elevator_of_roll <0.)
    h_ctl_elevator_of_roll = 0.;

  h_ctl_pitch_loop_setpoint =  h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi);

	float err = 0;

#ifdef USE_AOA
	switch (h_ctl_pitch_mode){
		case H_CTL_PITCH_MODE_THETA:
			err = att->theta - h_ctl_pitch_loop_setpoint;
		break;
		case H_CTL_PITCH_MODE_AOA:
			err = (*stateGetAngleOfAttack_f()) - h_ctl_pitch_loop_setpoint;
		break;
		default:
			err = att->theta - h_ctl_pitch_loop_setpoint;
		break;
	}
#else //NO_AOA
	err = att->theta - h_ctl_pitch_loop_setpoint;
#endif


  float d_err = err - last_err;
  last_err = err;
  float cmd = -h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
#ifdef LOITER_TRIM
  cmd += loiter();
#endif
  h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
static void send_att(struct transport_tx *trans, struct link_device *dev)
{
    struct FloatRates *body_rate = stateGetBodyRates_f();
    struct FloatEulers *att = stateGetNedToBodyEulers_f();
    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_quat.qx,
                                      &stabilization_att_sum_err_quat.qy,
                                      &stabilization_att_sum_err_quat.qz,
                                      &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(void) {
  struct FloatRates* body_rate = stateGetBodyRates_f();
  struct FloatEulers* att = stateGetNedToBodyEulers_f();
  float foo = 0.0;
  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],
      &foo, &foo, &foo);
}
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);
}
Exemple #5
0
/**
 * Poll lidar for data
 * for altitude hold 50Hz-100Hz should be enough,
 * in theory can go faster (see the datasheet for Lidar Lite v3
 */
void lidar_lite_periodic(void)
{
  switch (lidar_lite.status) {
    case LIDAR_LITE_INIT_RANGING:
      if (lidar_lite.trans.status == I2CTransDone) {
        // ask for i2c frame
        lidar_lite.trans.buf[0] = LIDAR_LITE_REG_ADDR; // sets register pointer to  (0x00)
        lidar_lite.trans.buf[1] = LIDAR_LITE_REG_VAL; // take acquisition & correlation processing with DC correction
        if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 2)){
          // transaction OK, increment status
          lidar_lite.status = LIDAR_LITE_REQ_READ;
        }
      }
      break;
    case LIDAR_LITE_REQ_READ:
      if (lidar_lite.trans.status == I2CTransDone) {
        lidar_lite.trans.buf[0] = LIDAR_LITE_READ_ADDR; // sets register pointer to results register
        if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 1)){
          // transaction OK, increment status
          lidar_lite.status = LIDAR_LITE_READ_DISTANCE;
        }
      }
      break;
    case LIDAR_LITE_READ_DISTANCE:
      if (lidar_lite.trans.status == I2CTransDone) {
        // clear buffer
        lidar_lite.trans.buf[0] = 0;
        lidar_lite.trans.buf[1] = 0;
        if (i2c_receive(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 2)){
          // transaction OK, increment status
          lidar_lite.status = LIDAR_LITE_PARSE;
        }
      }
      break;
    case LIDAR_LITE_PARSE: {
      // filter data
      uint32_t now_ts = get_sys_time_usec();
      lidar_lite.distance_raw = update_median_filter_i(
                                  &lidar_lite_filter,
                                  (uint32_t)((lidar_lite.trans.buf[0] << 8) | lidar_lite.trans.buf[1]));
      lidar_lite.distance = ((float)lidar_lite.distance_raw)/100.0;

      // compensate AGL measurement for body rotation
      if (lidar_lite.compensate_rotation) {
          float phi = stateGetNedToBodyEulers_f()->phi;
          float theta = stateGetNedToBodyEulers_f()->theta;
          float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
          lidar_lite.distance = lidar_lite.distance / gain;
      }

      // send message (if requested)
      if (lidar_lite.update_agl) {
        AbiSendMsgAGL(AGL_LIDAR_LITE_ID, now_ts, lidar_lite.distance);
      }

      // increment status
      lidar_lite.status = LIDAR_LITE_INIT_RANGING;
      break;
    }
    default:
      break;
  }
}
Exemple #6
0
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
{
  struct FloatEulers *att = stateGetNedToBodyEulers_f();
  pprz_msg_send_ATTITUDE(trans, dev, AC_ID, &(att->phi), &(att->psi), &(att->theta));
};
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;

  }
}
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;
}
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

}
Exemple #10
0
// Periodic
void takeoff_detect_periodic(void)
{
  // Run detection state machine here
  switch (takeoff_detect.state) {
    case TO_DETECT_ARMED:
      // test for "nose up" + AP in AUTO2, optionally AUTO1 not default since risky if one does not know what it does
      if (stateGetNedToBodyEulers_f()->theta > TAKEOFF_DETECT_LAUNCH_PITCH)
      {
#ifndef TAKEOFF_DETECT_ALSO_IN_AUTO1
      if (autopilot_get_mode() != AP_MODE_AUTO2)
#else
      if ((autopilot_get_mode() != AP_MODE_AUTO2) || (autopilot_get_mode() != AP_MODE_AUTO1))
#endif
        {
        takeoff_detect.timer++;
    	}
      }
      else {
        // else reset timer
        takeoff_detect.timer = 0;
      }
      // if timer is finished, start launching
      if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_TIMER)) {
        autopilot.launch = true;
        takeoff_detect.state = TO_DETECT_LAUNCHING;
        takeoff_detect.timer = 0;
      }
      break;

    case TO_DETECT_LAUNCHING:
      // abort if pitch goes below threshold while launching
      if (stateGetNedToBodyEulers_f()->theta < TAKEOFF_DETECT_ABORT_PITCH)
      {
#ifndef TAKEOFF_DETECT_ALSO_IN_AUTO1
      if (autopilot_get_mode() != AP_MODE_AUTO2)
#else
      if ((autopilot_get_mode() != AP_MODE_AUTO2) || (autopilot_get_mode() != AP_MODE_AUTO1))
#endif
        {
          // back to ARMED state
          autopilot.launch = false;
          takeoff_detect.state = TO_DETECT_ARMED;
        }
      }
      // increment timer and disable detection after some time
      takeoff_detect.timer++;
      if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_DISABLE_TIMER)) {
        takeoff_detect.state = TO_DETECT_DISABLED;
      }
      break;

    case TO_DETECT_DISABLED:
      // stop periodic call
      takeoff_detect_takeoff_detect_periodic_status = MODULES_STOP;
      break;

    default:
      // No kidding ?!
      takeoff_detect.state = TO_DETECT_DISABLED;
      break;
  }
}
static void send_tune_roll(void) {
  DOWNLINK_SEND_TUNE_ROLL(DefaultChannel, DefaultDevice,
      &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
}
Exemple #12
0
// GENERIC TRAJECTORY CONTROLLER
void gvf_control_2D(float ke, float kn, float e,
                    struct gvf_grad *grad, struct gvf_Hess *hess)
{
  struct FloatEulers *att = stateGetNedToBodyEulers_f();
  float ground_speed = stateGetHorizontalSpeedNorm_f();
  float course = stateGetHorizontalSpeedDir_f();
  float px_dot = ground_speed * sinf(course);
  float py_dot = ground_speed * cosf(course);
  int s = gvf_control.s;

  // gradient Phi
  float nx = grad->nx;
  float ny = grad->ny;

  // tangent to Phi
  float tx = s * grad->ny;
  float ty = -s * grad->nx;

  // Hessian
  float H11 = hess->H11;
  float H12 = hess->H12;
  float H21 = hess->H21;
  float H22 = hess->H22;

  // Calculation of the desired angular velocity in the vector field
  float pdx_dot = tx - ke * e * nx;
  float pdy_dot = ty - ke * e * ny;

  float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
  float md_x = pdx_dot / norm_pd_dot;
  float md_y = pdy_dot / norm_pd_dot;

  float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
  float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;

  float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
                        + ((-ke * e * H12) + s * H22) * py_dot;
  float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
                        - (s * H12 + (ke * e * H22)) * py_dot;

  float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
  float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;

  float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
                       / norm_pd_dot;

  float md_dot_x =  md_y * md_dot_const;
  float md_dot_y = -md_x * md_dot_const;

  float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);

  float mr_x = sinf(course);
  float mr_y = cosf(course);

  float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);

  // Coordinated turn
  if (autopilot_get_mode() == AP_MODE_AUTO2) {
    h_ctl_roll_setpoint =
      -atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
    BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);

    lateral_mode = LATERAL_MODE_ROLL;
  }
}
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();
}
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);
}
void stabilization_attitude_set_failsafe_setpoint(void) {
  stab_att_sp_euler.phi = 0.0;
  stab_att_sp_euler.theta = 0.0;
  stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi;
}
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_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;
  stabilization_att_ff_cmd[COMMAND_PITCH] =
    stabilization_gains.dd.y * stab_att_ref_accel.q;
  stabilization_att_ff_cmd[COMMAND_YAW] =
    stabilization_gains.dd.z * stab_att_ref_accel.r;

  /* 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;

  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;

  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;


  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]);

  /* bound the result */
  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
/// reset the heading for care-free mode to current heading
void stabilization_attitude_reset_care_free_heading(void) {
  care_free_heading = stateGetNedToBodyEulers_f()->psi;
}
Exemple #19
0
void dc_periodic_4Hz(void)
{
  static uint8_t dc_shutter_timer = 0;

  switch (dc_autoshoot) {

    case DC_AUTOSHOOT_PERIODIC:
      if (dc_shutter_timer) {
        dc_shutter_timer--;
      } else {
        dc_shutter_timer = dc_autoshoot_quartersec_period;
        dc_send_command(DC_SHOOT);
      }
      break;

    case DC_AUTOSHOOT_DISTANCE:
      {
        struct FloatVect2 cur_pos;
        cur_pos.x = stateGetPositionEnu_f()->x;
        cur_pos.y = stateGetPositionEnu_f()->y;
        struct FloatVect2 delta_pos;
        VECT2_DIFF(delta_pos, cur_pos, last_shot_pos);
        float dist_to_last_shot = float_vect2_norm(&delta_pos);
        if (dist_to_last_shot > dc_distance_interval) {
          dc_gps_count++;
          dc_send_command(DC_SHOOT);
          VECT2_COPY(last_shot_pos, cur_pos);
        }
      }
      break;

    case DC_AUTOSHOOT_CIRCLE:
      {
        float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle;
        if (course < 0.)
          course += 360.;
        float current_block = floorf(course/dc_circle_interval);

        if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) {
          dc_gps_count++;
          dc_circle_last_block = current_block;
          dc_send_command(DC_SHOOT);
        }
      }
      break;

    case DC_AUTOSHOOT_SURVEY:
      {
        float dist_x = dc_gps_x - stateGetPositionEnu_f()->x;
        float dist_y = dc_gps_y - stateGetPositionEnu_f()->y;

        if (dist_x*dist_x + dist_y*dist_y >= dc_gps_next_dist*dc_gps_next_dist) {
          dc_gps_next_dist += dc_survey_interval;
          dc_gps_count++;
          dc_send_command(DC_SHOOT);
        }
      }
      break;

    default:
      dc_autoshoot = DC_AUTOSHOOT_STOP;
  }
}
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;)

}
Exemple #21
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)

  }
void CN_vector_velocity(void)
{

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

  //Initalize
  int8_t disp_count = 0;
  float escape_angle = 0;
  float y_goal_frame;
  //float total_vel;
  float Distance_est;
  float Ca;
  float Cv;
  float angle_ver = 0;
  float angle_hor = 0;
  //struct FloatVect3 Repulsionforce_Kan = {0,0,0};
  Repulsionforce_Kan.x = 0;
  Repulsionforce_Kan.y = 0;
  Repulsionforce_Kan.z = 0;
  //struct FloatVect3 Attractforce_goal = {0,0,0};
  //Attractforce_goal.x = 0;
  //Attractforce_goal.y = 0;
  //Attractforce_goal.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;
  int8_t min_disparity = 45;

  //Flight plath angle calculation
  // TODO make algorithm dependent on angle of obstacle.....
  //     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 = heading_goal_f - stateGetNedToBodyEulers_f()->psi;
  //FLOAT_ANGLE_NORMALIZE(heading_goal_ref);

  //Calculate Attractforce_goal size = 1;
  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;


  //printf("current_heading, %f heading_goal_f: %f, heading_goal_ref: %f",stateGetNedToBodyEulers_f()->psi, heading_goal_f, heading_goal_ref);
  //Transform to body frame
  //current_attitude.psi = stateGetNedToBodyEulers_f()->psi;Attractforce_already in body frame, check when using data form Roland
  //float_rmat_of_eulers_312(&T, &current_attitude);
  //MAT33_VECT3_MUL(Attractforce_goal, T, Attractforce_goal);

  //Attractforce_goal_send.x = Attractforce_goal.x;
  //Attractforce_goal_send.y = Attractforce_goal.y;
  //Attractforce_goal_send.z = Attractforce_goal.z;


  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);

          printf("rep.x  %f index %d %d %d disp: %d cv: %f angle_hor: %f angle_ver: %f \n", Repulsionforce_Kan.x, i1, i2, i3,
                 stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3], Cv, angle_hor, angle_ver);

        }
//         else {
//          Distance_est = 2000;
//      }
//
//      if(pow(Cv/(READimageBuffer_offset[i1*size_matrix[1]+i2*size_matrix[0]*size_matrix[2] + i3]-0.2),2)<force_max){
//          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);
//      }
//      else{
//          Repulsionforce_Kan.x = Repulsionforce_Kan.x - force_max*sin(angle_hor)*cos(angle_ver);
//          Repulsionforce_Kan.y = Repulsionforce_Kan.y - force_max*cos(angle_hor)*cos(angle_ver);
//          Repulsionforce_Kan.z = Repulsionforce_Kan.z - force_max*sin(angle_ver);
//      }

      }
    }
  }

  //Normalize for ammount entries in Matrix
  //Repulsionforce_Kan = Repulsionforce_Kan/(float)(size_matrix[1]*size_matrix[2]);
  VECT3_SMUL(Repulsionforce_Kan, Repulsionforce_Kan, 1.0 / (float)disp_count);
  printf("After multiplication: %f", Repulsionforce_Kan.x);

  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);


  //hysteris
  if (sqrt(VECT2_NORM2(Total_Kan)) < V_hys_low && sqrt(VECT2_NORM2(pos_diff)) > 1) {
    hysteris_flag = 1;

    //x_goal_frame= cos() * Total_Kan.x + sin() * Total_Kan.y;
    y_goal_frame = -sin(heading_goal_ref) * Total_Kan.x + cos(heading_goal_ref) * Total_Kan.y;

    if (y_goal_frame >= 0) {
      escape_angle = 0.5 * M_PI;
    } else if (y_goal_frame < 0) {
      escape_angle = -0.5 * M_PI;
    }
  }


  if (sqrt(VECT2_NORM2(Total_Kan)) > V_hys_high || sqrt(VECT2_NORM2(pos_diff)) < 1) {
    hysteris_flag = 0;
  }

  if (hysteris_flag == 1) {

    Total_Kan.x = cos(heading_goal_ref + escape_angle) * V_hys_high;
    Total_Kan.y = -sin(heading_goal_ref + escape_angle) * V_hys_high;

  }


  ref_pitch = Total_Kan.x;
  ref_roll = Total_Kan.y;
  printf("ref_pitch:  %f ref_roll: %f disp_count: %d", ref_pitch, ref_roll, disp_count);
  //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;
}
Exemple #23
0
static void send_attitude(void) {
  struct FloatEulers* att = stateGetNedToBodyEulers_f();
  DOWNLINK_SEND_ATTITUDE(DefaultChannel, DefaultDevice,
      &(att->phi), &(att->psi), &(att->theta));
};
/**
 * \brief
 *
 */
void h_ctl_course_loop(void)
{
  static float last_err;

  // Ground path error
  float err = stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
  NormRadAngle(err);

#ifdef STRONG_WIND
  // Usefull path speed
  const float reference_advance = (NOMINAL_AIRSPEED / 2.);
  float advance = cos(err) * stateGetHorizontalSpeedNorm_f() / reference_advance;

  if (
    (advance < 1.)  &&                          // Path speed is small
    (stateGetHorizontalSpeedNorm_f() < reference_advance)  // Small path speed is due to wind (small groundspeed)
  ) {
    /*
    // rough crabangle approximation
    float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
    float wind_dir = atan2(wind_east,wind_north);

    float wind_course = h_ctl_course_setpoint - wind_dir;
    NormRadAngle(wind_course);

    estimator_hspeed_dir = estimator_psi;

    float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);
    //crab = estimator_hspeed_mod - estimator_psi;
    NormRadAngle(crab);
    */

    // Heading error
    float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
    NormRadAngle(herr);

    if (advance < -0.5) {            //<! moving in the wrong direction / big > 90 degree turn
      err = herr;
    } else if (advance < 0.) {       //<!
      err = (-advance) * 2. * herr;
    } else {
      err = advance * err;
    }

    // Reset differentiator when switching mode
    //if (h_ctl_course_heading_mode == 0)
    //  last_err = err;
    //h_ctl_course_heading_mode = 1;
  }
  /*  else
      {
      // Reset differentiator when switching mode
      if (h_ctl_course_heading_mode == 1)
      last_err = err;
      h_ctl_course_heading_mode = 0;
      }
  */
#endif //STRONG_WIND

  float d_err = err - last_err;
  last_err = err;

  NormRadAngle(d_err);

#ifdef H_CTL_COURSE_SLEW_INCREMENT
  /* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */
  static float h_ctl_course_slew_rate = 0.;
  float nav_angle_saturation = h_ctl_roll_max_setpoint / h_ctl_course_pgain; /* heading error corresponding to max_roll */
  float half_nav_angle_saturation = nav_angle_saturation / 2.;
  if (autopilot.launch) {  /* prevent accumulator run-up on the ground */
    if (err > half_nav_angle_saturation) {
      h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
      err = Min(err, (half_nav_angle_saturation + h_ctl_course_slew_rate));
      h_ctl_course_slew_rate += h_ctl_course_slew_increment;
    } else if (err < -half_nav_angle_saturation) {
      h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
      err = Max(err, (-half_nav_angle_saturation + h_ctl_course_slew_rate));
      h_ctl_course_slew_rate -= h_ctl_course_slew_increment;
    } else {
      h_ctl_course_slew_rate = 0.;
    }
  }
#endif

  float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;
  Bound(speed_depend_nav, 0.66, 1.5);

  float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);



#if defined(AGR_CLIMB) && !USE_AIRSPEED
  /** limit navigation during extreme altitude changes */
  if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
    if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE ||
        v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_BLENDED) {
      BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
      /* altitude: z-up is positive -> positive error -> too low */
      if (v_ctl_altitude_error > 0) {
        nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
                    (AGR_BLEND_START - AGR_BLEND_END));
        Bound(nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
      } else {
        nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
                    (AGR_BLEND_START - AGR_BLEND_END));
        Bound(nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
      }
      cmd *= nav_ratio;
    }
  }
#endif

  float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;

#ifdef H_CTL_ROLL_SLEW
  float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
  BoundAbs(diff_roll, h_ctl_roll_slew);
  h_ctl_roll_setpoint += diff_roll;
#else
  h_ctl_roll_setpoint = roll_setpoint;
#endif

  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
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);
}
Exemple #26
0
/**
 * Auto-throttle inner loop
 * \brief
 */
void v_ctl_climb_loop(void)
{
  // Airspeed setpoint rate limiter:
  // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1
  float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew;
  BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);
  v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;

#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
// Ground speed control loop (input: groundspeed error, output: airspeed controlled)
  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - stateGetHorizontalSpeedNorm_f());
  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
  BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
  v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) *
                                   v_ctl_auto_groundspeed_pgain;

  // Do not allow controlled airspeed below the setpoint
  if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) {
    v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;
    // reset integrator of ground speed loop
    v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled / (v_ctl_auto_groundspeed_pgain *
                                     v_ctl_auto_groundspeed_igain);
  }
#else
  v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;
#endif

  // Airspeed outerloop: positive means we need to accelerate
  float speed_error = v_ctl_auto_airspeed_controlled - stateGetAirspeed_f();

  // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
  v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
  BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration);

  // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
#ifndef SITL
  struct Int32Vect3 accel_meas_body;
  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
  float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
#else
  float vdot = 0;
#endif

  // Acceleration Error: positive means UAV needs to accelerate: needs extra energy
  float vdot_err = low_pass_vdot(v_ctl_desired_acceleration - vdot);

  // Flight Path Outerloop: positive means needs to climb more: needs extra energy
  float gamma_err  = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_controlled;

  // Total Energy Error: positive means energy should be added
  float en_tot_err = gamma_err + vdot_err;

  // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
  float en_dis_err = gamma_err - vdot_err;

  // Auto Cruise Throttle
  if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {
    v_ctl_auto_throttle_nominal_cruise_throttle +=
      v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude
      + en_tot_err * v_ctl_energy_total_igain * dt_attidude;
    Bound(v_ctl_auto_throttle_nominal_cruise_throttle, 0.0f, 1.0f);
  }

  // Total Controller
  float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle
                              + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
                              + v_ctl_auto_throttle_of_airspeed_pgain * speed_error
                              + v_ctl_energy_total_pgain * en_tot_err;

  if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle == 1)) {
    // If your energy supply is not sufficient, then neglect the climb requirement
    en_dis_err = -vdot_err;

    // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb
    if (v_ctl_climb_setpoint > 0) { v_ctl_climb_setpoint += - 30. * dt_attidude; }
    if (v_ctl_climb_setpoint < 0) { v_ctl_climb_setpoint +=   30. * dt_attidude; }
  }


  /* pitch pre-command */
  if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {
    v_ctl_auto_throttle_nominal_cruise_pitch +=  v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt_attidude
        + v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
    Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
  }
  float v_ctl_pitch_of_vz =
    + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
    - v_ctl_auto_pitch_of_airspeed_pgain * speed_error
    + v_ctl_auto_pitch_of_airspeed_dgain * vdot
    + v_ctl_energy_diff_pgain * en_dis_err
    + v_ctl_auto_throttle_nominal_cruise_pitch;
  if (kill_throttle) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; }

  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
  Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT)

  ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);

  v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
}
static void reset_psi_ref_from_body(void) {
    stab_att_ref_euler.psi = stateGetNedToBodyEulers_f()->psi;
    stab_att_ref_rate.r = 0;
    stab_att_ref_accel.r = 0;
}