diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 8bd340987d..76a32e1c6f 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -38,10 +38,12 @@ */ #include +using namespace matrix; +using namespace time_literals; -CollisionPrevention::CollisionPrevention() : - ModuleParams(nullptr) +CollisionPrevention::CollisionPrevention(ModuleParams *parent) : + ModuleParams(parent) { } @@ -105,17 +107,20 @@ void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint, void CollisionPrevention::update_range_constraints() { - obstacle_distance_s obstacle_distance = _sub_obstacle_distance->get(); + const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get(); if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { float max_detection_distance = obstacle_distance.max_distance / 100.0f; //convert to meters - for (int i = 0; i < 72; i++) { + 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) { float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters - float angle = i * obstacle_distance.increment * (M_PI / 180.0); + float angle = math::radians((float)i * obstacle_distance.increment); + //calculate normalized velocity reductions float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * cos(angle); float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * sin(angle); @@ -146,7 +151,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa _move_constraints_y = _move_constraints_y_normalized; // calculate the maximum velocity along x,y axis when moving in the demanded direction - float vel_mag = sqrt(original_setpoint(0) * original_setpoint(0) + original_setpoint(1) * original_setpoint(1)); + float vel_mag = original_setpoint.norm(); float v_max_x, v_max_y; if (vel_mag > 0.0f) { @@ -169,7 +174,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa _move_constraints_y(1) = v_max_y - _move_constraints_y(1); //constrain the velocity setpoint to respect the velocity limits - Vector2f new_setpoint = original_setpoint; + Vector2f new_setpoint; new_setpoint(0) = math::constrain(original_setpoint(0), -_move_constraints_x(0), _move_constraints_x(1)); new_setpoint(1) = math::constrain(original_setpoint(1), -_move_constraints_y(0), _move_constraints_y(1)); diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 11561c3bb5..527655611d 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -55,13 +55,10 @@ using uORB::Publication; #include #include -using namespace matrix; -using namespace time_literals; - class CollisionPrevention : public ModuleParams { public: - CollisionPrevention(); + CollisionPrevention(ModuleParams *parent); ~CollisionPrevention(); @@ -71,17 +68,9 @@ public: */ bool initializeSubscriptions(SubscriptionArray &subscription_array); - bool is_active() {return MPC_COL_PREV.get();} - - void update(); - - void update_range_constraints(); - - void reset_constraints(); - - void publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint); + bool is_active() {return MPC_COL_PREV_D.get() > 0 ;} - void modifySetpoint(Vector2f &original_setpoint, const float max_speed); + void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed); private: @@ -94,18 +83,25 @@ private: uORB::Subscription *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */ static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000; - static constexpr uint64_t MESSAGE_THROTTLE_US = 5_s; + static constexpr uint64_t MESSAGE_THROTTLE_US = 5000000; hrt_abstime _last_message; - Vector2f _move_constraints_x_normalized; - Vector2f _move_constraints_y_normalized; - Vector2f _move_constraints_x; - Vector2f _move_constraints_y; + matrix::Vector2f _move_constraints_x_normalized; + matrix::Vector2f _move_constraints_y_normalized; + matrix::Vector2f _move_constraints_x; + matrix::Vector2f _move_constraints_y; DEFINE_PARAMETERS( - (ParamInt) MPC_COL_PREV, /**< use range sensor measurements to prevent collision */ (ParamFloat) MPC_COL_PREV_D /**< collision prevention keep minimum distance */ ) + void update(); + + void update_range_constraints(); + + void reset_constraints(); + + void publish_constraints(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + }; diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index a518571f39..1b4175396b 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -39,22 +39,14 @@ * @author Tanja Baumann */ -/** - * Flag to enable the use of a MAVLink range sensor for collision avoidance. - * - * @boolean - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_COL_PREV, 0); - /** * Minimum distance the vehicle should keep to all obstacles * - * Only used in Position mode when collision avoidance is active (see MPC_COL_AVOID). + * Only used in Position mode. Collision avoidace is disable by setting this parameter to a negative value * - * @min 0 + * @min -1 * @max 15 * @unit meters * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, 4.0f); +PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f); diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index a6d18786dc..6b2ec40431 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -41,6 +41,11 @@ using namespace matrix; +FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this) +{ + +} + bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscription_array) { if (!FlightTaskManualAltitude::initializeSubscriptions(subscription_array)) { diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index f32393a18f..031c5cb1a4 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -46,7 +46,7 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude { public: - FlightTaskManualPosition() = default; + FlightTaskManualPosition(); virtual ~FlightTaskManualPosition() = default; bool initializeSubscriptions(SubscriptionArray &subscription_array) override;