Function main

src/simulator/cascadePID/src/cascadePID_node.cpp:122–182

}

int main(int argc, char **argv) {
  ros::init(argc, argv, "cascade_PID");

  ros::NodeHandle n("~");

  double init_x, init_y, init_z;
  double init_yaw;
  double controller_rate, angle_stable_time, damping_ratio;
  int drone_id;
  std::string quad_name;
  n.param("init_state_x", init_x, 0.0);
  n.param("init_state_y", init_y, 0.0);
  n.param("init_state_z", init_z, 1.0);
  n.param("init_state_yaw", init_yaw, 1.0);
  init_yaw = init_yaw / 180.0 * M_PI;
  yaw_des = init_yaw;
  n.param("controller_rate", controller_rate, 200.0);
  n.param("quadrotor_name", quad_name, std::string("quadrotor"));
  n.param("angle_stable_time", angle_stable_time, 0.1);
  n.param("damping_ratio", damping_ratio, 0.7);
  n.param("drone_id", drone_id, 0);

  control_RPM_pub = n.advertise<std_msgs::Float32MultiArray>("cmd_RPM", 100);
  ros::Subscriber odom_sub = n.subscribe("odom", 100, Odom_callback,
                                         ros::TransportHints().tcpNoDelay());
  ros::Subscriber cmd_sub = n.subscribe("cmd_pose", 100, cmd_callback,
                                        ros::TransportHints().tcpNoDelay());
  ros::Subscriber position_cmd_sub_ =
      n.subscribe("position_cmd", 10, fuel_position_cmd_callback,
                  ros::TransportHints().tcpNoDelay());

  quad_PID.setdroneid(drone_id);

  ros::Timer controller_timer =
      n.createTimer(ros::Duration(1.0 / controller_rate), run_control);

  Matrix3d Internal_mat;
  Internal_mat << 2.64e-3, 0, 0, 0, 2.64e-3, 0, 0, 0, 4.96e-3;
  double arm_length = 0.22;
  double k_F = 8.98132e-9;
  k_F = 3.0 * k_F;
  double mass = 1.9;

  // cascadePID quad_PID(controller_rate);
  quad_PID.setrate(controller_rate);
  quad_PID.setInternal(mass, Internal_mat, arm_length, k_F);
  quad_PID.setParam(angle_stable_time, damping_ratio);

  ros::Rate rate(controller_rate);
  rate.sleep();

  pos_des << init_x, init_y, init_z;
  vel_des << 0, 0, 0;
  acc_des << 0, 0, 0;

  t_init = ros::Time::now();

  ros::spin();