Skip to content

Commit 79bb056

Browse files
473867143473867143
committed
Add a new landing strategy, use the Land_mode parameter in px4_sender.yaml
1 parent abb9651 commit 79bb056

4 files changed

Lines changed: 95 additions & 34 deletions

File tree

‎Modules/control/src/px4_sender/px4_sender.cpp‎

Lines changed: 71 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@ float cur_time; //程序运行时间
3737
float Takeoff_height; //默认起飞高度
3838
float Disarm_height; //自动上锁高度
3939
float Land_speed; //降落速度
40+
int Land_mode; //降落策略选择
4041
//Geigraphical fence 地理围栏
4142
Eigen::Vector2f geo_fence_x;
4243
Eigen::Vector2f geo_fence_y;
@@ -137,6 +138,7 @@ int main(int argc, char **argv)
137138
nh.param<float>("Takeoff_height", Takeoff_height, 1.0);
138139
nh.param<float>("Disarm_height", Disarm_height, 0.15);
139140
nh.param<float>("Land_speed", Land_speed, 0.2);
141+
nh.param<int>("Land_mode",Land_mode,0);
140142

141143
nh.param<float>("geo_fence/x_min", geo_fence_x[0], -100.0);
142144
nh.param<float>("geo_fence/x_max", geo_fence_x[1], 100.0);
@@ -283,42 +285,80 @@ int main(int argc, char **argv)
283285

284286
break;
285287

286-
// 【Land】 降落。当前位置原地降落，降落后会自动上锁，且切换为mannual模式
288+
// 【Land】 降落。两种降落方式： 只有加载了参数Land_mode为1时，启用第二种降落方式；默认启用第一种降落方式。
289+
// 第一种：当前位置原地降落，降落后会自动上锁，且切换为mannual模式
290+
// 第二种：当前位置原地降落，降落中到达Disarm_height后，切换为飞控中land模式
287291
case prometheus_msgs::ControlCommand::Land:
288-
289-
if (Command_Last.Mode != prometheus_msgs::ControlCommand::Land)
290-
{
291-
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
292-
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
293-
Command_Now.Reference_State.position_ref[0] = _DroneState.position[0];
294-
Command_Now.Reference_State.position_ref[1] = _DroneState.position[1];
295-
Command_Now.Reference_State.yaw_ref = _DroneState.attitude[2]; //rad
296-
}
297292

298-
if(_DroneState.position[2] > Disarm_height)
299-
{
300-
Command_Now.Reference_State.position_ref[2] = _DroneState.position[2] - Land_speed * dt ;
301-
Command_Now.Reference_State.velocity_ref[0] = 0.0;
302-
Command_Now.Reference_State.velocity_ref[1] = 0.0;
303-
Command_Now.Reference_State.velocity_ref[2] = - Land_speed; //Land_speed
293+
if(Land_mode == 1){
294+
if (Command_Last.Mode != prometheus_msgs::ControlCommand::Land)
295+
{
296+
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XY_POS_Z_VEL;
297+
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
298+
Command_Now.Reference_State.position_ref[0] = _DroneState.position[0];
299+
Command_Now.Reference_State.position_ref[1] = _DroneState.position[1];
300+
Command_Now.Reference_State.yaw_ref = _DroneState.attitude[2]; //rad
301+
}
302+
//如果距离起飞高度小于30厘米，则直接切换为land模式；
303+
if(_DroneState.position[2] <= Disarm_height)
304+
{
305+
if(_DroneState.mode != "AUTO.LAND") // 无效
306+
{
307+
//此处切换会manual模式是因为:PX4默认在offboard模式且有控制的情况下没法上锁,直接使用飞控中的land模式
308+
_command_to_mavros.mode_cmd.request.custom_mode = "AUTO.LAND";
309+
_command_to_mavros.set_mode_client.call(_command_to_mavros.mode_cmd);
310+
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "LAND: inter AUTO LAND filght mode");
311+
}
312+
}
313+
else if(_DroneState.position[2] > Disarm_height)
314+
{
315+
Command_Now.Reference_State.position_ref[2] = _DroneState.position[2] - Land_speed * dt ;
316+
Command_Now.Reference_State.velocity_ref[0] = 0.0;
317+
Command_Now.Reference_State.velocity_ref[1] = 0.0;
318+
Command_Now.Reference_State.velocity_ref[2] = - Land_speed; //Land_speed
319+
state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
320+
state_sp_extra = Eigen::Vector3d(0.0,0.0,Command_Now.Reference_State.velocity_ref[2]);
321+
yaw_sp = Command_Now.Reference_State.yaw_ref;
322+
_command_to_mavros.send_pos_vel_xyz_setpoint(state_sp,state_sp_extra,yaw_sp);
323+
}
324+
325+
if(_DroneState.landed)
326+
{
327+
Command_Now.Mode = prometheus_msgs::ControlCommand::Idle;
328+
}
329+
}else{
330+
if (Command_Last.Mode != prometheus_msgs::ControlCommand::Land)
331+
{
332+
Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
333+
Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
334+
Command_Now.Reference_State.position_ref[0] = _DroneState.position[0];
335+
Command_Now.Reference_State.position_ref[1] = _DroneState.position[1];
336+
Command_Now.Reference_State.yaw_ref = _DroneState.attitude[2]; //rad
337+
}
338+
if(_DroneState.position[2] > Disarm_height)
339+
{
340+
Command_Now.Reference_State.position_ref[2] = _DroneState.position[2] - Land_speed * dt ;
341+
Command_Now.Reference_State.velocity_ref[0] = 0.0;
342+
Command_Now.Reference_State.velocity_ref[1] = 0.0;
343+
Command_Now.Reference_State.velocity_ref[2] = - Land_speed; //Land_speed
304344

305-
state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1], Command_Now.Reference_State.position_ref[2] );
306-
state_sp_extra = Eigen::Vector3d(0.0, 0.0 , Command_Now.Reference_State.velocity_ref[2]);
307-
yaw_sp = Command_Now.Reference_State.yaw_ref;
308-
_command_to_mavros.send_pos_vel_xyz_setpoint(state_sp, state_sp_extra, yaw_sp);
309-
}else
310-
{
311-
//此处切换会manual模式是因为:PX4默认在offboard模式且有控制的情况下没法上锁,直接使用飞控中的land模式
312-
_command_to_mavros.mode_cmd.request.custom_mode = "MANUAL";
313-
_command_to_mavros.set_mode_client.call(_command_to_mavros.mode_cmd);
345+
state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1], Command_Now.Reference_State.position_ref[2] );
346+
state_sp_extra = Eigen::Vector3d(0.0, 0.0 , Command_Now.Reference_State.velocity_ref[2]);
347+
yaw_sp = Command_Now.Reference_State.yaw_ref;
348+
_command_to_mavros.send_pos_vel_xyz_setpoint(state_sp, state_sp_extra, yaw_sp);
349+
}else
350+
{
351+
//此处切换会manual模式是因为:PX4默认在offboard模式且有控制的情况下没法上锁,直接使用飞控中的land模式
352+
_command_to_mavros.mode_cmd.request.custom_mode = "MANUAL";
353+
_command_to_mavros.set_mode_client.call(_command_to_mavros.mode_cmd);
314354

315-
_command_to_mavros.arm_cmd.request.value = false;
316-
_command_to_mavros.arming_client.call(_command_to_mavros.arm_cmd);
317-
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Disarming...");
355+
_command_to_mavros.arm_cmd.request.value = false;
356+
_command_to_mavros.arming_client.call(_command_to_mavros.arm_cmd);
357+
pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "Disarming...");
318358

319-
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "LAND: switch to MANUAL filght mode");
359+
pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "LAND: switch to MANUAL filght mode");
360+
}
320361
}
321-
322362
break;
323363

324364
case prometheus_msgs::ControlCommand::Move:
@@ -500,6 +540,7 @@ void printf_param()
500540
cout << "Takeoff_height : "<< Takeoff_height<<" [m] "<<endl;
501541
cout << "Disarm_height : "<< Disarm_height <<" [m] "<<endl;
502542
cout << "Land_speed : "<< Land_speed <<" [m/s] "<<endl;
543+
cout << "Land_mode : "<< Land_mode << endl;
503544
cout << "geo_fence_x : "<< geo_fence_x[0] << " [m] to "<<geo_fence_x[1] << " [m]"<< endl;
504545
cout << "geo_fence_y : "<< geo_fence_y[0] << " [m] to "<<geo_fence_y[1] << " [m]"<< endl;
505546
cout << "geo_fence_z : "<< geo_fence_z[0] << " [m] to "<<geo_fence_z[1] << " [m]"<< endl;

‎Simulator/gazebo_simulator/config/prometheus_control_config/px4_sender.yaml‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
## parameter for px4_sender.cpp
2+
3+
## 起飞高度,建议设置小一点
4+
## 2020.11.04测试发现飞机起飞有较大超调，未发现原因，但起飞后控制正常
5+
Takeoff_height : 0.5
6+
## 降落速度
7+
Land_speed : 0.2
8+
##　上锁高度
9+
Disarm_height : 0.25
10+
## 降落模式选择
11+
Land_mode : 1
12+
13+
##　地理围栏，根据实际情况设置
14+
geo_fence:
15+
x_min: -50
16+
x_max: 50
17+
y_min: -50
18+
y_max: 50
19+
z_min: -0.5
20+
z_max: 50

‎Simulator/gazebo_simulator/launch/sitl_px4_sender.launch‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -42,11 +42,11 @@
4242
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_ros/scripts/spawn_model -->
4343
<arg name="x" default="1.0"/>
4444
<arg name="y" default="1.0"/>
45-
<arg name="z" default="0.2"/>
45+
<arg name="z" default="0.16"/>
4646
<arg name="R" default="0"/>
4747
<arg name="P" default="0"/>
4848
<arg name="Y" default="0.0"/>
49-
<arg name="sdf" default="$(find prometheus_gazebo)/models/P300_basic/P300_basic.sdf"/>
49+
<arg name="sdf" default="$(find prometheus_gazebo)/models/AmovUAV/P300_amov.sdf"/>
5050
<arg name="model" default="P300_basic"/>
5151
<node name="$(anon vehicle_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen"
5252
args="-sdf -file $(arg sdf) -model $(arg model) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)">
@@ -82,7 +82,7 @@
8282

8383
<!-- run the px4_sender.cpp -->
8484
<node pkg="prometheus_control" type="px4_sender" name="px4_sender" output="screen">
85-
<rosparam command="load" file="$(find prometheus_gazebo)/config/prometheus_control_config/px4_pos_controller.yaml"/>
85+
<rosparam command="load" file="$(find prometheus_gazebo)/config/prometheus_control_config/px4_sender.yaml"/>
8686
</node>
8787

8888
<!-- run the ground_station.cpp -->

‎Simulator/gazebo_simulator/launch_control/sitl_px4_sender_control.launch‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<!-- Launch Gazebo Simulation -->
33
<arg name="x" default="0"/>
44
<arg name="y" default="0"/>
5-
<arg name="z" default="0.5"/>
5+
<arg name="z" default="0.16"/>
66
<arg name="world" default="$(find prometheus_gazebo)/worlds/empty.world"/>
77
<arg name="sdf" default="$(find prometheus_gazebo)/models/AmovUAV/P300_amov.sdf"/>
88
<arg name="model" default="P300_basic"/>

0 commit comments

Comments
 (0)