Skip to content

Commit 9b95bd7

Browse files
potato77potato77
committed
add px4_fw_controller.cpp for test
1 parent 7967cad commit 9b95bd7

File tree

3 files changed

+210
-1
lines changed

3 files changed

+210
-1
lines changed

‎CMakeLists.txt

+3-1
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,9 @@ target_link_libraries(ground_station ${catkin_LIBRARIES})
101101
target_link_libraries(ground_station OptiTrackFeedbackRigidBody)
102102

103103
###### Test File ##########
104-
104+
add_executable(px4_fw_controller src/Test/px4_fw_controller.cpp)
105+
add_dependencies(px4_fw_controller px4_command_gencpp)
106+
target_link_libraries(px4_fw_controller ${catkin_LIBRARIES})
105107
###### Utilities File ##########
106108

107109
add_executable(setpoint_track src/Utilities/setpoint_track.cpp)

‎sh/test/test_for_fw.sh

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
##sitl_gazebo
2+
gnome-terminal --window -e 'bash -c "roscore; exec bash"' \
3+
--tab -e 'bash -c "sleep 2; roslaunch mavros px4.launch fcu_url:="udp://:[email protected]:14557"; exec bash"' \
4+
--tab -e 'bash -c "sleep 2; roslaunch px4_command px4_pos_estimator.launch; exec bash"' \
5+
--tab -e 'bash -c "sleep 2; rosrun px4_command px4_fw_controller; exec bash"' \
6+
--tab -e 'bash -c "sleep 5; rosrun px4_command move; exec bash"' \
7+
--tab -e 'bash -c "sleep 2; rosrun px4_command set_mode; exec bash"' \
8+

‎src/Test/px4_fw_controller.cpp

+199
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,199 @@
1+
#include <ros/ros.h>
2+
3+
#include <state_from_mavros.h>
4+
#include <command_to_mavros.h>
5+
6+
#include <pos_controller_PID.h>
7+
8+
#include <px4_command/ControlCommand.h>
9+
#include <px4_command/DroneState.h>
10+
#include <px4_command/TrajectoryPoint.h>
11+
#include <px4_command/AttitudeReference.h>
12+
#include <px4_command_utils.h>
13+
#include <px4_command/Trajectory.h>
14+
15+
#include <Eigen/Eigen>
16+
17+
using namespace std;
18+
19+
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
20+
px4_command::ControlCommand Command_Now; //无人机当前执行命令
21+
px4_command::ControlCommand Command_Last; //无人机上一条执行命令
22+
23+
px4_command::DroneState _DroneState; //无人机状态量
24+
float cur_time;
25+
26+
Eigen::Vector3d pos_sp(0,0,0);
27+
Eigen::Vector3d vel_sp(0,0,0);
28+
double yaw_sp;
29+
30+
//Target pos of the drone [from fcu]
31+
Eigen::Vector3d pos_drone_fcu_target;
32+
//Target vel of the drone [from fcu]
33+
Eigen::Vector3d vel_drone_fcu_target;
34+
35+
36+
void Command_cb(const px4_command::ControlCommand::ConstPtr& msg)
37+
{
38+
Command_Now = *msg;
39+
}
40+
void drone_state_cb(const px4_command::DroneState::ConstPtr& msg)
41+
{
42+
_DroneState = *msg;
43+
44+
_DroneState.time_from_start = cur_time;
45+
}
46+
void pos_target_cb(const mavros_msgs::PositionTarget::ConstPtr& msg)
47+
{
48+
pos_drone_fcu_target = Eigen::Vector3d(msg->position.x, msg->position.y, msg->position.z);
49+
50+
vel_drone_fcu_target = Eigen::Vector3d(msg->velocity.x, msg->velocity.y, msg->velocity.z);
51+
}
52+
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
53+
int main(int argc, char **argv)
54+
{
55+
ros::init(argc, argv, "px4_fw_controller");
56+
ros::NodeHandle nh("~");
57+
58+
ros::Subscriber Command_sub = nh.subscribe<px4_command::ControlCommand>("/px4_command/control_command", 10, Command_cb);
59+
60+
//【订阅】无人机当前状态
61+
// 本话题来自根据需求自定px4_pos_estimator.cpp
62+
ros::Subscriber drone_state_sub = nh.subscribe<px4_command::DroneState>("/px4_command/drone_state", 10, drone_state_cb);
63+
64+
// 【订阅】无人机期望位置/速度/加速度 坐标系:ENU系
65+
// 本话题来自飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp读取), 对应Mavlink消息为POSITION_TARGET_LOCAL_NED, 对应的飞控中的uORB消息为vehicle_local_position_setpoint.msg
66+
ros::Subscriber position_target_sub = nh.subscribe<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/target_local", 10, pos_target_cb);
67+
68+
// 【发布】位置/速度/加速度期望值 坐标系 ENU系
69+
// 本话题要发送至飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp发送), 对应Mavlink消息为SET_POSITION_TARGET_LOCAL_NED (#84), 对应的飞控中的uORB消息为position_setpoint_triplet.msg
70+
ros::Publisher setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
71+
72+
// 频率 [50Hz]
73+
ros::Rate rate(50.0);
74+
75+
// 先读取一些飞控的数据
76+
for(int i=0;i<50;i++)
77+
{
78+
ros::spinOnce();
79+
rate.sleep();
80+
}
81+
82+
// 初始化命令-
83+
// 默认设置：Idle模式 电机怠速旋转 等待来自上层的控制指令
84+
Command_Now.Mode = command_to_mavros::Idle;
85+
Command_Now.Command_ID = 0;
86+
Command_Now.Reference_State.Sub_mode = command_to_mavros::XYZ_POS;
87+
Command_Now.Reference_State.position_ref[0] = 0;
88+
Command_Now.Reference_State.position_ref[1] = 0;
89+
Command_Now.Reference_State.position_ref[2] = 0;
90+
Command_Now.Reference_State.velocity_ref[0] = 0;
91+
Command_Now.Reference_State.velocity_ref[1] = 0;
92+
Command_Now.Reference_State.velocity_ref[2] = 0;
93+
Command_Now.Reference_State.acceleration_ref[0] = 0;
94+
Command_Now.Reference_State.acceleration_ref[1] = 0;
95+
Command_Now.Reference_State.acceleration_ref[2] = 0;
96+
Command_Now.Reference_State.yaw_ref = 0;
97+
98+
99+
// 记录启控时间
100+
ros::Time begin_time = ros::Time::now();
101+
float last_time = px4_command_utils::get_time_in_sec(begin_time);
102+
float dt = 0;
103+
104+
mavros_msgs::PositionTarget pos_setpoint;
105+
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 循 环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
106+
while(ros::ok())
107+
{
108+
// 当前时间
109+
cur_time = px4_command_utils::get_time_in_sec(begin_time);
110+
dt = cur_time - last_time;
111+
dt = constrain_function2(dt, 0.01, 0.03);
112+
last_time = cur_time;
113+
114+
//执行回调函数
115+
ros::spinOnce();
116+
117+
cout <<">>>>>>>>>>>>>>>>>>>>>> px4_fw_controller <<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
118+
// 打印无人机状态
119+
px4_command_utils::prinft_drone_state(_DroneState);
120+
121+
// 打印上层控制指令
122+
px4_command_utils::printf_command_control(Command_Now);
123+
124+
125+
switch (Command_Now.Mode)
126+
{
127+
case command_to_mavros::Idle:
128+
129+
break;
130+
131+
case command_to_mavros::Takeoff:
132+
133+
break;
134+
135+
// 不支持复合模式
136+
case command_to_mavros::Move_ENU:
137+
138+
139+
//Bitmask toindicate which dimensions should be ignored (1 means ignore,0 means not ignore; Bit 10 must set to 0)
140+
//Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
141+
//Bit 10 should set to 0, means is not force sp
142+
pos_setpoint.type_mask = 0b110111000000; // 110 111 000 000 vxvyvz + xyz
143+
144+
pos_setpoint.coordinate_frame = 1;
145+
146+
pos_setpoint.position.x = Command_Now.Reference_State.position_ref[0];
147+
pos_setpoint.position.y = Command_Now.Reference_State.position_ref[1];
148+
pos_setpoint.position.z = Command_Now.Reference_State.position_ref[2];
149+
150+
151+
152+
pos_setpoint.velocity.x = 0.0;
153+
pos_setpoint.velocity.y = 0.0;
154+
pos_setpoint.velocity.z = Command_Now.Reference_State.yaw_ref /3.14 * 180;
155+
156+
setpoint_raw_local_pub.publish(pos_setpoint);
157+
158+
// 检查飞控是否收到控制量
159+
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
160+
cout << "Pos_target [X Y Z] : " << pos_drone_fcu_target[0] << " [ m ] "<< pos_drone_fcu_target[1]<<" [ m ] "<<pos_drone_fcu_target[2]<<" [ m ] "<<endl;
161+
cout << "Vel_target [X Y Z] : " << vel_drone_fcu_target[0] << " [m/s] "<< vel_drone_fcu_target[1]<<" [m/s] "<<vel_drone_fcu_target[2]<<" [m/s] "<<endl;
162+
163+
break;
164+
165+
// 不支持复合模式
166+
case command_to_mavros::Move_Body:
167+
168+
169+
break;
170+
171+
case command_to_mavros::Hold:
172+
173+
break;
174+
175+
176+
case command_to_mavros::Land:
177+
178+
break;
179+
180+
case command_to_mavros::Disarm:
181+
182+
break;
183+
184+
case command_to_mavros::PPN_land:
185+
186+
break;
187+
188+
}
189+
190+
191+
192+
Command_Last = Command_Now;
193+
194+
rate.sleep();
195+
}
196+
197+
return 0;
198+
199+
}

0 commit comments

Comments
 (0)