├── config ├── estimatorsettings.yaml ├── square.yaml ├── payload_drop.yaml ├── indoorgeofence.yaml ├── outdoorgeofence.yaml ├── collision_avoidance_streo.yaml ├── controllersettings.yaml ├── collision_avoidance.yaml ├── target_tracking.yaml ├── autonomous_landing.yaml ├── payload_control.yaml ├── Parameter_for_control_2_drone.yaml ├── Parameter_for_control_gazebo.yaml ├── BaselineControllerParameters.yaml ├── Parameter_for_control.yaml └── Parameter_3drone_payload_gazebo.yaml ├── srv ├── Emergency.srv ├── GeneralInfo.srv ├── SetHome.srv ├── MultiPayloadAction.srv ├── SinglePayloadAction.srv └── ControlParameter.srv ├── msg ├── Trajectory.msg ├── Mocap.msg ├── Topic_for_log.msg ├── AttitudeReference.msg ├── ControlOutput.msg ├── HomePosition.msg ├── PayloadPoseCommand.msg ├── FleetStatus.msg ├── SimplifiedLog.msg ├── ControlCommand.msg ├── AddonForce.msg ├── AuxiliaryState.msg ├── TrajectoryPoint.msg ├── AuxiliaryState_singleUAV.msg └── DroneState.msg ├── images ├── Lab.PNG ├── Gazebo.PNG ├── Thumbnail.PNG ├── formation.PNG └── node_structure.PNG ├── sitlmodel ├── multi_iris │ └── multi_iris_3.zip └── launchfile │ ├── uav_single_drone_with_id.launch │ └── multi_drone_test.launch ├── launch ├── px4_outdoor_wifitest.launch ├── px4_interdrone_communication_gazebo.launch ├── px4_interdrone_communication_sitl.launch ├── px4_interdrone_communication.launch ├── mavros_multi_drone.launch ├── mavros_multi_drone_USBbridge.launch ├── px4_multidrone_pos_estimator.launch ├── px4_multidrone_pos_controller_gazebo.launch ├── px4_multidrone_pos_estimator_outdoor.launch ├── px4_multidrone_pos_estimator_pure_vision.launch ├── px4_multidrone_pos_controller.launch ├── px4_multidrone_pos_controller_sitl.launch ├── px4_multidrone_pos_controller_outdoor.launch ├── px4_multidrone_pos_controller_outdoor_gazebo.launch └── px4_payload_sitl.launch ├── sh └── sh_for_payload_control │ ├── pld_ctrl_launch.sh │ ├── payload_single_UAV.sh │ ├── payload_multi_UAV.sh │ ├── payload_multi_UAV_pure_vision.sh │ ├── payload_single_UAV_visionfeedback.sh │ ├── payload_single_UAV_sitl_gps_vision.sh │ ├── payload_multi_UAV_sitl.sh │ ├── payload_single_UAV_with_id.sh │ ├── payload_multi_UAV_sitl_3drones.sh │ └── payload_multi_UAV_sitl_2drones.sh ├── .vscode ├── c_cpp_properities.json └── settings.json ├── package.xml ├── README.md ├── include ├── Frame_tf_utils.h ├── serial.hpp ├── state_from_mavros.h ├── state_from_mavros_multidrone.h ├── circle_trajectory.h ├── rectangular_trajectory.h ├── pos_controller_PID.h ├── math_utils.h ├── quadrotor_drone.h └── pos_controller_cascade_PID.h ├── src ├── Utilities │ ├── outdoorwifitest.cpp │ ├── outdoorwifitest_ground.cpp │ ├── set_uav0_mode.cpp │ ├── set_uav1_mode.cpp │ └── set_uav2_mode.cpp ├── px4_multidrone_pos_estimator_pure_vision.cpp └── px4_multidrone_pos_estimator.cpp └── CMakeLists.txt /config/estimatorsettings.yaml: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /srv/Emergency.srv: -------------------------------------------------------------------------------- 1 | bool emergency_happened 2 | --- 3 | bool killcomfirmed -------------------------------------------------------------------------------- /msg/Trajectory.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | TrajectoryPoint[] points 4 | -------------------------------------------------------------------------------- /images/Lab.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LonghaoQian/px4_command/HEAD/images/Lab.PNG -------------------------------------------------------------------------------- /images/Gazebo.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LonghaoQian/px4_command/HEAD/images/Gazebo.PNG -------------------------------------------------------------------------------- /images/Thumbnail.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LonghaoQian/px4_command/HEAD/images/Thumbnail.PNG -------------------------------------------------------------------------------- /images/formation.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LonghaoQian/px4_command/HEAD/images/formation.PNG -------------------------------------------------------------------------------- /images/node_structure.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LonghaoQian/px4_command/HEAD/images/node_structure.PNG -------------------------------------------------------------------------------- /srv/GeneralInfo.srv: -------------------------------------------------------------------------------- 1 | string controllername 2 | int32 TargetdroneID 3 | bool isMulti 4 | --- 5 | bool oktostart 6 | -------------------------------------------------------------------------------- /srv/SetHome.srv: -------------------------------------------------------------------------------- 1 | float32 longitude 2 | float32 latitude 3 | float32 altitude 4 | --- 5 | bool homeset_ok 6 | -------------------------------------------------------------------------------- /config/square.yaml: -------------------------------------------------------------------------------- 1 | ##正方形大小 m 2 | size_square: 2 3 | ##飞行高度 4 | height_square: 1.0 5 | ##单边飞行时间 6 | sleep_time: 10.0 7 | -------------------------------------------------------------------------------- /srv/MultiPayloadAction.srv: -------------------------------------------------------------------------------- 1 | bool perform_action 2 | int32 action_type 3 | --- 4 | bool status_ok 5 | int32 trajectory_type 6 | -------------------------------------------------------------------------------- /srv/SinglePayloadAction.srv: -------------------------------------------------------------------------------- 1 | bool perform_action 2 | int32 action_type 3 | --- 4 | bool status_ok 5 | int32 trajectory_type 6 | -------------------------------------------------------------------------------- /sitlmodel/multi_iris/multi_iris_3.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LonghaoQian/px4_command/HEAD/sitlmodel/multi_iris/multi_iris_3.zip -------------------------------------------------------------------------------- /config/payload_drop.yaml: -------------------------------------------------------------------------------- 1 | ##payload_drop parameter 2 | fly_height: 1.0 3 | target_x: 0.0 4 | target_y: 1.0 5 | drop_height: 0.4 6 | limit_time: 10.0 7 | min_distance: 0.2 8 | -------------------------------------------------------------------------------- /launch/px4_outdoor_wifitest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /config/indoorgeofence.yaml: -------------------------------------------------------------------------------- 1 | # drone geo fence for indoor flight 2 | DroneGeoFence: 3 | x_min: -1.6 4 | x_max: 1.5 5 | y_min: -1.3 6 | y_max: 1.3 7 | z_min: -0.05 8 | z_max: 2.5 -------------------------------------------------------------------------------- /config/outdoorgeofence.yaml: -------------------------------------------------------------------------------- 1 | # drone geo fence for outdoor flight 2 | DroneGeoFence: 3 | x_min: -50.0 4 | x_max: 50.0 5 | y_min: -50.0 6 | y_max: 50.0 7 | z_min: 0.0 8 | z_max: 20.0 -------------------------------------------------------------------------------- /msg/Mocap.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## rigid-body position and velocity 4 | float32[3] position ## [m] 5 | float32[3] velocity ## [m/s] 6 | float32[3] angular_velocity ## [rad/s] 7 | float32[4] quaternion ## 8 | -------------------------------------------------------------------------------- /config/collision_avoidance_streo.yaml: -------------------------------------------------------------------------------- 1 | ##Parameter for collision_avoidance.cpp 2 | 3 | ## 期望位置_x; 4 | target_x : 8 5 | 6 | ## 期望位置_y 7 | target_y : 0 8 | 9 | ##追踪部分位置环P 10 | p_xy : 1 11 | 12 | ## 追踪部分速度限幅 13 | vel_track_max : 1.0 14 | -------------------------------------------------------------------------------- /msg/Topic_for_log.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## log时间 4 | float32 time ## [s] 5 | 6 | DroneState Drone_State 7 | 8 | ControlCommand Control_Command 9 | 10 | AttitudeReference Attitude_Reference 11 | 12 | ControlOutput Control_Output 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /msg/AttitudeReference.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## 位置控制器输出： 期望角度、期望角加速度 4 | float32[3] throttle_sp ## [0-1] 惯性系下的油门量 5 | float32 desired_throttle ## [0-1] 机体系z轴 6 | float32[3] desired_attitude ## [rad] 7 | geometry_msgs/Quaternion desired_att_q ## 四元数 8 | -------------------------------------------------------------------------------- /msg/ControlOutput.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## 位置控制器输出： 期望角度、期望角加速度 4 | float32[3] u_l ## [0-1] 惯性系下的油门量 5 | float32[3] u_d ## [rad] 6 | 7 | float32[3] NE ## [rad] 8 | 9 | float32[3] Thrust ## [rad] 10 | float32[3] Throttle -------------------------------------------------------------------------------- /sh/sh_for_payload_control/pld_ctrl_launch.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | xterm -e "roslaunch px4_command mavros_multi_drone.launch uavID:=uav0" & 4 | xterm -e "roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav0" & 5 | xterm -e "roslaunch px4_command px4_multidrone_pos_controller.launch uavID:=uav0" 6 | -------------------------------------------------------------------------------- /launch/px4_interdrone_communication_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /sh/sh_for_payload_control/payload_single_UAV.sh: -------------------------------------------------------------------------------- 1 | ## 2 | gnome-terminal --window -e 'bash -c "roslaunch mavros px4.launch fcu_url:="/dev/ttyTHS1:921600"; exec bash"' \ 3 | --tab -e 'bash -c "sleep 4; roslaunch px4_command px4_pos_estimator.launch; exec bash"' \ 4 | --tab -e 'bash -c "sleep 4; roslaunch px4_command px4_pos_controller.launch; exec bash"' \ 5 | -------------------------------------------------------------------------------- /config/controllersettings.yaml: -------------------------------------------------------------------------------- 1 | # settings for controller node 2 | 3 | ## true for print state 4 | PrintState : true 5 | 6 | ## single or multi-UAV mode 7 | CooperativeMode: 8 | isMulti: false 9 | droneID: 2 # single UAV payload mode is activated in drone 2 10 | 11 | ActionMode: 12 | type: 1 # 0 for circle path, 1 for rectangular path -------------------------------------------------------------------------------- /launch/px4_interdrone_communication_sitl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/px4_interdrone_communication.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /sh/sh_for_payload_control/payload_multi_UAV.sh: -------------------------------------------------------------------------------- 1 | ## 2 | gnome-terminal --window -e 'bash -c "roslaunch px4_command mavros_multi_drone.launch uavID:=uav0; exec bash"' \ 3 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator.launch uavID:=uav0; exec bash"' \ 4 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_controller.launch uavID:=uav0; exec bash"' \ 5 | -------------------------------------------------------------------------------- /msg/HomePosition.msg: -------------------------------------------------------------------------------- 1 | # MAVLink message: HOME_POSITION 2 | # https://mavlink.io/en/messages/common.html#HOME_POSITION 3 | 4 | std_msgs/Header header 5 | 6 | geographic_msgs/GeoPoint geo # geodetic coordinates in WGS-84 datum 7 | 8 | geometry_msgs/Point position # local position 9 | geometry_msgs/Quaternion orientation # XXX: verify field name (q[4]) 10 | geometry_msgs/Vector3 approach # position of the end of approach vector -------------------------------------------------------------------------------- /sh/sh_for_payload_control/payload_multi_UAV_pure_vision.sh: -------------------------------------------------------------------------------- 1 | ## 2 | gnome-terminal --window -e 'bash -c "roslaunch px4_command mavros_multi_drone.launch uavID:=uav0; exec bash"' \ 3 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav0; exec bash"' \ 4 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_gazebo.launch uavID:=uav0; exec bash"' \ 5 | -------------------------------------------------------------------------------- /msg/PayloadPoseCommand.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## Command_ID for safety, the command ID should increase with every command sent. 4 | uint32 Command_ID 5 | 6 | ## Payload Pose Setpoint 7 | float32 x_ref ## [m] 8 | float32 y_ref ## [m] 9 | float32 z_ref ## [m] 10 | float32 roll_ref ## [deg] 11 | float32 pitch_ref ## [deg] 12 | float32 yaw_ref ## [deg] 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/mavros_multi_drone.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/mavros_multi_drone_USBbridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/px4_multidrone_pos_estimator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/px4_multidrone_pos_controller_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/px4_multidrone_pos_estimator_outdoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/px4_multidrone_pos_estimator_pure_vision.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /msg/FleetStatus.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## fleet info for robust payload control 4 | float32 r_jx ## [m] 5 | float32 r_jy ## [m] 6 | float32 v_jx ## [m/s] 7 | float32 v_jy ## [m/s] 8 | float32 f_Ljx ## [N] 9 | float32 f_Ljy ## [N] 10 | float32 f_Ljz ## [N] 11 | float32 delta_jx ## [N] 12 | float32 delta_jy ## [N] 13 | float32 delta_jz ## [N] 14 | float32 rd_jx ## [m] 15 | float32 rd_jy ## [m] 16 | bool emergency ## -------------------------------------------------------------------------------- /launch/px4_multidrone_pos_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /config/collision_avoidance.yaml: -------------------------------------------------------------------------------- 1 | ##Parameter for collision_avoidance.cpp 2 | 3 | ## 期望位置_x; 4 | target_x : 4 5 | 6 | ## 期望位置_y 7 | target_y : -0.3 8 | 9 | ##追踪部分位置环P 10 | p_xy : 1 11 | 12 | ## 单位:米 13 | R_outside : 1.3 14 | 15 | ## 单位:米 16 | R_inside : 0.3 17 | 18 | ## 追踪部分速度限幅 19 | vel_track_max : 0.4 20 | 21 | ## 大圈比例系数 22 | p_R : 0.6 23 | 24 | ## 小圈比例系数 25 | p_r : 1 26 | 27 | ## 避障部分最大速度 28 | vel_collision_max : 0.5 29 | 30 | ## 总速度最大速度 31 | vel_sp_max : 0.4 32 | 33 | 34 | ## 激光雷达范围设定 35 | range_min : 45 36 | range_max : 315 37 | -------------------------------------------------------------------------------- /msg/SimplifiedLog.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## simplified log 4 | ## drone status 5 | bool connected ## 机载电脑是否连接上飞控，true已连接，false则不是 6 | bool armed ## 是否解锁，true为已解锁，false则不是 7 | string mode ## PX4飞控当前飞行模式 8 | float32 time_from_start ## [s] ## 系统启动时间 9 | bool mocapOK ## Flag showing whether the mocap msg has been updated very loop 10 | 11 | ControlCommand Control_Command ## control command 12 | 13 | float32[3] Throttle ## throttle calculated by controller. 14 | -------------------------------------------------------------------------------- /msg/ControlCommand.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## 控制命令的编号 防止接收到错误命令， 编号应该逐次递加 4 | uint32 Command_ID 5 | 6 | ## 控制命令的模式 7 | uint8 Mode 8 | # enum Mode 控制模式枚举 9 | uint8 Idle=0 10 | uint8 Takeoff=1 11 | uint8 Move_ENU=2 12 | uint8 Move_Body=3 13 | uint8 Hold=4 14 | uint8 Land=5 15 | uint8 Disarm=6 16 | uint8 Payload_Stabilization_SingleUAV=7 17 | uint8 Trajectory_Tracking=8 18 | uint8 Payload_Stabilization=9 19 | uint8 Payload_Land=10 20 | 21 | ## 控制参考量 22 | ## 位置参考量：位置、速度、加速度、加加速度、加加加速度 23 | ## 角度参考量：偏航角、偏航角速度、偏航角加速度 24 | TrajectoryPoint Reference_State 25 | -------------------------------------------------------------------------------- /launch/px4_multidrone_pos_controller_sitl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /config/target_tracking.yaml: -------------------------------------------------------------------------------- 1 | ##Parameter for target_tracking.cpp 2 | 3 | ##追踪算法比例参数，越大响应越快 4 | kpx_track : 0.6 5 | kpy_track : 0.6 6 | kpz_track : 0.8 7 | 8 | ##追踪算法最大追踪速度 9 | ##取决与使用场景，室内或者第一次实验时，建议选小一点 10 | track_max_vel_x : 0.5 11 | track_max_vel_y : 0.5 12 | track_max_vel_z : 0.5 13 | 14 | ##追踪算法速度死区 15 | track_thres_vel_x : 0.05 16 | track_thres_vel_y : 0.05 17 | track_thres_vel_z : 0.05 18 | 19 | ##前后方向是否追踪标志位 1 for track x, 0 for not track x 20 | flag_x : 1 21 | 22 | ##视觉丢失次数阈值 23 | count_vision_lost : 10 24 | 25 | ##追踪的前后间隔 26 | delta_x : 1.5 27 | 28 | ##追踪距离阈值 29 | distance_thres: 0.4 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properities.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "/home/longhao/fsc_ws/devel/include/px4_command" 8 | ], 9 | "defines": [], 10 | "compilerPath": "/usr/bin/gcc", 11 | "cStandard": "c11", 12 | "cppStandard": "c++17", 13 | "intelliSenseMode": "clang-x64", 14 | "compileCommands": "${workspaceFolder}/build/compile_commands.json" 15 | } 16 | ], 17 | 18 | "version": 4 19 | } 20 | -------------------------------------------------------------------------------- /launch/px4_multidrone_pos_controller_outdoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/px4_multidrone_pos_controller_outdoor_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /config/autonomous_landing.yaml: -------------------------------------------------------------------------------- 1 | # Parameter for autonomous_landing.cpp 2 | 3 | ##降落追踪 的比例参数 4 | kpx_land: 0.5 5 | kpy_land: 0.5 6 | kpz_land: 0.4 7 | 8 | ##降落追踪控制算法的最大速度 9 | Max_velx_land: 1 10 | Max_vely_land: 1 11 | Max_velz_land: 0.8 12 | 13 | ##降落追踪控制算法的速度死区 14 | Thres_velx_land: 0.0 15 | Thres_vely_land: 0.0 16 | Thres_velz_land: 0.0 17 | 18 | ##允许降落最大距离阈值 19 | Thres_distance_land: 0.16 20 | 21 | ##允许降落计数阈值 22 | Thres_count_land: 10 23 | 24 | ##允许降落最大高度阈值 25 | land_max_z: 0.40 26 | 27 | ##视觉丢失计数阈值 28 | Thres_vision_lost: 30 29 | 30 | ##允许飞行最低高度[这个高度是指降落板上方的相对高度] 31 | fly_min_z: 0.3 32 | 33 | ##降落板高度 34 | height_on_pad: -0.4 35 | 36 | -------------------------------------------------------------------------------- /sh/sh_for_payload_control/payload_single_UAV_visionfeedback.sh: -------------------------------------------------------------------------------- 1 | ## 2 | gnome-terminal --window -e 'bash -c "roslaunch px4 single_drone_payload_vision_sitl.launch; exec bash"' \ 3 | --tab -e 'bash -c "sleep 5; roslaunch optitrack_broadcast emulator_for_gazebo.launch; exec bash"' \ 4 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav2; exec bash"' \ 5 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_gazebo.launch uavID:=uav2; exec bash"' \ 6 | --tab -e 'bash -c "sleep 2; rosrun track_april_tag april_tag_opencv_emulate; exec bash"' \ 7 | --tab -e 'bash -c "sleep 2; rosrun px4_command set_uav2_mode; exec bash"' \ 8 | --tab -e 'bash -c "sleep 2; rosrun qt_ground_station qt_ground_station"' \ 9 | -------------------------------------------------------------------------------- /msg/AddonForce.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ##the total disturbance estimation 4 | 5 | float32 delta_Tx ## [N] 6 | float32 delta_Ty ## [N] 7 | float32 delta_Tz ## [N] 8 | 9 | float32 delta_Rx ## [N] 10 | float32 delta_Ry ## [N] 11 | float32 delta_Rz ## [N] 12 | 13 | ## the following terms are used for different purpose for TCST and JGCD method 14 | ## for TCST, they are the cross feeding terms 15 | ## for JGCD, they are the additive mpc command 16 | 17 | float32 R_1x ## [N] 18 | float32 R_1y ## [N] 19 | float32 R_1z ## [N] 20 | 21 | float32 R_2x ## [N] 22 | float32 R_2y ## [N] 23 | float32 R_2z ## [N] 24 | 25 | bool emergency ## flag for sending out emergency landing signal 26 | bool perform_action ## flag for performing action -------------------------------------------------------------------------------- /sh/sh_for_payload_control/payload_single_UAV_sitl_gps_vision.sh: -------------------------------------------------------------------------------- 1 | ## 2 | gnome-terminal --window -e 'bash -c "roslaunch px4 single_drone_payload_vision_sitl.launch; exec bash"' \ 3 | --tab -e 'bash -c "sleep 5; roslaunch optitrack_broadcast emulator_for_gazebo.launch; exec bash"' \ 4 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_outdoor.launch uavID:=uav2; exec bash"' \ 5 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_outdoor_gazebo.launch uavID:=uav2; exec bash"' \ 6 | --tab -e 'bash -c "sleep 2; rosrun track_april_tag april_tag_opencv_emulate; exec bash"' \ 7 | --tab -e 'bash -c "sleep 3; rosrun ade ade"' \ 8 | --tab -e 'bash -c "sleep 2; rosrun px4_command set_uav2_mode; exec bash"' \ 9 | --tab -e 'bash -c "sleep 2; rosrun qt_ground_station qt_ground_station"' \ 10 | 11 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | px4_command 4 | 0.0.0 5 | The px4_command package 6 | 7 | Yuhua Qi 8 | Yuhua Qi 9 | TODO 10 | 11 | http://www.amovauto.com/ 12 | https://github.com/potato77/px4_command.git 13 | 14 | message_generation 15 | message_runtime 16 | 17 | catkin 18 | std_msgs 19 | std_msgs 20 | std_msgs 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /msg/AuxiliaryState.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## quadrotor attitude 4 | 5 | float32 q_0 ## 6 | float32 q_1 ## 7 | float32 q_2 ## 8 | float32 q_3 ## 9 | 10 | float32 r_jx ## [m] 11 | float32 r_jy ## [m] 12 | 13 | float32 v_jx ## [m] 14 | float32 v_jy ## [m] 15 | 16 | float32 L_measured ## [m] measured L from mocap system 17 | 18 | ## payload pos error 19 | 20 | float32 pos_error_x ## [m] 21 | float32 pos_error_y ## [m] 22 | float32 pos_error_z ## [m] 23 | 24 | float32 angle_error_x ## 25 | float32 angle_error_y ## 26 | float32 angle_error_z ## 27 | 28 | ## quadrotor euler angle 29 | 30 | float32 Euler_roll ## 31 | float32 Euler_pitch ## 32 | float32 Euler_yaw ## 33 | 34 | float32 fLj_x ## [N] 35 | float32 fLj_y ## [N] 36 | float32 fLj_z ## [N] 37 | 38 | float32 Delta_jp_x ## [N] 39 | float32 Delta_jp_y ## [N] 40 | float32 Delta_jp_z ## [N] 41 | 42 | float32 acc_x ## 43 | float32 acc_y ## 44 | float32 acc_z ## 45 | 46 | float32 rd_jx ## 47 | float32 rd_jy ## -------------------------------------------------------------------------------- /sh/sh_for_payload_control/payload_multi_UAV_sitl.sh: -------------------------------------------------------------------------------- 1 | ## 2 | gnome-terminal --window -e 'bash -c "roslaunch px4 multi_drone_payload_sitl.launch; exec bash"' \ 3 | --tab -e 'bash -c "sleep 3; roslaunch optitrack_broadcast emulator_for_gazebo.launch; exec bash"' \ 4 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav0; exec bash"' \ 5 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_gazebo.launch uavID:=uav0; exec bash"' \ 6 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_interdrone_communication_gazebo.launch; exec bash"' \ 7 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav1; exec bash"' \ 8 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_gazebo.launch uavID:=uav1; exec bash"' \ 9 | --tab -e 'bash -c "sleep 2; rosrun px4_command set_uav0_mode; exec bash"' \ 10 | --tab -e 'bash -c "sleep 2; rosrun px4_command set_uav1_mode; exec bash"' \ 11 | --tab -e 'bash -c "sleep 2; rosrun qt_ground_station qt_ground_station"' \ 12 | -------------------------------------------------------------------------------- /msg/TrajectoryPoint.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## 时刻： 用于轨迹追踪 4 | float32 time_from_start ## [s] 5 | 6 | # sub_mode 2-bit value: 7 | # 0 for position, 1 for vel, 1st for xy, 2nd for z. 8 | # xy position xy velocity 9 | # z position 0b00(0) 0b10(2) 10 | # z velocity 0b01(1) 0b11(3) 11 | # 12 | ## 控制命令的子模式 13 | ## 可以选择 位置追踪、速度追踪或者其他复合模式， 14 | ## 默认为 XYZ位置追踪模式 （sub_mode = 0）； 速度追踪启用时，控制器不考虑位置参考量及位置状态反馈 15 | uint8 Sub_mode 16 | # enum sub_mode 17 | uint8 XYZ_POS = 0 18 | uint8 XY_POS_Z_VEL = 1 19 | uint8 XY_VEL_Z_POS = 2 20 | uint8 XY_VEL_Z_VEL = 3 21 | 22 | ## 参考量：位置、速度、加速度、加加速度、加加加速度 23 | float32[3] position_ref ## [m] 24 | float32[3] velocity_ref ## [m/s] 25 | float32[3] acceleration_ref ## [m/s^2] 26 | ## float32[3] jerk_ref ## [m/s^3] 27 | ## float32[3] snap_ref ## [m/s^4] 28 | 29 | ## 角度参考量：偏航角、偏航角速度、偏航角加速度 30 | float32 yaw_ref ## [rad] 31 | ## float32 yaw_rate_ref ## [rad/s] 32 | ## float32 yaw_acceleration_ref ## [rad/s] 33 | 34 | ## attitude angle reference for payload control 35 | float32 pitch_ref ## [rad] 36 | float32 roll_ref ## [rad] -------------------------------------------------------------------------------- /msg/AuxiliaryState_singleUAV.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## the estimated disturbances 4 | float32 W_x ## [N] 5 | float32 W_y ## [N] 6 | float32 W_z ## [N] 7 | 8 | ## quadrotor attitude 9 | float32 q_0 ## 10 | float32 q_1 ## 11 | float32 q_2 ## 12 | float32 q_3 ## 13 | 14 | ## cable motion 15 | float32 r_x ## [m] 16 | float32 r_y ## [m] 17 | 18 | float32 v_x ## [m] 19 | float32 v_y ## [m] 20 | 21 | ## tracking errors 22 | float32 pos_error_x ## [m] 23 | float32 pos_error_y ## [m] 24 | float32 pos_error_z ## [m] 25 | 26 | float32 vel_error_x ## [m/s] 27 | float32 vel_error_y ## [m/s] 28 | float32 vel_error_z ## [m/s] 29 | 30 | ## payload position measurement from mocap 31 | float32 Lm_x ## [m] 32 | float32 Lm_y ## [m] 33 | float32 Lm_z ## [m] 34 | 35 | ## payload position measurement from mocap 36 | float32 Vpm_x ## [m/s] 37 | float32 Vpm_y ## [m/s] 38 | float32 Vpm_z ## [m/s] 39 | 40 | ## the command lift 41 | float32 fL_x ## 42 | float32 fL_y ## 43 | float32 fL_z ## 44 | 45 | ## the quadrotor inerital acceleration 46 | float32 acc_x ## 47 | float32 acc_y ## 48 | float32 acc_z ## -------------------------------------------------------------------------------- /sh/sh_for_payload_control/payload_single_UAV_with_id.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Open the first tab (default terminal window) 4 | gnome-terminal -- bash -c "roslaunch px4 uav_single_drone_with_id.launch ; exec bash" 5 | 6 | # Wait for the terminal to open 7 | sleep 1 8 | 9 | # Use xdotool to open additional tabs in the same terminal 10 | xdotool key ctrl+shift+t 11 | xdotool type --delay 1 -- "sleep 5; roslaunch optitrack_broadcast emulator_for_gazebo.launch ; exec bash" 12 | xdotool key Return 13 | 14 | xdotool key ctrl+shift+t 15 | xdotool type --delay 1 -- "sleep 2; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav0; exec bash" 16 | xdotool key Return 17 | 18 | xdotool key ctrl+shift+t 19 | xdotool type --delay 1 -- "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_sitl.launch uavID:=uav0 paramfile:=Parameter_3drone_payload_gazebo.yaml ; exec bash" 20 | xdotool key Return 21 | 22 | xdotool key ctrl+shift+t 23 | xdotool type --delay 1 -- "sleep 3; rosrun px4_command set_uav0_mode; exec bash" 24 | xdotool key Return 25 | 26 | xdotool key ctrl+shift+t 27 | xdotool type --delay 1 -- "sleep 3; rosrun qt_ground_station qt_ground_station; exec bash" 28 | xdotool key Return 29 | -------------------------------------------------------------------------------- /msg/DroneState.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | ## 机载电脑是否连接上飞控，true已连接，false则不是 4 | bool connected 5 | ## 是否解锁，true为已解锁，false则不是 6 | bool armed 7 | ## PX4飞控当前飞行模式 8 | string mode 9 | 10 | ## 系统启动时间 11 | float32 time_from_start ## [s] 12 | 13 | ## drone position and attitude 14 | float32 longitude ## global position 15 | float32 latitude ## global position 16 | float32[3] position ## [m] local position if in outdoor mode 17 | float32[3] velocity ## [m/s] inertial velocity in ENU 18 | float32[3] attitude ## [rad] 19 | geometry_msgs/Quaternion attitude_q ## [] 20 | float32[3] attitude_rate ## [rad/s] 21 | float32[3] acceleration ## [m/s^2] defined in body fixed frame 22 | ## payload info from mocap system 23 | float32[3] payload_vel ## [m/s] 24 | float32[3] payload_pos ## [m] 25 | float32[4] payload_quaternion ## [] 26 | float32[3] payload_angular_vel ## [rad/s] 27 | ## battery status 28 | float32 battery_voltage ## [V] 29 | float32 battery_remaining ## 30 | ## Flag showing whether the mocap msg has been updated very loop 31 | bool mocapOK -------------------------------------------------------------------------------- /srv/ControlParameter.srv: -------------------------------------------------------------------------------- 1 | string controllername 2 | float32 dronemass 3 | float32 cablelength 4 | float32 a_j 5 | float32 payloadmass 6 | float32 motor_slope 7 | float32 motor_intercept 8 | int32 num_drone 9 | bool isPubAuxiliaryState 10 | bool isAddonForcedUsed 11 | bool isCrossFeedingTermsUsed 12 | float32 t_jx 13 | float32 t_jy 14 | float32 t_jz 15 | float32 kv_xy 16 | float32 kv_z 17 | float32 kR_xy 18 | float32 kR_z 19 | float32 kL 20 | float32 Kphi_xy 21 | float32 Kphi_z 22 | float32 kr1_x 23 | float32 kr1_y 24 | float32 kr1_z 25 | float32 kr2_x 26 | float32 kr2_y 27 | float32 kr2_z 28 | float32 kp_x 29 | float32 kp_y 30 | float32 kp_z 31 | float32 komega_x 32 | float32 komega_y 33 | float32 komega_z 34 | float32 lambdaj_x 35 | float32 lambdaj_y 36 | float32 lambdaj_z 37 | float32 lambda_T_x 38 | float32 lambda_T_y 39 | float32 lambda_T_z 40 | float32 lambda_R_x 41 | float32 lambda_R_y 42 | float32 lambda_R_z 43 | float32 lambda1_x 44 | float32 lambda1_y 45 | float32 lambda1_z 46 | float32 lambda2_x 47 | float32 lambda2_y 48 | float32 lambda2_z 49 | float32 pxy_error_max 50 | float32 pz_error_max 51 | float32 pxy_int_max 52 | float32 pz_int_max 53 | float32 tilt_max 54 | float32 int_start_error 55 | float32 fp_max_x 56 | float32 fp_max_y 57 | float32 fp_max_z 58 | --- 59 | bool oktostart -------------------------------------------------------------------------------- /sh/sh_for_payload_control/payload_multi_UAV_sitl_3drones.sh: -------------------------------------------------------------------------------- 1 | ## 2 | gnome-terminal --window -e 'bash -c "roslaunch px4 multi_drone_payload_3_sitl.launch; exec bash"' \ 3 | --tab -e 'bash -c "sleep 5; roslaunch optitrack_broadcast emulator_for_gazebo.launch; exec bash"' \ 4 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_interdrone_communication_sitl.launch paramfile:=Parameter_3drone_payload_gazebo.yaml; exec bash"' \ 5 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav0; exec bash"' \ 6 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_sitl.launch uavID:=uav0 paramfile:=Parameter_3drone_payload_gazebo.yaml; exec bash"' \ 7 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav1; exec bash"' \ 8 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_sitl.launch uavID:=uav1 paramfile:=Parameter_3drone_payload_gazebo.yaml; exec bash"' \ 9 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav2; exec bash"' \ 10 | --tab -e 'bash -c "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_sitl.launch uavID:=uav2 paramfile:=Parameter_3drone_payload_gazebo.yaml; exec bash"' \ 11 | --tab -e 'bash -c "sleep 2; rosrun px4_command set_uav0_mode; exec bash"' \ 12 | --tab -e 'bash -c "sleep 2; rosrun px4_command set_uav1_mode; exec bash"' \ 13 | --tab -e 'bash -c "sleep 2; rosrun px4_command set_uav2_mode; exec bash"' \ 14 | --tab -e 'bash -c "sleep 2; rosrun qt_ground_station qt_ground_station"' \ 15 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "iostream": "cpp", 4 | "random": "cpp", 5 | "cctype": "cpp", 6 | "clocale": "cpp", 7 | "cmath": "cpp", 8 | "cstdarg": "cpp", 9 | "cstddef": "cpp", 10 | "cstdio": "cpp", 11 | "cstdlib": "cpp", 12 | "cstring": "cpp", 13 | "ctime": "cpp", 14 | "cwchar": "cpp", 15 | "cwctype": "cpp", 16 | "array": "cpp", 17 | "atomic": "cpp", 18 | "strstream": "cpp", 19 | "*.tcc": "cpp", 20 | "bitset": "cpp", 21 | "chrono": "cpp", 22 | "complex": "cpp", 23 | "cstdint": "cpp", 24 | "deque": "cpp", 25 | "list": "cpp", 26 | "unordered_map": "cpp", 27 | "vector": "cpp", 28 | "exception": "cpp", 29 | "algorithm": "cpp", 30 | "functional": "cpp", 31 | "ratio": "cpp", 32 | "system_error": "cpp", 33 | "tuple": "cpp", 34 | "type_traits": "cpp", 35 | "fstream": "cpp", 36 | "initializer_list": "cpp", 37 | "iomanip": "cpp", 38 | "iosfwd": "cpp", 39 | "istream": "cpp", 40 | "limits": "cpp", 41 | "memory": "cpp", 42 | "new": "cpp", 43 | "ostream": "cpp", 44 | "numeric": "cpp", 45 | "sstream": "cpp", 46 | "stdexcept": "cpp", 47 | "streambuf": "cpp", 48 | "thread": "cpp", 49 | "cfenv": "cpp", 50 | "cinttypes": "cpp", 51 | "utility": "cpp", 52 | "typeindex": "cpp", 53 | "typeinfo": "cpp", 54 | "core": "cpp", 55 | "string": "cpp" 56 | } 57 | } -------------------------------------------------------------------------------- /launch/px4_payload_sitl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /sh/sh_for_payload_control/payload_multi_UAV_sitl_2drones.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Open the first tab (default terminal window) 4 | gnome-terminal -- bash -c "roslaunch px4 multi_drone_test.launch; exec bash" 5 | 6 | # Wait for the terminal to open 7 | sleep 1 8 | 9 | # Use xdotool to open additional tabs in the same terminal 10 | xdotool key ctrl+shift+t 11 | xdotool type --delay 1 -- "sleep 5; roslaunch optitrack_broadcast emulator_for_gazebo.launch; exec bash" 12 | xdotool key Return 13 | 14 | xdotool key ctrl+shift+t 15 | xdotool type --delay 1 -- "sleep 2; rosrun qt_ground_station qt_ground_station; exec bash" 16 | xdotool key Return 17 | 18 | xdotool key ctrl+shift+t 19 | xdotool type --delay 1 -- "sleep 3; roslaunch px4_command px4_interdrone_communication_sitl.launch paramfile:=Parameter_for_control_2_drone.yaml; exec bash" 20 | xdotool key Return 21 | 22 | xdotool key ctrl+shift+t 23 | xdotool type --delay 1 -- "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav0; exec bash" 24 | xdotool key Return 25 | 26 | xdotool key ctrl+shift+t 27 | xdotool type --delay 1 -- "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_sitl.launch uavID:=uav0 paramfile:=Parameter_for_control_2_drone.yaml; exec bash" 28 | xdotool key Return 29 | 30 | xdotool key ctrl+shift+t 31 | xdotool type --delay 1 -- "sleep 3; roslaunch px4_command px4_multidrone_pos_estimator_pure_vision.launch uavID:=uav1; exec bash" 32 | xdotool key Return 33 | 34 | xdotool key ctrl+shift+t 35 | xdotool type --delay 1 -- "sleep 3; roslaunch px4_command px4_multidrone_pos_controller_sitl.launch uavID:=uav1 paramfile:=Parameter_for_control_2_drone.yaml; exec bash" 36 | xdotool key Return 37 | 38 | xdotool key ctrl+shift+t 39 | xdotool type --delay 1 -- "sleep 2; rosrun px4_command set_uav0_mode; exec bash" 40 | xdotool key Return 41 | 42 | xdotool key ctrl+shift+t 43 | xdotool type --delay 1 -- "sleep 2; rosrun px4_command set_uav1_mode; exec bash" 44 | xdotool key Return 45 | -------------------------------------------------------------------------------- /sitlmodel/launchfile/uav_single_drone_with_id.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 54 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Flight controller for multiple quadrotors carrying a tethered payload 2 | 3 | [![Generic badge](https://img.shields.io/badge/controller%20node-latest-brightgreen)](https://shields.io/) [![Maintenance](https://img.shields.io/badge/Maintained%3F-yes-green.svg)](https://GitHub.com/Naereen/StrapDown.js/graphs/commit-activity) 4 | 5 | 6 | 7 | 8 | [![image alt text](https://github.com/LonghaoQian/px4_command/blob/master/images/Thumbnail.PNG)](https://www.youtube.com/watch?v=Z9-OlCR-daI&list=PLGJ05aPUKXH-Y6WUyEvXKBSRT5AAzUQf-&index=5) 9 | 10 | The controller runs on the Nvida Jeston Nano on each quadrotor. The command is sent by ground station from the ground station desktop. 11 | 12 | # Installation 13 | 14 | 1. Install the mavros packgae by Binary installation 15 | 16 | Please ref to: https://github.com/mavlink/mavros 17 | 18 | If you has install the mavros package by source code. Plaese read it. 19 | 20 | 2. Create a new ros space called "px4_ws" in your home folder 21 | 22 | `mkdir -p ~/px4_ws/src` 23 | 24 | `cd ~/px4_ws/src` 25 | 26 | `catkin_init_workspace` 27 | 28 | Please source manually, open a new terminal 29 | 30 | `gedit .bashrc` 31 | 32 | Add the path `source /home/$(your computer name)/px4_ws/devel/setup.bash` in the end of the bashrc.txt file 33 | 34 | 3. Git clone the px4_command package 35 | 36 | `cd ~/px4_ws/src` 37 | 38 | download from gitlab repository 39 | 40 | `git clone http://gitlab.amovauto.com/amovlab/px4_command` 41 | 42 | download from github repository 43 | 44 | `git clone https://github.com/potato77/px4_command` 45 | 46 | `cd ..` 47 | 48 | `catkin build` 49 | 50 | # Coordinate frames 51 | 52 | Here we are using **ENU** frames. 53 | 54 | > MAVROS does translate Aerospace NED frames, used in FCUs to ROS ENU frames and vice-versa. For translate airframe related data we simply apply rotation 180° about ROLL (X) axis. For local we apply 180° about ROLL (X) and 90° about YAW (Z) axes 55 | 56 | # Overal node structure 57 | 58 | 59 | 60 | # Quadrotor fleet 61 | 62 | 63 | 64 | 65 | # Run the software-in-the-loop (SITL) simulation 66 | 67 | ## To use the Gazebo SITL simulation 68 | - copy the launch files in ``sitlmodel/launch`` to ``/home//PX4-Autopilot/launch`` 69 | - unzip ``sitlmodel/multi_iris/multi_iris.zip`` and put it to ``/home//PX4-Autopilot/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models`` 70 | 71 | -------------------------------------------------------------------------------- /config/payload_control.yaml: -------------------------------------------------------------------------------- 1 | ## parameter for px4_pos_controller.cpp 2 | ## 起飞高度 3 | Takeoff_height : 0.3 4 | Disarm_height : 0.3 5 | ## 1 for use the accel command 6 | Use_accel : 0.0 7 | ## 1 for printf the state, 0 for block 8 | Flag_printf : 0.0 9 | 10 | ppn_kx : 0.2 11 | ppn_ky : 0.2 12 | ppn_kz : 0.2 13 | 14 | noise_a_xy : 0.0 15 | noise_b_xy : 0.00 16 | 17 | noise_a_z : 0.0 18 | noise_b_z : 0.00 19 | noise_T : 1.0 20 | 21 | noise_start_time : 100.0 22 | noise_end_time : 30.0 23 | 24 | ## parameter for px4_pos_estimator.cpp 25 | pos_estimator: 26 | ## 使用激光SLAM数据orVicon数据 0 for vicon， 1 for 激光SLAM 27 | flag_use_laser_or_vicon : 0 28 | ## 0 for use the data from fcu, 1 for use the mocap raw data(only position), 2 for use the mocap raw data(position and velocity) 29 | Use_mocap_raw : 0 30 | linear_window : 3 31 | angular_window : 3 32 | noise_a : 0.0 33 | noise_b : 0.0 34 | noise_T : 1.0 35 | 36 | 37 | ## 飞机参数 38 | Quad: 39 | mass: 1.6 40 | ## payload mass 41 | Payload: 42 | mass: 0.5 43 | ## cable length 44 | Cable: 45 | length: 0.9 46 | 47 | ##geo fence for fsc 48 | geo_fence: 49 | x_min: -1.2 50 | x_max: 1.2 51 | y_min: -1.0 52 | y_max: 1.0 53 | z_min: -0.05 54 | z_max: 1.8 55 | 56 | ## 限制参数 57 | Limit: 58 | pxy_error_max: 10.0 59 | vxy_error_max: 10.0 60 | pz_error_max: 10.0 61 | vz_error_max: 10.0 62 | pxy_int_max : 10.0 63 | pz_int_max : 10.0 64 | tilt_max: 20.0 65 | int_start_error: 10.0 66 | 67 | XY_VEL_MAX : 10.0 68 | Z_VEL_MAX : 10.0 69 | 70 | # 位置环参数 - cascade pid 71 | Pos_cascade_pid: 72 | Kp_xy : 0.95 73 | Kp_z : 1.0 74 | Kp_vxvy : 0.09 75 | Kp_vz : 0.2 76 | Ki_vxvy : 0.02 77 | Ki_vz : 0.02 78 | Kd_vxvy : 0.01 79 | Kd_vz : 0.0 80 | Hover_throttle : 0.45 81 | MPC_VELD_LP: 5.0 82 | 83 | # 位置环参数 - normal pid 84 | Pos_pid: 85 | Kp_xy : 2.5 86 | Kp_z : 2.5 87 | Ki_xy : 0.5 88 | Ki_z : 0.5 89 | Kd_xy : 3.0 90 | Kd_z : 3.0 91 | 92 | 93 | # 位置环参数 for ude 94 | Pos_ude: 95 | Kp_xy : 0.5 96 | Kp_z : 0.5 97 | Kd_xy : 2.0 98 | Kd_z : 2.0 99 | T_ude_xy : 2.0 100 | T_ude_z : 2.0 101 | 102 | 103 | # 位置环参数 for passivity 104 | Pos_passivity: 105 | Kp_xy : 0.5 106 | Kp_z : 0.5 107 | Kd_xy : 2.0 108 | Kd_z : 2.0 109 | T_ude_xy : 1.0 110 | T_ude_z : 1.0 111 | T_ps: 0.2 112 | 113 | # 位置环参数 for ne 114 | Pos_ne: 115 | Kp_xy : 0.5 116 | Kp_z : 0.5 117 | Kd_xy : 2.0 118 | Kd_z : 2.0 119 | T_ude_xy : 1.0 120 | T_ude_z : 1.0 121 | T_ne : 0.1 122 | 123 | # 圆轨迹参数 for Circle_Trajectory 124 | Circle_Trajectory: 125 | Center_x: 0.0 126 | Center_y: 0.0 127 | Center_z: 1.0 128 | radius: 1.0 129 | linear_vel: 1.0 130 | time_total: 50.0 131 | direction: 1.0 132 | 133 | # 位置环参数 for tie 134 | Pos_tie: 135 | Kp_xy : 0.5 136 | Kp_z : 0.5 137 | Kv_xy : 2.0 138 | Kv_z : 2.0 139 | T_tie_xy : 1.0 140 | T_tie_z : 1.0 141 | Integration: 1.0 142 | Kpv_xy: 1.0 143 | Kpv_z: 1.0 144 | KL : 1.0 145 | -------------------------------------------------------------------------------- /sitlmodel/launchfile/multi_drone_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /include/Frame_tf_utils.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************************************************************** 2 | * Frame_tf_utils.h 3 | * 4 | * Author: Qyp 5 | * 6 | * Update Time: 2019.3.16 7 | * 8 | * Introduction: utils functions for frame transform 9 | * 1. Coby from mavros source code /mavros/mavros/src/lib/ftf_frame_conversions.cpp 10 | ***************************************************************************************************************************/ 11 | #ifndef FRAME_TF_UTILS_H 12 | #define FRAME_TF_UTILS_H 13 | 14 | #include 15 | #include 16 | 17 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Rotation Transform<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 18 | // Transform the attitude representation from frame to frame. 19 | // The proof for this transform can be seen 20 | // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ 21 | 22 | //坐标系转换 transform_orientation_aircraft_to_baselink 23 | Eigen::Quaterniond transform_orientation_aircraft_to_baselink(const Eigen::Quaterniond &q) 24 | { 25 | // Static quaternion needed for rotating between aircraft and base_link frames 26 | // +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft) 27 | // Fto Forward, Left, Up (base_link) frames. 28 | Eigen::Vector3d aaa(M_PI, 0.0, 0.0); 29 | Eigen::Quaterniond AIRCRAFT_BASELINK_Q = quaternion_from_rpy(aaa); 30 | return q * AIRCRAFT_BASELINK_Q; 31 | } 32 | 33 | //坐标系转换 transform_orientation_baselink_to_aircraft 34 | Eigen::Quaterniond transform_orientation_baselink_to_aircraft(const Eigen::Quaterniond &q) 35 | { 36 | // AIRCRAFT_BASELINK_Q is the Static quaternion needed for rotating between aircraft and base_link frames 37 | // +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft) 38 | // Fto Forward, Left, Up (base_link) frames. 39 | Eigen::Vector3d aaa(M_PI, 0.0, 0.0); 40 | Eigen::Quaterniond AIRCRAFT_BASELINK_Q = quaternion_from_rpy(aaa); 41 | return q * AIRCRAFT_BASELINK_Q; 42 | } 43 | 44 | //坐标系转换 transform_orientation_ned_to_enu 45 | Eigen::Quaterniond transform_orientation_ned_to_enu(const Eigen::Quaterniond &q) 46 | { 47 | // NED_ENU_Q is the Static quaternion needed for rotating between ENU and NED frames 48 | // NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East) 49 | // ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North) 50 | Eigen::Vector3d bbb(M_PI, 0.0, M_PI_2); 51 | Eigen::Quaterniond NED_ENU_Q = quaternion_from_rpy(bbb); 52 | 53 | return NED_ENU_Q * q; 54 | } 55 | //坐标系转换 transform_orientation_enu_to_ned 56 | Eigen::Quaterniond transform_orientation_enu_to_ned(const Eigen::Quaterniond &q) 57 | { 58 | // NED_ENU_Q is the Static quaternion needed for rotating between ENU and NED frames 59 | // NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East) 60 | // ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North) 61 | Eigen::Vector3d bbb(M_PI, 0.0, M_PI_2); 62 | Eigen::Quaterniond NED_ENU_Q = quaternion_from_rpy(bbb); 63 | 64 | return NED_ENU_Q * q; 65 | } 66 | 67 | 68 | 69 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Translation Transform<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 70 | /** 71 | * @brief Static vector needed for rotating between ENU and NED frames 72 | * +PI rotation around X (North) axis follwed by +PI/2 rotation about Z (Down) 73 | * gives the ENU frame. Similarly, a +PI rotation about X (East) followed by 74 | * a +PI/2 roation about Z (Up) gives the NED frame. 75 | */ 76 | Eigen::Vector3d transform_enu_to_ned(const Eigen::Vector3d &vec) 77 | { 78 | Eigen::Vector3d bbb(M_PI, 0.0, M_PI_2); 79 | Eigen::Quaterniond NED_ENU_Q = quaternion_from_rpy(bbb); 80 | Eigen::Affine3d NED_ENU_AFFINE(NED_ENU_Q); 81 | 82 | return NED_ENU_AFFINE * vec; 83 | } 84 | 85 | Eigen::Vector3d transform_ned_to_enu(const Eigen::Vector3d &vec) 86 | { 87 | Eigen::Vector3d bbb(M_PI, 0.0, M_PI_2); 88 | Eigen::Quaterniond NED_ENU_Q = quaternion_from_rpy(bbb); 89 | Eigen::Affine3d NED_ENU_AFFINE(NED_ENU_Q); 90 | 91 | return NED_ENU_AFFINE * vec; 92 | } 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /src/Utilities/outdoorwifitest.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************************************************************** 2 | * outdoorwifitest.cpp 3 | * 4 | * Author: Longhao Qian 5 | * 6 | * Update Time: 2020.10.03 7 | * 8 | * Introduction: An outdoor wifi communincation testing tool. It sends out a fake drone state and a fake 9 | * auxiliary state at 50Hz to the ground station to test the wifi quality 10 | * computer 11 | * This node should run on the onboard computer of the drone 12 | ***************************************************************************************************************************/ 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | int main(int argc, char **argv) { 19 | ros::init(argc, argv, "outdoorwifitest"); 20 | ros::NodeHandle nh("~"); 21 | ros::Rate rate(50.0); 22 | float CurrentTime = 0.0; 23 | ros::Time begin_time = ros::Time::now(); 24 | 25 | px4_command::AuxiliaryState FakeAuxstate; 26 | px4_command::FleetStatus FaKeFleetStatus; 27 | 28 | ros::Publisher pubFakeAuxiliaryState = nh.advertise ("/test/px4_command/auxiliarystate", 1000); 29 | ros::Publisher pubFakeFleetStatus = nh.advertise ("/test/px4_command/fleetstatus", 1000); 30 | 31 | int tickfordisplay = 0; 32 | int ticknow = 0; 33 | bool displaytimestamp = false; 34 | int timedispstep = 10; 35 | 36 | while(ros::ok()) { 37 | 38 | FakeAuxstate.header.stamp = ros::Time::now(); 39 | FakeAuxstate.L_measured = 1.0; 40 | 41 | FakeAuxstate.q_0 = 0.3; 42 | FakeAuxstate.q_1 = 0.1; 43 | FakeAuxstate.q_2 = -0.2; 44 | FakeAuxstate.q_3 = 0.8; 45 | 46 | FakeAuxstate.r_jx = 0.2; 47 | FakeAuxstate.r_jy = -0.3; 48 | 49 | FakeAuxstate.v_jx = 0.2; 50 | FakeAuxstate.v_jy = 0.1; 51 | 52 | FakeAuxstate.pos_error_x = 0.1; 53 | FakeAuxstate.pos_error_y = 0.1; 54 | FakeAuxstate.pos_error_z = 0.1; 55 | 56 | FakeAuxstate.angle_error_x = 0.1; 57 | FakeAuxstate.angle_error_y = 0.1; 58 | FakeAuxstate.angle_error_z = 0.1; 59 | 60 | FakeAuxstate.Euler_roll = 3.0; 61 | FakeAuxstate.Euler_pitch = 4.0; 62 | FakeAuxstate.Euler_yaw = 20.0; 63 | 64 | FakeAuxstate.fLj_x = 20.0; 65 | FakeAuxstate.fLj_y = -1.0; 66 | FakeAuxstate.fLj_z = 2.0; 67 | 68 | FakeAuxstate.Delta_jp_x = 1.0; 69 | FakeAuxstate.Delta_jp_y = -4.0; 70 | FakeAuxstate.Delta_jp_z = 2.0; 71 | 72 | FakeAuxstate.acc_x = 0.4; 73 | FakeAuxstate.acc_y = 0.5; 74 | FakeAuxstate.acc_z = 0.3; 75 | 76 | FakeAuxstate.rd_jx = 0.4; 77 | FakeAuxstate.rd_jy = 0.1; 78 | 79 | pubFakeAuxiliaryState.publish(FakeAuxstate); 80 | 81 | FaKeFleetStatus.r_jx = 0.2; 82 | FaKeFleetStatus.r_jy = 0.1; 83 | FaKeFleetStatus.v_jx = 0.2; 84 | FaKeFleetStatus.v_jy = 0.1; 85 | FaKeFleetStatus.f_Ljx = 0.1; 86 | FaKeFleetStatus.f_Ljy = 0.1; 87 | FaKeFleetStatus.f_Ljz = 0.2; 88 | 89 | FaKeFleetStatus.delta_jx = 0.4; 90 | FaKeFleetStatus.delta_jy = 0.3; 91 | FaKeFleetStatus.delta_jz = 0.1; 92 | 93 | FaKeFleetStatus.rd_jx = 0.2; 94 | FaKeFleetStatus.rd_jy = 0.1; 95 | FaKeFleetStatus.emergency = true; 96 | 97 | pubFakeFleetStatus.publish(FaKeFleetStatus); 98 | 99 | CurrentTime = px4_command_utils::get_time_in_sec(begin_time); 100 | 101 | ticknow = int(floor(CurrentTime)); // record the tick 102 | if((ticknow % timedispstep == 0)) { // ticks now is a multiple of 10 103 | if(tickfordisplay == ticknow){ 104 | // if the display tick is equal to the current tick, display time and set the advance the tickfordisplay by 1 105 | ROS_INFO("Wifi testing node is running... Time stamp: %f [s]",CurrentTime); 106 | tickfordisplay += timedispstep; 107 | } 108 | } 109 | rate.sleep(); 110 | ros::spinOnce(); 111 | } 112 | 113 | return 0; 114 | } 115 | -------------------------------------------------------------------------------- /include/serial.hpp: -------------------------------------------------------------------------------- 1 | //串口读取库函数（用于读取超声波数据） 2 | //主要功能：读取串口数据 3 | //输入：需要先设置波特率、端口号等 4 | //输出：串口数据 5 | 6 | #ifndef _SERIAL_H 7 | #define _SERIAL_H 8 | 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | 23 | class Serial{ 24 | public: 25 | Serial(); 26 | ~Serial(); 27 | public: 28 | static int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop); 29 | static int open_portUSB(int comport); 30 | static int open_port_ultrasonic(); 31 | static int nwrite(int serialfd, unsigned char *data, int datalength); 32 | static int nread(int fd, unsigned char *data, int datalength); 33 | 34 | }; 35 | 36 | 37 | Serial::Serial( ) { 38 | } 39 | 40 | Serial::~Serial() { 41 | } 42 | 43 | 44 | int Serial::set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop) 45 | { 46 | struct termios newtio,oldtio; 47 | if ( tcgetattr( fd,&oldtio) != 0) { 48 | perror("SetupSerial 1"); 49 | return -1; 50 | } 51 | bzero(&newtio, sizeof(newtio)); 52 | newtio.c_cflag |= CLOCAL | CREAD; 53 | newtio.c_cflag &= ~CSIZE; 54 | 55 | switch( nBits ) 56 | { 57 | case 7: 58 | newtio.c_cflag |= CS7; 59 | break; 60 | case 8: 61 | newtio.c_cflag |= CS8; 62 | break; 63 | } 64 | 65 | switch( nEvent ) 66 | { 67 | case 'O': 68 | newtio.c_cflag |= PARENB; 69 | newtio.c_cflag |= PARODD; 70 | newtio.c_iflag |= (INPCK | ISTRIP); 71 | break; 72 | case 'E': 73 | newtio.c_iflag |= (INPCK | ISTRIP); 74 | newtio.c_cflag |= PARENB; 75 | newtio.c_cflag &= ~PARODD; 76 | break; 77 | case 'N': 78 | newtio.c_cflag &= ~PARENB; 79 | break; 80 | } 81 | 82 | switch( nSpeed ) 83 | { 84 | case 2400: 85 | cfsetispeed(&newtio, B2400); 86 | cfsetospeed(&newtio, B2400); 87 | break; 88 | case 4800: 89 | cfsetispeed(&newtio, B4800); 90 | cfsetospeed(&newtio, B4800); 91 | break; 92 | case 9600: 93 | cfsetispeed(&newtio, B9600); 94 | cfsetospeed(&newtio, B9600); 95 | break; 96 | case 115200: 97 | cfsetispeed(&newtio, B115200); 98 | cfsetospeed(&newtio, B115200); 99 | break; 100 | default: 101 | cfsetispeed(&newtio, B9600); 102 | cfsetospeed(&newtio, B9600); 103 | break; 104 | } 105 | if( nStop == 1 ) 106 | newtio.c_cflag &= ~CSTOPB; 107 | else if ( nStop == 2 ) 108 | newtio.c_cflag |= CSTOPB; 109 | newtio.c_cc[VTIME] = 0; 110 | newtio.c_cc[VMIN] = 0; 111 | tcflush(fd,TCIFLUSH); 112 | if((tcsetattr(fd,TCSANOW,&newtio))!=0) 113 | { 114 | perror("com set error"); 115 | return -1; 116 | } 117 | return 0; 118 | } 119 | 120 | int Serial::open_portUSB(int comport) //通过参数，打开相应的串口 121 | { 122 | string serial_port_adr("/dev/ttyUSB"+ to_string(comport)); 123 | cout << serial_port_adr << endl; 124 | int fd = open( serial_port_adr.c_str() , O_RDWR|O_NOCTTY); 125 | if (-1 == fd) 126 | { 127 | perror("Can't Open Serial Port!!"); 128 | } 129 | return fd; 130 | } 131 | 132 | 133 | int Serial::open_port_ultrasonic() //通过参数，打开相应的串口 134 | { 135 | string serial_port_adr("/dev/ultrasonic"); 136 | cout << serial_port_adr << endl; 137 | int fd = open( serial_port_adr.c_str() , O_RDWR|O_NOCTTY); 138 | if (-1 == fd) 139 | { 140 | perror("Can't Open Serial Port!!"); 141 | } 142 | return fd; 143 | } 144 | 145 | 146 | int Serial::nwrite (int serialfd,unsigned char *data, int datalength ) 147 | { 148 | return write(serialfd, data, datalength); 149 | } 150 | 151 | int Serial::nread(int fd, unsigned char *data,int datalength) { 152 | int readlen = read(fd,data,datalength); 153 | cout << "readlen: " << readlen< 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | using namespace std; 35 | 36 | class state_from_mavros 37 | { 38 | public: 39 | //constructed function 40 | state_from_mavros(void): 41 | state_nh("~") 42 | { 43 | // 【订阅】无人机当前状态 - 来自飞控 44 | // 本话题来自飞控(通过Mavros功能包 /plugins/sys_status.cpp) 45 | state_sub = state_nh.subscribe("/mavros/state", 10, &state_from_mavros::state_cb,this); 46 | 47 | // 【订阅】无人机当前位置 坐标系:ENU系 （此处注意，所有状态量在飞控中均为NED系，但在ros中mavros将其转换为ENU系处理。所以，在ROS中，所有和mavros交互的量都为ENU系） 48 | // 本话题来自飞控(通过Mavros功能包 /plugins/local_position.cpp读取), 对应Mavlink消息为LOCAL_POSITION_NED (#32), 对应的飞控中的uORB消息为vehicle_local_position.msg 49 | position_sub = state_nh.subscribe("/mavros/local_position/pose", 10, &state_from_mavros::pos_cb,this); 50 | 51 | // 【订阅】无人机当前速度 坐标系:ENU系 52 | // 本话题来自飞控(通过Mavros功能包 /plugins/local_position.cpp读取), 对应Mavlink消息为LOCAL_POSITION_NED (#32), 对应的飞控中的uORB消息为vehicle_local_position.msg 53 | velocity_sub = state_nh.subscribe("/mavros/local_position/velocity_local", 10, &state_from_mavros::vel_cb,this); 54 | 55 | // 【订阅】无人机当前欧拉角 坐标系:ENU系 56 | // 本话题来自飞控(通过Mavros功能包 /plugins/imu.cpp读取), 对应Mavlink消息为ATTITUDE (#30), 对应的飞控中的uORB消息为vehicle_attitude.msg 57 | attitude_sub = state_nh.subscribe("/mavros/imu/data", 10, &state_from_mavros::att_cb,this); 58 | } 59 | 60 | //变量声明 61 | px4_command::DroneState _DroneState; 62 | 63 | private: 64 | 65 | ros::NodeHandle state_nh; 66 | 67 | ros::Subscriber state_sub; 68 | ros::Subscriber position_sub; 69 | ros::Subscriber velocity_sub; 70 | ros::Subscriber attitude_sub; 71 | 72 | void state_cb(const mavros_msgs::State::ConstPtr &msg) 73 | { 74 | _DroneState.connected = msg->connected; 75 | _DroneState.armed = msg->armed; 76 | _DroneState.mode = msg->mode; 77 | } 78 | 79 | void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg) 80 | { 81 | _DroneState.position[0] = msg->pose.position.x; 82 | _DroneState.position[1] = msg->pose.position.y; 83 | _DroneState.position[2] = msg->pose.position.z; 84 | } 85 | 86 | void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg) 87 | { 88 | _DroneState.velocity[0] = msg->twist.linear.x; 89 | _DroneState.velocity[1] = msg->twist.linear.y; 90 | _DroneState.velocity[2] = msg->twist.linear.z; 91 | } 92 | 93 | void att_cb(const sensor_msgs::Imu::ConstPtr& msg) 94 | { 95 | Eigen::Quaterniond q_fcu = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); 96 | //Transform the Quaternion to euler Angles 97 | Eigen::Vector3d euler_fcu = quaternion_to_euler(q_fcu); 98 | 99 | _DroneState.attitude_q.w = q_fcu.w(); 100 | _DroneState.attitude_q.x = q_fcu.x(); 101 | _DroneState.attitude_q.y = q_fcu.y(); 102 | _DroneState.attitude_q.z = q_fcu.z(); 103 | 104 | _DroneState.attitude[0] = euler_fcu[0]; 105 | _DroneState.attitude[1] = euler_fcu[1]; 106 | _DroneState.attitude[2] = euler_fcu[2]; 107 | 108 | _DroneState.attitude_rate[0] = msg->angular_velocity.x; 109 | _DroneState.attitude_rate[1] = msg->angular_velocity.x; 110 | _DroneState.attitude_rate[2] = msg->angular_velocity.x; 111 | } 112 | 113 | 114 | }; 115 | 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /src/Utilities/outdoorwifitest_ground.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************************************************************** 2 | * outdoorwifitest_ground.cpp 3 | * 4 | * Author: Longhao Qian 5 | * 6 | * Update Time: 2020.10.03 7 | * 8 | * Introduction: An outdoor wifi communincation testing tool. It sends out a fake drone state and a fake 9 | * auxiliary state at 50Hz to the ground station to test the wifi quality 10 | * computer 11 | * This node should run on the ground station 12 | ***************************************************************************************************************************/ 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | void SubAuxiliaryState(const px4_command::AuxiliaryState::ConstPtr& msg, bool* IsReceived, px4_command::AuxiliaryState& state){ 24 | *IsReceived = true; 25 | state = *msg; 26 | } 27 | 28 | void SubFleetStatus(const px4_command::FleetStatus::ConstPtr& msg, bool* IsReceived, px4_command::FleetStatus& status){ 29 | *IsReceived = true; 30 | status = *msg; 31 | } 32 | 33 | void DispConnectionQuality(int& counter, std::string& name){ 34 | std::cout << name << " quality is: "; 35 | if(counter<2){ 36 | std::cout<< "Excellent. " << "\n"; 37 | }else if (counter <5){ 38 | std::cout<< "Good. " << "\n"; 39 | }else if (counter <8) { 40 | std::cout<< "Poor. " << "\n"; 41 | }else{ 42 | std::cout<< "Unstable. " << "\n"; 43 | } 44 | } 45 | 46 | int main(int argc, char **argv) { 47 | ros::init(argc, argv, "outdoorwifitest_ground"); 48 | ros::NodeHandle nh("~"); 49 | ros::Rate rate(50.0); 50 | 51 | float CurrentTime = 0.0; 52 | ros::Time begin_time = ros::Time::now(); 53 | 54 | int tickfordisplay = 0; 55 | int ticknow = 0; 56 | bool displaytimestamp = false; 57 | int timedispstep = 1; 58 | bool IsAuxiliaryReceived = false; 59 | bool IsFleetStatusReceived = false; 60 | 61 | px4_command::AuxiliaryState FakeAuxstate; 62 | px4_command::FleetStatus FaKeFleetStatus; 63 | 64 | namespace arg = std::placeholders; 65 | 66 | ros::Subscriber subAuxiliaryState = nh.subscribe("/test/px4_command/auxiliarystate", 67 | 100, 68 | std::bind(&SubAuxiliaryState, arg::_1, 69 | &IsAuxiliaryReceived, FakeAuxstate)); 70 | ros::Subscriber subFleetStatus = nh.subscribe("/test/px4_command/fleetstatus", 71 | 100, 72 | std::bind(&SubFleetStatus,arg::_1, 73 | &IsFleetStatusReceived , FaKeFleetStatus )); 74 | 75 | int AuxiliaryStateLossCounter = 10; 76 | int FleetStatusLossCounter = 10; 77 | std::string AuxName = "auxiliary state"; 78 | std::string FleetName = "fleet status "; 79 | while(ros::ok()) { 80 | 81 | if(IsAuxiliaryReceived){ 82 | AuxiliaryStateLossCounter = 0; //reset the 83 | IsAuxiliaryReceived = false; 84 | }else{ 85 | if (AuxiliaryStateLossCounter<20){ 86 | AuxiliaryStateLossCounter++; 87 | } 88 | } 89 | if(IsFleetStatusReceived){ 90 | FleetStatusLossCounter = 0; //reset the 91 | IsFleetStatusReceived = false; 92 | }else{ 93 | if (FleetStatusLossCounter<20){ 94 | FleetStatusLossCounter++; 95 | } 96 | } 97 | CurrentTime = px4_command_utils::get_time_in_sec(begin_time); 98 | ticknow = int(floor(CurrentTime)); // record the tick 99 | if((ticknow % timedispstep == 0)) { // ticks now is a multiple of 10 100 | if(tickfordisplay == ticknow){ 101 | // if the display tick is equal to the current tick, display time and set the advance the tickfordisplay by 1 102 | std::cout<< "--------------------------- \n"; 103 | std::cout<< " Time: " << CurrentTime << " [s]. \n"; 104 | DispConnectionQuality(AuxiliaryStateLossCounter, AuxName); 105 | DispConnectionQuality(FleetStatusLossCounter, FleetName); 106 | tickfordisplay += timedispstep; 107 | } 108 | } 109 | rate.sleep(); 110 | ros::spinOnce(); 111 | } 112 | 113 | return 0; 114 | 115 | 116 | } -------------------------------------------------------------------------------- /src/Utilities/set_uav0_mode.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************************************************************** 2 | * set_uav0_mode.cpp 3 | * 4 | * Author: Longhao Qian 5 | * 6 | * Update Time: 2019.9.13 7 | * 8 | * Introduction: Change mode for uav0 9 | ***************************************************************************************************************************/ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | mavros_msgs::State current_state; //无人机当前状态[包含上锁状态 模式] (从飞控中读取) 19 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 20 | void state_cb(const mavros_msgs::State::ConstPtr& msg) 21 | { 22 | current_state = *msg; 23 | } 24 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 25 | int main(int argc, char **argv) 26 | { 27 | ros::init(argc, argv, "set_uav0_mode"); 28 | ros::NodeHandle nh("~"); 29 | 30 | // 【订阅】无人机当前状态 - 来自飞控 31 | // 本话题来自飞控(通过/plugins/sys_status.cpp) 32 | ros::Subscriber state_sub = nh.subscribe("/uav0/mavros/state", 10, state_cb); 33 | 34 | // 【服务】修改系统模式 35 | ros::ServiceClient set_mode_client = nh.serviceClient("/uav0/mavros/set_mode"); 36 | 37 | ros::ServiceClient arming_client = nh.serviceClient("/uav0/mavros/cmd/arming"); 38 | 39 | mavros_msgs::SetMode mode_cmd; 40 | 41 | mavros_msgs::CommandBool arm_cmd; 42 | arm_cmd.request.value = true; 43 | 44 | ros::Rate rate(10.0); 45 | 46 | int Num_StateMachine = 0; 47 | int flag_1; 48 | 49 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 循 环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 50 | while(ros::ok()) 51 | { 52 | switch (Num_StateMachine) 53 | { 54 | // input 55 | case 0: 56 | cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>---UAV0 mode---<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl; 57 | cout << "Input the mode: 0 for OFFBOARD,1 for STABILIZED, 2 for POSCTL,3 for ALTCTL, 4 for arm "<> flag_1; 59 | 60 | //1000 降落 也可以指向其他任务 61 | if (flag_1 == 0) 62 | { 63 | Num_StateMachine = 1; 64 | break; 65 | } 66 | else if (flag_1 == 1) 67 | { 68 | Num_StateMachine = 2; 69 | break; 70 | } 71 | else if(flag_1 == 2) 72 | { 73 | Num_StateMachine = 3; 74 | }//惯性系移动 75 | else if(flag_1 == 3) 76 | { 77 | Num_StateMachine = 4; 78 | } 79 | else if(flag_1 == 4) 80 | { 81 | Num_StateMachine = 5; 82 | } 83 | 84 | break; 85 | 86 | //OFFBOARD 87 | case 1: 88 | if(current_state.mode != "OFFBOARD") 89 | { 90 | mode_cmd.request.custom_mode = "OFFBOARD"; 91 | set_mode_client.call(mode_cmd); 92 | cout << "Setting to OFFBOARD Mode..." < 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | mavros_msgs::State current_state; //无人机当前状态[包含上锁状态 模式] (从飞控中读取) 19 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 20 | void state_cb(const mavros_msgs::State::ConstPtr& msg) 21 | { 22 | current_state = *msg; 23 | } 24 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 25 | int main(int argc, char **argv) 26 | { 27 | ros::init(argc, argv, "set_uav1_mode"); 28 | ros::NodeHandle nh("~"); 29 | 30 | // 【订阅】无人机当前状态 - 来自飞控 31 | // 本话题来自飞控(通过/plugins/sys_status.cpp) 32 | ros::Subscriber state_sub = nh.subscribe("/uav1/mavros/state", 10, state_cb); 33 | 34 | // 【服务】修改系统模式 35 | ros::ServiceClient set_mode_client = nh.serviceClient("/uav1/mavros/set_mode"); 36 | 37 | ros::ServiceClient arming_client = nh.serviceClient("/uav1/mavros/cmd/arming"); 38 | 39 | mavros_msgs::SetMode mode_cmd; 40 | 41 | mavros_msgs::CommandBool arm_cmd; 42 | arm_cmd.request.value = true; 43 | 44 | ros::Rate rate(10.0); 45 | 46 | int Num_StateMachine = 0; 47 | int flag_1; 48 | 49 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 循 环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 50 | while(ros::ok()) 51 | { 52 | switch (Num_StateMachine) 53 | { 54 | // input 55 | case 0: 56 | cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>---UAV1 mode---<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl; 57 | cout << "Input the mode: 0 for OFFBOARD,1 for STABILIZED, 2 for POSCTL,3 for ALTCTL, 4 for arm "<> flag_1; 59 | 60 | //1000 降落 也可以指向其他任务 61 | if (flag_1 == 0) 62 | { 63 | Num_StateMachine = 1; 64 | break; 65 | } 66 | else if (flag_1 == 1) 67 | { 68 | Num_StateMachine = 2; 69 | break; 70 | } 71 | else if(flag_1 == 2) 72 | { 73 | Num_StateMachine = 3; 74 | }//惯性系移动 75 | else if(flag_1 == 3) 76 | { 77 | Num_StateMachine = 4; 78 | } 79 | else if(flag_1 == 4) 80 | { 81 | Num_StateMachine = 5; 82 | } 83 | 84 | break; 85 | 86 | //OFFBOARD 87 | case 1: 88 | if(current_state.mode != "OFFBOARD") 89 | { 90 | mode_cmd.request.custom_mode = "OFFBOARD"; 91 | set_mode_client.call(mode_cmd); 92 | cout << "Setting to OFFBOARD Mode..." < 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | mavros_msgs::State current_state; //无人机当前状态[包含上锁状态 模式] (从飞控中读取) 19 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 20 | void state_cb(const mavros_msgs::State::ConstPtr& msg) 21 | { 22 | current_state = *msg; 23 | } 24 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 25 | int main(int argc, char **argv) 26 | { 27 | ros::init(argc, argv, "set_uav2_mode"); 28 | ros::NodeHandle nh("~"); 29 | 30 | // 【订阅】无人机当前状态 - 来自飞控 31 | // 本话题来自飞控(通过/plugins/sys_status.cpp) 32 | ros::Subscriber state_sub = nh.subscribe("/uav2/mavros/state", 10, state_cb); 33 | 34 | // 【服务】修改系统模式 35 | ros::ServiceClient set_mode_client = nh.serviceClient("/uav2/mavros/set_mode"); 36 | 37 | ros::ServiceClient arming_client = nh.serviceClient("/uav2/mavros/cmd/arming"); 38 | 39 | mavros_msgs::SetMode mode_cmd; 40 | 41 | mavros_msgs::CommandBool arm_cmd; 42 | arm_cmd.request.value = true; 43 | 44 | ros::Rate rate(10.0); 45 | 46 | int Num_StateMachine = 0; 47 | int flag_1; 48 | 49 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 循 环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 50 | while(ros::ok()) 51 | { 52 | switch (Num_StateMachine) 53 | { 54 | // input 55 | case 0: 56 | cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>---UAV2 mode---<<<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl; 57 | cout << "Input the mode: 0 for OFFBOARD,1 for STABILIZED, 2 for POSCTL,3 for ALTCTL, 4 for arm "<> flag_1; 59 | 60 | //1000 降落 也可以指向其他任务 61 | if (flag_1 == 0) 62 | { 63 | Num_StateMachine = 1; 64 | break; 65 | } 66 | else if (flag_1 == 1) 67 | { 68 | Num_StateMachine = 2; 69 | break; 70 | } 71 | else if(flag_1 == 2) 72 | { 73 | Num_StateMachine = 3; 74 | }//惯性系移动 75 | else if(flag_1 == 3) 76 | { 77 | Num_StateMachine = 4; 78 | } 79 | else if(flag_1 == 4) 80 | { 81 | Num_StateMachine = 5; 82 | } 83 | 84 | break; 85 | 86 | //OFFBOARD 87 | case 1: 88 | if(current_state.mode != "OFFBOARD") 89 | { 90 | mode_cmd.request.custom_mode = "OFFBOARD"; 91 | set_mode_client.call(mode_cmd); 92 | cout << "Setting to OFFBOARD Mode..." < 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | using namespace std; 32 | 33 | class state_from_mavros_multidrone 34 | { 35 | public: 36 | //constructed function 37 | state_from_mavros_multidrone(const char* ID): // add drone ID as input argument 38 | state_nh("~") 39 | { 40 | 41 | char mavros_state[100];// 42 | char mavros_local_position_pose[100];// 43 | char mavros_local_position_velocity_local[100];// 44 | char mavros_imu_data[100];// 45 | 46 | strcpy (mavros_state,"/uav"); 47 | strcpy (mavros_local_position_pose,"/uav"); 48 | strcpy (mavros_local_position_velocity_local,"/uav"); 49 | strcpy (mavros_imu_data,"/uav"); 50 | 51 | strcat (mavros_state, ID); 52 | strcat (mavros_local_position_pose,ID); 53 | strcat (mavros_local_position_velocity_local,ID); 54 | strcat (mavros_imu_data,ID); 55 | 56 | strcat (mavros_state, "/mavros/state"); 57 | strcat (mavros_local_position_pose,"/mavros/local_position/pose"); 58 | strcat (mavros_local_position_velocity_local,"/mavros/local_position/velocity_local"); 59 | strcat (mavros_imu_data,"/mavros/imu/data"); 60 | 61 | // 【订阅】无人机当前状态 - 来自飞控 62 | // 本话题来自飞控(通过Mavros功能包 /plugins/sys_status.cpp) 63 | state_sub = state_nh.subscribe(mavros_state, 10, &state_from_mavros_multidrone::state_cb,this); 64 | 65 | // 【订阅】无人机当前位置 坐标系:ENU系 （此处注意，所有状态量在飞控中均为NED系，但在ros中mavros将其转换为ENU系处理。所以，在ROS中，所有和mavros交互的量都为ENU系） 66 | // 本话题来自飞控(通过Mavros功能包 /plugins/local_position.cpp读取), 对应Mavlink消息为LOCAL_POSITION_NED (#32), 对应的飞控中的uORB消息为vehicle_local_position.msg 67 | position_sub = state_nh.subscribe(mavros_local_position_pose, 10, &state_from_mavros_multidrone::pos_cb,this); 68 | 69 | // 【订阅】无人机当前速度 坐标系:ENU系 70 | // 本话题来自飞控(通过Mavros功能包 /plugins/local_position.cpp读取), 对应Mavlink消息为LOCAL_POSITION_NED (#32), 对应的飞控中的uORB消息为vehicle_local_position.msg 71 | velocity_sub = state_nh.subscribe(mavros_local_position_velocity_local, 10, &state_from_mavros_multidrone::vel_cb,this); 72 | 73 | // 【订阅】无人机当前欧拉角 坐标系:ENU系 74 | // 本话题来自飞控(通过Mavros功能包 /plugins/imu.cpp读取), 对应Mavlink消息为ATTITUDE (#30), 对应的飞控中的uORB消息为vehicle_attitude.msg 75 | attitude_sub = state_nh.subscribe(mavros_imu_data, 10, &state_from_mavros_multidrone::att_cb,this); 76 | 77 | ROS_INFO("Subscribe mavros State from: %s", mavros_state); 78 | ROS_INFO("Subscribe mavros local position pose from: %s ", mavros_local_position_pose); 79 | ROS_INFO("Subscribe mavros local velocity from: %s", mavros_local_position_velocity_local); 80 | ROS_INFO("Subscribe IMU from: %s", mavros_imu_data); 81 | } 82 | 83 | //变量声明 84 | px4_command::DroneState _DroneState; 85 | 86 | private: 87 | 88 | ros::NodeHandle state_nh; 89 | 90 | ros::Subscriber state_sub; 91 | ros::Subscriber position_sub; 92 | ros::Subscriber velocity_sub; 93 | ros::Subscriber attitude_sub; 94 | 95 | void state_cb(const mavros_msgs::State::ConstPtr &msg) 96 | { 97 | _DroneState.connected = msg->connected; 98 | _DroneState.armed = msg->armed; 99 | _DroneState.mode = msg->mode; 100 | } 101 | 102 | void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg) 103 | { 104 | _DroneState.position[0] = msg->pose.position.x; 105 | _DroneState.position[1] = msg->pose.position.y; 106 | _DroneState.position[2] = msg->pose.position.z; 107 | } 108 | 109 | void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg) 110 | { 111 | _DroneState.velocity[0] = msg->twist.linear.x; 112 | _DroneState.velocity[1] = msg->twist.linear.y; 113 | _DroneState.velocity[2] = msg->twist.linear.z; 114 | } 115 | 116 | void att_cb(const sensor_msgs::Imu::ConstPtr& msg) 117 | { 118 | Eigen::Quaterniond q_fcu = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); 119 | //Transform the Quaternion to euler Angles 120 | Eigen::Vector3d euler_fcu = quaternion_to_euler(q_fcu); 121 | 122 | _DroneState.attitude_q.w = q_fcu.w(); 123 | _DroneState.attitude_q.x = q_fcu.x(); 124 | _DroneState.attitude_q.y = q_fcu.y(); 125 | _DroneState.attitude_q.z = q_fcu.z(); 126 | 127 | _DroneState.attitude[0] = euler_fcu[0]; 128 | _DroneState.attitude[1] = euler_fcu[1]; 129 | _DroneState.attitude[2] = euler_fcu[2]; 130 | 131 | _DroneState.attitude_rate[0] = msg->angular_velocity.x; 132 | _DroneState.attitude_rate[1] = msg->angular_velocity.x; 133 | _DroneState.attitude_rate[2] = msg->angular_velocity.x; 134 | } 135 | 136 | 137 | }; 138 | 139 | 140 | #endif 141 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(px4_command) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | message_generation 12 | roscpp 13 | geometry_msgs 14 | sensor_msgs 15 | mavros 16 | nav_msgs 17 | std_msgs 18 | std_srvs 19 | tf2_ros 20 | tf2_eigen 21 | mavros_msgs 22 | ) 23 | 24 | ## System dependencies are found with CMake's conventions 25 | find_package(Boost REQUIRED COMPONENTS system) 26 | 27 | 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## Generate messages in the 'msg' folder 34 | add_message_files( 35 | DIRECTORY msg 36 | FILES 37 | AuxiliaryState.msg 38 | AuxiliaryState_singleUAV.msg 39 | ControlCommand.msg 40 | TrajectoryPoint.msg 41 | Trajectory.msg 42 | AttitudeReference.msg 43 | DroneState.msg 44 | Topic_for_log.msg 45 | ControlOutput.msg 46 | Mocap.msg 47 | AddonForce.msg 48 | FleetStatus.msg 49 | PayloadPoseCommand.msg 50 | HomePosition.msg 51 | ) 52 | 53 | add_service_files( 54 | FILES 55 | ControlParameter.srv 56 | Emergency.srv 57 | GeneralInfo.srv 58 | SinglePayloadAction.srv 59 | MultiPayloadAction.srv 60 | SetHome.srv 61 | ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | generate_messages( 65 | DEPENDENCIES 66 | geometry_msgs 67 | sensor_msgs 68 | std_msgs 69 | nav_msgs 70 | geographic_msgs 71 | ) 72 | 73 | catkin_package( 74 | INCLUDE_DIRS include 75 | CATKIN_DEPENDS message_runtime 76 | ) 77 | 78 | ########### 79 | ## Build ## 80 | ########### 81 | 82 | ## Specify additional locations of header files 83 | ## Your package locations should be listed before other locations 84 | include_directories( 85 | include 86 | ${catkin_INCLUDE_DIRS} 87 | ) 88 | 89 | 90 | ############################### 91 | ## executable list ## 92 | ############################### 93 | 94 | ##px4_multidrone_pos_controller.cpp 95 | add_executable(px4_multidrone_pos_controller src/px4_multidrone_pos_controller.cpp) 96 | add_dependencies(px4_multidrone_pos_controller px4_command_generate_messages_cpp) 97 | add_dependencies(px4_multidrone_pos_controller px4_command_gencpp) 98 | target_link_libraries(px4_multidrone_pos_controller ${catkin_LIBRARIES}) 99 | 100 | ##px4_multidrone_pos_estimator.cpp 101 | add_executable(px4_multidrone_pos_estimator src/px4_multidrone_pos_estimator.cpp) 102 | add_dependencies(px4_multidrone_pos_estimator px4_command_gencpp) 103 | target_link_libraries(px4_multidrone_pos_estimator ${catkin_LIBRARIES}) 104 | 105 | ##px4_multidrone_pos_estimator_pure_vision.cpp 106 | add_executable(px4_multidrone_pos_estimator_pure_vision src/px4_multidrone_pos_estimator_pure_vision.cpp) 107 | add_dependencies(px4_multidrone_pos_estimator_pure_vision px4_command_gencpp) 108 | target_link_libraries(px4_multidrone_pos_estimator_pure_vision ${catkin_LIBRARIES}) 109 | 110 | ##px4_interdrone_communication.cpp 111 | add_executable(px4_interdrone_communication src/px4_interdrone_communication.cpp) 112 | add_dependencies(px4_interdrone_communication px4_command_gencpp) 113 | target_link_libraries(px4_interdrone_communication ${catkin_LIBRARIES}) 114 | 115 | ##px4_multidrone_pos_estimator_outdoor.cpp 116 | add_executable(px4_multidrone_pos_estimator_outdoor src/px4_multidrone_pos_estimator_outdoor.cpp) 117 | add_dependencies(px4_multidrone_pos_estimator_outdoor px4_command_gencpp) 118 | target_link_libraries(px4_multidrone_pos_estimator_outdoor ${catkin_LIBRARIES}) 119 | 120 | ###### Utilities File ########## 121 | 122 | add_executable(set_uav0_mode src/Utilities/set_uav0_mode.cpp) 123 | add_dependencies(set_uav0_mode px4_command_gencpp) 124 | target_link_libraries(set_uav0_mode ${catkin_LIBRARIES}) 125 | 126 | add_executable(set_uav1_mode src/Utilities/set_uav1_mode.cpp) 127 | add_dependencies(set_uav1_mode px4_command_gencpp) 128 | target_link_libraries(set_uav1_mode ${catkin_LIBRARIES}) 129 | 130 | add_executable(set_uav2_mode src/Utilities/set_uav2_mode.cpp) 131 | add_dependencies(set_uav2_mode px4_command_gencpp) 132 | target_link_libraries(set_uav2_mode ${catkin_LIBRARIES}) 133 | 134 | add_executable(outdoorwifitest src/Utilities/outdoorwifitest.cpp) 135 | add_dependencies(outdoorwifitest px4_command_gencpp) 136 | target_link_libraries(outdoorwifitest ${catkin_LIBRARIES}) 137 | 138 | add_executable(outdoorwifitest_ground src/Utilities/outdoorwifitest_ground.cpp) 139 | add_dependencies(outdoorwifitest_ground px4_command_gencpp) 140 | target_link_libraries(outdoorwifitest_ground ${catkin_LIBRARIES}) 141 | 142 | 143 | 144 | ###### Application File ########## 145 | 146 | 147 | 148 | ############# 149 | ## Install ## 150 | ############# 151 | 152 | # all install targets should use catkin DESTINATION variables 153 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 154 | 155 | ## Mark executable scripts (Python etc.) for installation 156 | ## in contrast to setup.py, you can choose the destination 157 | # install(PROGRAMS 158 | # scripts/my_python_script 159 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 160 | # ) 161 | 162 | ## Mark executables and/or libraries for installation 163 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 164 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 165 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark cpp header files for installation 170 | # install(DIRECTORY include/${PROJECT_NAME}/ 171 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 172 | # FILES_MATCHING PATTERN "*.h" 173 | # PATTERN ".svn" EXCLUDE 174 | # ) 175 | 176 | ## Mark cpp header files for installation 177 | install(DIRECTORY include/${PROJECT_NAME}/ 178 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 179 | FILES_MATCHING PATTERN "*.h" 180 | ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | install(DIRECTORY launch/ 184 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 185 | ) 186 | 187 | ############# 188 | ## Testing ## 189 | ############# 190 | 191 | ## Add gtest based cpp test target and link libraries 192 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_px4_command.cpp) 193 | # if(TARGET ${PROJECT_NAME}-test) 194 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 195 | # endif() 196 | 197 | ## Add folders to be run by python nosetests 198 | # catkin_add_nosetests(test) 199 | -------------------------------------------------------------------------------- /include/circle_trajectory.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************************************************************** 2 | * circle_trajectory.h 3 | * 4 | * Author: Qyp 5 | * 6 | * Update Time: 2019.6.28 7 | * 8 | * Introduction: Circle trajectory generation code 9 | * 1. Generating the circle trajectory 10 | * 2. Parameter: center, radius, linear_vel, time_total, direction 11 | * 3. Input: time_from_start 12 | * 4. Output: position, velocity, acceleration, jerk, snap 13 | ***************************************************************************************************************************/ 14 | #ifndef CIRCLE_TRAJECTORY_H 15 | #define CIRCLE_TRAJECTORY_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | using namespace std; 24 | 25 | class Circle_Trajectory 26 | { 27 | //public表明该数据成员、成员函数是对全部用户开放的。全部用户都能够直接进行调用，在程序的不论什么其他地方訪问。 28 | public: 29 | 30 | //构造函数 31 | Circle_Trajectory(void): 32 | Circle_Trajectory_nh("~") 33 | { 34 | Circle_Trajectory_nh.param("Circle_Trajectory/Center_x", center[0], 0.0); 35 | Circle_Trajectory_nh.param("Circle_Trajectory/Center_y", center[1], 0.0); 36 | Circle_Trajectory_nh.param("Circle_Trajectory/Center_z", center[2], 1.0); 37 | Circle_Trajectory_nh.param("Circle_Trajectory/radius", radius, 1.0); 38 | Circle_Trajectory_nh.param("Circle_Trajectory/linear_vel", linear_vel, 0.5); 39 | Circle_Trajectory_nh.param("Circle_Trajectory/time_total", time_total, 10.0); 40 | Circle_Trajectory_nh.param("Circle_Trajectory/direction", direction, 1.0); 41 | } 42 | 43 | // Parameter 44 | Eigen::Vector3f center; 45 | float radius; 46 | float linear_vel; 47 | float time_total; 48 | float direction; //direction = 1 for CCW 逆时针, direction = -1 for CW 顺时针 49 | 50 | //Printf the Circle_Trajectory parameter 51 | void printf_param(); 52 | 53 | //Printf the Circle_Trajectory result 54 | void printf_result(px4_command::TrajectoryPoint& Circle_trajectory); 55 | 56 | //Circle_Trajectory Calculation [Input: time_from_start; Output: Circle_trajectory;] 57 | void Circle_trajectory_generation(float time_from_start, px4_command::TrajectoryPoint& Circle_trajectory); 58 | 59 | private: 60 | 61 | ros::NodeHandle Circle_Trajectory_nh; 62 | }; 63 | 64 | 65 | void Circle_Trajectory::Circle_trajectory_generation(float time_from_start, px4_command::TrajectoryPoint& Circle_trajectory) 66 | { 67 | float omega; 68 | if( radius != 0) 69 | { 70 | omega = direction * fabs(linear_vel / radius); 71 | }else 72 | { 73 | omega = 0.0; 74 | } 75 | const float angle = time_from_start * omega; 76 | const float cos_angle = cos(angle); 77 | const float sin_angle = sin(angle); 78 | 79 | // cout << "omega : " << omega * 180/M_PI <<" [deg/s] " <>>>>>>>>>>>>>>>>>>>>>>>>>> Circle_Trajectory <<<<<<<<<<<<<<<<<<<<<<<<<" <>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Circle_Trajectory Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" < 14 | #include 15 | #include 16 | 17 | namespace trajectory{ 18 | struct Rectangular_Trajectory_Parameter{ 19 | float a_x; 20 | float a_y; 21 | float h; 22 | float v_x; 23 | float v_y; 24 | float center_x; 25 | float center_y; 26 | float center_z; 27 | }; 28 | 29 | struct Reference_Path{ 30 | Eigen::Vector3f P; 31 | Eigen::Vector3f n; 32 | float vd; 33 | }; 34 | 35 | class Rectangular_Trajectory 36 | { 37 | public: 38 | Rectangular_Trajectory(); 39 | ~ Rectangular_Trajectory(); 40 | void LoadParameter(Rectangular_Trajectory_Parameter& parameter); 41 | void printf_param(); 42 | void printf_result(); 43 | Reference_Path UpdatePosition(Eigen::Vector3f& position); 44 | private: 45 | void DetermineState(); 46 | int state; // indicate which path the quadrotor should follow 47 | // Parameter 48 | Rectangular_Trajectory_Parameter param_; 49 | Eigen::Vector3f position; // drone position 50 | float theta[4];// the angles for all turning points 51 | float theta_now;// the angle cooresponding to the current drone position 52 | int theta_output; // the angle for outputing reference path 53 | Reference_Path path[4]; 54 | int target_theta[4];// the target angle cooresponding to each path. 55 | }; 56 | 57 | Rectangular_Trajectory::Rectangular_Trajectory(){ 58 | int state = 0; 59 | } 60 | 61 | Rectangular_Trajectory::~Rectangular_Trajectory(){ 62 | 63 | } 64 | 65 | void Rectangular_Trajectory::LoadParameter(Rectangular_Trajectory_Parameter& parameter) 66 | { 67 | param_ = parameter; 68 | path[0].P(math_utils::Vector_X) = parameter.a_x/2.0 + parameter.center_x; 69 | path[0].P(math_utils::Vector_Y) = parameter.a_y/2.0 + parameter.center_y; 70 | path[0].P(math_utils::Vector_Z) = parameter.h + parameter.center_z; 71 | 72 | path[0].n(math_utils::Vector_X) = -1.0; 73 | path[0].n(math_utils::Vector_Y) = 0.0; 74 | path[0].n(math_utils::Vector_Z) = 0.0; 75 | path[0].vd = parameter.v_x; 76 | 77 | path[1].P(math_utils::Vector_X) = -parameter.a_x/2.0 + parameter.center_x ; 78 | path[1].P(math_utils::Vector_Y) = parameter.a_y/2.0 + parameter.center_y; 79 | path[1].P(math_utils::Vector_Z) = parameter.h + + parameter.center_z; 80 | 81 | path[1].n(math_utils::Vector_X) = 0.0; 82 | path[1].n(math_utils::Vector_Y) = -1.0; 83 | path[1].n(math_utils::Vector_Z) = 0.0; 84 | 85 | path[1].vd = parameter.v_y; 86 | 87 | path[2].P(math_utils::Vector_X) = -parameter.a_x/2.0 + parameter.center_x; 88 | path[2].P(math_utils::Vector_Y) = -parameter.a_y/2.0 + parameter.center_y; 89 | path[2].P(math_utils::Vector_Z) = parameter.h + parameter.center_z; 90 | 91 | path[2].n(math_utils::Vector_X) = 1.0; 92 | path[2].n(math_utils::Vector_Y) = 0.0; 93 | path[2].n(math_utils::Vector_Z) = 0.0; 94 | 95 | path[2].vd = parameter.v_x; 96 | 97 | path[3].P(math_utils::Vector_X) = parameter.a_x/2.0 + parameter.center_x; 98 | path[3].P(math_utils::Vector_Y) = -parameter.a_y/2.0 + parameter.center_y; 99 | path[3].P(math_utils::Vector_Z) = parameter.h + parameter.center_z; 100 | 101 | path[3].n(math_utils::Vector_X) = 0.0; 102 | path[3].n(math_utils::Vector_Y) = 1.0; 103 | path[3].n(math_utils::Vector_Z) = 0.0; 104 | 105 | path[3].vd = parameter.v_y; 106 | for(int i = 0;i<4;i++){ 107 | theta[i] = atan2(path[i].P(math_utils::Vector_Y)-param_.center_y,path[i].P(math_utils::Vector_X)-param_.center_x); 108 | } 109 | target_theta[0] = 1; 110 | target_theta[1] = 2; 111 | target_theta[2] = 3; 112 | target_theta[3] = 0; 113 | theta_output = 0; 114 | } 115 | 116 | Reference_Path Rectangular_Trajectory::UpdatePosition(Eigen::Vector3f& position){ 117 | theta_now = atan2(position(math_utils::Vector_Y)-param_.center_y,position(math_utils::Vector_X)-param_.center_x);// get the theta at the position 118 | DetermineState();// determine state 119 | // determine whether the drone is close to the turning point 120 | Eigen::Vector3f n_q; 121 | n_q(math_utils::Vector_X) = cos(theta_now); 122 | n_q(math_utils::Vector_Y) = sin(theta_now); 123 | n_q(math_utils::Vector_Z) = 0.0; 124 | 125 | Eigen::Vector3f n_p; 126 | n_p(math_utils::Vector_X) = cos(theta[target_theta[state]]); 127 | n_p(math_utils::Vector_Y) = sin(theta[target_theta[state]]); 128 | n_p(math_utils::Vector_Z) = 0.0; 129 | 130 | if(n_q.cross(n_p)(math_utils::Vector_Z)<0.05) 131 | { 132 | // drone has reached the switching boundary 133 | theta_output = target_theta[state]; 134 | }else{ 135 | // not reaching the switching boundary yet 136 | theta_output = state; 137 | } 138 | // output reference path based on theta to follow 139 | return path[theta_output]; 140 | } 141 | 142 | void Rectangular_Trajectory::DetermineState(){ 143 | if(theta_now>0){ 144 | if(theta_now>theta[1]) 145 | { 146 | state = 1; 147 | }else if(theta_now>theta[0]){ 148 | state = 0; 149 | }else{ 150 | state = 3; 151 | } 152 | }else{ 153 | if(theta_now>>>>>>>>>>>>>> Rectrangular Trajectory State <<<<<<<<<<<<<<" <>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Rectangular Trajectory Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" < 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | using namespace std; 24 | 25 | class pos_controller_PID 26 | { 27 | //public表明该数据成员、成员函数是对全部用户开放的。全部用户都能够直接进行调用，在程序的不论什么其他地方訪问。 28 | public: 29 | 30 | //构造函数 31 | pos_controller_PID(void): 32 | pos_pid_nh("~") 33 | { 34 | pos_pid_nh.param("Quad/mass", Quad_MASS, 1.0); 35 | 36 | pos_pid_nh.param("Pos_pid/Kp_xy", Kp[0], 1.0); 37 | pos_pid_nh.param("Pos_pid/Kp_xy", Kp[1], 1.0); 38 | pos_pid_nh.param("Pos_pid/Kp_z" , Kp[2], 2.0); 39 | pos_pid_nh.param("Pos_pid/Kd_xy", Kd[0], 0.5); 40 | pos_pid_nh.param("Pos_pid/Kd_xy", Kd[1], 0.5); 41 | pos_pid_nh.param("Pos_pid/Kd_z" , Kd[2], 0.5); 42 | pos_pid_nh.param("Pos_pid/Ki_xy", Ki[0], 0.2); 43 | pos_pid_nh.param("Pos_pid/Ki_xy", Ki[1], 0.2); 44 | pos_pid_nh.param("Pos_pid/Ki_z" , Ki[2], 0.2); 45 | 46 | pos_pid_nh.param("Limit/pxy_error_max", pos_error_max[0], 0.6); 47 | pos_pid_nh.param("Limit/pxy_error_max", pos_error_max[1], 0.6); 48 | pos_pid_nh.param("Limit/pz_error_max" , pos_error_max[2], 1.0); 49 | pos_pid_nh.param("Limit/vxy_error_max", vel_error_max[0], 0.3); 50 | pos_pid_nh.param("Limit/vxy_error_max", vel_error_max[1], 0.3); 51 | pos_pid_nh.param("Limit/vz_error_max" , vel_error_max[2], 1.0); 52 | pos_pid_nh.param("Limit/pxy_int_max" , int_max[0], 0.5); 53 | pos_pid_nh.param("Limit/pxy_int_max" , int_max[1], 0.5); 54 | pos_pid_nh.param("Limit/pz_int_max" , int_max[2], 0.5); 55 | pos_pid_nh.param("Limit/tilt_max", tilt_max, 20.0); 56 | pos_pid_nh.param("Limit/int_start_error" , int_start_error, 0.3); 57 | 58 | integral = Eigen::Vector3f(0.0,0.0,0.0); 59 | } 60 | 61 | //Quadrotor Parameter 62 | float Quad_MASS; 63 | 64 | //PID parameter for the control law 65 | Eigen::Vector3f Kp; 66 | Eigen::Vector3f Kd; 67 | Eigen::Vector3f Ki; 68 | 69 | //Limitation 70 | Eigen::Vector3f pos_error_max; 71 | Eigen::Vector3f vel_error_max; 72 | Eigen::Vector3f int_max; 73 | float tilt_max; 74 | float int_start_error; 75 | 76 | //积分项 77 | Eigen::Vector3f integral; 78 | 79 | //输出 80 | px4_command::ControlOutput _ControlOutput; 81 | 82 | 83 | //Printf the PID parameter 84 | void printf_param(); 85 | 86 | void printf_result(); 87 | 88 | // Position control main function 89 | // [Input: Current state, Reference state, sub_mode, dt; Output: AttitudeReference;] 90 | px4_command::ControlOutput pos_controller(const px4_command::DroneState& _DroneState, const px4_command::TrajectoryPoint& _Reference_State, float dt); 91 | 92 | private: 93 | ros::NodeHandle pos_pid_nh; 94 | 95 | }; 96 | 97 | px4_command::ControlOutput pos_controller_PID::pos_controller( 98 | const px4_command::DroneState& _DroneState, 99 | const px4_command::TrajectoryPoint& _Reference_State, float dt) 100 | { 101 | Eigen::Vector3d accel_sp; 102 | 103 | // 计算误差项 104 | Eigen::Vector3f pos_error; 105 | Eigen::Vector3f vel_error; 106 | 107 | px4_command_utils::cal_pos_error(_DroneState, _Reference_State, pos_error); 108 | px4_command_utils::cal_vel_error(_DroneState, _Reference_State, vel_error); 109 | 110 | // 误差项限幅 111 | for (int i=0; i<3; i++) 112 | { 113 | pos_error[i] = constrain_function(pos_error[i], pos_error_max[i]); 114 | vel_error[i] = constrain_function(vel_error[i], vel_error_max[i]); 115 | } 116 | 117 | // 期望加速度 = 加速度前馈 + PID 118 | for (int i=0; i<3; i++) 119 | { 120 | accel_sp[i] = _Reference_State.acceleration_ref[i] + Kp[i] * pos_error[i] + Kd[i] * vel_error[i] + Ki[i] * integral[i]; 121 | } 122 | 123 | accel_sp[2] = accel_sp[2] + 9.8; 124 | 125 | // 更新积分项 126 | for (int i=0; i<3; i++) 127 | { 128 | if(abs(pos_error[i]) < int_start_error) 129 | { 130 | integral[i] += pos_error[i] * dt; 131 | 132 | if(abs(integral[i]) > int_max[i]) 133 | { 134 | cout << "Integral saturation! " << " [0-1-2] "<< i <>>>>>>>>>>>>>>>>>>>>> PID Position Controller <<<<<<<<<<<<<<<<<<<<<<" <>>>>>>>>>>>>>>>>>>>>>>>>>>>>>PID Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" < 16 | #include 17 | 18 | using namespace std; 19 | 20 | namespace math_utils{ 21 | 22 | enum vector_component{ 23 | Vector_X = 0, 24 | Vector_Y = 1, 25 | Vector_Z = 2 26 | }; 27 | enum quaternion_component{ 28 | Quat_w = 0, 29 | Quat_x = 1, 30 | Quat_y = 2, 31 | Quat_z = 3 32 | }; 33 | 34 | enum euler_component{ 35 | EULER_ROLL = 0, 36 | EULER_PITCH, 37 | EULER_YAW 38 | }; 39 | 40 | Eigen::Vector3f GetGravitationalAcc(){ 41 | Eigen::Vector3f g_I; 42 | g_I(Vector_X) = 0.0; 43 | g_I(Vector_Y) = 0.0; 44 | g_I(Vector_Z) = -9.810; 45 | return g_I; 46 | }; 47 | } 48 | 49 | // 四元数转欧拉角 50 | Eigen::Vector3d quaternion_to_rpy2(const Eigen::Quaterniond &q) 51 | { 52 | // YPR - ZYX 53 | return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); 54 | } 55 | 56 | // 从(roll,pitch,yaw)创建四元数 by a 3-2-1 intrinsic Tait-Bryan rotation sequence 57 | Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy) 58 | { 59 | // YPR - ZYX 60 | return Eigen::Quaterniond( 61 | Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * 62 | Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * 63 | Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()) 64 | ); 65 | } 66 | 67 | // 将四元数转换至(roll,pitch,yaw) by a 3-2-1 intrinsic Tait-Bryan rotation sequence 68 | // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles 69 | // q0 q1 q2 q3 70 | // w x y z 71 | Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond &q) 72 | { 73 | float quat[4]; 74 | quat[0] = q.w(); 75 | quat[1] = q.x(); 76 | quat[2] = q.y(); 77 | quat[3] = q.z(); 78 | 79 | Eigen::Vector3d ans; 80 | ans[0] = atan2(2.0 * (quat[3] * quat[2] + quat[0] * quat[1]), 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])); 81 | ans[1] = asin(2.0 * (quat[2] * quat[0] - quat[3] * quat[1])); 82 | ans[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), 1.0 - 2.0 * (quat[2] * quat[2] + quat[3] * quat[3])); 83 | return ans; 84 | } 85 | 86 | Eigen::Vector3d quaternion_to_euler2(const Eigen::Vector4f& quat) 87 | { 88 | Eigen::Vector3d ans; 89 | ans[0] = atan2(2.0 * (quat[3] * quat[2] + quat[0] * quat[1]), 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])); 90 | ans[1] = asin(2.0 * (quat[2] * quat[0] - quat[3] * quat[1])); 91 | ans[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), 1.0 - 2.0 * (quat[2] * quat[2] + quat[3] * quat[3])); 92 | return ans; 93 | } 94 | 95 | //rotation matrix to euler anlge 96 | void rotation_to_euler(const Eigen::Matrix3d& dcm, Eigen::Vector3d& euler_angle) 97 | { 98 | double phi_val = atan2(dcm(2, 1), dcm(2, 2)); 99 | double theta_val = asin(-dcm(2, 0)); 100 | double psi_val = atan2(dcm(1, 0), dcm(0, 0)); 101 | double pi = M_PI; 102 | 103 | if (fabs(theta_val - pi / 2.0) < 1.0e-3) { 104 | phi_val = 0.0; 105 | psi_val = atan2(dcm(1, 2), dcm(0, 2)); 106 | 107 | } else if (fabs(theta_val + pi / 2.0) < 1.0e-3) { 108 | phi_val = 0.0; 109 | psi_val = atan2(-dcm(1, 2), -dcm(0, 2)); 110 | } 111 | 112 | euler_angle(0) = phi_val; 113 | euler_angle(1) = theta_val; 114 | euler_angle(2) = psi_val; 115 | } 116 | 117 | //constrain_function 118 | float constrain_function(float data, float Max) 119 | { 120 | if(abs(data)>Max) 121 | { 122 | return (data > 0) ? Max : -Max; 123 | } 124 | else 125 | { 126 | return data; 127 | } 128 | } 129 | 130 | 131 | //constrain_function2 132 | float constrain_function2(float data, float Min,float Max) 133 | { 134 | if(data > Max) 135 | { 136 | return Max; 137 | } 138 | else if (data < Min) 139 | { 140 | return Min; 141 | }else 142 | { 143 | return data; 144 | } 145 | } 146 | 147 | // vector based constraint 148 | 149 | Eigen::VectorXf constrain_vector(const Eigen::VectorXf& input, float Max_norm) 150 | { 151 | Eigen::VectorXf result; 152 | 153 | float norm = input.norm(); 154 | 155 | if (norm > Max_norm) 156 | { 157 | result = input / norm * Max_norm; 158 | return result; 159 | } else { 160 | return input; 161 | } 162 | } 163 | 164 | //sign_function 165 | float sign_function(float data) 166 | { 167 | if(data>0) 168 | { 169 | return 1.0; 170 | } 171 | else if(data<0) 172 | { 173 | return -1.0; 174 | } 175 | else if(data == 0) 176 | { 177 | return 0.0; 178 | } 179 | } 180 | 181 | // min function 182 | float min(float data1,float data2) { 183 | if(data1>=data2) 184 | { 185 | return data2; 186 | } 187 | else 188 | { 189 | return data1; 190 | } 191 | } 192 | 193 | Eigen::Vector3f Veemap(const Eigen::Matrix3f& cross_matrix) { 194 | Eigen::Vector3f vector; 195 | vector(0) = -cross_matrix(1,2); 196 | vector(1) = cross_matrix(0,2); 197 | vector(2) = -cross_matrix(0,1); 198 | return vector; 199 | } 200 | Eigen::Matrix3f Hatmap(const Eigen::Vector3f& vector) { 201 | /* 202 | 203 | r^x = [0 -r3 r2; 204 | r3 0 -r1; 205 | -r2 r1 0] 206 | */ 207 | Eigen::Matrix3f cross_matrix; 208 | cross_matrix(0,0) = 0.0; 209 | cross_matrix(0,1) = - vector(2); 210 | cross_matrix(0,2) = vector(1); 211 | 212 | cross_matrix(1,0) = vector(2); 213 | cross_matrix(1,1) = 0.0; 214 | cross_matrix(1,2) = - vector(0); 215 | 216 | cross_matrix(2,0) = - vector(1); 217 | cross_matrix(2,1) = vector(0); 218 | cross_matrix(2,2) = 0.0; 219 | return cross_matrix; 220 | } 221 | Eigen::Matrix3f QuaterionToRotationMatrix(const Eigen::Vector4f& quaternion) { 222 | Eigen::Matrix3f R_IB; 223 | 224 | /* take a special note at the order of the quaterion 225 | pose[1].q0 = OptiTrackdata.pose.orientation.w; 226 | pose[1].q1 = OptiTrackdata.pose.orientation.x; 227 | pose[1].q2 = OptiTrackdata.pose.orientation.y; 228 | pose[1].q3 = OptiTrackdata.pose.orientation.z; 229 | 230 | //update the auxiliary matrix 231 | /* 232 | L = [-q1 q0 q3 -q2; 233 | -q2 -q3 q0 q1; 234 | -q3 q2 -q1 q0] 235 | R = [-q1 q0 -q3 q2; 236 | -q2 q3 q0 -q1; 237 | -q3 -q2 q1 q0] 238 | R_IB = RL^T 239 | */ 240 | Eigen::Matrix L,R; 241 | L(0,0) = - quaternion(1); 242 | L(1,0) = - quaternion(2); 243 | L(2,0) = - quaternion(3); 244 | 245 | L(0,1) = quaternion(0); 246 | L(1,2) = quaternion(0); 247 | L(2,3) = quaternion(0); 248 | 249 | L(0,2) = quaternion(3); 250 | L(0,3) = - quaternion(2); 251 | L(1,1) = - quaternion(3); 252 | L(1,3) = quaternion(1); 253 | L(2,1) = quaternion(2); 254 | L(2,2) = - quaternion(1); 255 | 256 | R(0,0) = - quaternion(1); 257 | R(1,0) = - quaternion(2); 258 | R(2,0) = - quaternion(3); 259 | 260 | R(0,1) = quaternion(0); 261 | R(1,2) = quaternion(0); 262 | R(2,3) = quaternion(0); 263 | 264 | R(0,2) = - quaternion(3); 265 | R(0,3) = quaternion(2); 266 | R(1,1) = quaternion(3); 267 | R(1,3) = -quaternion(1); 268 | R(2,1) = -quaternion(2); 269 | R(2,2) = quaternion(1); 270 | 271 | R_IB = R * L.transpose(); 272 | return R_IB; 273 | } 274 | 275 | #endif 276 | -------------------------------------------------------------------------------- /include/quadrotor_drone.h: -------------------------------------------------------------------------------- 1 | #ifndef QUADROTOR_DRONE_COMMAND_H 2 | #define QUADROTOR_DRONE_COMMAND_H 3 | /***************************************************** 4 | 5 | quadrotor_drone.h 6 | 7 | Author: Longhao Qian 8 | Date : 2020 08 01 9 | 10 | A class containing all necessary parameters, functions, for a quadrotor drone 11 | 12 | ++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | namespace experiment_drone { 20 | struct liftcurve{ 21 | double motor_slope; 22 | double motor_intercept; 23 | }; 24 | 25 | struct IMUdata{ 26 | Eigen::Vector4f Quad_Drone; // quadrotor attitude 27 | Eigen::Vector3f AccBody; // acc in body fixed frame 28 | Eigen::Vector3f AccInertial; // acc in inertial frame 29 | Eigen::Matrix3f R_Ij; // drone attitude 30 | }; 31 | 32 | struct drone_command{ 33 | Eigen::Vector3f accelSetpoint; // acceleration setpoint based on translational control 34 | Eigen::Vector3f thrustSetpoint; // thrust setpoint under tilt angle constraint 35 | Eigen::Vector3f throttleSetpoint; // calculate the required throttle command based on the lift model 36 | }; 37 | 38 | struct quadrotor_parameter{ 39 | liftcurve liftmodel; 40 | float Quad_MASS; 41 | float tiltlimit;// maximum allowed tilt angle for drones (DEG) 42 | std::string uav_name;// uav name tag 43 | }; 44 | 45 | class quadrotor_drone 46 | { 47 | public: 48 | quadrotor_drone(); 49 | ~quadrotor_drone(); 50 | std::string getUAVname(); 51 | IMUdata getProcessedIMU(); 52 | drone_command getDroneCommand(); 53 | void loadparameter(const quadrotor_parameter& param); 54 | void updatestate(const px4_command::DroneState& _DroneState); 55 | px4_command::ControlOutput outputdronecommand(const Eigen::Vector3f& accel_command, 56 | const float& effective_mass, 57 | const Eigen::Vector3f& u_l, 58 | const Eigen::Vector3f& u_d); 59 | px4_command::ControlOutput outputdronecommand(const Eigen::Vector3f& desiredlift); 60 | void printf_param(); 61 | void printf_state(); 62 | private: 63 | quadrotor_parameter parameter; 64 | drone_command command; 65 | px4_command::ControlOutput output; 66 | IMUdata IMU; 67 | Eigen::Vector3f g_I; // gravity acc in inertial frame 68 | }; 69 | 70 | quadrotor_drone::quadrotor_drone(){ 71 | g_I = math_utils::GetGravitationalAcc(); 72 | } 73 | 74 | quadrotor_drone::~quadrotor_drone(){ 75 | 76 | } 77 | 78 | void quadrotor_drone::loadparameter(const quadrotor_parameter& param){ 79 | parameter = param; 80 | } 81 | 82 | void quadrotor_drone::updatestate(const px4_command::DroneState& _DroneState){ 83 | IMU.Quad_Drone(math_utils::Quat_w) = _DroneState.attitude_q.w; 84 | IMU.Quad_Drone(math_utils::Quat_x) = _DroneState.attitude_q.x; 85 | IMU.Quad_Drone(math_utils::Quat_y) = _DroneState.attitude_q.y; 86 | IMU.Quad_Drone(math_utils::Quat_z) = _DroneState.attitude_q.z; 87 | IMU.R_Ij = QuaterionToRotationMatrix(IMU.Quad_Drone); 88 | // get the acceleration reading from IMU 89 | for( int i = 0; i < 3 ; i ++) { 90 | IMU.AccBody(i) = _DroneState.acceleration[i]; 91 | } 92 | IMU.AccInertial = IMU.R_Ij * IMU.AccBody + g_I;// calculate true acc in inertial frame dot_vqj in TCST paper 93 | } 94 | 95 | px4_command::ControlOutput quadrotor_drone::outputdronecommand(const Eigen::Vector3f& accelCommand, 96 | const float& effective_mass, 97 | const Eigen::Vector3f& u_l, 98 | const Eigen::Vector3f& u_d){ 99 | command.accelSetpoint = accelCommand; 100 | command.thrustSetpoint = px4_command_utils::accelToThrust(command.accelSetpoint, 101 | effective_mass, 102 | parameter.tiltlimit); 103 | // calculate the required throttle command 104 | command.throttleSetpoint = px4_command_utils::thrustToThrottleLinear(command.thrustSetpoint, 105 | parameter.liftmodel.motor_slope, 106 | parameter.liftmodel.motor_intercept); 107 | 108 | for (int i=0; i<3; i++) { 109 | output.u_l[i] = u_l[i]; 110 | output.u_d[i] = u_d[i]; 111 | output.Thrust[i] = command.thrustSetpoint(i); 112 | output.Throttle[i] = command.throttleSetpoint(i); 113 | } 114 | 115 | return output; 116 | } 117 | px4_command::ControlOutput quadrotor_drone::outputdronecommand(const Eigen::Vector3f& desiredlift) { 118 | 119 | command.thrustSetpoint = px4_command_utils::ForceToThrust(desiredlift,parameter.tiltlimit); 120 | // calculate the required throttle command 121 | command.throttleSetpoint = px4_command_utils::thrustToThrottleLinear(command.thrustSetpoint, 122 | parameter.liftmodel.motor_slope, 123 | parameter.liftmodel.motor_intercept); 124 | for (int i=0; i<3; i++) { 125 | output.u_l[i] = 0.0; 126 | output.u_d[i] = 0.0; 127 | output.Thrust[i] = command.thrustSetpoint(i); 128 | output.Throttle[i] = command.throttleSetpoint(i); 129 | } 130 | return output; 131 | } 132 | std::string quadrotor_drone::getUAVname(){ 133 | return parameter.uav_name; 134 | } 135 | IMUdata quadrotor_drone::getProcessedIMU(){ 136 | return IMU; 137 | } 138 | drone_command quadrotor_drone::getDroneCommand(){ 139 | return command; 140 | } 141 | void quadrotor_drone::printf_param(){ 142 | cout << parameter.uav_name << " parameter: \n"; 143 | cout <<"Quad_MASS : "<< parameter.Quad_MASS << " [kg] \n"; 144 | cout << "Motor Curve Slop: " << parameter.liftmodel.motor_slope << " Motor Curve Intercept: "< 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | using namespace std; 41 | struct SubTopic 42 | { 43 | char str[100]; 44 | }; 45 | struct PubTopic 46 | { 47 | char str[100]; 48 | }; 49 | px4_command::DroneState _DroneState; 50 | px4_command::Mocap UAV_motion; 51 | px4_command::Mocap Payload_motion; 52 | bool UAVsubFlag; 53 | bool PaylaodsubFlag; 54 | bool MocapOK; 55 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Callbacks <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 56 | void GetUAVBattery(const sensor_msgs::BatteryState::ConstPtr &msg) { 57 | _DroneState.battery_voltage = msg->voltage; 58 | _DroneState.battery_remaining = msg->percentage; 59 | } 60 | void GetUAVState(const px4_command::Mocap::ConstPtr& msg) { 61 | UAV_motion = *msg; 62 | for ( int i = 0; i < 3; i ++) { 63 | _DroneState.position[i] = UAV_motion.position[i]; 64 | _DroneState.velocity[i] = UAV_motion.velocity[i]; 65 | } 66 | UAVsubFlag = true; 67 | } 68 | void GetPayloadState(const px4_command::Mocap::ConstPtr& msg) { 69 | Payload_motion = *msg; 70 | for ( int i = 0; i < 3; i ++) { 71 | _DroneState.payload_vel[i] = Payload_motion.velocity[i]; 72 | _DroneState.payload_pos[i] = Payload_motion.position[i]; 73 | _DroneState.payload_angular_vel[i] = Payload_motion.angular_velocity[i]; 74 | } 75 | 76 | for (int i = 0; i < 4; i++) { 77 | _DroneState.payload_quaternion[i] = Payload_motion.quaternion[i]; 78 | } 79 | 80 | PaylaodsubFlag = true; 81 | } 82 | void GetMavrosState(const mavros_msgs::State::ConstPtr &msg) { 83 | _DroneState.connected = msg->connected; 84 | _DroneState.armed = msg->armed; 85 | _DroneState.mode = msg->mode; 86 | } 87 | void GetAttitude(const sensor_msgs::Imu::ConstPtr& msg) { 88 | Eigen::Quaterniond q_fcu = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); 89 | //Transform the Quaternion to euler Angles 90 | Eigen::Vector3d euler_fcu = quaternion_to_euler(q_fcu); 91 | _DroneState.attitude_q.w = q_fcu.w(); 92 | _DroneState.attitude_q.x = q_fcu.x(); 93 | _DroneState.attitude_q.y = q_fcu.y(); 94 | _DroneState.attitude_q.z = q_fcu.z(); 95 | _DroneState.attitude[0] = euler_fcu[0]; 96 | _DroneState.attitude[1] = euler_fcu[1]; 97 | _DroneState.attitude[2] = euler_fcu[2]; 98 | _DroneState.attitude_rate[0] = msg->angular_velocity.x; 99 | _DroneState.attitude_rate[1] = msg->angular_velocity.y; 100 | _DroneState.attitude_rate[2] = msg->angular_velocity.z; 101 | _DroneState.acceleration[0] = msg->linear_acceleration.x; 102 | _DroneState.acceleration[1] = msg->linear_acceleration.y; 103 | _DroneState.acceleration[2] = msg->linear_acceleration.z; 104 | } 105 | int main(int argc, 106 | char **argv) { 107 | ros::init(argc, argv, "px4_multidrone_pos_estimator_pure_vision"); 108 | ros::NodeHandle nh("~"); 109 | ros::Rate rate(50.0); 110 | 111 | /*----------- determine the ID of the drone -----------------------------*/ 112 | SubTopic mocap_UAV; 113 | SubTopic mavros_state; 114 | SubTopic mavros_imu_data; 115 | SubTopic mavros_battery; 116 | PubTopic px4_command_drone_state; 117 | PubTopic mavros_vision_pose_pose; 118 | // add preflex: (mavros and px4_command use lower case uav, while mocap use upper case UAV) 119 | strcpy (mocap_UAV.str,"/mocap/UAV"); 120 | strcpy (mavros_state.str,"/uav"); 121 | strcpy (mavros_imu_data.str,"/uav"); 122 | strcpy (px4_command_drone_state.str,"/uav"); 123 | strcpy (mavros_vision_pose_pose.str,"/uav"); 124 | strcpy (mavros_battery.str,"/uav"); 125 | if ( argc > 1) { 126 | // if ID is specified as the second argument 127 | strcat (mocap_UAV.str,argv[1]); 128 | strcat (mavros_state.str,argv[1]); 129 | strcat (mavros_imu_data.str,argv[1]); 130 | strcat (mavros_battery.str,argv[1]); 131 | strcat (px4_command_drone_state.str,argv[1]); 132 | strcat (mavros_vision_pose_pose.str,argv[1]); 133 | ROS_INFO("UAV ID specified as: UAV%s", argv[1]); 134 | } else { 135 | // if ID is not specified, then set the drone to UAV0 136 | strcat (mocap_UAV.str,"0"); 137 | strcat (mavros_state.str,"0"); 138 | strcat (mavros_imu_data.str,"0"); 139 | strcat (px4_command_drone_state.str,"0"); 140 | strcat (mavros_vision_pose_pose.str,"0"); 141 | strcat (mavros_battery.str,"0"); 142 | ROS_WARN("NO UAV ID specified, set ID to 0."); 143 | } 144 | 145 | strcat (mavros_state.str,"/mavros/state"); 146 | strcat (mavros_imu_data.str,"/mavros/imu/data"); 147 | strcat (px4_command_drone_state.str,"/px4_command/drone_state"); 148 | strcat (mavros_vision_pose_pose.str,"/mavros/vision_pose/pose"); 149 | strcat (mavros_battery.str,"/mavros/battery"); 150 | 151 | ROS_INFO("Subscribe uav Mocap from: %s", mocap_UAV.str); 152 | ROS_INFO("Subscribe payload from: /mocap/Payload"); 153 | ROS_INFO("Subscribe mavros_msgs::State from: %s", mavros_state.str); 154 | ROS_INFO("Subscribe IMU from: %s", mavros_imu_data.str); 155 | ROS_INFO("Subscribe battery info from: %s", mavros_battery.str); 156 | ROS_INFO("Publish DroneState to: %s", px4_command_drone_state.str); 157 | ROS_INFO("Publish PoseStamped to: %s", mavros_vision_pose_pose.str); 158 | 159 | // subscriber 160 | ros::Subscriber UAV_motion_sub = nh.subscribe(mocap_UAV.str, 1000, GetUAVState); 161 | ros::Subscriber MavrosBattery_sub = nh.subscribe (mavros_battery.str,1000,GetUAVBattery); 162 | ros::Subscriber Payload_motion_sub = nh.subscribe("/mocap/Payload", 1000, GetPayloadState); 163 | ros::Subscriber MavrosState_sub = nh.subscribe(mavros_state.str, 100, GetMavrosState); 164 | ros::Subscriber Attitude_sub = nh.subscribe(mavros_imu_data.str, 100, GetAttitude); 165 | // publisher 166 | ros::Publisher drone_state_pub = nh.advertise(px4_command_drone_state.str, 100); 167 | /*【发布】无人机位置和偏航角 坐标系 ENU系 168 | 本话题要发送飞控(通过mavros_extras/src/plugins/vision_pose_estimate.cpp发送), 169 | 对应Mavlink消息为VISION_POSITION_ESTIMATE(#??), 对应的飞控中的uORB消息为vehicle_vision_position.msg 170 | 及 vehicle_vision_attitude.msg */ 171 | ros::Publisher vision_pub = nh.advertise(mavros_vision_pose_pose.str, 100); 172 | geometry_msgs::PoseStamped vision;// vision data 173 | 174 | const int MocapTolerance = 2; 175 | int NoFeedBackCounter = 0; 176 | ROS_INFO("Start the estimator..."); 177 | while(ros::ok()) 178 | { 179 | //回调一次 更新传感器状态 180 | ros::spinOnce(); 181 | vision.pose.position.x = UAV_motion.position[0]; 182 | vision.pose.position.y = UAV_motion.position[1]; 183 | vision.pose.position.z = UAV_motion.position[2]; 184 | vision.pose.orientation.w = UAV_motion.quaternion[0]; 185 | vision.pose.orientation.x = UAV_motion.quaternion[1]; 186 | vision.pose.orientation.y = UAV_motion.quaternion[2]; 187 | vision.pose.orientation.z = UAV_motion.quaternion[3]; 188 | vision.header.stamp = ros::Time::now(); 189 | vision_pub.publish(vision);// send vision measurment to fcu 190 | 191 | _DroneState.header.stamp = ros::Time::now(); 192 | MocapOK = true; // reset the mocap flag to false 193 | if ( !UAVsubFlag ) { 194 | MocapOK = false; 195 | } 196 | // reset the sub flag to false 197 | UAVsubFlag = false; 198 | // determine whether the mocap feedback is normal 199 | _DroneState.mocapOK = true; 200 | if (MocapOK) { 201 | NoFeedBackCounter = 0; 202 | _DroneState.mocapOK = true; 203 | } else { 204 | NoFeedBackCounter++; 205 | if ( NoFeedBackCounter >= MocapTolerance ) { 206 | _DroneState.mocapOK = false; 207 | } else { 208 | _DroneState.mocapOK = true; 209 | } 210 | } 211 | 212 | drone_state_pub.publish(_DroneState); 213 | rate.sleep(); 214 | } 215 | return 0; 216 | } -------------------------------------------------------------------------------- /config/Parameter_3drone_payload_gazebo.yaml: -------------------------------------------------------------------------------- 1 | ## parameter for 3 drone gazebo simulation 2 | Takeoff_height : 0.3 3 | Disarm_height : 0.3 4 | ## 1 for use the accel command 5 | Use_accel : 0.0 6 | ## 1 for printf the state, 0 for block 7 | PrintState : true 8 | 9 | ppn_kx : 0.2 10 | ppn_ky : 0.2 11 | ppn_kz : 0.2 12 | 13 | noise_a_xy : 0.0 14 | noise_b_xy : 0.00 15 | 16 | noise_a_z : 0.0 17 | noise_b_z : 0.00 18 | noise_T : 1.0 19 | 20 | noise_start_time : 100.0 21 | noise_end_time : 30.0 22 | 23 | ## parameter for px4_pos_estimator.cpp 24 | pos_estimator: 25 | ## 使用激光SLAM数据orVicon数据 0 for vicon， 1 for 激光SLAM 26 | flag_use_laser_or_vicon : 0 27 | ## 0 for use the data from fcu, 1 for use the mocap raw data(only position), 2 for use the mocap raw data(position and velocity) 28 | Use_mocap_raw : 0 29 | linear_window : 3 30 | angular_window : 3 31 | noise_a : 0.0 32 | noise_b : 0.0 33 | noise_T : 1.0 34 | 35 | ## 飞机参数 36 | Quad: 37 | mass: 1.5 38 | ## payload mass 39 | Payload: 40 | mass: 0.95 41 | ## cable length 42 | Cable: 43 | length: 0.91 44 | 45 | ## single or multi-UAV mode 46 | CooperativeMode: 47 | isMulti: true 48 | droneID: 2 # single UAV payload mode is activated in drone 2 49 | 50 | ActionMode: 51 | type: 1 # 0 for circle path, 1 for rectangular path 52 | 53 | ##geo fence for fsc 54 | geo_fence: 55 | x_min: -1.6 56 | x_max: 1.5 57 | y_min: -1.3 58 | y_max: 1.3 59 | z_min: -0.05 60 | z_max: 2.5 61 | 62 | ## 限制参数 63 | Limit: 64 | pxy_error_max: 10.0 65 | vxy_error_max: 10.0 66 | pz_error_max: 10.0 67 | vz_error_max: 10.0 68 | pxy_int_max : 10.0 69 | pz_int_max : 10.0 70 | tilt_max: 35.0 71 | int_start_error: 10.0 72 | 73 | XY_VEL_MAX : 10.0 74 | Z_VEL_MAX : 10.0 75 | 76 | # 位置环参数 - cascade pid 77 | Pos_cascade_pid: 78 | Kp_xy : 0.95 79 | Kp_z : 1.0 80 | Kp_vxvy : 0.09 81 | Kp_vz : 0.2 82 | Ki_vxvy : 0.02 83 | Ki_vz : 0.02 84 | Kd_vxvy : 0.01 85 | Kd_vz : 0.0 86 | Hover_throttle : 0.45 87 | MPC_VELD_LP: 5.0 88 | 89 | # 位置环参数 - normal pid 90 | Pos_pid: 91 | Kp_xy : 2.5 92 | Kp_z : 2.5 93 | Ki_xy : 0.5 94 | Ki_z : 0.5 95 | Kd_xy : 3.0 96 | Kd_z : 3.0 97 | 98 | 99 | # 位置环参数 for ude 100 | Pos_ude: 101 | Kp_xy : 0.5 102 | Kp_z : 0.5 103 | Kd_xy : 2.0 104 | Kd_z : 2.0 105 | T_ude_xy : 2.0 106 | T_ude_z : 2.0 107 | 108 | # 位置环参数 for passivity 109 | Pos_passivity: 110 | Kp_xy : 0.5 111 | Kp_z : 0.5 112 | Kd_xy : 2.0 113 | Kd_z : 2.0 114 | T_ude_xy : 1.0 115 | T_ude_z : 1.0 116 | T_ps: 0.2 117 | 118 | # 位置环参数 for ne 119 | Pos_ne: 120 | Kp_xy : 0.5 121 | Kp_z : 0.5 122 | Kd_xy : 2.0 123 | Kd_z : 2.0 124 | T_ude_xy : 1.0 125 | T_ude_z : 1.0 126 | T_ne : 0.1 127 | 128 | # 圆轨迹参数 for Circle_Trajectory 129 | Circle_Trajectory: 130 | Center_x: 0.0 131 | Center_y: 0.0 132 | Center_z: 1.0 133 | radius: 0.8 134 | linear_vel: 0.3 135 | time_total: 50.0 136 | direction: 1.0 137 | 138 | Rectangular_Trajectory: 139 | a_x: 0.8 # length along x axis 140 | a_y: 0.5 # length along y axis 141 | h: 0.4 # hight 142 | vel_x: 0.05 # velocity along x axis 143 | vel_y: 0.05 # velocity along y axis 144 | center_x: 0.15 145 | center_y: 0.0 146 | center_z: 0.0 147 | 148 | payload_geofence: 149 | x_min: -0.5 150 | x_max: 0.6 151 | y_min: -0.45 152 | y_max: 0.45 153 | z_min: -0.05 154 | z_max: 0.6 155 | 156 | # parameter for single UAV payload using method in TIE paper 157 | Pos_tie: 158 | issingleUAVused: true # is single UAV payload used in control 159 | isVisionused: false 160 | isIntegrationOn: true 161 | isPubAuxiliaryState: true 162 | payload_mass: 0.5 163 | quadrotor_mass : 1.45 164 | cablelength : 1 165 | motor_slope: 0.082 166 | motor_intercept: 0.26 167 | Kp_xy : 0.4 168 | Kp_z : 0.6 169 | Kv_xy : 1.7 170 | Kv_z : 2 171 | T_tie_xy : 1.0 172 | T_tie_z : 1.0 173 | Kpv_xy: 0.3 174 | Kpv_z: 0.3 175 | KL : 0.1 176 | 177 | # cooperative method 178 | CooperativePayload: 0 179 | IntegrationStartHeight: 0.15 180 | 181 | # parameter for payload cooperative control 182 | uav0_Pos_GNC: 183 | mass : 1.55 184 | cablelength : 0.98 185 | TetherOffset_x: 1.085 186 | TetherOffset_y: 0 187 | TetherOffset_z: 0 188 | PayloadSharingPortion : 0.333 189 | motor_slope: 0.082 190 | motor_intercept: 0.26 191 | 192 | uav1_Pos_GNC: 193 | mass : 1.55 194 | cablelength : 0.98 195 | TetherOffset_x: -0.5425 196 | TetherOffset_y: -0.9396 197 | TetherOffset_z: 0 198 | PayloadSharingPortion: 0.333 199 | motor_slope: 0.082 200 | motor_intercept: 0.26 201 | 202 | uav2_Pos_GNC: 203 | mass : 1.55 204 | cablelength : 0.98 205 | TetherOffset_x: -0.5425 206 | TetherOffset_y: 0.9396 207 | TetherOffset_z: 0 208 | PayloadSharingPortion: 0.333 209 | motor_slope: 0.082 210 | motor_intercept: 0.26 211 | 212 | Pos_GNC: 213 | ControllerInfo: -1 214 | Type: 0 # 0 for TCST method 215 | MaxInclination: 50.0 # maximum safty angle 216 | CableLengthTolerance: 1.2 217 | # debugging options 218 | UseAddonForce: true 219 | UseCrossFeedingTerms: true 220 | PubAuxiliaryState : true 221 | # controller parametrers: 222 | num_drone : 3 223 | kv_xy : 0.3 224 | Kv_z : 0.7 225 | kR_xy : 0.3 226 | kR_z : 0.2 227 | kL : 0.15 228 | Kphi_xy : 3 229 | Kphi_z : 4.5 230 | fp_max_x : 5 231 | fp_max_y : 5 232 | fp_max_z : 5 233 | lambda_j : 1 234 | lambda_Txy : 0.09 235 | lambda_Tz : 0.3 236 | lambda_Rxy : 0.09 237 | lambda_Rz : 0.09 238 | kF_xy : 0.4 239 | kF_z : 0.5 240 | kr1_x : 0.15 241 | kr1_y : 0.15 242 | kr1_z : 0.15 243 | kr2_x : 0.15 244 | kr2_y : 0.15 245 | kr2_z : 0.15 246 | 247 | kp_x : 0.1 248 | kp_y : 0.1 249 | kp_z : 0.1 250 | 251 | komega_x : 0.1 252 | komega_y : 0.1 253 | komega_z : 0.1 254 | 255 | lambda1_x : 0.3 256 | lambda1_y : 0.3 257 | lambda1_z : 0.3 258 | lambda2_x : 0.3 259 | lambda2_y : 0.3 260 | lambda2_z : 0.3 261 | pos_int_max_x: 10 262 | pos_int_max_y: 10 263 | pos_int_max_z: 10 264 | ang_int_max_x : 1 265 | ang_int_max_y : 1 266 | ang_int_max_z : 1 267 | pos_int_start_error: 0.3 268 | angle_int_start_error: 0.5 269 | ## temporary disabled: (integral gain for payload stabilization) 270 | kvi_xy : 0.00 271 | kvi_z : 0.00 272 | kRi_xy : 0.0 273 | kRi_z : 0.0 274 | 275 | 276 | Pos_JGCD: 277 | MaxInclination: 50.0 # maximum safty angle 278 | CableLengthTolerance: 1.2 279 | PubAuxiliaryState : true 280 | # controller parametrers: 281 | num_drone : 3 282 | kv_xy : 0.2 283 | Kv_z : 0.7 284 | kR_xy : 0.3 285 | kR_z : 0.2 286 | kL : 0.12 287 | Kphi_xy : 3.0 288 | Kphi_z : 4.5 289 | fp_max_x : 10.0 290 | fp_max_y : 10.0 291 | fp_max_z : 10.0 292 | lambda_j : 1.0 293 | 294 | MPCgain: 295 | Kmpc_0_0: 0.034149 296 | Kmpc_0_1: 0 297 | Kmpc_0_2: 0 298 | Kmpc_0_3: 0 299 | Kmpc_0_4: 0 300 | Kmpc_0_5: 0 301 | Kmpc_0_6: 0.01913 302 | Kmpc_0_7: 0 303 | Kmpc_0_8: 0.01913 304 | Kmpc_0_9: 0 305 | Kmpc_0_10: 0.01913 306 | Kmpc_0_11: 0 307 | Kmpc_0_12: 0.039987 308 | Kmpc_0_13: 0 309 | Kmpc_0_14: 0 310 | Kmpc_0_15: 0 311 | Kmpc_0_16: 0 312 | Kmpc_0_17: 0 313 | Kmpc_0_18: 0.059665 314 | Kmpc_0_19: 0 315 | Kmpc_0_20: 0.059665 316 | Kmpc_0_21: 0 317 | Kmpc_0_22: 0.059665 318 | Kmpc_0_23: 0 319 | Kmpc_1_0: 0 320 | Kmpc_1_1: 0.034149 321 | Kmpc_1_2: 0 322 | Kmpc_1_3: 0 323 | Kmpc_1_4: 0 324 | Kmpc_1_5: 0 325 | Kmpc_1_6: 0 326 | Kmpc_1_7: 0.01913 327 | Kmpc_1_8: 0 328 | Kmpc_1_9: 0.01913 329 | Kmpc_1_10: 0 330 | Kmpc_1_11: 0.01913 331 | Kmpc_1_12: 0 332 | Kmpc_1_13: 0.039987 333 | Kmpc_1_14: 0 334 | Kmpc_1_15: 0 335 | Kmpc_1_16: 0 336 | Kmpc_1_17: 0 337 | Kmpc_1_18: 0 338 | Kmpc_1_19: 0.059665 339 | Kmpc_1_20: 0 340 | Kmpc_1_21: 0.059665 341 | Kmpc_1_22: 0 342 | Kmpc_1_23: 0.059665 343 | Kmpc_2_0: 0 344 | Kmpc_2_1: 0 345 | Kmpc_2_2: 0.029668 346 | Kmpc_2_3: 0 347 | Kmpc_2_4: 0 348 | Kmpc_2_5: 0 349 | Kmpc_2_6: 0 350 | Kmpc_2_7: 0 351 | Kmpc_2_8: 0 352 | Kmpc_2_9: 0 353 | Kmpc_2_10: 0 354 | Kmpc_2_11: 0 355 | Kmpc_2_12: 0 356 | Kmpc_2_13: 0 357 | Kmpc_2_14: 0.037247 358 | Kmpc_2_15: 0 359 | Kmpc_2_16: 0 360 | Kmpc_2_17: 0 361 | Kmpc_2_18: 0 362 | Kmpc_2_19: 0 363 | Kmpc_2_20: 0 364 | Kmpc_2_21: 0 365 | Kmpc_2_22: 0 366 | Kmpc_2_23: 0 367 | Kmpc_3_0: 0 368 | Kmpc_3_1: 0 369 | Kmpc_3_2: 0 370 | Kmpc_3_3: 0.028949 371 | Kmpc_3_4: 0 372 | Kmpc_3_5: 0 373 | Kmpc_3_6: 0 374 | Kmpc_3_7: 0 375 | Kmpc_3_8: 0 376 | Kmpc_3_9: 0 377 | Kmpc_3_10: 0 378 | Kmpc_3_11: 0 379 | Kmpc_3_12: 0 380 | Kmpc_3_13: 0 381 | Kmpc_3_14: 0 382 | Kmpc_3_15: 0.03769 383 | Kmpc_3_16: 0 384 | Kmpc_3_17: 0 385 | Kmpc_3_18: 0 386 | Kmpc_3_19: 0 387 | Kmpc_3_20: 0 388 | Kmpc_3_21: 0 389 | Kmpc_3_22: 0 390 | Kmpc_3_23: 0 391 | Kmpc_4_0: 0 392 | Kmpc_4_1: 0 393 | Kmpc_4_2: 0 394 | Kmpc_4_3: 0 395 | Kmpc_4_4: 0.028949 396 | Kmpc_4_5: 0 397 | Kmpc_4_6: 0 398 | Kmpc_4_7: 0 399 | Kmpc_4_8: 0 400 | Kmpc_4_9: 0 401 | Kmpc_4_10: 0 402 | Kmpc_4_11: 0 403 | Kmpc_4_12: 0 404 | Kmpc_4_13: 0 405 | Kmpc_4_14: 0 406 | Kmpc_4_15: 0 407 | Kmpc_4_16: 0.03769 408 | Kmpc_4_17: 0 409 | Kmpc_4_18: 0 410 | Kmpc_4_19: 0 411 | Kmpc_4_20: 0 412 | Kmpc_4_21: 0 413 | Kmpc_4_22: 0 414 | Kmpc_4_23: 0 415 | Kmpc_5_0: 0 416 | Kmpc_5_1: 0 417 | Kmpc_5_2: 0 418 | Kmpc_5_3: 0 419 | Kmpc_5_4: 0 420 | Kmpc_5_5: 0.028147 421 | Kmpc_5_6: 0 422 | Kmpc_5_7: 0.024841 423 | Kmpc_5_8: 0.021512 424 | Kmpc_5_9: -0.012421 425 | Kmpc_5_10: -0.021512 426 | Kmpc_5_11: -0.012421 427 | Kmpc_5_12: 0 428 | Kmpc_5_13: 0 429 | Kmpc_5_14: 0 430 | Kmpc_5_15: 0 431 | Kmpc_5_16: 0 432 | Kmpc_5_17: 0.036876 433 | Kmpc_5_18: 0 434 | Kmpc_5_19: 0.077785 435 | Kmpc_5_20: 0.067361 436 | Kmpc_5_21: -0.038892 437 | Kmpc_5_22: -0.067361 438 | Kmpc_5_23: -0.038892 -------------------------------------------------------------------------------- /src/px4_multidrone_pos_estimator.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************************************************************** 2 | * px4_multidrone_pos_estimator.cpp 3 | * 4 | * Author: Longhao Qian 5 | * 6 | * Update Time: 2019.8.31 7 | * 8 | * 说明: mavros位置估计程序 9 | * 1. 订阅激光SLAM (cartorgrapher_ros节点) 发布的位置信息,从laser坐标系转换至NED坐标系 10 | * 2. 订阅Mocap设备 (vrpn-client-ros节点) 发布的位置信息，从mocap坐标系转换至NED坐标系 11 | * 3. 订阅飞控发布的位置、速度及欧拉角信息，作对比用 12 | * 4. 存储飞行数据，实验分析及作图使用 13 | * 5. 选择激光SLAM或者Mocap设备作为位置来源，发布位置及偏航角(xyz+yaw)给飞控 14 | * 15 | ***************************************************************************************************************************/ 16 | //头文件 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | //msg 头文件 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | using namespace std; 45 | struct SubTopic 46 | { 47 | char str[100]; 48 | }; 49 | struct PubTopic 50 | { 51 | char str[100]; 52 | }; 53 | //---------------------------------------相关参数----------------------------------------------- 54 | int flag_use_laser_or_vicon; //0:使用mocap数据作为定位数据 1:使用laser数据作为定位数据 55 | //---------------------------------------vicon定位相关------------------------------------------ 56 | Eigen::Vector3d pos_drone_mocap; //无人机当前位置 (vicon) 57 | Eigen::Quaterniond q_mocap; 58 | Eigen::Vector3d Euler_mocap; //无人机当前姿态 (vicon) 59 | //---------------------------------------laser定位相关------------------------------------------ 60 | Eigen::Vector3d pos_drone_laser; //无人机当前位置 (laser) 61 | Eigen::Quaterniond q_laser; 62 | Eigen::Vector3d Euler_laser; //无人机当前姿态(laser) 63 | 64 | geometry_msgs::TransformStamped laser; //当前时刻cartorgrapher发布的数据 65 | geometry_msgs::TransformStamped laser_last; 66 | //---------------------------------------无人机位置及速度-------------------------------------------- 67 | Eigen::Vector3d pos_drone_fcu; //无人机当前位置 (来自fcu) 68 | Eigen::Vector3d vel_drone_fcu; //无人机上一时刻位置 (来自fcu) 69 | Eigen::Vector3d Att_fcu; //无人机当前欧拉角(来自fcu) 70 | Eigen::Vector3d Att_rate_fcu; 71 | //---------------------------------------发布相关变量-------------------------------------------- 72 | ros::Publisher vision_pub; 73 | ros::Publisher drone_state_pub; 74 | px4_command::DroneState _DroneState; 75 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> callbacks <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 76 | void laser_cb(const tf2_msgs::TFMessage::ConstPtr& msg) 77 | { 78 | //确定是cartographer发出来的/tf信息 79 | //有的时候/tf这个消息的发布者不止一个 80 | if (msg->transforms[0].header.frame_id == "map") 81 | { 82 | laser = msg->transforms[0]; 83 | 84 | float dt_laser; 85 | 86 | dt_laser = (laser.header.stamp.sec - laser_last.header.stamp.sec) + (laser.header.stamp.nsec - laser_last.header.stamp.nsec)/10e9; 87 | 88 | //这里需要做这个判断是因为cartographer发布位置时有一个小bug，ENU到NED不展开讲。 89 | if (dt_laser != 0) 90 | { 91 | //位置 xy [将解算的位置从laser坐标系转换至ENU坐标系]??? 92 | pos_drone_laser[0] = laser.transform.translation.x; 93 | pos_drone_laser[1] = laser.transform.translation.y; 94 | 95 | // Read the Quaternion from the Carto Package [Frame: Laser[ENU]] 96 | Eigen::Quaterniond q_laser_enu(laser.transform.rotation.w, laser.transform.rotation.x, laser.transform.rotation.y, laser.transform.rotation.z); 97 | 98 | q_laser = q_laser_enu; 99 | 100 | // Transform the Quaternion to Euler Angles 101 | Euler_laser = quaternion_to_euler(q_laser); 102 | } 103 | 104 | laser_last = laser; 105 | } 106 | } 107 | void sonic_cb(const std_msgs::UInt16::ConstPtr& msg) 108 | { 109 | std_msgs::UInt16 sonic; 110 | 111 | sonic = *msg; 112 | 113 | //位置 114 | pos_drone_laser[2] = (float)sonic.data / 1000; 115 | } 116 | 117 | void tfmini_cb(const sensor_msgs::Range::ConstPtr& msg) 118 | { 119 | sensor_msgs::Range tfmini; 120 | 121 | tfmini = *msg; 122 | 123 | //位置 124 | pos_drone_laser[2] = tfmini.range ; 125 | 126 | } 127 | 128 | void optitrack_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) 129 | { 130 | //位置 -- optitrack系 到 ENU系 131 | //Frame convention 0: Z-up -- 1: Y-up (See the configuration in the motive software) 132 | int optitrack_frame = 0; 133 | if(optitrack_frame == 0) 134 | { 135 | // Read the Drone Position from the Vrpn Package [Frame: Vicon] (Vicon to ENU frame) 136 | pos_drone_mocap = Eigen::Vector3d(msg->pose.position.x,msg->pose.position.y,msg->pose.position.z); 137 | // Read the Quaternion from the Vrpn Package [Frame: Vicon[ENU]] 138 | q_mocap = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z); 139 | } 140 | else 141 | { 142 | // Read the Drone Position from the Vrpn Package [Frame: Vicon] (Vicon to ENU frame) 143 | pos_drone_mocap = Eigen::Vector3d(-msg->pose.position.x,msg->pose.position.z,msg->pose.position.y); 144 | // Read the Quaternion from the Vrpn Package [Frame: Vicon[ENU]] 145 | q_mocap = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.z, msg->pose.orientation.y); //Y-up convention, switch the q2 & q3 146 | } 147 | 148 | // Transform the Quaternion to Euler Angles 149 | Euler_mocap = quaternion_to_euler(q_mocap); 150 | 151 | } 152 | 153 | void send_to_fcu() 154 | { 155 | geometry_msgs::PoseStamped vision; 156 | 157 | //vicon 158 | if(flag_use_laser_or_vicon == 0) 159 | { 160 | vision.pose.position.x = pos_drone_mocap[0] ; 161 | vision.pose.position.y = pos_drone_mocap[1] ; 162 | vision.pose.position.z = pos_drone_mocap[2] ; 163 | 164 | vision.pose.orientation.x = q_mocap.x(); 165 | vision.pose.orientation.y = q_mocap.y(); 166 | vision.pose.orientation.z = q_mocap.z(); 167 | vision.pose.orientation.w = q_mocap.w(); 168 | 169 | }//laser 170 | else if (flag_use_laser_or_vicon == 1) 171 | { 172 | vision.pose.position.x = pos_drone_laser[0]; 173 | vision.pose.position.y = pos_drone_laser[1]; 174 | vision.pose.position.z = pos_drone_laser[2]; 175 | 176 | vision.pose.orientation.x = q_laser.x(); 177 | vision.pose.orientation.y = q_laser.y(); 178 | vision.pose.orientation.z = q_laser.z(); 179 | vision.pose.orientation.w = q_laser.w(); 180 | } 181 | 182 | vision.header.stamp = ros::Time::now(); 183 | vision_pub.publish(vision); 184 | } 185 | //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 186 | int main(int argc, char **argv) 187 | { 188 | ros::init(argc, argv, "px4_multidrone_pos_estimator"); 189 | ros::NodeHandle nh("~"); 190 | // 频率 191 | ros::Rate rate(50.0); 192 | //读取参数表中的参数 193 | // 使用激光SLAM数据orVicon数据 0 for vicon， 1 for 激光SLAM 194 | nh.param("pos_estimator/flag_use_laser_or_vicon", flag_use_laser_or_vicon, 0); 195 | /*----------- determine the ID of the drone -----------------------------*/ 196 | SubTopic vrpn_client_node_UAV_pose; 197 | SubTopic tf; 198 | SubTopic sonic; 199 | SubTopic TFmini_TFmini; 200 | PubTopic mavros_vision_pose_pose; 201 | PubTopic px4_command_drone_state; 202 | // add preflex: (mavros and px4_command use lower case uav, while /vrpn_client_node/UAV use upper case UAV) 203 | strcpy (vrpn_client_node_UAV_pose.str,"/vrpn_client_node/UAV"); 204 | strcpy (tf.str,"/uav"); 205 | strcpy (sonic.str,"/uav"); 206 | strcpy (TFmini_TFmini.str,"/uav"); 207 | strcpy (mavros_vision_pose_pose.str,"/uav"); 208 | strcpy (px4_command_drone_state.str,"/uav"); 209 | 210 | char ID[20]; // ID of the uav 211 | 212 | if ( argc > 1) { 213 | // if ID is specified as the second argument 214 | strcat (vrpn_client_node_UAV_pose.str,argv[1]); 215 | strcat (tf.str,argv[1]); 216 | strcat (sonic.str,argv[1]); 217 | strcat (TFmini_TFmini.str,argv[1]); 218 | strcat (mavros_vision_pose_pose.str,argv[1]); 219 | strcat (px4_command_drone_state.str,argv[1]); 220 | ROS_INFO("UAV ID specified as: UAV%s", argv[1]); 221 | strcpy (ID,argv[1]); 222 | } else { 223 | // if ID is not specified, then set the drone to UAV0 224 | strcat (vrpn_client_node_UAV_pose.str,"0"); 225 | strcat (tf.str,"0"); 226 | strcat (sonic.str,"0"); 227 | strcat (TFmini_TFmini.str,"0"); 228 | strcat (mavros_vision_pose_pose.str,"0"); 229 | strcat (px4_command_drone_state.str,"0"); 230 | ROS_WARN("NO UAV ID specified, set ID to 0."); 231 | strcpy (ID,"0"); 232 | } 233 | 234 | state_from_mavros_multidrone _state_from_mavros(ID); // define the state_from_mavros 235 | strcat (vrpn_client_node_UAV_pose.str,"/pose"); 236 | strcat (tf.str,"/tf"); 237 | strcat (sonic.str,"/sonic"); 238 | strcat (TFmini_TFmini.str,"/TFmini/TFmini"); 239 | strcat (mavros_vision_pose_pose.str,"/mavros/vision_pose/pose"); 240 | strcat (px4_command_drone_state.str,"/px4_command/drone_state"); 241 | 242 | ROS_INFO("Subscribe uav Mocap from: %s", vrpn_client_node_UAV_pose.str); 243 | ROS_INFO("Subscribe TFMessage from: %s", tf.str); 244 | ROS_INFO("Subscribe sonic from: %s", sonic.str); 245 | ROS_INFO("Subscribe TFmini from: %s", TFmini_TFmini.str); 246 | ROS_INFO("Publish mavros_vision_pose_pose to: %s", mavros_vision_pose_pose.str); 247 | ROS_INFO("Publish PoseStamped to: %s", px4_command_drone_state.str); 248 | 249 | // 【订阅】cartographer估计位置 250 | ros::Subscriber laser_sub = nh.subscribe(tf.str, 1000, laser_cb); 251 | 252 | // 【订阅】超声波的数据 253 | ros::Subscriber sonic_sub = nh.subscribe(sonic.str, 100, sonic_cb); 254 | 255 | // 【订阅】tf mini的数据 256 | ros::Subscriber tfmini_sub = nh.subscribe(TFmini_TFmini.str, 100, tfmini_cb); 257 | 258 | // 【订阅】optitrack估计位置 259 | ros::Subscriber optitrack_sub = nh.subscribe(vrpn_client_node_UAV_pose.str, 1000, optitrack_cb); 260 | 261 | // 【发布】无人机位置和偏航角 坐标系 ENU系 262 | // 本话题要发送飞控(通过mavros_extras/src/plugins/vision_pose_estimate.cpp发送), 对应Mavlink消息为VISION_POSITION_ESTIMATE(#??), 对应的飞控中的uORB消息为vehicle_vision_position.msg 及 vehicle_vision_attitude.msg 263 | vision_pub = nh.advertise(mavros_vision_pose_pose.str, 100); 264 | 265 | drone_state_pub = nh.advertise(px4_command_drone_state.str, 100); 266 | 267 | ROS_INFO("Start the estimator..."); 268 | 269 | while(ros::ok()) 270 | { 271 | //回调一次 更新传感器状态 272 | ros::spinOnce(); 273 | 274 | // 将定位信息及偏航角信息发送至飞控，根据参数flag_use_laser_or_vicon选择定位信息来源 275 | send_to_fcu(); 276 | 277 | for (int i=0;i<3;i++) 278 | { 279 | pos_drone_fcu[i] = _state_from_mavros._DroneState.position[i]; 280 | vel_drone_fcu[i] = _state_from_mavros._DroneState.velocity[i]; 281 | Att_fcu[i] = _state_from_mavros._DroneState.attitude[i]; 282 | Att_rate_fcu[i] = _state_from_mavros._DroneState.attitude_rate[i]; 283 | } 284 | 285 | // 发布无人机状态至px4_pos_controller.cpp节点，根据参数Use_mocap_raw选择位置速度消息来源 286 | // get drone state from _state_from_mavros 287 | _DroneState = _state_from_mavros._DroneState; 288 | _DroneState.header.stamp = ros::Time::now(); 289 | drone_state_pub.publish(_DroneState); 290 | /*TODO add a vrpn detector*/ 291 | rate.sleep(); 292 | } 293 | 294 | return 0; 295 | 296 | } -------------------------------------------------------------------------------- /include/pos_controller_cascade_PID.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************************************************************** 2 | * pos_controller_cascade_PID.h 3 | * 4 | * Author: Qyp 5 | * 6 | * Update Time: 2019.5.1 7 | * 8 | * Introduction: Position Controller using PID (P for pos loop, pid for vel loop) 9 | * 1. Similiar to the position controller in PX4 (1.8.2) 10 | * 2. Ref to : https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/PositionControl.cpp 11 | * 3. Here we didn't consider the mass of the drone, we treat accel_sp is the thrust_sp. 12 | * 4. ThrottleToAttitude ref to https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/Utility/ControlMath.cpp 13 | * 5. For the derrive of the velocity error, we use a low-pass filter as same in PX4. 14 | * Ref to: https://github.com/PX4/Firmware/blob/master/src/lib/controllib/BlockDerivative.cpp 15 | * https://github.com/PX4/Firmware/blob/master/src/lib/controllib/BlockLowPass.cpp 16 | * 6. 没有考虑积分器清零的情况，在降落时 或者突然换方向机动时，积分器需要清0 17 | * 7. 推力到欧拉角基本与PX4吻合，但是在极端情况下不吻合。如：z轴期望值为-100时。 18 | ***************************************************************************************************************************/ 19 | #ifndef POS_CONTROLLER_CASCADE_PID_H 20 | #define POS_CONTROLLER_CASCADE_PID_H 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | using namespace std; 32 | 33 | class pos_controller_cascade_PID 34 | { 35 | //public表明该数据成员、成员函数是对全部用户开放的。全部用户都能够直接进行调用，在程序的不论什么其他地方訪问。 36 | public: 37 | 38 | //构造函数 39 | pos_controller_cascade_PID(void): 40 | pos_cascade_pid_nh("~") 41 | { 42 | pos_cascade_pid_nh.param("Pos_cascade_pid/Kp_xy", Kp_xy, 1.0); 43 | pos_cascade_pid_nh.param("Pos_cascade_pid/Kp_z", Kp_z, 1.0); 44 | pos_cascade_pid_nh.param("Pos_cascade_pid/Kp_vxvy", Kp_vxvy, 0.1); 45 | pos_cascade_pid_nh.param("Pos_cascade_pid/Kp_vz", Kp_vz, 0.1); 46 | pos_cascade_pid_nh.param("Pos_cascade_pid/Ki_vxvy", Ki_vxvy, 0.02); 47 | pos_cascade_pid_nh.param("Pos_cascade_pid/Ki_vz", Ki_vz, 0.02); 48 | pos_cascade_pid_nh.param("Pos_cascade_pid/Kd_vxvy", Kd_vxvy, 0.01); 49 | pos_cascade_pid_nh.param("Pos_cascade_pid/Kd_vz", Kd_vz, 0.01); 50 | pos_cascade_pid_nh.param("Pos_cascade_pid/Hover_throttle", Hover_throttle, 0.5); 51 | pos_cascade_pid_nh.param("Pos_cascade_pid/MPC_VELD_LP", MPC_VELD_LP, 5.0); 52 | 53 | pos_cascade_pid_nh.param("Limit/XY_VEL_MAX", MPC_XY_VEL_MAX, 1.0); 54 | pos_cascade_pid_nh.param("Limit/Z_VEL_MAX", MPC_Z_VEL_MAX, 0.5); 55 | pos_cascade_pid_nh.param("Limit/THR_MIN", MPC_THR_MIN, 0.1); 56 | pos_cascade_pid_nh.param("Limit/THR_MAX", MPC_THR_MAX, 0.9); 57 | pos_cascade_pid_nh.param("Limit/tilt_max", tilt_max, 5.0); 58 | 59 | 60 | 61 | vel_setpoint = Eigen::Vector3d(0.0,0.0,0.0); 62 | thrust_sp = Eigen::Vector3d(0.0,0.0,0.0); 63 | vel_P_output = Eigen::Vector3d(0.0,0.0,0.0); 64 | thurst_int = Eigen::Vector3d(0.0,0.0,0.0); 65 | vel_D_output = Eigen::Vector3d(0.0,0.0,0.0); 66 | error_vel_dot_last = Eigen::Vector3d(0.0,0.0,0.0); 67 | error_vel_last = Eigen::Vector3d(0.0,0.0,0.0); 68 | delta_time = 0.02; 69 | } 70 | 71 | //PID parameter for the control law 72 | float Kp_xy; 73 | float Kp_z; 74 | float Kp_vxvy; 75 | float Kp_vz; 76 | float Ki_vxvy; 77 | float Ki_vz; 78 | float Kd_vxvy; 79 | float Kd_vz; 80 | 81 | //Limitation of the velocity 82 | float MPC_XY_VEL_MAX; 83 | float MPC_Z_VEL_MAX; 84 | 85 | //Hover thrust of drone (decided by the mass of the drone) 86 | float Hover_throttle; 87 | 88 | //Limitation of the thrust 89 | float MPC_THR_MIN; 90 | float MPC_THR_MAX; 91 | 92 | //Limitation of the tilt angle (roll and pitch) [degree] 93 | float tilt_max; 94 | 95 | //输出 96 | 97 | //Desired position and velocity of the drone 98 | Eigen::Vector3d vel_setpoint; 99 | 100 | //Desired thurst of the drone[the output of this class] 101 | Eigen::Vector3d thrust_sp; 102 | 103 | //Output of the vel loop in PID [thurst_int is the I] 104 | Eigen::Vector3d vel_P_output; 105 | Eigen::Vector3d thurst_int; 106 | Eigen::Vector3d vel_D_output; 107 | 108 | float MPC_VELD_LP; 109 | 110 | //The delta time between now and the last step 111 | float delta_time; 112 | 113 | //Derriv of the velocity error in last step [used for the D-output in vel loop] 114 | Eigen::Vector3d error_vel_dot_last; 115 | Eigen::Vector3d error_vel_last; 116 | 117 | //Current state of the drone 118 | mavros_msgs::State current_state; 119 | 120 | px4_command::ControlOutput _ControlOutput; 121 | 122 | //Printf the PID parameter 123 | void printf_param(); 124 | 125 | //Printf the control result 126 | void printf_result(); 127 | 128 | // Position control main function 129 | // [Input: Current state, Reference state, _Reference_State.Sub_mode, dt; Output: AttitudeReference;] 130 | px4_command::ControlOutput pos_controller(const px4_command::DroneState& _DroneState, const px4_command::TrajectoryPoint& _Reference_State, float dt); 131 | 132 | //Position control loop [Input: current pos, desired pos; Output: desired vel] 133 | void _positionController(const px4_command::DroneState& _DroneState, const px4_command::TrajectoryPoint& _Reference_State, Eigen::Vector3d& vel_setpoint); 134 | 135 | //Velocity control loop [Input: current vel, desired vel; Output: desired thrust] 136 | void _velocityController(const px4_command::DroneState& _DroneState, const px4_command::TrajectoryPoint& _Reference_State, float dt, Eigen::Vector3d& thrust_sp); 137 | 138 | void cal_vel_error_deriv(const Eigen::Vector3d& error_now, Eigen::Vector3d& vel_error_deriv); 139 | 140 | private: 141 | 142 | ros::NodeHandle pos_cascade_pid_nh; 143 | }; 144 | 145 | px4_command::ControlOutput pos_controller_cascade_PID::pos_controller 146 | (const px4_command::DroneState& _DroneState, 147 | const px4_command::TrajectoryPoint& _Reference_State, 148 | float dt) 149 | { 150 | delta_time = dt; 151 | 152 | _positionController(_DroneState, _Reference_State, vel_setpoint); 153 | 154 | Eigen::Vector3d thrust_sp; 155 | 156 | _velocityController(_DroneState, _Reference_State, delta_time, thrust_sp); 157 | 158 | _ControlOutput.Throttle[0] = thrust_sp[0]; 159 | _ControlOutput.Throttle[1] = thrust_sp[1]; 160 | _ControlOutput.Throttle[2] = thrust_sp[2]; 161 | 162 | return _ControlOutput; 163 | } 164 | 165 | 166 | void pos_controller_cascade_PID::_positionController 167 | (const px4_command::DroneState& _DroneState, 168 | const px4_command::TrajectoryPoint& _Reference_State, 169 | Eigen::Vector3d& vel_setpoint) 170 | { 171 | //# _Reference_State.Sub_mode 2-bit value: 172 | //# 0 for position, 1 for vel, 1st for xy, 2nd for z. 173 | //# xy position xy velocity 174 | //# z position 0b00[0] 0b10[2] 175 | //# z velocity 0b01[1] 0b11(3) 176 | 177 | if((_Reference_State.Sub_mode & 0b10) == 0) //xy channel 178 | { 179 | vel_setpoint[0] = _Reference_State.velocity_ref[0] + Kp_xy * (_Reference_State.position_ref[0] - _DroneState.position[0]); 180 | vel_setpoint[1] = _Reference_State.velocity_ref[1] + Kp_xy * (_Reference_State.position_ref[1] - _DroneState.position[1]); 181 | } 182 | else 183 | { 184 | vel_setpoint[0] = _Reference_State.velocity_ref[0]; 185 | vel_setpoint[1] = _Reference_State.velocity_ref[1]; 186 | } 187 | 188 | if((_Reference_State.Sub_mode & 0b01) == 0) //z channel 189 | { 190 | vel_setpoint[2] = _Reference_State.velocity_ref[2] + Kp_z * (_Reference_State.position_ref[2] - _DroneState.position[2]); 191 | } 192 | else 193 | { 194 | vel_setpoint[2] = _Reference_State.velocity_ref[2]; 195 | } 196 | 197 | // Limit the velocity setpoint 198 | vel_setpoint[0] = constrain_function(vel_setpoint[0], MPC_XY_VEL_MAX); 199 | vel_setpoint[1] = constrain_function(vel_setpoint[1], MPC_XY_VEL_MAX); 200 | vel_setpoint[2] = constrain_function(vel_setpoint[2], MPC_Z_VEL_MAX); 201 | } 202 | 203 | void pos_controller_cascade_PID::_velocityController 204 | (const px4_command::DroneState& _DroneState, 205 | const px4_command::TrajectoryPoint& _Reference_State, float dt, 206 | Eigen::Vector3d& thrust_sp) 207 | { 208 | // Generate desired thrust setpoint. 209 | // PID 210 | // u_des = P(error_vel) + D(error_vel_dot) + I(vel_integral) 211 | // Umin <= u_des <= Umax 212 | // 213 | // Anti-Windup: 214 | // u_des = _thrust_sp; y = vel_sp; r = _vel 215 | // u_des >= Umax and r - y >= 0 => Saturation = true 216 | // u_des >= Umax and r - y <= 0 => Saturation = false 217 | // u_des <= Umin and r - y <= 0 => Saturation = true 218 | // u_des <= Umin and r - y >= 0 => Saturation = false 219 | // 220 | // Notes: 221 | // - control output in Z-direction has priority over XY-direction 222 | // - the equilibrium point for the PID is at hover-thrust 223 | 224 | // - the desired thrust in Z-direction is limited by the thrust limits 225 | // - the desired thrust in XY-direction is limited by the thrust excess after 226 | // consideration of the desired thrust in Z-direction. In addition, the thrust in 227 | // XY-direction is also limited by the maximum tilt. 228 | 229 | Eigen::Vector3d error_vel; 230 | error_vel[0] = vel_setpoint[0] - _DroneState.velocity[0]; 231 | error_vel[1] = vel_setpoint[1] - _DroneState.velocity[1]; 232 | error_vel[2] = vel_setpoint[2] - _DroneState.velocity[2]; 233 | 234 | vel_P_output[0] = Kp_vxvy * error_vel[0]; 235 | vel_P_output[1] = Kp_vxvy * error_vel[1]; 236 | vel_P_output[2] = Kp_vz * error_vel[2]; 237 | 238 | Eigen::Vector3d vel_error_deriv; 239 | cal_vel_error_deriv(error_vel, vel_error_deriv); 240 | 241 | vel_D_output[0] = Kd_vxvy * vel_error_deriv[0]; 242 | vel_D_output[1] = Kd_vxvy * vel_error_deriv[1]; 243 | vel_D_output[2] = Kd_vz * vel_error_deriv[2]; 244 | 245 | 246 | // Consider thrust in Z-direction. [Here Hover_throttle is added] 247 | float thrust_desired_Z = _Reference_State.acceleration_ref[2] + vel_P_output[2] + thurst_int[2] + vel_D_output[2] + Hover_throttle; 248 | 249 | // Apply Anti-Windup in Z-direction. 250 | // 两种情况：期望推力大于最大推力，且速度误差朝上；期望推力小于最小推力，且速度误差朝下 251 | bool stop_integral_Z = ( thrust_desired_Z >= MPC_THR_MAX && error_vel[2] >= 0.0f) || 252 | ( thrust_desired_Z <= MPC_THR_MIN && error_vel[2] <= 0.0f); 253 | if (!stop_integral_Z) { 254 | thurst_int[2] += Ki_vz * error_vel[2] * delta_time; 255 | 256 | // limit thrust integral 257 | thurst_int[2] = min(fabs(thurst_int[2]), MPC_THR_MAX ) * sign_function(thurst_int[2]); 258 | } 259 | 260 | // Saturate thrust setpoint in Z-direction. 261 | thrust_sp[2] = constrain_function2( thrust_desired_Z , MPC_THR_MIN, MPC_THR_MAX); 262 | 263 | // PID-velocity controller for XY-direction. 264 | float thrust_desired_X; 265 | float thrust_desired_Y; 266 | thrust_desired_X = _Reference_State.acceleration_ref[0] + vel_P_output[0] + thurst_int[0] + vel_D_output[0]; 267 | thrust_desired_Y = _Reference_State.acceleration_ref[1] + vel_P_output[1] + thurst_int[1] + vel_D_output[1]; 268 | 269 | // Get maximum allowed thrust in XY based on tilt angle and excess thrust. 270 | float thrust_max_XY_tilt = fabs(thrust_sp[2]) * tanf(tilt_max/180.0*M_PI); 271 | float thrust_max_XY = sqrtf(MPC_THR_MAX * MPC_THR_MAX - thrust_sp[2] * thrust_sp[2]); 272 | thrust_max_XY = min(thrust_max_XY_tilt, thrust_max_XY); 273 | 274 | // Saturate thrust in XY-direction. 275 | thrust_sp[0] = thrust_desired_X; 276 | thrust_sp[1] = thrust_desired_Y; 277 | 278 | 279 | if ((thrust_desired_X * thrust_desired_X + thrust_desired_Y * thrust_desired_Y) > thrust_max_XY * thrust_max_XY) { 280 | float mag = sqrtf((thrust_desired_X * thrust_desired_X + thrust_desired_Y * thrust_desired_Y)); 281 | thrust_sp[0] = thrust_desired_X / mag * thrust_max_XY; 282 | thrust_sp[1] = thrust_desired_Y / mag * thrust_max_XY; 283 | } 284 | 285 | // Use tracking Anti-Windup for XY-direction: during saturation, the integrator is used to unsaturate the output 286 | // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 287 | // Actually, I dont understand here. 288 | float arw_gain = 2.f / Kp_vxvy; 289 | 290 | float vel_err_lim_x,vel_err_lim_y; 291 | vel_err_lim_x = error_vel[0] - (thrust_desired_X - thrust_sp[0]) * arw_gain; 292 | vel_err_lim_y = error_vel[1] - (thrust_desired_Y - thrust_sp[1]) * arw_gain; 293 | 294 | // cout << "vel_err_lim_x : " << vel_err_lim_x << " [deg] " <>>>>>>>>>>>>>>>>>> cascade PID Position Controller <<<<<<<<<<<<<<<<<<" <>>>>>>>>>>>>>>>>>>>>>>>>>>>>>PID Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <