AP_Camera/AP_Mount/GCS_MAVLink: Siyi implements camera-thermal-range

Tools/scripts/build_options.py

@@ -136,6 +136,7 @@ def __init__(self,
     Feature('Camera', 'Camera_Servo', 'AP_CAMERA_SERVO_ENABLED', 'Enable Camera Trigger via Servo support', 0, 'Camera'),
     Feature('Camera', 'Camera_Solo', 'AP_CAMERA_SOLOGIMBAL_ENABLED', 'Enable Solo Gimbal support', 0, 'Camera'),
     Feature('Camera', 'Camera_FOV_Status', 'AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'Enable Camera FOV Status send to GCS', 0, 'Camera,MOUNT'),  # noqa: E501
+    Feature('Camera', 'Camera_ThermalRange', 'AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'Enable Camera Thermal Range send to GCS', 0, 'Camera,MOUNT'),  # noqa: E501
 
     Feature('Camera', 'RUNCAM', 'HAL_RUNCAM_ENABLED', 'Enable RunCam Control', 0, None),
 

Tools/scripts/extract_features.py

@@ -136,6 +136,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_CAMERA_ENABLED', 'AP_Camera::var_info',),
             ('AP_CAMERA_{type}_ENABLED', 'AP_Camera_(?P<type>.*)::trigger_pic',),
             ('AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'AP_Camera::send_camera_fov_status'),
+            ('AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'AP_Camera::send_camera_thermal_range'),
             ('HAL_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',),
 
             ('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',),

libraries/AP_Camera/AP_Camera.cpp

@@ -461,6 +461,12 @@ bool AP_Camera::send_mavlink_message(GCS_MAVLINK &link, const enum ap_message ms
         CHECK_PAYLOAD_SIZE2(CAMERA_CAPTURE_STATUS);
         send_camera_capture_status(chan);
         break;
+#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+    case MSG_CAMERA_THERMAL_RANGE:
+        CHECK_PAYLOAD_SIZE2(CAMERA_THERMAL_RANGE);
+        send_camera_thermal_range(chan);
+        break;
+#endif
 
     default:
         // should not reach this; should only be called for specific IDs
@@ -615,6 +621,21 @@ void AP_Camera::send_camera_capture_status(mavlink_channel_t chan)
     }
 }
 
+#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+// send camera thermal range message to GCS
+void AP_Camera::send_camera_thermal_range(mavlink_channel_t chan)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    // call each instance
+    for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->send_camera_thermal_range(chan);
+        }
+    }
+}
+#endif
+
 /*
   update; triggers by distance moved and camera trigger
 */

libraries/AP_Camera/AP_Camera.h

@@ -240,6 +240,11 @@ class AP_Camera {
     // send camera capture status message to GCS
     void send_camera_capture_status(mavlink_channel_t chan);
 
+#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+    // send camera thermal range message to GCS
+    void send_camera_thermal_range(mavlink_channel_t chan);
+#endif
+
     HAL_Semaphore _rsem;                // semaphore for multi-thread access
     AP_Camera_Backend *primary;         // primary camera backed
     bool _is_in_auto_mode;              // true if in AUTO mode

libraries/AP_Camera/AP_Camera_Backend.h

@@ -126,6 +126,11 @@ class AP_Camera_Backend
     // send camera capture status message to GCS
     virtual void send_camera_capture_status(mavlink_channel_t chan) const;
 
+#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+    // send camera thermal range message to GCS
+    virtual void send_camera_thermal_range(mavlink_channel_t chan) const {};
+#endif
+
 #if AP_CAMERA_SCRIPTING_ENABLED
     // accessor to allow scripting backend to retrieve state
     // returns true on success and cam_state is filled in

libraries/AP_Camera/AP_Camera_Mount.cpp

@@ -109,6 +109,19 @@ void AP_Camera_Mount::send_camera_capture_status(mavlink_channel_t chan) const
     }
 }
 
+#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+// send camera thermal range message to GCS
+void AP_Camera_Mount::send_camera_thermal_range(mavlink_channel_t chan) const
+{
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+    AP_Mount* mount = AP::mount();
+    if (mount != nullptr) {
+        mount->send_camera_thermal_range(get_mount_instance(), chan);
+    }
+#endif
+}
+#endif // AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+
 #if AP_CAMERA_SCRIPTING_ENABLED
 // change camera settings not normally used by autopilot
 bool AP_Camera_Mount::change_setting(CameraSetting setting, float value)

libraries/AP_Camera/AP_Camera_Mount.h

@@ -70,6 +70,11 @@ class AP_Camera_Mount : public AP_Camera_Backend
     // send camera capture status message to GCS
     void send_camera_capture_status(mavlink_channel_t chan) const override;
 
+#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+    // send camera thermal range message to GCS
+    void send_camera_thermal_range(mavlink_channel_t chan) const override;
+#endif
+
 #if AP_CAMERA_SCRIPTING_ENABLED
     // change camera settings not normally used by autopilot
     bool change_setting(CameraSetting setting, float value) override;

libraries/AP_Camera/AP_Camera_config.h

@@ -50,6 +50,11 @@
 #define AP_CAMERA_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
 #endif
 
+// send thermal range is supported on a few thermal cameras but all are within mounts
+#ifndef AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+#define AP_CAMERA_SEND_THERMAL_RANGE_ENABLED AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+#endif
+
 #ifndef HAL_RUNCAM_ENABLED
 #define HAL_RUNCAM_ENABLED 1
 #endif

libraries/AP_Mount/AP_Mount.cpp

@@ -907,6 +907,18 @@ void AP_Mount::send_camera_capture_status(uint8_t instance, mavlink_channel_t ch
     backend->send_camera_capture_status(chan);
 }
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+// send camera thermal range message to GCS
+void AP_Mount::send_camera_thermal_range(uint8_t instance, mavlink_channel_t chan) const
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return;
+    }
+    backend->send_camera_thermal_range(chan);
+}
+#endif
+
 // change camera settings not normally used by autopilot
 // setting values from AP_Camera::Setting enum
 bool AP_Mount::change_setting(uint8_t instance, CameraSetting setting, float value)

libraries/AP_Mount/AP_Mount.h

@@ -271,6 +271,11 @@ class AP_Mount
     // send camera capture status message to GCS
     void send_camera_capture_status(uint8_t instance, mavlink_channel_t chan) const;
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+    // send camera thermal range message to GCS
+    void send_camera_thermal_range(uint8_t instance, mavlink_channel_t chan) const;
+#endif
+
     // change camera settings not normally used by autopilot
     // setting values from AP_Camera::Setting enum
     bool change_setting(uint8_t instance, CameraSetting setting, float value);

libraries/AP_Mount/AP_Mount_Backend.h

@@ -198,6 +198,11 @@ class AP_Mount_Backend
     // send camera capture status message to GCS
     virtual void send_camera_capture_status(mavlink_channel_t chan) const {}
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+    // send camera thermal status message to GCS
+    virtual void send_camera_thermal_range(mavlink_channel_t chan) const {}
+#endif
+
     // change camera settings not normally used by autopilot
     virtual bool change_setting(CameraSetting setting, float value) { return false; }
 

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,11 @@ void AP_Mount_Siyi::update()
         _last_rangefinder_req_ms = now_ms;
     }
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+    // request thermal min/max from ZT30 or ZT6
+    request_thermal_minmax();
+#endif
+
     // send attitude to gimbal at 10Hz
     if (now_ms - _last_attitude_send_ms > 100) {
         _last_attitude_send_ms = now_ms;
@@ -544,6 +550,25 @@ void AP_Mount_Siyi::process_packet()
         break;
     }
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+    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]);
+        break;
+    }
+#endif
+
     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();
@@ -1098,6 +1123,29 @@ void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const
         NaN);               // focusLevel float, percentage from 0 to 100, NaN if unknown
 }
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+// send camera thermal range message to GCS
+void AP_Mount_Siyi::send_camera_thermal_range(mavlink_channel_t chan) const
+{
+    const float NaN = nanf("0x4152");
+    const uint32_t now_ms = AP_HAL::millis();
+    bool timeout = now_ms - _thermal.last_update_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS;
+
+    // send CAMERA_THERMAL_RANGE message
+    mavlink_msg_camera_thermal_range_send(
+        chan,
+        now_ms,             // time_boot_ms
+        _instance + 1,      // video stream id (assume one-to-one mapping with camera id)
+        _instance + 1,      // camera device id
+        timeout ? NaN : _thermal.max_C,     // max in degC
+        timeout ? NaN : _thermal.max_pos.x, // max x position
+        timeout ? NaN : _thermal.max_pos.y, // max y position
+        timeout ? NaN : _thermal.min_C,     // min in degC
+        timeout ? NaN : _thermal.min_pos.x, // min x position
+        timeout ? NaN : _thermal.min_pos.y);// min y position
+}
+#endif
+
 // change camera settings not normally used by autopilot
 // THERMAL_PALETTE: 0:WhiteHot, 2:Sepia, 3:IronBow, 4:Rainbow, 5:Night, 6:Aurora, 7:RedHot, 8:Jungle, 9:Medical, 10:BlackHot, 11:GloryHot
 // THERMAL_GAIN: 0:Low gain (50C ~ 550C), 1:High gain (-20C ~ 150C)
@@ -1193,6 +1241,27 @@ void AP_Mount_Siyi::check_firmware_version() const
     }
 }
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+// 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;
+    }
+}
+#endif
+
 /*
   send ArduPilot attitude and position to gimbal
 */

libraries/AP_Mount/AP_Mount_Siyi.h

@@ -82,6 +82,11 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial
     // send camera settings message to GCS
     void send_camera_settings(mavlink_channel_t chan) const override;
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+    // send camera thermal range message to GCS
+    void send_camera_thermal_range(mavlink_channel_t chan) const override;
+#endif
+
     // change camera settings not normally used by autopilot
     // THERMAL_PALETTE: 0:WhiteHot, 2:Sepia, 3:IronBow, 4:Rainbow, 5:Night, 6:Aurora, 7:RedHot, 8:Jungle, 9:Medical, 10:BlackHot, 11:GloryHot
     // THERMAL_GAIN: 0:Low gain (50C ~ 550C), 1:High gain (-20C ~ 150C)
@@ -123,6 +128,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 +306,11 @@ 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;
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+    // get thermal min/max if available at 5hz
+    void request_thermal_minmax();
+#endif
+
     // internal variables
     bool _got_hardware_id;                          // true once hardware id ha been received
 
@@ -354,6 +365,18 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial
     };
     static const HWInfo hardware_lookup_table[];
 
+#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+    // 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;
+#endif
+
     // count of SET_TIME packets, we send 5 times to cope with packet loss
     uint8_t sent_time_count;
 };

libraries/AP_Mount/AP_Mount_config.h

@@ -64,3 +64,8 @@
 #ifndef HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
 #define HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SIYI_ENABLED || HAL_MOUNT_XACTI_ENABLED || HAL_MOUNT_VIEWPRO_ENABLED
 #endif
+
+// send thermal range is only support on Siyi cameras
+#ifndef AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
+#define AP_MOUNT_SEND_THERMAL_RANGE_ENABLED HAL_MOUNT_SIYI_ENABLED
+#endif

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1060,7 +1060,10 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
         { MAVLINK_MSG_ID_CAMERA_SETTINGS,       MSG_CAMERA_SETTINGS},
         { MAVLINK_MSG_ID_CAMERA_FOV_STATUS,     MSG_CAMERA_FOV_STATUS},
         { MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, MSG_CAMERA_CAPTURE_STATUS},
-#endif
+#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+        { MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE,  MSG_CAMERA_THERMAL_RANGE},
+#endif // AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+#endif // AP_CAMERA_ENABLED
 #if HAL_MOUNT_ENABLED
         { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS},
         { MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE},
@@ -6131,6 +6134,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
     case MSG_CAMERA_FOV_STATUS:
 #endif
     case MSG_CAMERA_CAPTURE_STATUS:
+#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
+    case MSG_CAMERA_THERMAL_RANGE:
+#endif
         {
             AP_Camera *camera = AP::camera();
             if (camera == nullptr) {

libraries/GCS_MAVLink/ap_message.h

@@ -57,6 +57,7 @@ enum ap_message : uint8_t {
     MSG_CAMERA_SETTINGS,
     MSG_CAMERA_FOV_STATUS,
     MSG_CAMERA_CAPTURE_STATUS,
+    MSG_CAMERA_THERMAL_RANGE,
     MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
     MSG_GIMBAL_MANAGER_INFORMATION,
     MSG_GIMBAL_MANAGER_STATUS,