From aa65a2ab542243586202cfa3d65f887676937dfd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 24 Aug 2024 11:16:45 +0900 Subject: [PATCH] AP_Mount: siyi supports camera-thermal-range --- libraries/AP_Mount/AP_Mount.cpp | 12 +++++ libraries/AP_Mount/AP_Mount.h | 5 ++ libraries/AP_Mount/AP_Mount_Backend.h | 5 ++ libraries/AP_Mount/AP_Mount_Siyi.cpp | 69 +++++++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_Siyi.h | 23 +++++++++ libraries/AP_Mount/AP_Mount_config.h | 5 ++ 6 files changed, 119 insertions(+) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index c88856d40f..fe7f29f7ea 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/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) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 8db5c31a00..54762e9153 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -271,6 +271,11 @@ public: // 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); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 28e7dfd478..16082d58f4 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -198,6 +198,11 @@ public: // 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; } diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index c89b8cb676..1c3c8cf831 100644 --- 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,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 */ diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index bef712e5fd..20285b0b5b 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -82,6 +82,11 @@ public: // 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 @@ private: 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 @@ private: // 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 @@ private: }; 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; }; diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index a2da37fac5..f40a39cd05 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/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