From e6f07bde2aa1daef45ea6336d892efb4ef89b1fa Mon Sep 17 00:00:00 2001
From: Konrad <konrad@auterion.com>
Date: Mon, 9 Sep 2024 10:35:35 +0200
Subject: [PATCH] lightware_laser: add option to listen to system to
 enable/disable distance sensor

---
 msg/CMakeLists.txt                               |  1 +
 msg/DistanceSensorModeChangeRequest.msg          |  5 +++++
 .../lightware_laser_i2c/lightware_laser_i2c.cpp  | 16 +++++++++++++++-
 .../lightware_laser_i2c/parameters.c             |  2 +-
 4 files changed, 22 insertions(+), 2 deletions(-)
 create mode 100644 msg/DistanceSensorModeChangeRequest.msg

diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 91c38488f5..596ca30fcd 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -73,6 +73,7 @@ set(msg_files
 	DebugVect.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
+	DistanceSensorModeChangeRequest.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg
diff --git a/msg/DistanceSensorModeChangeRequest.msg b/msg/DistanceSensorModeChangeRequest.msg
new file mode 100644
index 0000000000..294e225e35
--- /dev/null
+++ b/msg/DistanceSensorModeChangeRequest.msg
@@ -0,0 +1,5 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 req_mode 			# requested mode of operation
+uint8 MODE_DISABLED = 0
+uint8 MODE_ENABLED  = 1
diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
index 6c1dfbcea0..152073f2ac 100644
--- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
+++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
@@ -55,6 +55,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/distance_sensor_mode_change_request.h>
 
 using namespace time_literals;
 
@@ -143,6 +144,8 @@ private:
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN};
+	uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)};
+	typeof(px4::msg::DistanceSensorModeChangeRequest::req_mode) _req_mode{px4::msg::DistanceSensorModeChangeRequest::MODE_DISABLED};
 	bool _restriction{false};
 	bool _auto_restriction{false};
 	bool _prev_restriction{false};
@@ -412,6 +415,17 @@ void LightwareLaser::start()
 
 int LightwareLaser::updateRestriction()
 {
+	if (_dist_sense_mode_change_sub.updated()) {
+		distance_sensor_mode_change_request_s dist_sense_mode_change;
+
+		if (_dist_sense_mode_change_sub.copy(&dist_sense_mode_change)) {
+			_req_mode = dist_sense_mode_change.req_mode;
+
+		} else {
+			_req_mode = distance_sensor_mode_change_request_s::MODE_DISABLED;
+		}
+	}
+
 	px4::msg::VehicleStatus vehicle_status;
 
 	if (_vehicle_status_sub.update(&vehicle_status)) {
@@ -452,7 +466,7 @@ int LightwareLaser::updateRestriction()
 		break;
 
 	case 2:
-		_restriction = _auto_restriction;
+		_restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::MODE_ENABLED;
 		break;
 	}
 
diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c b/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c
index defe335751..161e9d3cab 100644
--- a/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c
+++ b/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c
@@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
  *
  * @value 0 Disabled
  * @value 1 Enabled
- * @value 2 Disabled during VTOL fast forward flight
+ * @value 2 Enabled in VTOL MC mode, listen to request from system in FW mode
  *
  * @min 0
  * @max 2