From 8c24ba9255a85169acef11f8c889e5e10078f037 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Thu, 21 Feb 2019 16:28:33 +0100
Subject: [PATCH] move injection of avoidance setpoints to flight task library
---
.../FlightTaskAutoLineSmoothVel.cpp | 2 +-
.../AutoMapper2/FlightTaskAutoMapper2.cpp | 8 +++
.../AutoMapper2/FlightTaskAutoMapper2.hpp | 4 +-
.../FlightTasks/tasks/Utility/CMakeLists.txt | 3 +-
.../tasks/Utility/ObstacleAvoidance.cpp | 71 +++++++++++++++++++
.../tasks/Utility/ObstacleAvoidance.hpp | 66 +++++++++++++++++
.../mc_pos_control/mc_pos_control_main.cpp | 54 --------------
7 files changed, 151 insertions(+), 57 deletions(-)
create mode 100644 src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
create mode 100644 src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index c7ab200be9..226f0231bf 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -75,7 +75,7 @@ void FlightTaskAutoLineSmoothVel::_generateSetpoints()
_prepareSetpoints();
_generateTrajectory();
- if (!PX4_ISFINITE(_yaw_setpoint)) {
+ if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) {
// no valid heading -> generate heading in this flight task
_generateHeading();
}
diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
index b3a1c404a0..e2d47bc8ba 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
@@ -40,6 +40,12 @@
using namespace matrix;
+FlightTaskAutoMapper2::FlightTaskAutoMapper2() :
+ _obstacle_avoidance(this)
+{
+
+}
+
bool FlightTaskAutoMapper2::activate()
{
bool ret = FlightTaskAuto::activate();
@@ -91,6 +97,8 @@ bool FlightTaskAutoMapper2::update()
break;
}
+ _obstacle_avoidance.prepareAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, _yawspeed_setpoint);
+
_generateSetpoints();
// during mission and reposition, raise the landing gears but only
diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
index 226a54427f..032a5210f5 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
@@ -41,11 +41,12 @@
#pragma once
#include "FlightTaskAuto.hpp"
+#include "lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp"
class FlightTaskAutoMapper2 : public FlightTaskAuto
{
public:
- FlightTaskAutoMapper2() = default;
+ FlightTaskAutoMapper2();
virtual ~FlightTaskAutoMapper2() = default;
bool activate() override;
bool update() override;
@@ -80,4 +81,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/CMakeLists.txt b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
index 4e8164182d..94f6d1173a 100644
--- a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
+++ b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt
@@ -34,9 +34,10 @@
px4_add_library(FlightTaskUtility
ManualSmoothingZ.cpp
ManualSmoothingXY.cpp
+ ObstacleAvoidance.cpp
StraightLine.cpp
VelocitySmoothing.cpp
)
target_link_libraries(FlightTaskUtility PUBLIC FlightTask)
-target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
\ No newline at end of file
+target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
new file mode 100644
index 0000000000..1fbe629aec
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ * 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 ObstacleAvoidance.cpp
+ */
+
+#include "ObstacleAvoidance.hpp"
+#include <drivers/drv_hrt.h>
+#include <uORB/Subscription.hpp>
+
+
+using namespace matrix;
+using namespace time_literals;
+
+/** Timeout in us for trajectory data to get considered invalid */
+static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
+
+ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
+ ModuleParams(parent)
+{
+
+}
+
+void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp) {
+
+ if (!COM_OBS_AVOID.get()) {
+ 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_point_valid = _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;
+ }
+}
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
new file mode 100644
index 0000000000..304cbc40f5
--- /dev/null
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ * 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 ObstacleAvoidance.hpp
+ * This class is used to inject the setpoints of an obstacle avoidance system
+ * into the FlightTasks
+ *
+ * @author Martina Rivizzigno
+ */
+
+#pragma once
+
+#include <px4_module_params.h>
+#include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
+#include <matrix/matrix/math.hpp>
+#include <px4_defines.h>
+
+class ObstacleAvoidance : public ModuleParams
+{
+public:
+ ObstacleAvoidance(ModuleParams *parent);
+ ~ObstacleAvoidance() = default;
+
+ 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)};
+
+ DEFINE_PARAMETERS(
+ (ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID /**< obstacle avoidance enabled */
+ );
+
+};
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 505628afe3..78690bbb60 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -283,21 +283,11 @@ private:
*/
void update_avoidance_waypoint_desired(PositionControlStates &states, vehicle_local_position_setpoint_s &setpoint);
- /**
- * Check whether or not use the obstacle avoidance waypoint
- */
- bool use_obstacle_avoidance();
-
/**
* Reset setpoints to NAN
*/
void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);
- /**
- * Overwrite setpoint with waypoint coming from obstacle avoidance
- */
- void execute_avoidance_waypoint(vehicle_local_position_setpoint_s &setpoint);
-
/**
* Publish desired vehicle_trajectory_waypoint
*/
@@ -736,11 +726,6 @@ MulticopterPositionControl::run()
limit_altitude(setpoint);
}
- // adjust setpoints based on avoidance
- if (use_obstacle_avoidance()) {
- execute_avoidance_waypoint(setpoint);
- }
-
// Update states, setpoints and constraints.
_control.updateConstraints(constraints);
_control.updateState(_states);
@@ -1168,24 +1153,6 @@ MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlSta
}
}
-void
-MulticopterPositionControl::execute_avoidance_waypoint(vehicle_local_position_setpoint_s &setpoint)
-{
- setpoint.x = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0];
- setpoint.y = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1];
- setpoint.z = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2];
-
- setpoint.vx = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0];
- setpoint.vy = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1];
- setpoint.vz = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2];
-
- setpoint.acc_x = setpoint.acc_y = setpoint.acc_z = NAN;
-
- setpoint.yaw = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
- setpoint.yawspeed = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
- Vector3f(NAN, NAN, NAN).copyTo(setpoint.thrust);
-}
-
void
MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
{
@@ -1196,27 +1163,6 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
}
-bool
-MulticopterPositionControl::use_obstacle_avoidance()
-{
- if (_param_com_obs_avoid.get()) {
- const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
- const bool avoidance_point_valid = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
- const bool in_mission = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
- const bool in_rtl = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
-
- // switch to hold mode to stop when we loose external avoidance data during a mission
- if (avoidance_data_timeout && in_mission) {
- send_vehicle_cmd_do(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
- }
-
- if ((in_mission || in_rtl) && !avoidance_data_timeout && avoidance_point_valid) {
- return true;
- }
- }
- return false;
-}
-
void
MulticopterPositionControl::publish_avoidance_desired_waypoint()
{
--
GitLab