From 2c37958c3f44ab85d4bdc436dbb22cf50ee8c33d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 12 Jun 2023 12:10:19 +0900 Subject: [PATCH] AP_Mount: support send-camera-info and settings --- libraries/AP_Mount/AP_Mount.cpp | 22 ++++++++ libraries/AP_Mount/AP_Mount.h | 6 ++ libraries/AP_Mount/AP_Mount_Backend.h | 6 ++ libraries/AP_Mount/AP_Mount_Siyi.cpp | 81 +++++++++++++++++++++++++-- libraries/AP_Mount/AP_Mount_Siyi.h | 11 ++++ 5 files changed, 122 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 66d2d8e8f4..a81151281e 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -742,6 +742,28 @@ bool AP_Mount::set_focus(uint8_t instance, FocusType focus_type, float focus_val return backend->set_focus(focus_type, focus_value); } +// send camera information message to GCS +void AP_Mount::send_camera_information(mavlink_channel_t chan) const +{ + // call send_camera_information for each instance + for (uint8_t instance=0; instancesend_camera_information(chan); + } + } +} + +// send camera settings message to GCS +void AP_Mount::send_camera_settings(mavlink_channel_t chan) const +{ + // call send_camera_settings for each instance + for (uint8_t instance=0; instancesend_camera_settings(chan); + } + } +} + AP_Mount_Backend *AP_Mount::get_primary() const { return get_instance(_primary); diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 6048e29fd0..690d3930c4 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -212,6 +212,12 @@ public: // focus in = -1, focus hold = 0, focus out = 1 bool set_focus(uint8_t instance, FocusType focus_type, float focus_value); + // send camera information message to GCS + void send_camera_information(mavlink_channel_t chan) const; + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan) const; + // parameter var table static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 63e400b898..4859e9e4e3 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -154,6 +154,12 @@ public: // focus in = -1, focus hold = 0, focus out = 1 virtual bool set_focus(FocusType focus_type, float focus_value) { return false; } + // send camera information message to GCS + virtual void send_camera_information(mavlink_channel_t chan) const {} + + // send camera settings message to GCS + virtual void send_camera_settings(mavlink_channel_t chan) const {} + protected: enum class MountTargetType { diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 488ace08ec..ec10882dbf 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -310,11 +310,15 @@ void AP_Mount_Siyi::process_packet() // set hardware version based on message length _hardware_model = (_parsed_msg.data_bytes_received <= 8) ? HardwareModel::A8 : HardwareModel::ZR10; - // display camera firmware version + // consume and display camera firmware version + _cam_firmware_version = {_msg_buff[_msg_buff_data_start+2], // firmware major version + _msg_buff[_msg_buff_data_start+1], // firmware minor version + _msg_buff[_msg_buff_data_start+0]}; // firmware revision (aka patch) + debug("Mount: SiyiCam fw:%u.%u.%u", - (unsigned)_msg_buff[_msg_buff_data_start+2], // firmware major version - (unsigned)_msg_buff[_msg_buff_data_start+1], // firmware minor version - (unsigned)_msg_buff[_msg_buff_data_start+0]); // firmware revision + (unsigned)_cam_firmware_version.major, // firmware major version + (unsigned)_cam_firmware_version.minor, // firmware minor version + (unsigned)_cam_firmware_version.patch); // firmware revision // display gimbal info to user gcs().send_text(MAV_SEVERITY_INFO, "Mount: Siyi fw:%u.%u.%u", @@ -779,4 +783,73 @@ bool AP_Mount_Siyi::set_focus(FocusType focus_type, float focus_value) return false; } +// send camera information message to GCS +void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const +{ + // exit immediately if not initialised + if (!_initialised || !_got_firmware_version) { + return; + } + + const uint8_t vendor_name[32] = "Siyi"; + uint8_t model_name[32] = "Unknown"; + uint32_t fw_version = _cam_firmware_version.major | (_cam_firmware_version.minor << 8) | (_cam_firmware_version.patch << 16); + const char cam_definition_uri[140] {}; + + // focal length + float focal_length_mm = 0; + switch (_hardware_model) { + case HardwareModel::UNKNOWN: + break; + case HardwareModel::A8: + strncpy((char *)model_name, "A8", sizeof(model_name)); + focal_length_mm = 21; + break; + case HardwareModel::ZR10: + strncpy((char *)model_name, "ZR10", sizeof(model_name)); + // focal length range from 5.15 ~ 47.38 + focal_length_mm = 5.15; + break; + } + + // capability flags + const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO | + CAMERA_CAP_FLAGS_CAPTURE_IMAGE | + CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM | + CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS; + + // send CAMERA_INFORMATION message + mavlink_msg_camera_information_send( + chan, + AP_HAL::millis(), // time_boot_ms + vendor_name, // vendor_name uint8_t[32] + model_name, // model_name uint8_t[32] + fw_version, // firmware version uint32_t + focal_length_mm, // focal_length float (mm) + 0, // sensor_size_h float (mm) + 0, // sensor_size_v float (mm) + 0, // resolution_h uint16_t (pix) + 0, // resolution_v uint16_t (pix) + 0, // lens_id uint8_t + flags, // flags uint32_t (CAMERA_CAP_FLAGS) + 0, // cam_definition_version uint16_t + cam_definition_uri); // cam_definition_uri char[140] +} + +// send camera settings message to GCS +void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const +{ + const float NaN = nanf("0x4152"); + const float zoom_mult_max = get_zoom_mult_max(); + const float zoom_pct = is_positive(zoom_mult_max) ? (_zoom_mult / zoom_mult_max * 100) : 0; + + // send CAMERA_SETTINGS message + mavlink_msg_camera_settings_send( + chan, + AP_HAL::millis(), // time_boot_ms + _last_record_video ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey) + zoom_pct, // zoomLevel float, percentage from 0 to 100, NaN if unknown + NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown +} + #endif // HAL_MOUNT_SIYI_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 7855942bcd..74cefafab2 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -72,6 +72,12 @@ public: // focus in = -1, focus hold = 0, focus out = 1 bool set_focus(FocusType focus_type, float focus_value) override; + // send camera information message to GCS + void send_camera_information(mavlink_channel_t chan) const override; + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan) const override; + protected: // get attitude as a quaternion. returns true on success @@ -200,6 +206,11 @@ private: AP_HAL::UARTDriver *_uart; // uart connected to gimbal bool _initialised; // true once the driver has been initialised bool _got_firmware_version; // true once gimbal firmware version has been received + struct { + uint8_t major; + uint8_t minor; + uint8_t patch; + } _cam_firmware_version; // camera firmware version (for reporting for GCS) // buffer holding bytes from latest packet. This is only used to calculate the crc uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX];