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

bugfixes and cleanup

Browse Source
This commit is contained in:
baumanta
2019-05-15 16:30:03 +02:00
committed by Mathieu Bresciani
parent 4212ae8b87
commit f50a1d58b0
2 changed files with 8 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+8 -15
View File
@@ -65,11 +65,6 @@ bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscriptio
return true;
}
bool CollisionPrevention::isZero(const Vector2f &vec, const float limit) const
{
return (vec(0) < limit && vec(0) > -limit) && (vec(1) < limit && vec(1) > -limit);
}
void CollisionPrevention::calculate_constrained_setpoint(const float max_acc, Vector2f &setpoint)
{
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get();
@@ -81,11 +76,12 @@ void CollisionPrevention::calculate_constrained_setpoint(const float max_acc, Ve
//Limit the deviation of the adapted setpoint from the originally given joystick input (slightly less than 90 degrees)
float max_slide_angle_rad = 1.2f;
if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && !isZero(setpoint, 1e-3)) {
if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && setpoint.norm() > 0.001f) {
int distances_array_size = sizeof(obstacle_distance.distances) / sizeof(obstacle_distance.distances[0]);
for (int i = 0; i < distances_array_size; i++) {
//determine if distance bin is valid and contains a valid distance measurement
if (obstacle_distance.distances[i] < obstacle_distance.max_distance &&
obstacle_distance.distances[i] > obstacle_distance.min_distance && i * obstacle_distance.increment < 360) {
@@ -93,11 +89,7 @@ void CollisionPrevention::calculate_constrained_setpoint(const float max_acc, Ve
float angle = math::radians((float)i * obstacle_distance.increment);
//max velocity squared in current bin direction
float vel_max_sqrd = max_acc * (distance - _param_mpc_col_prev_d.get());
if (distance < _param_mpc_col_prev_d.get()) {
vel_max_sqrd = 0.f;
}
float vel_max_sqrd = math::max(0.f, max_acc * (distance - _param_mpc_col_prev_d.get()));
//split current setpoint into parallel and orthogonal components with respect to the current bin direction
Vector2f bin_direction = {cos(angle), sin(angle)};
@@ -111,10 +103,11 @@ void CollisionPrevention::calculate_constrained_setpoint(const float max_acc, Ve
//limit sliding angle
float angle_diff_temp_orig = acos(setpoint_temp.dot(setpoint) / (setpoint_temp.norm() * setpoint.norm()));
float angle_diff_temp_bin = acos(setpoint_temp.dot(bin_direction) / (setpoint_temp.norm() * bin_direction.norm()));
if (angle_diff_temp_orig > max_slide_angle_rad) {
float angle_diff_bin = acos(sp_parallel / setpoint_sqrd.norm());
float orth_len = vel_max_sqrd * tan(angle_diff_bin + max_slide_angle_rad);
if (angle_diff_temp_orig > max_slide_angle_rad && setpoint_temp.norm() > 0.001f) {
float angle_temp_bin_cropped = angle_diff_temp_bin - (angle_diff_temp_orig - max_slide_angle_rad);
float orth_len = vel_max_sqrd * tan(angle_temp_bin_cropped);
if (sp_orth > 0) {
setpoint_temp = vel_max_sqrd * bin_direction + orth_len * orth_direction;
@@ -130,7 +123,7 @@ void CollisionPrevention::calculate_constrained_setpoint(const float max_acc, Ve
}
//take the squared root
if (!isZero(setpoint_sqrd, 1e-3)) {
if (setpoint_sqrd.norm() > 0.001f) {
setpoint = setpoint_sqrd.normalized() * std::sqrt(setpoint_sqrd.norm());
} else {
src/lib/CollisionPrevention/CollisionPrevention.hpp
-3
View File
@@ -74,7 +74,6 @@ private:
bool _interfering = false; /**< states if the collision prevention interferes with the user input */
orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
uORB::SubscriptionPollable<obstacle_distance_s> *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */
@@ -90,8 +89,6 @@ private:
void update();
bool isZero(const matrix::Vector2f &vec, const float limit) const;
void calculate_constrained_setpoint(const float max_acc, matrix::Vector2f &setpoint);
};