diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp
index 8bd340987de39d291338284d5bcc41ddb079d85f..76a32e1c6f0a2d01cea6912aeb2d38349486b239 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.cpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp
@@ -38,10 +38,12 @@
*/
#include <CollisionPrevention/CollisionPrevention.hpp>
+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 11561c3bb502c5f2713ba0fa3489603328d7c144..527655611de859e5b19cd39265705015823a20a0 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.hpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp
@@ -55,13 +55,10 @@ using uORB::Publication;
#include <systemlib/mavlink_log.h>
#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
-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<obstacle_distance_s> *_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<px4::params::MPC_COL_PREV>) MPC_COL_PREV, /**< use range sensor measurements to prevent collision */
(ParamFloat<px4::params::MPC_COL_PREV_D>) 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 a518571f39275a6366fad3c04d8d1d59661390b7..1b4175396b6a8afe074d8388c94741a13b5a9f52 100644
--- a/src/lib/CollisionPrevention/collisionprevention_params.c
+++ b/src/lib/CollisionPrevention/collisionprevention_params.c
@@ -39,22 +39,14 @@
* @author Tanja Baumann <tanja@auterion.com>
*/
-/**
- * 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 a6d18786dcbc2fa4c15021eb5491fb2c76a49425..6b2ec40431d8793869e38ca17c67d7081fbec925 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 f32393a18f33cb43536b8576f256f62848a8df73..031c5cb1a423bbe13c46b5c2dfe6442753fc55f6 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;