fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 02:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: add execution of velocity waypoint coming from the

Browse Source 
obstacle avoidance module
This commit is contained in:
Martina
2018-07-12 16:36:11 -04:00
committed by Daniel Agar
parent f511d3a399
commit 5d6771753d
1 changed files with 48 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+48 -1
View File
@@ -115,6 +115,8 @@ private:
/** Time in us that direction change condition has to be true for direction change state */
static constexpr uint64_t DIRECTION_CHANGE_TRIGGER_TIME_US = 100000;
/** Timeout in us for trajectory data to get considered invalid */
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500000;
bool _task_should_exit = false; /**<true if task should exit */
bool _gear_state_initialized = false; /**<true if the gear state has been initialized */
@@ -405,6 +407,15 @@ private:
void set_idle_state();
/**
* trajctory generation
*/
void execute_avoidance_velocity_waypoint();
bool use_obstacle_avoidance();
bool use_vel_wp_avoidance();
/**
* Temporary method for flight control compuation
*/
@@ -2480,13 +2491,21 @@ MulticopterPositionControl::calculate_velocity_setpoint()
}
constrain_velocity_setpoint();
/* check obstacle avoidance */
if (use_vel_wp_avoidance() && !_in_smooth_takeoff) {
execute_avoidance_velocity_waypoint();
} else {
_vel_sp_prev = _vel_sp;
}
/* make sure velocity setpoint is constrained in all directions (xyz) */
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
/* check if the velocity demand is significant */
_vel_sp_significant = vel_norm_xy > 0.5f * _vel_max_xy;
_vel_sp_prev = _vel_sp;
}
void
@@ -3353,6 +3372,34 @@ MulticopterPositionControl::set_idle_state()
_att_sp.thrust = 0.0f;
}
void
MulticopterPositionControl::execute_avoidance_velocity_waypoint()
{
_vel_sp(0) = _traj_wp_avoidance.point_0[trajectory_waypoint_s::VX];
_vel_sp(1) = _traj_wp_avoidance.point_0[trajectory_waypoint_s::VY];
_vel_sp(2) = _traj_wp_avoidance.point_0[trajectory_waypoint_s::VZ];
/* we always constrain velocity since we do not know what the avoidance module sends out */
constrain_velocity_setpoint();
}
bool
MulticopterPositionControl::use_obstacle_avoidance()
{
/* check that external obstacle avoidance is sending data and that the first point is valid */
return (hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) <
TRAJECTORY_STREAM_TIMEOUT_US && (_traj_wp_avoidance.point_valid[trajectory_waypoint_s::POINT_0] == true));
}
bool
MulticopterPositionControl::use_vel_wp_avoidance()
{
return use_obstacle_avoidance() && PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::VX]) &&
PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::VY])
&& PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::VZ]);
}
void
MulticopterPositionControl::updateConstraints(Controller::Constraints &constraints)
{