dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Check for OA interface failures only after it has been activated 

The OA interface is 'activated' when it receives the first trajectory message.
This commit is contained in:
Daniel Mesham 2022-11-08 14:10:27 +01:00 committed by Daniel Agar
parent e721d8dd8f
commit 1e39c4828f
2 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -86,8 +86,12 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
}
if (avoidance_invalid) {
PX4_WARN("Obstacle Avoidance system failed, loitering");
_publishVehicleCmdDoLoiter();
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
@ -98,10 +102,15 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
vel_sp.setNaN();
yaw_sp = NAN;
yaw_speed_sp = NAN;
// Do nothing further - wait until activation
return;
} else {
} 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) {

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

@ -130,6 +130,8 @@ protected:
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 */