AP_Mount: Viewpro supports get rangefinder distance

rmackay9 · Aug 22, 2023 · 6ec4fac · 6ec4fac
1 parent 37a3f85
commit 6ec4fac
Show file tree

Hide file tree

Showing 5 changed files with 48 additions and 0 deletions.
diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
@@ -824,6 +824,16 @@ void AP_Mount::send_camera_settings(uint8_t instance, mavlink_channel_t chan) co
     backend->send_camera_settings(chan);
 }
 
+// get rangefinder distance.  Returns true on success
+bool AP_Mount::get_rangefinder_distance(uint8_t instance, float& distance_m) const
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+    return backend->get_rangefinder_distance(distance_m);
+}
+
 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
@@ -247,6 +247,13 @@ class AP_Mount
     // send camera settings message to GCS
     void send_camera_settings(uint8_t instance, mavlink_channel_t chan) const;
 
+    //
+    // rangefinder
+    //
+
+    // get rangefinder distance.  Returns true on success
+    bool get_rangefinder_distance(uint8_t instance, float& distance_m) const;
+
     // 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
@@ -178,6 +178,13 @@ class AP_Mount_Backend
     // send camera settings message to GCS
     virtual void send_camera_settings(mavlink_channel_t chan) const {}
 
+    //
+    // rangefinder
+    //
+
+    // get rangefinder distance.  Returns true on success
+    virtual bool get_rangefinder_distance(float& distance_m) const { return false; }
+
 protected:
 
     enum class MountTargetType {

diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp
@@ -370,6 +370,9 @@ void AP_Mount_Viewpro::process_packet()
 
         // get optical zoom times
         _zoom_times = UINT16_VALUE(_msg_buff[_msg_buff_data_start+39], _msg_buff[_msg_buff_data_start+40]) * 0.1;
+
+        // get laser rangefinder distance
+        _rangefinder_dist_m = UINT16_VALUE(_msg_buff[_msg_buff_data_start+33], _msg_buff[_msg_buff_data_start+34]) * 0.1;
         break;
     }
 
@@ -915,4 +918,17 @@ void AP_Mount_Viewpro::send_camera_settings(mavlink_channel_t chan) const
         NaN);               // focusLevel float, percentage from 0 to 100, NaN if unknown
 }
 
+// get rangefinder distance.  Returns true on success
+bool AP_Mount_Viewpro::get_rangefinder_distance(float& distance_m) const
+{
+    // if not healthy or zero distance return false
+    // healthy() checks attitude timeout which is in same message as rangefinder distance
+    if (!healthy()) {
+        return false;
+    }
+
+    distance_m = _rangefinder_dist_m;
+    return true;
+}
+
 #endif // HAL_MOUNT_VIEWPRO_ENABLED
diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h
@@ -84,6 +84,13 @@ class AP_Mount_Viewpro : public AP_Mount_Backend
     // send camera settings message to GCS
     void send_camera_settings(mavlink_channel_t chan) const override;
 
+    //
+    // rangefinder
+    //
+
+    // get rangefinder distance.  Returns true on success
+    bool get_rangefinder_distance(float& distance_m) const override;
+
 protected:
 
     // get attitude as a quaternion.  returns true on success
@@ -384,6 +391,7 @@ class AP_Mount_Viewpro : public AP_Mount_Backend
     bool _got_firmware_version;                     // true once we have received the firmware version
     uint8_t _model_name[11] {};                     // model name received from gimbal
     bool _got_model_name;                           // true once we have received model name
+    float _rangefinder_dist_m;                      // latest rangefinder distance (in meters)
 };
 
 #endif // HAL_MOUNT_VIEWPRO_ENABLED