Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

mc_pos_control: add vel_sp_desired to send desired velocity to the

Browse Source 
obstacle avoidance
This commit is contained in:
Martina
2018-04-05 15:36:04 +02:00
committed by Daniel Agar
parent 47f2db67b6
commit b33a708215
1 changed files with 11 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
+11
View File
@@ -285,6 +285,7 @@ private:
matrix::Vector3f _vel_prev; /**< velocity on previous step */
matrix::Vector3f _vel_sp_prev;
matrix::Vector3f _vel_err_d; /**< derivative of current velocity */
matrix::Vector3f _vel_sp_desired; /**< the desired velocity can be overwritten by obstacle avoidance */
matrix::Vector3f _curr_pos_sp; /**< current setpoint of the triplets */
matrix::Vector3f _prev_pos_sp; /**< previous setpoint of the triples */
matrix::Vector2f _stick_input_xy_prev; /**< for manual controlled mode to detect direction change */
@@ -519,6 +520,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_prev.zero();
_vel_sp_prev.zero();
_vel_err_d.zero();
_vel_sp_desired.zero();
_curr_pos_sp.zero();
_prev_pos_sp.zero();
_stick_input_xy_prev.zero();
@@ -2529,12 +2531,21 @@ MulticopterPositionControl::calculate_velocity_setpoint()
set_takeoff_velocity(_vel_sp(2));
}
/* constrain velocity: this is desired velocity before any obstacle avoidance */
constrain_velocity_setpoint();
_vel_sp_desired = matrix::Vector3f(_vel_sp(0), _vel_sp(1), _vel_sp(2));
/* check obstacle avoidance */
if (use_vel_wp_avoidance() && !_in_smooth_takeoff) {
execute_avoidance_velocity_waypoint();
/* If obstacle avoidanc is active, the previous velocity
* setpoint will be set to desired setpoint to ensure that setpoint
* increases linearly with acceleration.
*/
_vel_sp_prev = _vel_sp_desired;
} else {
_vel_sp_prev = _vel_sp;
}