From 7e276bc16310d4cae92e511a0207d6209052f7d7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 5 Nov 2018 21:44:15 +0100 Subject: [PATCH] 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 7122865792..f8a18304a6 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 bf1f5539c2..53b3a8b652 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp @@ -72,7 +72,7 @@ public: 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 6e0eb06bac..abcd1a0466 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); }