Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: siyi supports camera-thermal-range

Browse Source
This commit is contained in:
Randy Mackay 2024-08-24 11:16:45 +09:00
parent 6f2a88f707
commit aa65a2ab54
6 changed files with 119 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Mount/AP_Mount.cpp
View File

@ -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)

5
libraries/AP_Mount/AP_Mount.h
View File

@ -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);

5
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -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; }

69
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -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
*/

23
libraries/AP_Mount/AP_Mount_Siyi.h
View File

@ -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;
};

5
libraries/AP_Mount/AP_Mount_config.h
View File

@ -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