fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 00:11:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: add execution of position waypoint coming from the

Browse Source 
obstacle avoidance
This commit is contained in:
Martina
2018-07-12 16:36:11 -04:00
committed by Daniel Agar
parent 5d6771753d
commit df19610e69
1 changed files with 24 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+24
View File
@@ -410,10 +410,14 @@ private:
/** /**
* trajctory generation * trajctory generation
*/ */
void execute_avoidance_position_waypoint();
void execute_avoidance_velocity_waypoint(); void execute_avoidance_velocity_waypoint();
bool use_obstacle_avoidance(); bool use_obstacle_avoidance();
bool use_pos_wp_avoidance();
bool use_vel_wp_avoidance(); bool use_vel_wp_avoidance();
/** /**
@@ -2396,6 +2400,10 @@ MulticopterPositionControl::control_position()
void void
MulticopterPositionControl::calculate_velocity_setpoint() MulticopterPositionControl::calculate_velocity_setpoint()
{ {
if (use_pos_wp_avoidance()) {
execute_avoidance_position_waypoint();
}
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) { if (_run_pos_control) {
@@ -3372,6 +3380,14 @@ MulticopterPositionControl::set_idle_state()
_att_sp.thrust = 0.0f; _att_sp.thrust = 0.0f;
} }
void
MulticopterPositionControl::execute_avoidance_position_waypoint()
{
_pos_sp(0) = _traj_wp_avoidance.point_0[trajectory_waypoint_s::X];
_pos_sp(1) = _traj_wp_avoidance.point_0[trajectory_waypoint_s::Y];
_pos_sp(2) = _traj_wp_avoidance.point_0[trajectory_waypoint_s::Z];
}
void void
MulticopterPositionControl::execute_avoidance_velocity_waypoint() MulticopterPositionControl::execute_avoidance_velocity_waypoint()
{ {
@@ -3392,6 +3408,14 @@ MulticopterPositionControl::use_obstacle_avoidance()
TRAJECTORY_STREAM_TIMEOUT_US && (_traj_wp_avoidance.point_valid[trajectory_waypoint_s::POINT_0] == true)); TRAJECTORY_STREAM_TIMEOUT_US && (_traj_wp_avoidance.point_valid[trajectory_waypoint_s::POINT_0] == true));
} }
bool
MulticopterPositionControl::use_pos_wp_avoidance()
{
return use_obstacle_avoidance() && PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::X]) &&
PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::Y])
&& PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::Z]);
}
bool bool
MulticopterPositionControl::use_vel_wp_avoidance() MulticopterPositionControl::use_vel_wp_avoidance()
{ {