From db514fe4418173ebf1118b6559733c7fec1741f7 Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 30 Oct 2018 16:16:56 +0100 Subject: [PATCH] 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 5780155feb..c66a748587 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 0000000000..4c08ac801d --- /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 45956ee7f8..e440cf07d9 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 0000000000..8bf01178bb --- /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 0000000000..823719c0c3 --- /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 0000000000..bf1f5539c2 --- /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 3a07d25ae3..5cca46e062 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 @@ public: */ 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 9efe587ee4..30ef82a3fd 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 @@ public: 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 12bc4586de..f61e8728d9 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 04a2064fdf..337dbe1d31 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -56,6 +56,12 @@ public: */ 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 @@ 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 *_ext_collision_avoidance = nullptr; /**< external collision avoidance library*/ }; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index b930dae9b5..fbd65614f4 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 f60e2abe39..247a266a6c 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 0b50c5d7b4..9a513393e7 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 @@ private: 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 @@ private: 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 @@ private: 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 4ac55f0dec..b09af23e3b 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);