px4_command 2.0

Author: potato77

.vscode/ipch/1ad808d9e5aebffe/mmap_address.bin

@@ -8,6 +8,7 @@ project(px4_command)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  message_generation  
   roscpp
   geometry_msgs
   sensor_msgs
@@ -18,7 +19,6 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   tf2_eigen
   mavros_msgs
-  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -34,54 +34,28 @@ find_package(Boost REQUIRED COMPONENTS system)
 add_message_files(
   DIRECTORY msg
   FILES
-  command.msg
+  ControlCommand.msg
   data_log.msg
+  TrajectoryPoint.msg
+  Trajectory.msg
+  AttitudeReference.msg
+  DroneState.msg
 )
 
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
+  geometry_msgs 
+  sensor_msgs
   std_msgs
 )
 
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-
 catkin_package(
   INCLUDE_DIRS include
   CATKIN_DEPENDS  message_runtime
 )
 
 
-
-
 ###########
 ## Build ##
 ###########

.vscode/ipch/1af179b751394c17/mmap_address.bin

@@ -15,18 +15,28 @@ geo_fence:
 
 ## 限制参数
 Limit:
+  pxy_error_max: 0.8
+  vxy_error_max: 1.0
+  pz_error_max: 0.5
+  vz_error_max: 1.0
+  pxy_int_max : 5.0
+  pz_int_max : 5.0
   tilt_max: 20.0
   THR_MIN: 0.1
   THR_MAX: 0.9
+  int_start_error: 100.0
+
   XY_VEL_MAX : 0.8
   Z_VEL_MAX : 0.5
-
+  
+  
 ## 起飞高度
 Takeoff_height : 0.8
 Disarm_height : 0.15
-flag_att_sp : 1
+Use_mocap_raw : 0
+
 # 位置环参数
-Pos_pid:
+Pos_cascade_pid:
   Kp_xy : 0.95
   Kp_z : 1.0
   Kp_vxvy : 0.09
@@ -38,6 +48,16 @@ Pos_pid:
   Hover_throttle : 0.5
   MPC_VELD_LP: 5.0
 
+# 位置环参数
+Pos_pid:
+  Kp_xy : 2.5
+  Kp_z : 2.5
+  Ki_xy : 0.5
+  Ki_z : 0.5
+  Kd_xy : 3.0
+  Kd_vz : 3.0
+
+
 # 位置环参数 for ude
 Pos_ude:
   Kp_xy : 0.5
@@ -46,9 +66,6 @@ Pos_ude:
   Kd_Zz : 2.0
   T_ude_xy : 1.0
   T_ude_z : 1.0
-  INT_LIM_X : 5.0
-  INT_LIM_Y : 5.0
-  INT_LIM_Z : 10.0
 
 # 位置环参数 for passivity
 Pos_passivity:
@@ -58,10 +75,7 @@ Pos_passivity:
   Kd_z : 2.0
   T_ude_xy : 1.0
   T_ude_z : 1.0
-  T1: 0.1
-  INT_LIM_X : 5.0
-  INT_LIM_Y : 5.0
-  INT_LIM_Z : 10.0
+  T_ps: 0.1
 
 # 位置环参数 for ne
 Pos_ne:
@@ -71,12 +85,17 @@ Pos_ne:
   Kd_z : 2.0
   T_ude_xy : 1.0
   T_ude_z : 1.0
-  Tn: 0.1
-  INT_LIM_X : 5.0
-  INT_LIM_Y : 5.0
-  INT_LIM_Z : 10.0
-
+  T_ne: 0.1
 
+# 圆轨迹参数 for Circle_Trajectory
+Circle_Trajectory:
+  Center_x: 0.0
+  Center_y: 0.0
+  Center_z: 1.0
+  radius: 1.0
+  linear_vel: 0.31415926
+  time_total: 50.0
+  direction: 1.0
 
 
 

.vscode/ipch/1ba3f7e1ff0a1044/mmap_address.bin

@@ -0,0 +1,155 @@
+/***************************************************************************************************************************
+* circle_trajectory.h
+*
+* Author: Qyp
+*
+* Update Time: 2019.6.28
+*
+* Introduction:  Circle trajectory generation code
+*         1. Generating the circle trajectory
+*         2. Parameter: center, radius, linear_vel, time_total, direction
+*         3. Input: time_from_start
+*         4. Output: position, velocity, acceleration, jerk, snap
+***************************************************************************************************************************/
+#ifndef CIRCLE_TRAJECTORY_H
+#define CIRCLE_TRAJECTORY_H
+
+#include <Eigen/Eigen>
+#include <math.h>
+#include <math_utils.h>
+#include <px4_command/TrajectoryPoint.h>
+using namespace std;
+
+class Circle_Trajectory
+{
+     //public表明该数据成员、成员函数是对全部用户开放的。全部用户都能够直接进行调用，在程序的不论什么其他地方訪问。
+    public:
+
+        //构造函数
+        Circle_Trajectory(void):
+            Circle_Trajectory_nh("~")
+        {
+            Circle_Trajectory_nh.param<float>("Circle_Trajectory/Center_x", center[0], 0.0);
+            Circle_Trajectory_nh.param<float>("Circle_Trajectory/Center_y", center[1], 0.0);
+            Circle_Trajectory_nh.param<float>("Circle_Trajectory/Center_z", center[2], 1.0);
+            Circle_Trajectory_nh.param<float>("Circle_Trajectory/radius", radius, 1.0);
+            Circle_Trajectory_nh.param<float>("Circle_Trajectory/linear_vel", linear_vel, 0.5);
+            Circle_Trajectory_nh.param<float>("Circle_Trajectory/time_total", time_total, 10.0);
+            Circle_Trajectory_nh.param<float>("Circle_Trajectory/direction", direction, 1.0);
+ 
+            time_from_start  = 0.0;
+        }
+        
+        // Parameter
+        Eigen::Vector3f center;
+        float radius;
+        float linear_vel;
+        float time_total;
+        float direction;         //direction = 1 for CCW 逆时针, direction = -1 for CW 顺时针
+
+        // Input
+        float time_from_start;
+
+        // Output: position, velocity, acceleration, jerk, snap
+        px4_command::TrajectoryPoint Circle_trajectory;
+
+        //Printf the Circle_Trajectory parameter
+        void printf_param();
+
+        //Printf the Circle_Trajectory result
+        void printf_result();
+
+        //Circle_Trajectory Calculation [Input: time_from_start; Output: Circle_trajectory;]
+        px4_command::TrajectoryPoint Circle_trajectory_generation(float time_from_start);
+
+    private:
+
+        ros::NodeHandle Circle_Trajectory_nh;
+
+};
+
+
+px4_command::TrajectoryPoint Circle_Trajectory::Circle_trajectory_generation(float time_from_start)
+{
+    const float omega = direction * fabs(linear_vel / radius);
+    const float angle = time_from_start * omega;
+    const float cos_angle = cos(angle);
+    const float sin_angle = sin(angle);
+
+    // cout << "omega : " << omega  * 180/M_PI <<" [deg/s] " <<endl;
+    // cout << "angle : " << angle  * 180/M_PI <<" [deg] " <<endl;
+
+    Circle_trajectory.time_from_start = time_from_start;
+
+    Circle_trajectory.position_ref[0] = radius * cos_angle + center[0];
+    Circle_trajectory.position_ref[1] = radius * sin_angle + center[1];
+    Circle_trajectory.position_ref[2] = center[2];
+
+    Circle_trajectory.velocity_ref[0] = - radius * omega * sin_angle;
+    Circle_trajectory.velocity_ref[1] = radius * omega * cos_angle;
+    Circle_trajectory.velocity_ref[2] = 0;
+
+    Circle_trajectory.acceleration_ref[0] = - radius * pow(omega, 2.0) * cos_angle;
+    Circle_trajectory.acceleration_ref[1] = - radius * pow(omega, 2.0) * sin_angle;
+    Circle_trajectory.acceleration_ref[2] = 0;
+
+    Circle_trajectory.jerk_ref[0] = radius * pow(omega, 3.0) * sin_angle;
+    Circle_trajectory.jerk_ref[1] = - radius * pow(omega, 3.0) * cos_angle;
+    Circle_trajectory.jerk_ref[2] = 0;
+
+    Circle_trajectory.snap_ref[0] = radius * pow(omega, 4.0) * cos_angle;
+    Circle_trajectory.snap_ref[1] = radius * pow(omega, 4.0) * sin_angle;
+    Circle_trajectory.snap_ref[2] = 0;
+
+    Circle_trajectory.yaw_ref = 0;
+    Circle_trajectory.yaw_rate_ref = 0;
+    Circle_trajectory.yaw_acceleration_ref = 0;
+    return Circle_trajectory;
+}
+
+
+void Circle_Trajectory::printf_result()
+{
+    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Circle_Trajectory<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+
+    //固定的浮点显示
+    cout.setf(ios::fixed);
+    //左对齐
+    cout.setf(ios::left);
+    // 强制显示小数点
+    cout.setf(ios::showpoint);
+    // 强制显示符号
+    cout.setf(ios::showpos);
+
+    cout<<setprecision(2);
+
+    cout << "time_from_start : " << Circle_trajectory.time_from_start<< " [s] " <<endl;
+
+    cout << "position  [X Y Z] : " << Circle_trajectory.position_ref[0] << " [m] "<< Circle_trajectory.position_ref[1]<<" [m] "<<Circle_trajectory.position_ref[2]<<" [m] "<<endl;
+
+    cout << "velocity [X Y Z] : " << Circle_trajectory.velocity_ref[0] << " [m/s] "<< Circle_trajectory.velocity_ref[1]<<" [m/s] "<<Circle_trajectory.velocity_ref[2]<<" [m/s] "<<endl;
+
+    cout << "acceleration [X Y Z] : " << Circle_trajectory.acceleration_ref[0] << " [m/s^2] "<< Circle_trajectory.acceleration_ref[1]<<" [m/s^2] "<< Circle_trajectory.acceleration_ref[2]<<" [m/s^2] "<<endl;
+
+    cout << "jerk [X Y Z] : " << Circle_trajectory.jerk_ref[0] << " [m/s^3] "<< Circle_trajectory.jerk_ref[1]<<" [m/s^3] "<<Circle_trajectory.jerk_ref[2]<<" [m/s^3] "<<endl;
+
+    cout << "snap [X Y Z] : " << Circle_trajectory.snap_ref[0] << " [m/s^4] "<< Circle_trajectory.snap_ref[1]<<" [m/s^4] "<<Circle_trajectory.snap_ref[2]<<" [m/s^4] "<<endl;
+
+}
+
+// 【打印参数函数】
+void Circle_Trajectory::printf_param()
+{
+    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Circle_Trajectory Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
+
+    cout <<"center :  "<< center <<endl;
+    cout <<"radius :  "<< radius << endl;
+    cout <<"linear_vel : "<< linear_vel << endl;
+    cout <<"time_total : "<< time_total << endl;
+    cout <<"direction : "<< direction << endl;
+    //direction = 1 for CCW 逆时针, direction = -1 for CW 顺时针
+}
+
+
+
+#endif