diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index e440cf07d9..c0734c4101 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -56,4 +56,4 @@ add_subdirectory(terrain_estimation) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(WeatherVane) -add_subdirectory(CollisionAvoidance) +add_subdirectory(CollisionPrevention) diff --git a/src/lib/CollisionAvoidance/CMakeLists.txt b/src/lib/CollisionPrevention/CMakeLists.txt similarity index 96% rename from src/lib/CollisionAvoidance/CMakeLists.txt rename to src/lib/CollisionPrevention/CMakeLists.txt index 8bf01178bb..b9ef18f7c3 100644 --- a/src/lib/CollisionAvoidance/CMakeLists.txt +++ b/src/lib/CollisionPrevention/CMakeLists.txt @@ -31,4 +31,4 @@ # ############################################################################ -px4_add_library(CollisionAvoidance CollisionAvoidance.cpp) +px4_add_library(CollisionPrevention CollisionPrevention.cpp) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp similarity index 88% rename from src/lib/CollisionAvoidance/CollisionAvoidance.cpp rename to src/lib/CollisionPrevention/CollisionPrevention.cpp index 9b712cc553..38e49a41a1 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -32,31 +32,33 @@ ****************************************************************************/ /** - * @file CollisionAvoidance.cpp - * CollisionAvoidance controller. + * @file CollisionPrevention.cpp + * CollisionPrevention controller. * */ -#include "CollisionAvoidance.hpp" +#include -CollisionAvoidance::CollisionAvoidance() : +CollisionPrevention::CollisionPrevention() : ModuleParams(nullptr) { } -CollisionAvoidance::~CollisionAvoidance(){ +CollisionPrevention::~CollisionPrevention() +{ //unadvertise publishers if (_constraints_pub != nullptr) { orb_unadvertise(_constraints_pub); } + if (_mavlink_log_pub != nullptr) { orb_unadvertise(_mavlink_log_pub); } } -bool CollisionAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array) +bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscription_array) { if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) { return false; @@ -65,7 +67,7 @@ bool CollisionAvoidance::initializeSubscriptions(SubscriptionArray &subscription return true; } -void CollisionAvoidance::reset_constraints() +void CollisionPrevention::reset_constraints() { _move_constraints_x_normalized(0) = 0.0f; //normalized constraint in negative x-direction @@ -80,7 +82,7 @@ void CollisionAvoidance::reset_constraints() } -void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) +void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) { collision_constraints_s constraints; /**< collision constraints message */ @@ -107,10 +109,10 @@ void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, } -void CollisionAvoidance::update() +void CollisionPrevention::update() { - // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter - if (collision_avoidance_enabled()) { + // activate/deactivate the collision prevention based on MPC_COL_PREV parameter + if (collision_prevention_enabled()) { activate(); } else { @@ -118,7 +120,7 @@ void CollisionAvoidance::update() } } -void CollisionAvoidance::update_range_constraints() +void CollisionPrevention::update_range_constraints() { obstacle_distance_s obstacle_distance = _sub_obstacle_distance->get(); @@ -132,8 +134,8 @@ void CollisionAvoidance::update_range_constraints() float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters float angle = i * obstacle_distance.increment * (M_PI / 180.0); //calculate normalized velocity reductions - float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * cos(angle); - float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * sin(angle); + 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); if (vel_lim_x > 0 && vel_lim_x > _move_constraints_x_normalized(1)) { _move_constraints_x_normalized(1) = vel_lim_x; } @@ -151,7 +153,7 @@ void CollisionAvoidance::update_range_constraints() } } -void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float max_speed) +void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed) { update(); reset_constraints(); @@ -191,7 +193,7 @@ void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float 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)); - //warn user if collision avoidance starts to interfere + //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)); diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp similarity index 86% rename from src/lib/CollisionAvoidance/CollisionAvoidance.hpp rename to src/lib/CollisionPrevention/CollisionPrevention.hpp index 66a72d75d4..ec82401bd9 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -32,10 +32,10 @@ ****************************************************************************/ /** - * @file CollisionAvoidance.hpp + * @file CollisionPrevention.hpp * @author Tanja Baumann * - * CollisionAvoidance controller. + * CollisionPrevention controller. * */ @@ -58,12 +58,12 @@ using uORB::Publication; using namespace matrix; using namespace time_literals; -class CollisionAvoidance : public ModuleParams +class CollisionPrevention : public ModuleParams { public: - CollisionAvoidance(); + CollisionPrevention(); - ~CollisionAvoidance(); + ~CollisionPrevention(); /** * Initialize the uORB subscriptions using an array @@ -77,7 +77,7 @@ public: bool is_active() {return _is_active;} - bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); } + bool collision_prevention_enabled() { return MPC_COL_PREV.get(); } void update(); @@ -92,7 +92,7 @@ public: private: bool _is_active = true; - bool _interfering = false; /**< states if the collision avoidance interferes with the user input */ + 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 */ @@ -110,8 +110,8 @@ private: Vector2f _move_constraints_y; DEFINE_PARAMETERS( - (ParamInt) MPC_COL_AVOID, /**< use range sensor measurements to avoid collision */ - (ParamFloat) MPC_COL_AVOID_D /**< collision avoidance keep minimum distance */ + (ParamInt) MPC_COL_PREV, /**< use range sensor measurements to prevent collision */ + (ParamFloat) MPC_COL_PREV_D /**< collision prevention keep minimum distance */ ) }; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 74457e8e92..a6d18786dc 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -47,7 +47,7 @@ bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscr return false; } - if (!_collision_avoidance.initializeSubscriptions(subscription_array)) { + if (!_collision_prevention.initializeSubscriptions(subscription_array)) { return false; } @@ -127,9 +127,9 @@ void FlightTaskManualPosition::_scaleSticks() /* Rotate setpoint into local frame. */ _rotateIntoHeadingFrame(vel_sp_xy); - // collision avoidance - if (_collision_avoidance.is_active()) { - _collision_avoidance.modifySetpoint(vel_sp_xy, _constraints.speed_xy); + // collision prevention + if (_collision_prevention.is_active()) { + _collision_prevention.modifySetpoint(vel_sp_xy, _constraints.speed_xy); } _velocity_setpoint(0) = vel_sp_xy(0); diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 59da9ee057..f32393a18f 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -40,8 +40,8 @@ #pragma once +#include #include "FlightTaskManualAltitude.hpp" -#include class FlightTaskManualPosition : public FlightTaskManualAltitude { @@ -77,5 +77,5 @@ private: WeatherVane *_weathervane_yaw_handler = nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - CollisionAvoidance _collision_avoidance; /**< collision avoidance setpoint amendment */ + CollisionPrevention _collision_prevention; /**< collision avoidance setpoint amendment */ }; diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 247a266a6c..f649cecbef 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -45,5 +45,5 @@ px4_add_module( git_ecl ecl_geo WeatherVane - CollisionAvoidance + CollisionPrevention ) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index a2e1d04542..f418ac5403 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -744,7 +744,7 @@ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); * @boolean * @group Multicopter Position Control */ -PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); +PARAM_DEFINE_INT32(MPC_COL_PREV, 0); /** * Minimum distance the vehicle should keep to all obstacles @@ -756,4 +756,4 @@ PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); * @unit meters * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_COL_AVOID_D, 4.0f); +PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, 4.0f);