reintroduce obstacle avoidance failsafe. If OA fails, switch to loiter

src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp

@@ -80,6 +80,10 @@ bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_
 		return false;
 	}
 
+	if (!subscription_array.get(ORB_ID(vehicle_status), _sub_vehicle_status)) {
+		return false;
+	}
+
 	return true;
 }
 
@@ -95,13 +99,18 @@ void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &ve
 					    > TRAJECTORY_STREAM_TIMEOUT_US;
 	const bool avoidance_point_valid =
 		_sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
+	const bool is_loitering = _sub_vehicle_status->get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 
-	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;
+	if ((avoidance_data_timeout || !avoidance_point_valid) && !is_loitering) {
+		PX4_WARN("Obstacle Avoidance system failed, loitering");
+		_publish_vehicle_cmd_do_loiter();
+		return;
 	}
+
+	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;
 }
 
 void ObstacleAvoidance::updateAvoidanceWaypoints(const Vector3f &curr_wp, const float curr_yaw,
@@ -194,3 +203,28 @@ ObstacleAvoidance::_publish_avoidance_desired_waypoint()
 						 &_desired_waypoint);
 	}
 }
+
+void ObstacleAvoidance::_publish_vehicle_cmd_do_loiter()
+{
+	vehicle_command_s command{};
+	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
+	if (_pub_vehicle_command == nullptr) {
+		_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
+				       vehicle_command_s::ORB_QUEUE_LENGTH);
+
+	} else {
+		orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
+	}
+}

src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp

@@ -44,7 +44,10 @@
 #include <px4_module_params.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_status.h>
 #include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
+#include <commander/px4_custom_mode.h>
 #include <matrix/matrix/math.hpp>
 #include <px4_defines.h>
 
@@ -67,6 +70,7 @@ public:
 private:
 
 	uORB::Subscription<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr};
+	uORB::Subscription<vehicle_status_s> *_sub_vehicle_status{nullptr};
 
   DEFINE_PARAMETERS(
     (ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID,             /**< obstacle avoidance enabled */
@@ -76,7 +80,9 @@ private:
 	vehicle_trajectory_waypoint_s _desired_waypoint = {};
 	orb_advert_t _traj_wp_avoidance_desired_pub{nullptr}; /**< trajectory waypoint desired publication */
 	orb_advert_t _pub_pos_control_status{nullptr};
+	orb_advert_t _pub_vehicle_command{nullptr};
 
 	void _publish_avoidance_desired_waypoint();
+	void _publish_vehicle_cmd_do_loiter();
 
 };