yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Collision prevention improvements (#11866)

Browse Source 
* match max vel in col-prev to regular pos ctrl
* change warning criterium to avoid float error
* disable push back from obstacles
* use floats consistently
This commit is contained in:
baumanta
2019-04-17 19:20:29 +02:00
committed by Daniel Agar
parent 463b8a7528
commit c0a1f5b72f
2 changed files with 15 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -149,31 +149,19 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
//calculate movement constraints based on range data
update_range_constraints();
_move_constraints_x = _move_constraints_x_normalized;
_move_constraints_y = _move_constraints_y_normalized;
// calculate the maximum velocity along x,y axis when moving in the demanded direction
float vel_mag = original_setpoint.norm();
float v_max_x, v_max_y;
if (vel_mag > 0.0f) {
v_max_x = abs(max_speed / vel_mag * original_setpoint(0));
v_max_y = abs(max_speed / vel_mag * original_setpoint(1));
} else {
v_max_x = 0.0f;
v_max_y = 0.0f;
}
//scale the velocity reductions with the maximum possible velocity along the respective axis
_move_constraints_x *= v_max_x;
_move_constraints_y *= v_max_y;
//clamp constraints to be in [0,1]. Constraints > 1 occur if the vehicle is closer than _param_mpc_col_prev_d to the obstacle.
//they would lead to the vehicle being pushed back from the obstacle which we do not yet support
_move_constraints_x_normalized(0) = math::constrain(_move_constraints_x_normalized(0), 0.f, 1.f);
_move_constraints_x_normalized(1) = math::constrain(_move_constraints_x_normalized(1), 0.f, 1.f);
_move_constraints_y_normalized(0) = math::constrain(_move_constraints_y_normalized(0), 0.f, 1.f);
_move_constraints_y_normalized(1) = math::constrain(_move_constraints_y_normalized(1), 0.f, 1.f);
//apply the velocity reductions to form velocity limits
_move_constraints_x(0) = v_max_x - _move_constraints_x(0);
_move_constraints_x(1) = v_max_x - _move_constraints_x(1);
_move_constraints_y(0) = v_max_y - _move_constraints_y(0);
_move_constraints_y(1) = v_max_y - _move_constraints_y(1);
_move_constraints_x(0) = max_speed * (1.f - _move_constraints_x_normalized(0));
_move_constraints_x(1) = max_speed * (1.f - _move_constraints_x_normalized(1));
_move_constraints_y(0) = max_speed * (1.f - _move_constraints_y_normalized(0));
_move_constraints_y(1) = max_speed * (1.f - _move_constraints_y_normalized(1));
//constrain the velocity setpoint to respect the velocity limits
Vector2f new_setpoint;
@@ -181,9 +169,10 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
new_setpoint(1) = math::constrain(original_setpoint(1), -_move_constraints_y(0), _move_constraints_y(1));
//warn user if collision prevention starts to interfere
bool currently_interfering = (new_setpoint(0) < 0.95f * original_setpoint(0)
|| new_setpoint(0) > 1.05f * original_setpoint(0) || new_setpoint(1) < 0.95f * original_setpoint(1)
|| new_setpoint(1) > 1.05f * original_setpoint(1));
bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
|| new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
|| new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
|| new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);
if (currently_interfering && (currently_interfering != _interfering)) {
mavlink_log_critical(&_mavlink_log_pub, "Collision Warning");

2
src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
View File

@@ -134,7 +134,7 @@ void FlightTaskManualPosition::_scaleSticks()
// collision prevention
if (_collision_prevention.is_active()) {
_collision_prevention.modifySetpoint(vel_sp_xy, _constraints.speed_xy);
_collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale);
}
_velocity_setpoint(0) = vel_sp_xy(0);