fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

remove avoidance library and logic

Browse Source 
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan 2024-12-26 21:13:51 +01:00 committed by Silvan Fuhrer
parent d2cbe10243
commit 1f2dba68d2
21 changed files with 4 additions and 895 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -404,7 +404,7 @@ check_newlines:
# Testing
# --------------------------------------------------------------------
.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance
.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard
.PHONY: rostest python_coverage
tests:

2
ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid
View File

@ -28,5 +28,3 @@ param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
param set-default COM_OBS_AVOID 1

1
ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision
View File

@ -19,7 +19,6 @@ param set-default SENS_EN_BAROSIM 0
param set-default SENS_EN_MAGSIM 1
# Commander Parameters
param set-default COM_OBS_AVOID 0
param set-default COM_DISARM_LAND 0.5
# EKF2 parameters

1
ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision
View File

@ -12,7 +12,6 @@
. ${R}etc/init.d/rc.mc_defaults
# Commander Parameters
param set-default COM_OBS_AVOID 1
param set-default COM_DISARM_LAND 0.5
# EKF2 parameters

1
ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5
View File

@ -12,7 +12,6 @@
. ${R}etc/init.d/rc.mc_defaults
# Commander Parameters
param set-default COM_OBS_AVOID 1
param set-default COM_DISARM_LAND 0.5
# EKF2 parameters

3
msg/versioned/VehicleStatus.msg
View File

@ -131,9 +131,6 @@ bool open_drone_id_system_healthy
bool parachute_system_present
bool parachute_system_healthy
bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
bool avoidance_system_valid # Status of the obstacle avoidance system
bool rc_calibration_in_progress
bool calibration_enabled

1
src/lib/CMakeLists.txt
View File

@ -34,7 +34,6 @@
add_subdirectory(adsb EXCLUDE_FROM_ALL)
add_subdirectory(airspeed EXCLUDE_FROM_ALL)
add_subdirectory(atmosphere EXCLUDE_FROM_ALL)
add_subdirectory(avoidance EXCLUDE_FROM_ALL)
add_subdirectory(battery EXCLUDE_FROM_ALL)
add_subdirectory(bezier EXCLUDE_FROM_ALL)
add_subdirectory(button EXCLUDE_FROM_ALL)

39
src/lib/avoidance/CMakeLists.txt
View File

@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2020 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.
#
############################################################################
px4_add_library(avoidance
ObstacleAvoidance.cpp
)
target_link_libraries(avoidance PUBLIC hysteresis bezier)
px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS avoidance)

271
src/lib/avoidance/ObstacleAvoidance.cpp
View File

@ -1,271 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 "bezier/BezierN.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;
/** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */
static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms;
static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s;
ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
ModuleParams(parent)
{
_desired_waypoint = empty_trajectory_waypoint;
_failsafe_position.setNaN();
_avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE);
_no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US);
}
void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp,
float &yaw_speed_sp)
{
_sub_vehicle_status.update();
_sub_vehicle_trajectory_waypoint.update();
_sub_vehicle_trajectory_bezier.update();
const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get();
const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get();
const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(
bezier_msg.control_points[bezier_msg.bezier_order - 1].delta * 1e6f);
const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout;
const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid;
const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0;
_avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid
&& !avoidance_bezier_valid, hrt_absolute_time());
const bool avoidance_invalid = (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state());
if ((_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) && avoidance_invalid) {
// if in nav_state LOITER and avoidance isn't healthy, don't inject setpoints from avoidance system
return;
}
if (avoidance_invalid) {
if (_avoidance_activated) {
// Invalid point received: deactivate
PX4_WARN("Obstacle Avoidance system failed, loitering");
_publishVehicleCmdDoLoiter();
_avoidance_activated = false;
}
if (!_failsafe_position.isAllFinite()) {
// save vehicle position when entering failsafe
_failsafe_position = _position;
}
pos_sp = _failsafe_position;
vel_sp.setNaN();
yaw_sp = NAN;
yaw_speed_sp = NAN;
// Do nothing further - wait until activation
return;
} else if (!_avoidance_activated) {
// First setpoint has been received: activate
PX4_INFO("Obstacle Avoidance system activated");
_failsafe_position.setNaN();
_avoidance_activated = true;
}
if (avoidance_point_valid && !wp_msg_timeout) {
const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0];
pos_sp = Vector3f(point0.position);
vel_sp = Vector3f(point0.velocity);
if (!_ext_yaw_active) {
// inject yaw setpoints only if weathervane isn't active
yaw_sp = point0.yaw;
yaw_speed_sp = point0.yaw_speed;
}
} else if (avoidance_bezier_valid && !bezier_msg_timeout) {
float yaw = NAN, yaw_speed = NAN;
_generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed);
if (!_ext_yaw_active) {
// inject yaw setpoints only if weathervane isn't active
yaw_sp = yaw;
yaw_speed_sp = yaw_speed;
}
}
}
void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity,
float &yaw, float &yaw_velocity)
{
const auto &msg = _sub_vehicle_trajectory_bezier.get();
int bezier_order = msg.bezier_order;
matrix::Vector3f bezier_points[bezier_order];
float bezier_yaws[bezier_order];
for (int i = 0; i < bezier_order; i++) {
bezier_points[i] = Vector3f(msg.control_points[i].position);
bezier_yaws[i] = msg.control_points[i].yaw;
}
const float duration_s = msg.control_points[bezier_order - 1].delta;
const hrt_abstime now = hrt_absolute_time();
const hrt_abstime start = msg.timestamp;
const hrt_abstime end = start + hrt_abstime(duration_s * 1e6f);
float T = NAN;
if (bezier::calculateT(start, end, now, T) &&
bezier::calculateBezierPosVel(bezier_points, bezier_order, T, position, velocity) &&
bezier::calculateBezierYaw(bezier_yaws, bezier_order, T, yaw, yaw_velocity)
) {
// translate bezier velocities T [0;1] into real velocities m/s
yaw_velocity /= duration_s;
velocity /= duration_s;
} else {
PX4_WARN("Obstacle Avoidance system failed, bad trajectory");
}
}
void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw,
const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed,
const bool ext_yaw_active, const int wp_type)
{
_desired_waypoint.timestamp = hrt_absolute_time();
_desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
_curr_wp = curr_wp;
_ext_yaw_active = ext_yaw_active;
curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration);
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type;
next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration);
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
}
void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp, const int type)
{
pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].type = type;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
_pub_traj_wp_avoidance_desired.publish(_desired_waypoint);
_desired_waypoint = empty_trajectory_waypoint;
}
void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,
float target_acceptance_radius, const Vector2f &closest_pt)
{
_position = pos;
position_controller_status_s pos_control_status = {};
pos_control_status.timestamp = hrt_absolute_time();
// vector from previous triplet to current target
Vector2f prev_to_target = Vector2f(_curr_wp - prev_wp);
// vector from previous triplet to the vehicle projected position on the line previous-target triplet
Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp);
// fraction of the previous-tagerget line that has been flown
const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
Vector2f pos_to_target = Vector2f(_curr_wp - _position);
if (prev_curr_travelled > 1.0f) {
// if the vehicle projected position on the line previous-target is past the target waypoint,
// increase the target acceptance radius such that navigator will update the triplets
pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f;
}
const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2));
bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z);
_no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time());
if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()
&& _no_progress_z_hysteresis.get_state()) {
// vehicle above or below the target waypoint
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
}
_prev_pos_to_target_z = pos_to_target_z;
// do not check for waypoints yaw acceptance in navigator
pos_control_status.yaw_acceptance = NAN;
_pub_pos_control_status.publish(pos_control_status);
}
void ObstacleAvoidance::_publishVehicleCmdDoLoiter()
{
vehicle_command_s command{};
command.timestamp = hrt_absolute_time();
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = (float)1; // base mode
command.param3 = (float)0; // sub mode
command.target_system = 1;
command.target_component = 1;
command.source_system = 1;
command.source_component = 1;
command.confirmation = false;
command.from_external = false;
command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
// publish the vehicle command
_pub_vehicle_command.publish(command);
}

152
src/lib/avoidance/ObstacleAvoidance.hpp
View File

@ -1,152 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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_platform_common/defines.h>
#include <px4_platform_common/module_params.h>
#include <commander/px4_custom_mode.h>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_bezier.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/position_setpoint.h>
#include <lib/hysteresis/hysteresis.h>
#include <matrix/matrix/math.hpp>
const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
{ {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}
}
};
class ObstacleAvoidance : public ModuleParams
{
public:
ObstacleAvoidance(ModuleParams *parent);
~ObstacleAvoidance() = default;
/**
* Inject setpoints from obstacle avoidance system into FlightTasks.
* @param pos_sp, position setpoint
* @param vel_sp, velocity setpoint
* @param yaw_sp, yaw setpoint
* @param yaw_speed_sp, yaw speed setpoint
*/
void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
/**
* Updates the desired waypoints to send to the obstacle avoidance system. These messages don't have any direct impact on the flight.
* @param curr_wp, current position triplet
* @param curr_yaw, current yaw triplet
* @param curr_yawspeed, current yaw speed triplet
* @param next_wp, next position triplet
* @param next_yaw, next yaw triplet
* @param next_yawspeed, next yaw speed triplet
* @param wp_type, current triplet type
*/
void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed,
const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active,
const int wp_type);
/**
* Updates the desired setpoints to send to the obstacle avoidance system.
* @param pos_sp, desired position setpoint computed by the active FlightTask
* @param vel_sp, desired velocity setpoint computed by the active FlightTask
* @param type, current triplet type
*/
void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type);
/**
* Checks the vehicle progress between previous and current position waypoint of the triplet.
* @param pos, vehicle position
* @param prev_wp, previous position triplet
* @param target_acceptance_radius, current position triplet xy acceptance radius
* @param closest_pt, closest point to the vehicle on the line previous-current position triplet
*/
void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp,
float target_acceptance_radius, const matrix::Vector2f &closest_pt);
protected:
uORB::SubscriptionData<vehicle_trajectory_bezier_s> _sub_vehicle_trajectory_bezier{ORB_ID(vehicle_trajectory_bezier)}; /**< vehicle trajectory waypoint subscription */
uORB::SubscriptionData<vehicle_trajectory_waypoint_s> _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */
uORB::Publication<vehicle_trajectory_waypoint_s> _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */
uORB::Publication<position_controller_status_s> _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */
uORB::Publication<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
matrix::Vector3f _curr_wp = {}; /**< current position triplet */
matrix::Vector3f _position = {}; /**< current vehicle position */
matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */
bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */
systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */
float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */
bool _ext_yaw_active = false; /**< true, if external yaw handling is active */
/**
* Publishes vehicle command.
*/
void _publishVehicleCmdDoLoiter();
void _generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, float &yaw, float &yaw_velocity);
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */
);
};

242
src/lib/avoidance/ObstacleAvoidanceTest.cpp
View File

@ -1,242 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2019 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.
*
****************************************************************************/
#include <gtest/gtest.h>
#include <avoidance/ObstacleAvoidance.hpp>
#include <uORB/Subscription.hpp>
using namespace matrix;
// to run: make tests TESTFILTER=ObstacleAvoidance
class ObstacleAvoidanceTest : public ::testing::Test
{
public:
Vector3f pos_sp;
Vector3f vel_sp;
float yaw_sp = 0.123f;
float yaw_speed_sp = NAN;
void SetUp() override
{
param_control_autosave(false);
param_reset_all();
pos_sp = Vector3f(1.f, 1.2f, 0.1f);
vel_sp = Vector3f(0.3f, 0.4f, 0.1f);
}
};
class TestObstacleAvoidance : public ::ObstacleAvoidance
{
public:
TestObstacleAvoidance() : ObstacleAvoidance(nullptr) {}
void paramsChanged() {ObstacleAvoidance::updateParamsImpl();}
void test_setPosition(Vector3f &pos) {_position = pos;}
};
TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); }
TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy)
{
// GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming
// from offboard
TestObstacleAvoidance oa;
vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint;
message.timestamp = hrt_absolute_time();
message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f;
message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f;
message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2] = 2.7f;
message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = 0.23f;
message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
// GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message
uORB::Publication<vehicle_trajectory_waypoint_s> vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
vehicle_trajectory_waypoint_pub.publish(message);
vehicle_status_s vehicle_status{};
vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
vehicle_status_pub.publish(vehicle_status);
// WHEN: we inject the vehicle_trajectory_waypoint in the interface
oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
// THEN: the setpoints should be injected
EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0], pos_sp(0));
EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1], pos_sp(1));
EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2], pos_sp(2));
EXPECT_TRUE(vel_sp.isAllNan());
EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw, yaw_sp);
EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp));
}
TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier)
{
// GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming
// from offboard
TestObstacleAvoidance oa;
vehicle_trajectory_bezier_s message {};
message.timestamp = hrt_absolute_time();
message.bezier_order = 2;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0] = 2.6f;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1] = 2.4f;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2] = 2.7f;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw = 0.23f;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = NAN;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[0] = 2.6f;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[1] = 2.4f;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[2] = 3.7f;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].yaw = 0.23f;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].delta = 0.5f;
// GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message
uORB::Publication<vehicle_trajectory_bezier_s> vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
vehicle_trajectory_bezier_pub.publish(message);
vehicle_status_s vehicle_status{};
vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
vehicle_status_pub.publish(vehicle_status);
// WHEN: we inject the vehicle_trajectory_waypoint in the interface
oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
// THEN: the setpoints should be injected
EXPECT_FLOAT_EQ(2.6f, pos_sp(0));
EXPECT_FLOAT_EQ(2.4f, pos_sp(1));
EXPECT_LT(2.7f, pos_sp(2));
EXPECT_GT(2.8f, pos_sp(2)); // probably only a tiny bit above 2.7, but let's not have flakey tests
EXPECT_FLOAT_EQ(vel_sp.xy().norm(), 0);
EXPECT_FLOAT_EQ(vel_sp(2), (3.7f - 2.7f) / 0.5f);
EXPECT_FLOAT_EQ(0.23, yaw_sp);
EXPECT_FLOAT_EQ(yaw_speed_sp, 0);
}
TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy)
{
// GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message
TestObstacleAvoidance oa;
vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint;
Vector3f pos(3.1f, 4.7f, 5.2f);
oa.test_setPosition(pos);
// GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status
uORB::Publication<vehicle_trajectory_waypoint_s> vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
vehicle_trajectory_waypoint_pub.publish(message);
vehicle_status_s vehicle_status{};
vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
vehicle_status_pub.publish(vehicle_status);
// WHEN: we inject the vehicle_trajectory_waypoint in the interface
oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
// THEN: the setpoints shouldn't be injected
EXPECT_FLOAT_EQ(pos(0), pos_sp(0));
EXPECT_FLOAT_EQ(pos(1), pos_sp(1));
EXPECT_FLOAT_EQ(pos(2), pos_sp(2));
EXPECT_TRUE(vel_sp.isAllNan());
EXPECT_FALSE(PX4_ISFINITE(yaw_sp));
EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp));
}
TEST_F(ObstacleAvoidanceTest, oa_desired)
{
// GIVEN: the flight controller setpoints from FlightTaskAuto and the waypoints from FLightTaskAuto
TestObstacleAvoidance oa;
pos_sp = Vector3f(1.f, 1.2f, NAN);
vel_sp = Vector3f(NAN, NAN, 0.7f);
int type = 4;
Vector3f curr_wp(1.f, 1.2f, 5.0f);
float curr_yaw = 1.02f;
float curr_yawspeed = NAN;
Vector3f next_wp(11.f, 1.2f, 5.0f);
float next_yaw = 0.82f;
float next_yawspeed = NAN;
bool ext_yaw_active = false;
// WHEN: we inject the setpoints and waypoints in the interface
oa.updateAvoidanceDesiredWaypoints(curr_wp, curr_yaw, curr_yawspeed, next_wp, next_yaw, next_yawspeed, ext_yaw_active,
type);
oa.updateAvoidanceDesiredSetpoints(pos_sp, vel_sp, type);
// WHEN: we subscribe to the uORB message out of the interface
uORB::SubscriptionData<vehicle_trajectory_waypoint_s> _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)};
// THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2
EXPECT_FLOAT_EQ(pos_sp(0),
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]);
EXPECT_FLOAT_EQ(pos_sp(1),
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]);
EXPECT_FALSE(PX4_ISFINITE(
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2]));
EXPECT_FALSE(PX4_ISFINITE(
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0]));
EXPECT_FALSE(PX4_ISFINITE(
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1]));
EXPECT_FLOAT_EQ(vel_sp(2),
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]);
EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].type);
EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid);
EXPECT_FLOAT_EQ(curr_wp(0),
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[0]);
EXPECT_FLOAT_EQ(curr_wp(1),
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[1]);
EXPECT_FLOAT_EQ(curr_wp(2),
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[2]);
EXPECT_FLOAT_EQ(curr_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw);
EXPECT_FALSE(PX4_ISFINITE(
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed));
EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].type);
EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid);
EXPECT_FLOAT_EQ(next_wp(0),
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[0]);
EXPECT_FLOAT_EQ(next_wp(1),
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[1]);
EXPECT_FLOAT_EQ(next_wp(2),
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[2]);
EXPECT_FLOAT_EQ(next_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw);
EXPECT_FALSE(PX4_ISFINITE(
_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed));
EXPECT_EQ(UINT8_MAX, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].type);
EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid);
}

93
src/lib/avoidance/ObstacleAvoidance_dummy.hpp
View File

@ -1,93 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 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_dummy.hpp
* This is a dummy class to reduce flash space for when obstacle avoidance is not required
*
* @author Julian Kent
*/
#pragma once
#include <px4_platform_common/defines.h>
#include <commander/px4_custom_mode.h>
#include <matrix/matrix/math.hpp>
class ObstacleAvoidance
{
public:
ObstacleAvoidance(void *) {} // takes void* argument to be compatible with ModuleParams constructor
void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp,
float &yaw_speed_sp)
{
notify_dummy();
};
void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw,
const float curr_yawspeed,
const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active,
const int wp_type)
{
notify_dummy();
};
void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp,
const int type)
{
notify_dummy();
}
void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp,
float target_acceptance_radius, const matrix::Vector2f &closest_pt)
{
notify_dummy();
};
protected:
bool _logged_error = false;
void notify_dummy()
{
if (!_logged_error) {
PX4_ERR("Dummy avoidance library called!");
_logged_error = true;
}
}
};

26
src/modules/commander/Commander.cpp
View File

@ -1717,8 +1717,6 @@ void Commander::updateParameters()
_vehicle_status.system_type = value_int32;
}
_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
const bool is_rotary = is_rotary_wing(_vehicle_status) || (is_vtol(_vehicle_status)
@ -1799,9 +1797,6 @@ void Commander::run()
_status_changed = true;
}
/* Update OA parameter */
_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
handlePowerButtonState();
systemPowerUpdate();
@ -2814,16 +2809,6 @@ void Commander::dataLinkCheck()
_vehicle_status.open_drone_id_system_present = true;
_vehicle_status.open_drone_id_system_healthy = healthy;
}
if (telemetry.heartbeat_component_obstacle_avoidance) {
if (_avoidance_system_lost) {
_avoidance_system_lost = false;
_status_changed = true;
}
_datalink_last_heartbeat_avoidance_system = telemetry.timestamp;
_vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy;
}
}
}
@ -2875,17 +2860,6 @@ void Commander::dataLinkCheck()
_open_drone_id_system_lost = true;
_status_changed = true;
}
// AVOIDANCE SYSTEM state check (only if it is enabled)
if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) {
// if heartbeats stop
if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
&& (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
_avoidance_system_lost = true;
_vehicle_status.avoidance_system_valid = false;
}
}
}
void Commander::battery_status_check()

3
src/modules/commander/Commander.hpp
View File

@ -241,7 +241,6 @@ private:
Hysteresis _auto_disarm_killed{false};
hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
hrt_abstime _datalink_last_heartbeat_gcs{0};
hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
hrt_abstime _datalink_last_heartbeat_parachute_system{0};
@ -268,7 +267,6 @@ private:
bool _failsafe_user_override_request{false}; ///< override request due to stick movements
bool _open_drone_id_system_lost{true};
bool _avoidance_system_lost{false};
bool _onboard_controller_lost{false};
bool _parachute_system_lost{true};
@ -342,7 +340,6 @@ private:
(ParamBool<px4::params::COM_FORCE_SAFETY>) _param_com_force_safety,
(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_com_rc_override,

21
src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp
View File

@ -121,27 +121,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
}
}
// avoidance system
if (context.status().avoidance_system_required) {
if (context.status().avoidance_system_valid) {
reporter.setIsPresent(health_component_t::avoidance);
} else {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_OBS_AVOID</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_avoidance_not_ready"),
events::Log::Error, "Avoidance system not ready");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avoidance system not ready");
}
}
}
// VTOL in transition
if (context.status().is_vtol && !context.isArmed()) {
if (context.status().in_transition_mode) {

8
src/modules/commander/commander_params.c
View File

@ -640,14 +640,6 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
*/
PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0);
/**
* Flag to enable obstacle avoidance.
*
* @boolean
* @group Commander
*/
PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
/**
* Expect and require a healthy MAVLink parachute system
*

2
src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt
View File

@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto
FlightTaskAuto.cpp
)
target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane)
target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility WeatherVane)
target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

17
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -156,12 +156,6 @@ bool FlightTaskAuto::update()
break;
}
if (_param_com_obs_avoid.get()) {
_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
_yawspeed_setpoint);
}
_checkEmergencyBraking();
Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp};
@ -477,17 +471,6 @@ bool FlightTaskAuto::_evaluateTriplets()
_updateInternalWaypoints();
}
if (_param_com_obs_avoid.get()
&& _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
_triplet_next_wp,
_sub_triplet_setpoint.get().next.yaw,
(float)NAN,
_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
_obstacle_avoidance.checkAvoidanceProgress(
_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
}
// set heading
_weathervane.update();

10
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@ -53,13 +53,6 @@
#include "StickAccelerationXY.hpp"
#include "StickYaw.hpp"
// TODO: make this switchable in the board config, like a module
#if CONSTRAINED_FLASH
#include <lib/avoidance/ObstacleAvoidance_dummy.hpp>
#else
#include <lib/avoidance/ObstacleAvoidance.hpp>
#endif
/**
* This enum has to agree with position_setpoint_s type definition
* The only reason for not using the struct position_setpoint is because
@ -147,8 +140,6 @@ protected:
AlphaFilter<float> _yawspeed_filter;
bool _yaw_sp_aligned{false};
ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */
PositionSmoothing _position_smoothing;
Vector3f _unsmoothed_velocity_setpoint;
Sticks _sticks{this};
@ -165,7 +156,6 @@ protected:
(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight

2
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp
View File

@ -68,5 +68,5 @@ private:
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
CollisionPrevention _collision_prevention{this}; /**< collision avoidance setpoint amendment */
CollisionPrevention _collision_prevention{this}; /**< collision prevention setpoint amendment */
};

2
src/modules/logger/params.c
View File

@ -123,7 +123,7 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0);
* 4 : Full rates for analysis of fast maneuvers (RC, attitude, rates and actuators)
* 5 : Debugging topics (debug_*.msg topics, for custom code)
* 6 : Topics for sensor comparison (low rate raw IMU, Baro and magnetometer data)
* 7 : Topics for computer vision and collision avoidance
* 7 : Topics for computer vision and collision prevention
* 8 : Raw FIFO high-rate IMU (Gyro)
* 9 : Raw FIFO high-rate IMU (Accel)
* 10: Logging of mavlink tunnel message (useful for payload communication debugging)