From eba0bb389a1c475312b2086a820eedd4eed3ca92 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Wed, 24 Apr 2019 08:30:40 +0200
Subject: [PATCH] ObstacleAvoidance: fix comment, update failsafe position if
one axis is NAN
---
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp | 4 ++--
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp | 2 +-
2 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
index 4f78b9c6fe..080abaa6e0 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
@@ -103,8 +103,8 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
PX4_WARN("Obstacle Avoidance system failed, loitering");
_publishVehicleCmdDoLoiter();
- if (!PX4_ISFINITE(_failsafe_position(0)) && !PX4_ISFINITE(_failsafe_position(1))
- && !PX4_ISFINITE(_failsafe_position(2))) {
+ if (!PX4_ISFINITE(_failsafe_position(0)) || !PX4_ISFINITE(_failsafe_position(1))
+ || !PX4_ISFINITE(_failsafe_position(2))) {
// save vehicle position when entering failsafe
_failsafe_position = _position;
}
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
index fcb97816bf..0472882a6d 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
@@ -116,7 +116,7 @@ private:
orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */
matrix::Vector3f _curr_wp = {}; /**< current position triplet */
- matrix::Vector3f _position = {}; /**< current position triplet */
+ matrix::Vector3f _position = {}; /**< current vehicle position */
matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */
/**
--
GitLab