AP_Mount: Viewpro supports get rangefinder distance

rmackay9 · Aug 22, 2023 · 5bf6f10 · 5bf6f10
1 parent 37a3f85
commit 5bf6f10
Show file tree

Hide file tree

Showing 7 changed files with 56 additions and 1 deletion.
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.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp
@@ -387,6 +387,10 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us)
     bool target_yaw_is_ef = false;
     IGNORE_RETURN(get_angle_target(target_roll, target_pitch, target_yaw, target_yaw_is_ef));
 
+    // get rangefinder distance
+    float rangefinder_dist = nanf;
+    IGNORE_RETURN(get_rangefinder_distance(rangefinder_dist));
+
     const struct log_Mount pkt {
         LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(LOG_MOUNT_MSG)),
         time_us       : (timestamp_us > 0) ? timestamp_us : AP_HAL::micros64(),
@@ -399,6 +403,7 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us)
         actual_yaw_bf : yaw_bf,
         desired_yaw_ef: target_yaw_is_ef ? target_yaw : nanf,
         actual_yaw_ef : yaw_ef,
+        rangefinder_dist : rangefinder_dist,
     };
     AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
 }

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
diff --git a/libraries/AP_Mount/LogStructure.h b/libraries/AP_Mount/LogStructure.h
@@ -17,6 +17,7 @@
 // @Field: YawB: mount's actual yaw in body frame
 // @Field: DesYawE: mount's desired yaw in earth frame
 // @Field: YawE: mount's actual yaw in earth frame
+// @Field: Dist: rangefinder distance
 
 struct PACKED log_Mount {
     LOG_PACKET_HEADER;
@@ -30,9 +31,10 @@ struct PACKED log_Mount {
     float    actual_yaw_bf;
     float    desired_yaw_ef;
     float    actual_yaw_ef;
+    float    rangefinder_dist;
 };
 
 #define LOG_STRUCTURE_FROM_MOUNT \
     { LOG_MOUNT_MSG, sizeof(log_Mount), \
-      "MNT", "QBffffffff","TimeUS,I,DesRoll,Roll,DesPitch,Pitch,DesYawB,YawB,DesYawE,YawE", "s#dddddddd", "F---------" },
+      "MNT", "QBfffffffff","TimeUS,I,DesRoll,Roll,DesPitch,Pitch,DesYawB,YawB,DesYawE,YawE,Dist", "s#ddddddddm", "F---------0" },