AP_Mount: Siyi supports rangefinder distance

libraries/AP_Mount/AP_Mount_Siyi.cpp

@@ -77,6 +77,12 @@ void AP_Mount_Siyi::update()
         _last_req_current_angle_rad_ms = now_ms;
     }
 
+    // request rangefinder distance from ZT30 at 10hz
+    if ((_hardware_model == HardwareModel::ZT30) && (now_ms - _last_rangefinder_req_ms > 100)) {    
+        request_rangefinder_distance();
+        _last_rangefinder_req_ms = now_ms;
+    }
+
     // run zoom control
     update_zoom_control();
 
@@ -484,6 +490,11 @@ void AP_Mount_Siyi::process_packet()
         //const float pitch_rate_deg = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]) * 0.1;   // pitch rate
         //const float roll_rate_deg = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]) * 0.1;  // roll rate
         break;
+    
+    case SiyiCommandId::READ_RANGEFINDER:
+        _rangefinder_dist_m = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]);
+        _last_rangefinder_dist_ms = AP_HAL::millis();
+        break;
     }
 
     default:
@@ -926,4 +937,22 @@ const char* AP_Mount_Siyi::get_model_name() const
     return hardware_lookup_table[0].model_name;
 }
 
+// get rangefinder distance.  Returns true on success
+bool AP_Mount_Siyi::get_rangefinder_distance(float& distance_m) const
+{
+    // only supported on ZT30
+    if (_hardware_model != HardwareModel::ZT30) {
+        return false;
+    }
+
+    // unhealthy if distance not received recently
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - _last_rangefinder_dist_ms > AP_MOUNT_SIYI_TIMEOUT_MS) {
+        return false;
+    }
+
+    distance_m = _rangefinder_dist_m;
+    return true;
+}
+
 #endif // HAL_MOUNT_SIYI_ENABLED

libraries/AP_Mount/AP_Mount_Siyi.h

@@ -81,6 +81,13 @@ public:
     // 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
@@ -103,6 +110,7 @@ private:
         ACQUIRE_GIMBAL_ATTITUDE = 0x0D,
         ABSOLUTE_ZOOM = 0x0F,
         SET_CAMERA_IMAGE_TYPE = 0x11,
+        READ_RANGEFINDER = 0x15,
     };
 
     // Function Feedback Info packet info_type values
@@ -187,6 +195,7 @@ private:
     void request_configuration() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO, nullptr, 0); }
     void request_function_feedback_info() { send_packet(SiyiCommandId::FUNCTION_FEEDBACK_INFO, nullptr, 0); }
     void request_gimbal_attitude() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE, nullptr, 0); }
+    void request_rangefinder_distance() { send_packet(SiyiCommandId::READ_RANGEFINDER, nullptr, 0); }
 
     // rotate gimbal.  pitch_rate and yaw_rate are scalars in the range -100 ~ +100
     // yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock)
@@ -265,6 +274,11 @@ private:
     float _zoom_mult;                               // most recent actual zoom multiple received from camera
     uint32_t _last_zoom_control_ms;                 // system time that zoom control was last run
 
+    // rangefinder variables
+    uint32_t _last_rangefinder_req_ms;              // system time of last request for rangefinder distance
+    uint32_t _last_rangefinder_dist_ms;             // system time of last successful read of rangefinder distance
+    float _rangefinder_dist_m;                      // distance received from rangefinder
+
     // hardware lookup table indexed by HardwareModel enum values (see above)
     struct HWInfo {
         uint8_t hwid[2];