CollisionAvoidance: move activation logic to library

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) {

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();
 

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);
 		}