AP_Mount: add set_rangefinder_enable for ViewPro

libraries/AP_Mount/AP_Mount.cpp

@@ -873,6 +873,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);

libraries/AP_Mount/AP_Mount.h

@@ -266,6 +266,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[];
 

libraries/AP_Mount/AP_Mount_Backend.h

@@ -203,6 +203,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 {

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)
         }
     };
 
@@ -939,4 +940,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

libraries/AP_Mount/AP_Mount_Viewpro.h

@@ -91,6 +91,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
@@ -157,6 +160,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
@@ -347,7 +359,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);