From bd8f7bfaac54cbb036381dd68b918f9d453db2a3 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 15 Mar 2024 09:58:24 +0900
Subject: [PATCH] AP_Mount: add set_rangefinder_enable for ViewPro

---
 libraries/AP_Mount/AP_Mount.cpp         | 10 ++++++++++
 libraries/AP_Mount/AP_Mount.h           |  3 +++
 libraries/AP_Mount/AP_Mount_Backend.h   |  3 +++
 libraries/AP_Mount/AP_Mount_Viewpro.cpp | 13 ++++++++++---
 libraries/AP_Mount/AP_Mount_Viewpro.h   | 14 +++++++++++++-
 5 files changed, 39 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 0a156dd1d8..3bfed9c292 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -898,6 +898,16 @@ bool AP_Mount::get_rangefinder_distance(uint8_t instance, float& distance_m) con
     return backend->get_rangefinder_distance(distance_m);
 }
 
+// enable/disable rangefinder.  Returns true on success
+bool AP_Mount::set_rangefinder_enable(uint8_t instance, bool enable)
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+    return backend->set_rangefinder_enable(enable);
+}
+
 AP_Mount_Backend *AP_Mount::get_primary() const
 {
     return get_instance(_primary);
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index 0a860d07c5..d67526302a 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -272,6 +272,9 @@ public:
     // get rangefinder distance.  Returns true on success
     bool get_rangefinder_distance(uint8_t instance, float& distance_m) const;
 
+    // enable/disable rangefinder.  Returns true on success
+    bool set_rangefinder_enable(uint8_t instance, bool enable);
+
     // parameter var table
     static const struct AP_Param::GroupInfo        var_info[];
 
diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
index adc2fcc8bb..56706d7463 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.h
+++ b/libraries/AP_Mount/AP_Mount_Backend.h
@@ -209,6 +209,9 @@ public:
     // get rangefinder distance.  Returns true on success
     virtual bool get_rangefinder_distance(float& distance_m) const { return false; }
 
+    // enable/disable rangefinder.  Returns true on success
+    virtual bool set_rangefinder_enable(bool enable) { return false; }
+
 protected:
 
     enum class MountTargetType {
diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp
index edce186adf..4dd78a82c5 100644
--- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp
+++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp
@@ -573,22 +573,23 @@ bool AP_Mount_Viewpro::send_target_angles(float pitch_rad, float yaw_rad, bool y
 }
 
 // send camera command, affected image sensor and value (e.g. zoom speed)
-bool AP_Mount_Viewpro::send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value)
+bool AP_Mount_Viewpro::send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value, LRFCommand lrf_cmd)
 {
     // fill in 2 bytes containing sensor, zoom speed, operation command and LRF
     // bit0~2: sensor
     // bit3~5: zoom speed
     // bit6~12: operation command no
-    // bit13~15: LRF command (unused)
+    // bit13~15: LRF command
     const uint16_t sensor_id = (uint16_t)img_sensor;
     const uint16_t zoom_speed = ((uint16_t)value & 0x07) << 3;
     const uint16_t operation_cmd = ((uint16_t)cmd & 0x7F) << 6;
+    const uint16_t rangefinder_cmd = ((uint16_t)lrf_cmd & 0x07) << 13;
 
     // fill in packet
     const C1Packet c1_packet {
         .content = {
             frame_id: FrameId::C1,
-            sensor_zoom_cmd_be:  htobe16(sensor_id | zoom_speed | operation_cmd)
+            sensor_zoom_cmd_be:  htobe16(sensor_id | zoom_speed | operation_cmd | rangefinder_cmd)
         }
     };
 
@@ -980,4 +981,10 @@ bool AP_Mount_Viewpro::get_rangefinder_distance(float& distance_m) const
     return true;
 }
 
+// enable/disable rangefinder.  Returns true on success
+bool AP_Mount_Viewpro::set_rangefinder_enable(bool enable)
+{
+    return send_camera_command(ImageSensor::NO_ACTION, CameraCommand::NO_ACTION, 0, enable ? LRFCommand::CONTINUOUS_RANGING_START : LRFCommand::STOP_RANGING);
+}
+
 #endif // HAL_MOUNT_VIEWPRO_ENABLED
diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h
index 552ce6d7b4..1ddaa28321 100644
--- a/libraries/AP_Mount/AP_Mount_Viewpro.h
+++ b/libraries/AP_Mount/AP_Mount_Viewpro.h
@@ -95,6 +95,9 @@ public:
     // get rangefinder distance.  Returns true on success
     bool get_rangefinder_distance(float& distance_m) const override;
 
+    // enable/disable rangefinder.  Returns true on success
+    bool set_rangefinder_enable(bool enable) override;
+
 protected:
 
     // get attitude as a quaternion.  returns true on success
@@ -161,6 +164,15 @@ private:
         MANUAL_FOCUS = 0x1A
     };
 
+    // C1 rangefinder commands
+    enum class LRFCommand : uint8_t {
+        NO_ACTION = 0x00,
+        SINGLE_RANGING = 0x01,
+        CONTINUOUS_RANGING_START = 0x02,
+        LPCL_CONTINUOUS_RANGING_START = 0x03,
+        STOP_RANGING = 0x05
+    };
+
     // C2 camera commands
     enum class CameraCommand2 : uint8_t {
         SET_EO_ZOOM = 0x53
@@ -351,7 +363,7 @@ private:
     bool send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef);
 
     // send camera command, affected image sensor and value (e.g. zoom speed)
-    bool send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value);
+    bool send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value, LRFCommand lrf_cmd = LRFCommand::NO_ACTION);
 
     // send camera command2 and corresponding value (e.g. zoom as absolute value)
     bool send_camera_command2(CameraCommand2 cmd, uint16_t value);