diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 9ab3b343ee95d..9f5ed474634a7 100644 --- a/Tools/scripts/build_options.py +++ b/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), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index b189ff25c81bf..54c13e39c92c4 100755 --- a/Tools/scripts/extract_features.py +++ b/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.*)::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',), diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index c37243723ed43..06a1147dc2ada 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/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 */ diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 28a27b0fa36b6..37f33942273fe 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index d52944f5c0e51..c9b55949e61db 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera_Mount.cpp b/libraries/AP_Camera/AP_Camera_Mount.cpp index b472d068eb5d6..9599097d8ce1c 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.cpp +++ b/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) diff --git a/libraries/AP_Camera/AP_Camera_Mount.h b/libraries/AP_Camera/AP_Camera_Mount.h index b3dd450652d14..19e56ff3c078b 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.h +++ b/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; diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index 24b51275be72c..6ff54ec978b57 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/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 diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index c88856d40f989..fe7f29f7eac3c 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 8db5c31a00689..54762e915307f 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/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); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 28e7dfd478fec..16082d58f450b 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/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; } diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index c89b8cb676e48..1c3c8cf8316d9 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 bef712e5fd585..20285b0b5bd6c 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/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; }; diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index a2da37fac5880..f40a39cd05b06 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 diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index bd3027072deef..75ae487b11ec8 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/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) { diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 4923317266025..d041127dfa163 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/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, diff --git a/modules/mavlink b/modules/mavlink index 60aa4ff8472cf..cdfe7a82380b4 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 60aa4ff8472cf91506256082bea8a1c3bbf9068b +Subproject commit cdfe7a82380b4dae477cf5fac2fe0089817ac560