dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

add parameter for detection angle

This commit is contained in:
baumanta 2019-07-29 17:51:08 +02:00 committed by Roman Bapst
parent 150b5df7cb
commit 14f128b89d
3 changed files with 26 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/lib/CollisionPrevention/CollisionPrevention.cpp
View File

@ -212,21 +212,22 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
angle += math::radians(obstacle.angle_offset); angle += math::radians(obstacle.angle_offset);
} }
//check if the bin must be considered regarding the given stick input
Vector2f bin_direction = {cos(angle), sin(angle)}; Vector2f bin_direction = {cos(angle), sin(angle)};
float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
//calculate max allowed velocity with a P-controller (same gain as in the position controller) if (setpoint_dir.dot(bin_direction) > 0 && setpoint_dir.dot(bin_direction) > cosf(_param_mpc_col_prev_ang.get())) {
float delay_distance = curr_vel_parallel * _param_mpc_col_prev_dly.get(); //calculate max allowed velocity with a P-controller (same gain as in the position controller)
float vel_max_posctrl = math::max(0.f, float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
_param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance)); float delay_distance = curr_vel_parallel * _param_mpc_col_prev_dly.get();
Vector2f vel_max_vec = bin_direction * vel_max_posctrl; float vel_max_posctrl = math::max(0.f,
float vel_max_bin = vel_max_vec.dot(setpoint_dir); _param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance));
float vel_setpoint_bin = setpoint.dot(bin_direction); Vector2f vel_max_vec = bin_direction * vel_max_posctrl;
float vel_max_bin = vel_max_vec.dot(setpoint_dir);
//limit the velocity //constrain the velocity
//do not react to obstacles more than 45 degree off the given stick input cos(45deg) = 0.71 if (vel_max_bin >= 0) {
if (vel_setpoint_bin > 0.71f * setpoint_length && vel_max_bin >= 0) { vel_max = math::min(vel_max, vel_max_bin);
vel_max = math::min(vel_max, vel_max_bin); }
} }
} }
} }

1
src/lib/CollisionPrevention/CollisionPrevention.hpp
View File

@ -94,6 +94,7 @@ private:
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */ (ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */
(ParamFloat<px4::params::MPC_COL_PREV_ANG>) _param_mpc_col_prev_ang, /**< collision prevention detection angle */
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/ (ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
(ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly /**< delay of the range measurement data*/ (ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly /**< delay of the range measurement data*/
) )

12
src/lib/CollisionPrevention/collisionprevention_params.c
View File

@ -62,3 +62,15 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f); PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f);
/**
* Angle at which the vehicle can fly away from obstacles
*
* Only used in Position mode.
*
* @min 0
* @max 1.570796
* @unit [ ]
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_COL_PREV_ANG, 0.785f);