Include a VisionTargetEstTask DEBUG which always runs the VTE

JonasPerolini · Oct 9, 2024 · 456cc47 · 456cc47
1 parent 02f4c0a
commit 456cc47
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 0 deletions.
diff --git a/src/modules/vision_target_estimator/VisionTargetEst.cpp b/src/modules/vision_target_estimator/VisionTargetEst.cpp
@@ -134,6 +134,8 @@ bool VisionTargetEst::init()
 	} else {
 
 		if (_vte_task_mask & VisionTargetEstTask::VTE_FOR_PREC_LAND) { PX4_INFO("VTE for precision landing.");}
+
+		if (_vte_task_mask & VisionTargetEstTask::VTE_DEBUG) { PX4_WARN("VTE for DEBUG. Always active.");}
 	}
 
 	if (_param_vte_pos_en.get()) {
@@ -243,6 +245,16 @@ bool VisionTargetEst::new_task_available()
 		_vte_current_task = VisionTargetEstTask::VTE_FOR_PREC_LAND;
 		return true;
 
+	} else if (_vte_task_mask & VisionTargetEstTask::VTE_DEBUG) {
+
+		// DEBUG task already running
+		if (_vte_current_task == VisionTargetEstTask::VTE_DEBUG) {
+			return false;
+		}
+
+		PX4_WARN("VTE, DEBUG task requested.");
+		_vte_current_task = VisionTargetEstTask::VTE_DEBUG;
+		return true;
 	}
 
 	// To add a new task:

diff --git a/src/modules/vision_target_estimator/VisionTargetEst.h b/src/modules/vision_target_estimator/VisionTargetEst.h
@@ -167,6 +167,7 @@ class VisionTargetEst : public ModuleBase<VisionTargetEst>, ModuleParams, px4::S
 		// Bit locations for VTE tasks
 		VTE_NO_TASK = 0,
 		VTE_FOR_PREC_LAND  = (1 << 0),    ///< set to true if target GPS position data is ready to be fused
+		VTE_DEBUG = (2 << 0) ///< set to true if target GPS position data is ready to be fused
 	};
 
 	int _vte_current_task{VisionTargetEstTask::VTE_NO_TASK};