From 320ed40806bffdf099311bd0dbcbce6fd31bb197 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Thu, 28 Feb 2019 23:53:24 +0100
Subject: [PATCH] use subscription array in ObstacleAvoidance library
---
.../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 10 ++++++++
.../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 7 +++++-
.../AutoMapper2/FlightTaskAutoMapper2.cpp | 4 ----
.../AutoMapper2/FlightTaskAutoMapper2.hpp | 5 ++--
.../tasks/Utility/ObstacleAvoidance.cpp | 24 ++++++++++++-------
.../tasks/Utility/ObstacleAvoidance.hpp | 4 +++-
6 files changed, 37 insertions(+), 17 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index 8275b70c2a..de7125d499 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -42,6 +42,12 @@ using namespace matrix;
static constexpr float SIGMA_NORM = 0.001f;
+FlightTaskAuto::FlightTaskAuto() :
+ _obstacle_avoidance(this)
+{
+
+}
+
bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_array)
{
if (!FlightTask::initializeSubscriptions(subscription_array)) {
@@ -60,6 +66,10 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr
return false;
}
+ if (!_obstacle_avoidance.initializeSubscriptions(subscription_array)) {
+ return false;
+ }
+
return true;
}
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
index d2101037c1..bb0b1b6585 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -46,6 +46,8 @@
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/vehicle_status.h>
#include <lib/ecl/geo/geo.h>
+#include "lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp"
+
/**
* This enum has to agree with position_setpoint_s type definition
@@ -73,7 +75,7 @@ enum class State {
class FlightTaskAuto : public FlightTask
{
public:
- FlightTaskAuto() = default;
+ FlightTaskAuto();
virtual ~FlightTaskAuto() = default;
bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
@@ -84,6 +86,7 @@ public:
* Sets an external yaw handler which can be used to implement a different yaw control strategy.
*/
void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
+ ObstacleAvoidance _obstacle_avoidance;
protected:
void _setDefaultConstraints() override;
@@ -113,6 +116,8 @@ protected:
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid // obstacle avoidance active
);
+
+
private:
matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */
diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
index 23c5748a2b..894a4389c1 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
@@ -40,11 +40,7 @@
using namespace matrix;
-FlightTaskAutoMapper2::FlightTaskAutoMapper2() :
- _obstacle_avoidance(this)
-{
-}
bool FlightTaskAutoMapper2::activate()
{
diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
index 032a5210f5..dbcc69e7b9 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
@@ -41,12 +41,11 @@
#pragma once
#include "FlightTaskAuto.hpp"
-#include "lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp"
class FlightTaskAutoMapper2 : public FlightTaskAuto
{
public:
- FlightTaskAutoMapper2();
+ FlightTaskAutoMapper2() = default;
virtual ~FlightTaskAutoMapper2() = default;
bool activate() override;
bool update() override;
@@ -81,5 +80,5 @@ private:
void _reset(); /**< Resets member variables to current vehicle state */
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
- ObstacleAvoidance _obstacle_avoidance;
+
};
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
index 78a9475076..c402d03cb8 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
@@ -52,6 +52,15 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
}
+bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+ if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) {
+ return false;
+ }
+
+ return true;
+}
+
void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp,
float &yaw_speed_sp)
{
@@ -60,16 +69,15 @@ void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &ve
return;
}
- _sub_vehicle_trajectory_waypoint.update();
- const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp) >
- TRAJECTORY_STREAM_TIMEOUT_US;
+ const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint->get().timestamp)
+ > TRAJECTORY_STREAM_TIMEOUT_US;
const bool avoidance_point_valid =
- _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
+ _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
if (!avoidance_data_timeout && avoidance_point_valid) {
- pos_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position;
- vel_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity;
- yaw_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
- yaw_speed_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
+ pos_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position;
+ vel_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity;
+ yaw_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
+ yaw_speed_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
}
}
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
index 64c18645a0..3378f59def 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
@@ -53,11 +53,13 @@ public:
ObstacleAvoidance(ModuleParams *parent);
~ObstacleAvoidance() = default;
+ bool initializeSubscriptions(SubscriptionArray &subscription_array);
+
void prepareAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
private:
- uORB::Subscription<vehicle_trajectory_waypoint_s> _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)};
+ uORB::Subscription<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr};
DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID /**< obstacle avoidance enabled */
--
GitLab