AP_Mount: Siyi retrieves thermal min/max at 5hz

rmackay9 · Aug 24, 2024 · f1a79b4 · f1a79b4
1 parent 306b9f6
commit f1a79b4
Show file tree

Hide file tree

Showing 2 changed files with 63 additions and 0 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp
@@ -17,6 +17,7 @@ extern const AP_HAL::HAL& hal;
 #define AP_MOUNT_SIYI_PITCH_P       1.50    // pitch controller P gain (converts pitch angle error to target rate)
 #define AP_MOUNT_SIYI_YAW_P         1.50    // yaw controller P gain (converts yaw angle error to target rate)
 #define AP_MOUNT_SIYI_TIMEOUT_MS    1000    // timeout for health and rangefinder readings
+#define AP_MOUNT_SIYI_THERM_TIMEOUT_MS  3000// timeout for temp min/max readings
 
 #define AP_MOUNT_SIYI_DEBUG 0
 #define debug(fmt, args ...) do { if (AP_MOUNT_SIYI_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Siyi: " fmt, ## args); } } while (0)
@@ -82,6 +83,9 @@ void AP_Mount_Siyi::update()
         _last_rangefinder_req_ms = now_ms;
     }
 
+    // request thermal min/max from ZT30 or ZT6
+    request_thermal_minmax();
+
     // send attitude to gimbal at 10Hz
     if (now_ms - _last_attitude_send_ms > 100) {
         _last_attitude_send_ms = now_ms;
@@ -544,6 +548,32 @@ void AP_Mount_Siyi::process_packet()
         break;
     }
 
+    case SiyiCommandId::GET_TEMP_FULL_IMAGE: {
+        if (_parsed_msg.data_bytes_received != 12) {
+#if AP_MOUNT_SIYI_DEBUG
+            unexpected_len = true;
+#endif
+            break;
+        }
+        _thermal.last_update_ms = AP_HAL::millis();
+        _thermal.max_C = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.01;
+        _thermal.min_C = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+3], _msg_buff[_msg_buff_data_start+2]) * 0.01;
+        _thermal.max_pos.x = UINT16_VALUE(_msg_buff[_msg_buff_data_start+5], _msg_buff[_msg_buff_data_start+4]);
+        _thermal.max_pos.y = UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]);
+        _thermal.min_pos.x = UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]);
+        _thermal.min_pos.y = UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]);
+
+        gcs().send_named_float("ThermMaxC", _thermal.max_C);
+        gcs().send_named_float("ThermMinC", _thermal.min_C);
+        gcs().send_named_float("ThermMaxX", _thermal.max_pos.x);
+        gcs().send_named_float("ThermMaxY", _thermal.max_pos.y);
+        gcs().send_named_float("ThermMinX", _thermal.min_pos.x);
+        gcs().send_named_float("ThermMinY", _thermal.min_pos.y);
+
+        //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: therm max:%.2f min:%.2f pos:%u,%u pos:%u,%u", (double)_thermal.max_C, (double)_thermal.min_C, (unsigned)_thermal.max_pos.x, (unsigned)_thermal.max_pos.y, (unsigned)_thermal.min_pos.x, (unsigned)_thermal.min_pos.y);
+        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();
@@ -1193,6 +1223,25 @@ void AP_Mount_Siyi::check_firmware_version() const
     }
 }
 
+// get thermal min/max if available at 5hz
+void AP_Mount_Siyi::request_thermal_minmax()
+{
+    // only supported on ZT6 and ZT30
+    if (_hardware_model != HardwareModel::ZT6 &&
+        _hardware_model != HardwareModel::ZT30) {
+        return;
+    }
+
+    // check for timeout
+    uint32_t now_ms = AP_HAL::millis();
+    if ((now_ms - _thermal.last_update_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS) &&
+        (now_ms - _thermal.last_req_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS)) {
+        // request thermal min/max at 5hz
+        send_1byte_packet(SiyiCommandId::GET_TEMP_FULL_IMAGE, 2);
+        _thermal.last_req_ms = now_ms;
+    }
+}
+
 /*
   send ArduPilot attitude and position to gimbal
 */

diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h
@@ -123,6 +123,7 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial
         ACQUIRE_GIMBAL_ATTITUDE = 0x0D,
         ABSOLUTE_ZOOM = 0x0F,
         SET_CAMERA_IMAGE_TYPE = 0x11,
+        GET_TEMP_FULL_IMAGE = 0x14,
         READ_RANGEFINDER = 0x15,
         SET_THERMAL_PALETTE = 0x1B,
         EXTERNAL_ATTITUDE = 0x22,
@@ -300,6 +301,9 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial
     // Checks that the firmware version on the Gimbal meets the minimum supported version.
     void check_firmware_version() const;
 
+    // get thermal min/max if available at 5hz
+    void request_thermal_minmax();
+
     // internal variables
     bool _got_hardware_id;                          // true once hardware id ha been received
 
@@ -354,6 +358,16 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial
     };
     static const HWInfo hardware_lookup_table[];
 
+    // thermal variables
+    struct {
+        uint32_t last_req_ms;       // system time of last request for thermal min/max temperatures
+        uint32_t last_update_ms;    // system time of last update of thermal min/max temperatures
+        float max_C;                // thermal max temp in C
+        float min_C;                // thermal min temp in C
+        Vector2ui max_pos;          // thermal max temp position on image in pixels. x=0 is left, y=0 is top
+        Vector2ui min_pos;          // thermal min temp position on image in pixels. x=0 is left, y=0 is top
+    } _thermal;
+
     // count of SET_TIME packets, we send 5 times to cope with packet loss
     uint8_t sent_time_count;
 };