From a8a55d41274fad201d926c5373c5e782e11341a8 Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 30 Oct 2018 16:16:56 +0100 Subject: [PATCH 01/10] Add a collision avoidance library which uses range data --- msg/CMakeLists.txt | 1 + msg/collision_constraints.msg | 12 ++ src/lib/CMakeLists.txt | 1 + src/lib/CollisionAvoidance/CMakeLists.txt | 34 ++++ .../CollisionAvoidance/CollisionAvoidance.cpp | 175 ++++++++++++++++++ .../CollisionAvoidance/CollisionAvoidance.hpp | 110 +++++++++++ src/lib/FlightTasks/FlightTasks.hpp | 6 + .../tasks/FlightTask/FlightTask.hpp | 7 + .../FlightTaskManualPosition.cpp | 6 + .../FlightTaskManualPosition.hpp | 7 + src/modules/logger/logger.cpp | 2 + src/modules/mc_pos_control/CMakeLists.txt | 1 + .../mc_pos_control/mc_pos_control_main.cpp | 34 ++++ .../mc_pos_control/mc_pos_control_params.c | 19 ++ 14 files changed, 415 insertions(+) create mode 100644 msg/collision_constraints.msg create mode 100644 src/lib/CollisionAvoidance/CMakeLists.txt create mode 100644 src/lib/CollisionAvoidance/CollisionAvoidance.cpp create mode 100644 src/lib/CollisionAvoidance/CollisionAvoidance.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5780155febd8..c66a74858756 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -42,6 +42,7 @@ set(msg_files camera_capture.msg camera_trigger.msg collision_report.msg + collision_constraints.msg commander_state.msg cpuload.msg debug_array.msg diff --git a/msg/collision_constraints.msg b/msg/collision_constraints.msg new file mode 100644 index 000000000000..4c08ac801ddd --- /dev/null +++ b/msg/collision_constraints.msg @@ -0,0 +1,12 @@ +# Local setpoint constraints in NED frame +# setting something to NaN means that no limit is provided + +uint64 timestamp # time since system start (microseconds) + +#value of 0 means no constraint along this axis, value of 1 means no movements alowed, value bigger than 1 forces negative movement. +float32[2] constraints_normalized_x # constraints determined by range sensor measurements [x negative, x positive] +float32[2] constraints_normalized_y # constraintss determined by range sensor measurements [y negative, y positive] + +float32[2] original_setpoint # velocities demanded +float32[2] adapted_setpoint # velocities allowed + diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 45956ee7f8b5..e440cf07d971 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -56,3 +56,4 @@ add_subdirectory(terrain_estimation) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(WeatherVane) +add_subdirectory(CollisionAvoidance) diff --git a/src/lib/CollisionAvoidance/CMakeLists.txt b/src/lib/CollisionAvoidance/CMakeLists.txt new file mode 100644 index 000000000000..8bf01178bbe0 --- /dev/null +++ b/src/lib/CollisionAvoidance/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(CollisionAvoidance CollisionAvoidance.cpp) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp new file mode 100644 index 000000000000..823719c0c390 --- /dev/null +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file CollisionAvoidance.cpp + * CollisionAvoidance controller. + * + */ + +#include "CollisionAvoidance.hpp" + + +CollisionAvoidance::CollisionAvoidance() : + ModuleParams(nullptr) +{ + +} + +void CollisionAvoidance::reset_constraints() +{ + + _move_constraints_x_normalized(0) = 0.0f; //normalized constraint in negative x-direction + _move_constraints_x_normalized(1) = 0.0f; //normalized constraint in positive x-direction + _move_constraints_y_normalized(0) = 0.0f; //normalized constraint in negative y-direction + _move_constraints_y_normalized(1) = 0.0f; //normalized constraint in positive y-direction + + _move_constraints_x(0) = 0.0f; //constraint in negative x-direction + _move_constraints_x(1) = 0.0f; //constraint in positive x-direction + _move_constraints_y(0) = 0.0f; //constraint in negative y-direction + _move_constraints_y(1) = 0.0f; //constraint in positive y-direction + +} + +void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) +{ + + collision_constraints_s constraints; /**< collision constraints message */ + + //fill in values + constraints.timestamp = hrt_absolute_time(); + constraints.constraints_normalized_x[0] = _move_constraints_x_normalized(0); + constraints.constraints_normalized_x[1] = _move_constraints_x_normalized(1); + constraints.constraints_normalized_y[0] = _move_constraints_y_normalized(0); + constraints.constraints_normalized_y[1] = _move_constraints_y_normalized(1); + + constraints.original_setpoint[0] = original_setpoint(0); + constraints.original_setpoint[1] = original_setpoint(1); + constraints.adapted_setpoint[0] = adapted_setpoint(0); + constraints.adapted_setpoint[1] = adapted_setpoint(1); + + // publish constraints + if (_constraints_pub != nullptr) { + orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints); + + } else { + _constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints); + } + +} + +void CollisionAvoidance::update_range_constraints() +{ + if (hrt_elapsed_time((hrt_abstime *)&_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++) { + //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); + //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); + + if (vel_lim_x > 0 && vel_lim_x > _move_constraints_x_normalized(1)) { _move_constraints_x_normalized(1) = vel_lim_x; } + + if (vel_lim_y > 0 && vel_lim_y > _move_constraints_y_normalized(1)) { _move_constraints_y_normalized(1) = vel_lim_y; } + + if (vel_lim_x < 0 && -vel_lim_x > _move_constraints_x_normalized(0)) { _move_constraints_x_normalized(0) = -vel_lim_x; } + + if (vel_lim_y < 0 && -vel_lim_y > _move_constraints_y_normalized(0)) { _move_constraints_y_normalized(0) = -vel_lim_y; } + } + } + + } else if (_last_message + MESSAGE_THROTTLE_US < hrt_absolute_time()) { + mavlink_log_critical(&_mavlink_log_pub, "No range data received"); + _last_message = hrt_absolute_time(); + } +} + +void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float max_speed) +{ + + reset_constraints(); + + //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 = sqrt(original_setpoint(0) * original_setpoint(0) + original_setpoint(1) * original_setpoint(1)); + 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(0) *= v_max_x; + _move_constraints_x(1) *= v_max_x; + _move_constraints_y(0) *= v_max_y; + _move_constraints_y(1) *= v_max_y; + + //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); + + //constrain the velocity setpoint to respect the velocity limits + Vector2f new_setpoint = original_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)); + + //warn user if collision avoidance 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)); + + if (currently_interfering && (currently_interfering != _interfering)) { + mavlink_log_critical(&_mavlink_log_pub, "Collision Avoidance starts interfering"); + } + + _interfering = currently_interfering; + + publish_constraints(original_setpoint, new_setpoint); + original_setpoint = new_setpoint; +} diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp new file mode 100644 index 000000000000..bf1f5539c219 --- /dev/null +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file CollisionAvoidance.hpp + * @author Tanja Baumann + * + * CollisionAvoidance controller. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +using uORB::Publication; +#include +#include + +using namespace matrix; +using namespace time_literals; + +class CollisionAvoidance : public ModuleParams +{ +public: + CollisionAvoidance(); + + ~CollisionAvoidance() = default; + + void activate() {_is_active = true;} + + void deactivate() {_is_active = false;} + + bool is_active() {return _is_active;} + + bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); } + + void update(const obstacle_distance_s &distance_measurements) {_obstacle_distance = distance_measurements;} + + void update_range_constraints(); + + void reset_constraints(); + + void publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint); + + void modifySetpoint(Vector2f &original_setpoint, const float max_speed); + +private: + + obstacle_distance_s _obstacle_distance{}; /**< obstacle distances received form a range sensor */ + + bool _is_active = true; + bool _interfering = false; /**< states if the collision avoidance interferes with the user input */ + + orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */ + orb_advert_t _mavlink_log_pub = nullptr; /**< Mavlink log uORB handle */ + + static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000; + static constexpr uint64_t MESSAGE_THROTTLE_US = 5_s; + + hrt_abstime _last_message; + + Vector2f _move_constraints_x_normalized; + Vector2f _move_constraints_y_normalized; + Vector2f _move_constraints_x; + 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 */ + ) + +}; diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 3a07d25ae349..5cca46e0626f 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -45,6 +45,7 @@ #include "SubscriptionArray.hpp" #include "FlightTasks_generated.hpp" #include +#include #include @@ -131,6 +132,11 @@ class FlightTasks */ void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);} + /** + * Sets an external collision avoidance. The active flight task can use the the collision avoidance to modify the setpoint. + */ + void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {_current_task.task->setCollisionAvoidance(ext_collision_avoidance);} + /** * This method will re-activate current task. */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 9efe587ee47d..30ef82a3fde8 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include "SubscriptionArray.hpp" class FlightTask : public ModuleParams @@ -154,6 +155,12 @@ class FlightTask : public ModuleParams void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; } + + /** + * Sets an external collision avoidance which can be used by any flight task to implement a different setpoint + * This method does nothing, each flighttask which wants to use the collision avoidance needs to override this method. + */ + virtual void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {}; protected: diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 12bc4586dea8..f61e8728d985 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -113,6 +113,12 @@ void FlightTaskManualPosition::_scaleSticks() /* Rotate setpoint into local frame. */ _rotateIntoHeadingFrame(vel_sp_xy); + + //collision avoidance + if (_ext_collision_avoidance != nullptr && _ext_collision_avoidance->is_active()) { + _ext_collision_avoidance->modifySetpoint(vel_sp_xy, _constraints.speed_xy); + } + _velocity_setpoint(0) = vel_sp_xy(0); _velocity_setpoint(1) = vel_sp_xy(1); } diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 04a2064fdfa1..337dbe1d3146 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -56,6 +56,12 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude */ void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } + /** + * Sets an external collision avoidance which can be used to modify setpoints + */ + void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) override {_ext_collision_avoidance = ext_collision_avoidance;} + + protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */ void _updateSetpoints() override; @@ -74,4 +80,5 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude WeatherVane *_weathervane_yaw_handler = nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + CollisionAvoidance *_ext_collision_avoidance = nullptr; /**< external collision avoidance library*/ }; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index b930dae9b5bb..fbd65614f463 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -622,6 +622,7 @@ void Logger::add_default_topics() add_topic("battery_status", 500); add_topic("camera_capture"); add_topic("camera_trigger"); + add_topic("collision_constraints"); add_topic("cpuload"); add_topic("distance_sensor", 100); add_topic("ekf2_innovations", 200); @@ -633,6 +634,7 @@ void Logger::add_default_topics() add_topic("manual_control_setpoint", 200); add_topic("mission"); add_topic("mission_result"); + add_topic("obstacle_distance"); add_topic("optical_flow", 50); add_topic("position_setpoint_triplet", 200); add_topic("radio_status"); diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index f60e2abe39a7..247a266a6c4a 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -45,4 +45,5 @@ px4_add_module( git_ecl ecl_geo WeatherVane + CollisionAvoidance ) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0b50c5d7b4c2..9a513393e74e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -64,6 +65,7 @@ #include #include +#include #include "PositionControl.hpp" #include "Utility/ControlMath.hpp" @@ -117,6 +119,7 @@ class MulticopterPositionControl : public ModuleBase int _att_sub{-1}; /**< vehicle attitude */ int _home_pos_sub{-1}; /**< home position */ int _traj_wp_avoidance_sub{-1}; /**< trajectory waypoint */ + int _range_sensor_sub{-1}; /**< obstacle distances */ int _task_failure_count{0}; /**< counter for task failures */ @@ -133,6 +136,7 @@ class MulticopterPositionControl : public ModuleBase home_position_s _home_pos{}; /**< home position */ vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */ vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */ + obstacle_distance_s _obstacle_distance{}; /**< obstacle distances received form a range sensor */ DEFINE_PARAMETERS( (ParamFloat) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ @@ -181,6 +185,7 @@ class MulticopterPositionControl : public ModuleBase systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ WeatherVane *_wv_controller{nullptr}; + CollisionAvoidance *_ca_controller{nullptr}; /** * Update our local parameter cache. @@ -355,6 +360,9 @@ MulticopterPositionControl::~MulticopterPositionControl() if (_wv_controller != nullptr) { delete _wv_controller; } + if (_ca_controller != nullptr) { + delete _ca_controller; + } } void @@ -425,6 +433,10 @@ MulticopterPositionControl::poll_subscriptions() if (_wv_controller == nullptr && _vehicle_status.is_vtol) { _wv_controller = new WeatherVane(); } + // if the vehicle is a rotary wing, enable collision avoidance capabilities + if (_ca_controller == nullptr && _vehicle_status.is_rotary_wing) { + _ca_controller = new CollisionAvoidance(); + } } orb_check(_vehicle_land_detected_sub, &updated); @@ -465,6 +477,12 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_trajectory_waypoint), _traj_wp_avoidance_sub, &_traj_wp_avoidance); } + + orb_check(_range_sensor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(obstacle_distance), _range_sensor_sub, &_obstacle_distance); + } } void @@ -582,6 +600,7 @@ MulticopterPositionControl::run() _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _traj_wp_avoidance_sub = orb_subscribe(ORB_ID(vehicle_trajectory_waypoint)); + _range_sensor_sub = orb_subscribe(ORB_ID(obstacle_distance)); parameters_update(true); @@ -644,6 +663,20 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } + // activate the collision avoidance if required. If activated a flighttask can use it modify the setpoint + if (_ca_controller != nullptr) { + + // in manual mode we just want to use weathervane if position is controlled as well + if (_ca_controller->collision_avoidance_enabled()) { + _ca_controller->activate(); + + } else { + _ca_controller->deactivate(); + } + + _ca_controller->update(_obstacle_distance); + } + if (_control_mode.flag_armed) { // as soon vehicle is armed check for flighttask start_flight_task(); @@ -665,6 +698,7 @@ MulticopterPositionControl::run() vehicle_local_position_setpoint_s setpoint; _flight_tasks.setYawHandler(_wv_controller); + _flight_tasks.setCollisionAvoidance(_ca_controller); // update task if (!_flight_tasks.update()) { 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 4ac55f0decf5..b09af23e3b57 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -737,3 +737,22 @@ PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0); * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); + +/** + * Flag to enable the use of a range sensor for collision avoidance. + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); +/** + * Minimum Obstacle Distance at which the vehicle should not get closer + * + * Only relevant if in Position control and the range sensor is active + * + * @min 0 + * @max 15 + * @unit meters + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_COL_AVOID_D, 4.0f); From f381d1584fe0307ad8dda7bc98718d5309435de6 Mon Sep 17 00:00:00 2001 From: baumanta Date: Fri, 2 Nov 2018 15:09:41 +0100 Subject: [PATCH 02/10] Change wrong comment --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 9a513393e74e..6e0eb06bac6a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -663,7 +663,7 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - // activate the collision avoidance if required. If activated a flighttask can use it modify the setpoint + // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter if (_ca_controller != nullptr) { // in manual mode we just want to use weathervane if position is controlled as well From 59e039297a5c3a010048466005b3f5bd71549630 Mon Sep 17 00:00:00 2001 From: baumanta Date: Mon, 5 Nov 2018 08:42:30 +0100 Subject: [PATCH 03/10] Improve comments --- src/lib/CollisionAvoidance/CollisionAvoidance.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_params.c | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp index 823719c0c390..712286579279 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -165,7 +165,7 @@ void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float || new_setpoint(1) > 1.05f * original_setpoint(1)); if (currently_interfering && (currently_interfering != _interfering)) { - mavlink_log_critical(&_mavlink_log_pub, "Collision Avoidance starts interfering"); + mavlink_log_critical(&_mavlink_log_pub, "Collision Warning"); } _interfering = currently_interfering; 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 b09af23e3b57..6d9e8ad1ac15 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -739,16 +739,16 @@ PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0); PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); /** - * Flag to enable the use of a range sensor for collision avoidance. + * Flag to enable the use of a MAVLink range sensor for collision avoidance. * * @boolean * @group Multicopter Position Control */ PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); /** - * Minimum Obstacle Distance at which the vehicle should not get closer + * Minimum distance the vehicle should keep to all obstacles * - * Only relevant if in Position control and the range sensor is active + * Only used in Position mode when collision avoidance is active (see MPC_COL_AVOID). * * @min 0 * @max 15 From 2bd310939ab6acddc8d8c6049251ed013afb7ba1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 5 Nov 2018 21:44:15 +0100 Subject: [PATCH 04/10] CollisionAvoidance: move activation logic to library --- src/lib/CollisionAvoidance/CollisionAvoidance.cpp | 12 ++++++++++++ src/lib/CollisionAvoidance/CollisionAvoidance.hpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ---------- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp index 712286579279..f8a18304a6ea 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -88,6 +88,18 @@ void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, } +void CollisionAvoidance::update(const obstacle_distance_s &distance_measurements) { + // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter + if (collision_avoidance_enabled()) { + activate(); + + } else { + deactivate(); + } + + _obstacle_distance = distance_measurements; +} + void CollisionAvoidance::update_range_constraints() { if (hrt_elapsed_time((hrt_abstime *)&_obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp index bf1f5539c219..53b3a8b65262 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp @@ -72,7 +72,7 @@ class CollisionAvoidance : public ModuleParams bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); } - void update(const obstacle_distance_s &distance_measurements) {_obstacle_distance = distance_measurements;} + void update(const obstacle_distance_s &distance_measurements); void update_range_constraints(); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6e0eb06bac6a..abcd1a0466c6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -663,17 +663,7 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter if (_ca_controller != nullptr) { - - // in manual mode we just want to use weathervane if position is controlled as well - if (_ca_controller->collision_avoidance_enabled()) { - _ca_controller->activate(); - - } else { - _ca_controller->deactivate(); - } - _ca_controller->update(_obstacle_distance); } From e790a31dac090c4f4631890b66cf7b30e2642404 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 5 Nov 2018 22:33:52 +0100 Subject: [PATCH 05/10] ColisionAvoidance: move instantiation to FlightTask --- .../CollisionAvoidance/CollisionAvoidance.cpp | 30 ++++++++++++------- .../CollisionAvoidance/CollisionAvoidance.hpp | 13 ++++++-- src/lib/FlightTasks/FlightTasks.hpp | 6 ---- .../tasks/FlightTask/FlightTask.hpp | 7 ----- .../FlightTaskManualPosition.cpp | 19 ++++++++++-- .../FlightTaskManualPosition.hpp | 9 ++---- .../mc_pos_control/mc_pos_control_main.cpp | 24 --------------- .../mc_pos_control/mc_pos_control_params.c | 1 + 8 files changed, 50 insertions(+), 59 deletions(-) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp index f8a18304a6ea..1afc2f5c2687 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -46,6 +46,15 @@ CollisionAvoidance::CollisionAvoidance() : } +bool CollisionAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array) +{ + if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) { + return false; + } + + return true; +} + void CollisionAvoidance::reset_constraints() { @@ -88,7 +97,8 @@ void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, } -void CollisionAvoidance::update(const obstacle_distance_s &distance_measurements) { +void CollisionAvoidance::update() +{ // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter if (collision_avoidance_enabled()) { activate(); @@ -96,21 +106,21 @@ void CollisionAvoidance::update(const obstacle_distance_s &distance_measurements } else { deactivate(); } - - _obstacle_distance = distance_measurements; } void CollisionAvoidance::update_range_constraints() { - if (hrt_elapsed_time((hrt_abstime *)&_obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { - float max_detection_distance = _obstacle_distance.max_distance / 100.0f; //convert to meters + 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++) { //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); + 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); //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); @@ -133,7 +143,7 @@ void CollisionAvoidance::update_range_constraints() void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float max_speed) { - + update(); reset_constraints(); //calculate movement constraints based on range data diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp index 53b3a8b65262..39a05c9bd0c8 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp @@ -53,6 +53,7 @@ using uORB::Publication; #include #include +#include using namespace matrix; using namespace time_literals; @@ -64,6 +65,12 @@ class CollisionAvoidance : public ModuleParams ~CollisionAvoidance() = default; + /** + * Initialize the uORB subscriptions using an array + * @return true on success, false on error + */ + bool initializeSubscriptions(SubscriptionArray &subscription_array); + void activate() {_is_active = true;} void deactivate() {_is_active = false;} @@ -72,7 +79,7 @@ class CollisionAvoidance : public ModuleParams bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); } - void update(const obstacle_distance_s &distance_measurements); + void update(); void update_range_constraints(); @@ -84,14 +91,14 @@ class CollisionAvoidance : public ModuleParams private: - obstacle_distance_s _obstacle_distance{}; /**< obstacle distances received form a range sensor */ - bool _is_active = true; bool _interfering = false; /**< states if the collision avoidance 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::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; diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 5cca46e0626f..3a07d25ae349 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -45,7 +45,6 @@ #include "SubscriptionArray.hpp" #include "FlightTasks_generated.hpp" #include -#include #include @@ -132,11 +131,6 @@ class FlightTasks */ void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);} - /** - * Sets an external collision avoidance. The active flight task can use the the collision avoidance to modify the setpoint. - */ - void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {_current_task.task->setCollisionAvoidance(ext_collision_avoidance);} - /** * This method will re-activate current task. */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 30ef82a3fde8..9efe587ee47d 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -52,7 +52,6 @@ #include #include #include -#include #include "SubscriptionArray.hpp" class FlightTask : public ModuleParams @@ -155,12 +154,6 @@ class FlightTask : public ModuleParams void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; } - - /** - * Sets an external collision avoidance which can be used by any flight task to implement a different setpoint - * This method does nothing, each flighttask which wants to use the collision avoidance needs to override this method. - */ - virtual void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {}; protected: diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index f61e8728d985..74457e8e929d 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -41,6 +41,19 @@ using namespace matrix; +bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscription_array) +{ + if (!FlightTaskManualAltitude::initializeSubscriptions(subscription_array)) { + return false; + } + + if (!_collision_avoidance.initializeSubscriptions(subscription_array)) { + return false; + } + + return true; +} + bool FlightTaskManualPosition::updateInitialize() { bool ret = FlightTaskManualAltitude::updateInitialize(); @@ -114,9 +127,9 @@ void FlightTaskManualPosition::_scaleSticks() /* Rotate setpoint into local frame. */ _rotateIntoHeadingFrame(vel_sp_xy); - //collision avoidance - if (_ext_collision_avoidance != nullptr && _ext_collision_avoidance->is_active()) { - _ext_collision_avoidance->modifySetpoint(vel_sp_xy, _constraints.speed_xy); + // collision avoidance + if (_collision_avoidance.is_active()) { + _collision_avoidance.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 337dbe1d3146..59da9ee05745 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -41,6 +41,7 @@ #pragma once #include "FlightTaskManualAltitude.hpp" +#include class FlightTaskManualPosition : public FlightTaskManualAltitude { @@ -48,6 +49,7 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude FlightTaskManualPosition() = default; virtual ~FlightTaskManualPosition() = default; + bool initializeSubscriptions(SubscriptionArray &subscription_array) override; bool activate() override; bool updateInitialize() override; @@ -56,11 +58,6 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude */ void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } - /** - * Sets an external collision avoidance which can be used to modify setpoints - */ - void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) override {_ext_collision_avoidance = ext_collision_avoidance;} - protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */ @@ -80,5 +77,5 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude WeatherVane *_weathervane_yaw_handler = nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - CollisionAvoidance *_ext_collision_avoidance = nullptr; /**< external collision avoidance library*/ + CollisionAvoidance _collision_avoidance; /**< collision avoidance setpoint amendment */ }; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index abcd1a0466c6..0b50c5d7b4c2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -47,7 +47,6 @@ #include #include -#include #include #include #include @@ -65,7 +64,6 @@ #include #include -#include #include "PositionControl.hpp" #include "Utility/ControlMath.hpp" @@ -119,7 +117,6 @@ class MulticopterPositionControl : public ModuleBase int _att_sub{-1}; /**< vehicle attitude */ int _home_pos_sub{-1}; /**< home position */ int _traj_wp_avoidance_sub{-1}; /**< trajectory waypoint */ - int _range_sensor_sub{-1}; /**< obstacle distances */ int _task_failure_count{0}; /**< counter for task failures */ @@ -136,7 +133,6 @@ class MulticopterPositionControl : public ModuleBase home_position_s _home_pos{}; /**< home position */ vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */ vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */ - obstacle_distance_s _obstacle_distance{}; /**< obstacle distances received form a range sensor */ DEFINE_PARAMETERS( (ParamFloat) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ @@ -185,7 +181,6 @@ class MulticopterPositionControl : public ModuleBase systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ WeatherVane *_wv_controller{nullptr}; - CollisionAvoidance *_ca_controller{nullptr}; /** * Update our local parameter cache. @@ -360,9 +355,6 @@ MulticopterPositionControl::~MulticopterPositionControl() if (_wv_controller != nullptr) { delete _wv_controller; } - if (_ca_controller != nullptr) { - delete _ca_controller; - } } void @@ -433,10 +425,6 @@ MulticopterPositionControl::poll_subscriptions() if (_wv_controller == nullptr && _vehicle_status.is_vtol) { _wv_controller = new WeatherVane(); } - // if the vehicle is a rotary wing, enable collision avoidance capabilities - if (_ca_controller == nullptr && _vehicle_status.is_rotary_wing) { - _ca_controller = new CollisionAvoidance(); - } } orb_check(_vehicle_land_detected_sub, &updated); @@ -477,12 +465,6 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_trajectory_waypoint), _traj_wp_avoidance_sub, &_traj_wp_avoidance); } - - orb_check(_range_sensor_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(obstacle_distance), _range_sensor_sub, &_obstacle_distance); - } } void @@ -600,7 +582,6 @@ MulticopterPositionControl::run() _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _traj_wp_avoidance_sub = orb_subscribe(ORB_ID(vehicle_trajectory_waypoint)); - _range_sensor_sub = orb_subscribe(ORB_ID(obstacle_distance)); parameters_update(true); @@ -663,10 +644,6 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - if (_ca_controller != nullptr) { - _ca_controller->update(_obstacle_distance); - } - if (_control_mode.flag_armed) { // as soon vehicle is armed check for flighttask start_flight_task(); @@ -688,7 +665,6 @@ MulticopterPositionControl::run() vehicle_local_position_setpoint_s setpoint; _flight_tasks.setYawHandler(_wv_controller); - _flight_tasks.setCollisionAvoidance(_ca_controller); // update task if (!_flight_tasks.update()) { 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 6d9e8ad1ac15..a2e1d045429c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -745,6 +745,7 @@ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); * @group Multicopter Position Control */ PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); + /** * Minimum distance the vehicle should keep to all obstacles * From a0f7b0ab314279d1b63472317ef2ac50e7709b62 Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 6 Nov 2018 10:24:41 +0100 Subject: [PATCH 06/10] unadvertise publishers in the destructor --- src/lib/CollisionAvoidance/CollisionAvoidance.cpp | 10 ++++++++++ src/lib/CollisionAvoidance/CollisionAvoidance.hpp | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp index 1afc2f5c2687..9b712cc553b6 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -46,6 +46,16 @@ CollisionAvoidance::CollisionAvoidance() : } +CollisionAvoidance::~CollisionAvoidance(){ + //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) { if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) { diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp index 39a05c9bd0c8..66a72d75d4d3 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp @@ -63,7 +63,7 @@ class CollisionAvoidance : public ModuleParams public: CollisionAvoidance(); - ~CollisionAvoidance() = default; + ~CollisionAvoidance(); /** * Initialize the uORB subscriptions using an array From c3d7ac76872db5c57d5049df9db2ff2d005b1211 Mon Sep 17 00:00:00 2001 From: baumanta Date: Wed, 7 Nov 2018 10:10:56 +0100 Subject: [PATCH 07/10] change feature name from CollisionAvoidance to CollisionPrevention --- src/lib/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CollisionPrevention.cpp} | 34 ++++++++++--------- .../CollisionPrevention.hpp} | 18 +++++----- .../FlightTaskManualPosition.cpp | 8 ++--- .../FlightTaskManualPosition.hpp | 4 +-- src/modules/mc_pos_control/CMakeLists.txt | 2 +- .../mc_pos_control/mc_pos_control_params.c | 4 +-- 8 files changed, 38 insertions(+), 36 deletions(-) rename src/lib/{CollisionAvoidance => CollisionPrevention}/CMakeLists.txt (96%) rename src/lib/{CollisionAvoidance/CollisionAvoidance.cpp => CollisionPrevention/CollisionPrevention.cpp} (88%) rename src/lib/{CollisionAvoidance/CollisionAvoidance.hpp => CollisionPrevention/CollisionPrevention.hpp} (86%) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index e440cf07d971..c0734c41019d 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 8bf01178bbe0..b9ef18f7c361 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 9b712cc553b6..38e49a41a10a 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 66a72d75d4d3..ec82401bd954 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 @@ class CollisionAvoidance : public ModuleParams 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 @@ class CollisionAvoidance : public ModuleParams 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 @@ class CollisionAvoidance : public ModuleParams 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 74457e8e929d..a6d18786dcbc 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 59da9ee05745..f32393a18f33 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 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude 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 247a266a6c4a..f649cecbef69 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 a2e1d045429c..f418ac540383 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); From fc22ea90dbc10efc274bd8285c1db41a426e0302 Mon Sep 17 00:00:00 2001 From: baumanta Date: Thu, 15 Nov 2018 14:43:04 +0100 Subject: [PATCH 08/10] Put parameters inside library. Get rid of unnecessary activation logic --- .../CollisionPrevention.cpp | 13 ---- .../CollisionPrevention.hpp | 8 +-- .../collisionprevention_params.c | 60 +++++++++++++++++++ .../mc_pos_control/mc_pos_control_params.c | 19 ------ 4 files changed, 61 insertions(+), 39 deletions(-) create mode 100644 src/lib/CollisionPrevention/collisionprevention_params.c diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 38e49a41a10a..98cf2ec2f871 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -106,18 +106,6 @@ void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint, } else { _constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints); } - -} - -void CollisionPrevention::update() -{ - // activate/deactivate the collision prevention based on MPC_COL_PREV parameter - if (collision_prevention_enabled()) { - activate(); - - } else { - deactivate(); - } } void CollisionPrevention::update_range_constraints() @@ -155,7 +143,6 @@ void CollisionPrevention::update_range_constraints() void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed) { - update(); reset_constraints(); //calculate movement constraints based on range data diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index ec82401bd954..11561c3bb502 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -71,13 +71,7 @@ class CollisionPrevention : public ModuleParams */ bool initializeSubscriptions(SubscriptionArray &subscription_array); - void activate() {_is_active = true;} - - void deactivate() {_is_active = false;} - - bool is_active() {return _is_active;} - - bool collision_prevention_enabled() { return MPC_COL_PREV.get(); } + bool is_active() {return MPC_COL_PREV.get();} void update(); diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c new file mode 100644 index 000000000000..a518571f3927 --- /dev/null +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file collisionprevention_params.c + * + * Parameters defined by the collisionprevention lib. + * + * @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). + * + * @min 0 + * @max 15 + * @unit meters + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, 4.0f); 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 f418ac540383..f86eb782061c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -738,22 +738,3 @@ PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0); */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); -/** - * 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). - * - * @min 0 - * @max 15 - * @unit meters - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, 4.0f); From 2043246e2f35e40ffe01f5b53fbcefa200777ae0 Mon Sep 17 00:00:00 2001 From: baumanta Date: Mon, 19 Nov 2018 08:46:58 +0100 Subject: [PATCH 09/10] clean up --- .../CollisionPrevention.cpp | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 98cf2ec2f871..8bd340987de3 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -70,16 +70,11 @@ bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscriptio void CollisionPrevention::reset_constraints() { - _move_constraints_x_normalized(0) = 0.0f; //normalized constraint in negative x-direction - _move_constraints_x_normalized(1) = 0.0f; //normalized constraint in positive x-direction - _move_constraints_y_normalized(0) = 0.0f; //normalized constraint in negative y-direction - _move_constraints_y_normalized(1) = 0.0f; //normalized constraint in positive y-direction - - _move_constraints_x(0) = 0.0f; //constraint in negative x-direction - _move_constraints_x(1) = 0.0f; //constraint in positive x-direction - _move_constraints_y(0) = 0.0f; //constraint in negative y-direction - _move_constraints_y(1) = 0.0f; //constraint in positive y-direction + _move_constraints_x_normalized.zero(); //normalized constraint in x-direction + _move_constraints_y_normalized.zero(); //normalized constraint in y-direction + _move_constraints_x.zero(); //constraint in x-direction + _move_constraints_y.zero(); //constraint in y-direction } void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) @@ -164,10 +159,8 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa } //scale the velocity reductions with the maximum possible velocity along the respective axis - _move_constraints_x(0) *= v_max_x; - _move_constraints_x(1) *= v_max_x; - _move_constraints_y(0) *= v_max_y; - _move_constraints_y(1) *= v_max_y; + _move_constraints_x *= v_max_x; + _move_constraints_y *= v_max_y; //apply the velocity reductions to form velocity limits _move_constraints_x(0) = v_max_x - _move_constraints_x(0); From 0dccec2cf1711d8e633e03babaafec00e638ed5a Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 20 Nov 2018 12:31:52 +0100 Subject: [PATCH 10/10] use only one parameter and enable parameter change in flight. clean up code --- .../CollisionPrevention.cpp | 19 ++++++---- .../CollisionPrevention.hpp | 36 +++++++++---------- .../collisionprevention_params.c | 14 ++------ .../FlightTaskManualPosition.cpp | 5 +++ .../FlightTaskManualPosition.hpp | 2 +- 5 files changed, 37 insertions(+), 39 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 8bd340987de3..76a32e1c6f0a 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 11561c3bb502..527655611de8 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 @@ class CollisionPrevention : public ModuleParams */ 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 @@ class CollisionPrevention : public ModuleParams 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 a518571f3927..1b4175396b6a 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 a6d18786dcbc..6b2ec40431d8 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 f32393a18f33..031c5cb1a423 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;