From 81ec6c5b1e95c5db6c8e11733cf07ec68782045b Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 5 Nov 2018 22:33:52 +0100
Subject: [PATCH] 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 f8a18304a6..1afc2f5c26 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 53b3a8b652..39a05c9bd0 100644
--- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp
+++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp
@@ -53,6 +53,7 @@
using uORB::Publication;
#include <uORB/uORB.h>
#include <systemlib/mavlink_log.h>
+#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
using namespace matrix;
using namespace time_literals;
@@ -64,6 +65,12 @@ public:
~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 @@ public:
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 @@ public:
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<obstacle_distance_s> *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */
+
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000;
static constexpr uint64_t MESSAGE_THROTTLE_US = 5_s;
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 5cca46e062..3a07d25ae3 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 <lib/WeatherVane/WeatherVane.hpp>
-#include <lib/CollisionAvoidance/CollisionAvoidance.hpp>
#include <new>
@@ -132,11 +131,6 @@ 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 30ef82a3fd..9efe587ee4 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
@@ -52,7 +52,6 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <lib/WeatherVane/WeatherVane.hpp>
-#include <lib/CollisionAvoidance/CollisionAvoidance.hpp>
#include "SubscriptionArray.hpp"
class FlightTask : public ModuleParams
@@ -155,12 +154,6 @@ 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 f61e8728d9..74457e8e92 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 337dbe1d31..59da9ee057 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 <lib/CollisionAvoidance/CollisionAvoidance.hpp>
class FlightTaskManualPosition : public FlightTaskManualAltitude
{
@@ -48,6 +49,7 @@ public:
FlightTaskManualPosition() = default;
virtual ~FlightTaskManualPosition() = default;
+ bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
bool activate() override;
bool updateInitialize() override;
@@ -56,11 +58,6 @@ 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 */
@@ -80,5 +77,5 @@ private:
WeatherVane *_weathervane_yaw_handler =
nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
- CollisionAvoidance *_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 abcd1a0466..0b50c5d7b4 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 <commander/px4_custom_mode.h>
#include <uORB/topics/home_position.h>
-#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -65,7 +64,6 @@
#include <lib/FlightTasks/FlightTasks.hpp>
#include <lib/WeatherVane/WeatherVane.hpp>
-#include <lib/CollisionAvoidance/CollisionAvoidance.hpp>
#include "PositionControl.hpp"
#include "Utility/ControlMath.hpp"
@@ -119,7 +117,6 @@ 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 */
@@ -136,7 +133,6 @@ 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<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */
@@ -185,7 +181,6 @@ 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.
@@ -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 6d9e8ad1ac..a2e1d04542 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
*
--
GitLab