inline static void h_ctl_pitch_loop( void ) {
  static float last_err;
  /* 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(estimator_phi);
#ifdef USE_PITCH_TRIM
  loiter();
#endif

#ifdef 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*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * 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 = estimator_theta - h_ctl_ref_pitch_angle;
  float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
  last_err = err;

  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);
}
Beispiel #2
0
//光流定点模式
rt_err_t loiter_mode(u8 height)
{
    const u16 basic_thought = 450;
    if (check_safe(SAFE_ADNS3080))
        return RT_EIO;
    if (pwm.throttle > 0.3f && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40)
    {
        loiter(pos_X, pos_y, yaw_exp);

        althold(height);

        motor_hupdate(basic_thought);
    }
    else
    Motor_Set(60, 60, 60, 60);
    return RT_EOK;
}
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.;
  }

  if (v_ctl_mode == V_CTL_MODE_LANDING) {
    h_ctl_pitch_loop_setpoint =  h_ctl_pitch_setpoint;
  }
  else {
    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);
}
inline static void h_ctl_pitch_loop( void ) {
  static float last_err;
  /* 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(estimator_phi);

  float err = estimator_theta - h_ctl_pitch_loop_setpoint;
  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);
}
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);
}
Beispiel #6
0
void FlyLine::localCallBack(const geometry_msgs::PoseStampedConstPtr &msg)
{
    double roll, pitch, yaw;
    tf::Quaternion q;
    tf::quaternionMsgToTF(msg->pose.orientation, q);
    tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
    if(fabs(msg->pose.position.x)                       < AllowError_d
       && fabs(msg->pose.position.y)                    < AllowError_d
       && fabs(msg->pose.position.z - beginpoint_(2))   < AllowError_d
       && fabs(yaw)                                     < AllowError_rad
       && !in_line
       && !in_land)
    {
        in_line = true;
        ROS_INFO("Begin to fly line!!!");
    }
    if(in_line == false && in_land == false)
    {
        ROS_INFO("Taking off to beginpoint,  waiting line!!!! ");
        takeoff(beginpoint_);
    }
    if(in_line == true)
    {

        t_now = ros::Time::now();
        double dt = (t_now.toNSec() - t_prev.toNSec())/1e+9;
        dt = dt < 0.1 ? dt : 0;
        line_t += dt;
        if(line_t < loiter_t)
        {
            ROS_INFO("Waiting 5 second in begin point!!!");
            loiter(beginpoint_);
        }
        else if(line_t >= loiter_t && line_t <= allline_t + loiter_t)
        {
            ROS_INFO("Fly line!!!");
            line(beginpoint_, line_t, endpoint_);
        }
        else if(line_t  > allline_t + loiter_t && line_t  <= allline_t + 2 * loiter_t)
        {

                ROS_INFO("line end!!!Waiting endpoint 5 second!!!");
                loiter(endpoint_);
        }
        else
        {
            in_land = true;
            in_line = false;
        }
    }
    if(in_land == true)
    {
        ROS_INFO("it is landing!!!");
        t_now = ros::Time::now();
        double dt = (t_now.toNSec() - t_prev.toNSec())/1e+9;
        dt = dt < 0.1 ? dt : 0;
        land_t += dt;
        land(endpoint_, land_t);
    }
    t_prev = t_now;
    publish();
}