AP_Mount: support send-camera-info and settings

This commit is contained in:
Randy Mackay 2023-06-12 12:10:19 +09:00
parent c0cf43d5b5
commit 2c37958c3f
5 changed files with 122 additions and 4 deletions

View File

@ -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); 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; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != nullptr) {
_backends[instance]->send_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; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != nullptr) {
_backends[instance]->send_camera_settings(chan);
}
}
}
AP_Mount_Backend *AP_Mount::get_primary() const AP_Mount_Backend *AP_Mount::get_primary() const
{ {
return get_instance(_primary); return get_instance(_primary);

View File

@ -212,6 +212,12 @@ public:
// focus in = -1, focus hold = 0, focus out = 1 // focus in = -1, focus hold = 0, focus out = 1
bool set_focus(uint8_t instance, FocusType focus_type, float focus_value); 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 // parameter var table
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];

View File

@ -154,6 +154,12 @@ public:
// focus in = -1, focus hold = 0, focus out = 1 // focus in = -1, focus hold = 0, focus out = 1
virtual bool set_focus(FocusType focus_type, float focus_value) { return false; } 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: protected:
enum class MountTargetType { enum class MountTargetType {

View File

@ -310,11 +310,15 @@ void AP_Mount_Siyi::process_packet()
// set hardware version based on message length // set hardware version based on message length
_hardware_model = (_parsed_msg.data_bytes_received <= 8) ? HardwareModel::A8 : HardwareModel::ZR10; _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", debug("Mount: SiyiCam fw:%u.%u.%u",
(unsigned)_msg_buff[_msg_buff_data_start+2], // firmware major version (unsigned)_cam_firmware_version.major, // firmware major version
(unsigned)_msg_buff[_msg_buff_data_start+1], // firmware minor version (unsigned)_cam_firmware_version.minor, // firmware minor version
(unsigned)_msg_buff[_msg_buff_data_start+0]); // firmware revision (unsigned)_cam_firmware_version.patch); // firmware revision
// display gimbal info to user // display gimbal info to user
gcs().send_text(MAV_SEVERITY_INFO, "Mount: Siyi fw:%u.%u.%u", 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; 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 #endif // HAL_MOUNT_SIYI_ENABLED

View File

@ -72,6 +72,12 @@ public:
// focus in = -1, focus hold = 0, focus out = 1 // focus in = -1, focus hold = 0, focus out = 1
bool set_focus(FocusType focus_type, float focus_value) override; 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: protected:
// get attitude as a quaternion. returns true on success // get attitude as a quaternion. returns true on success
@ -200,6 +206,11 @@ private:
AP_HAL::UARTDriver *_uart; // uart connected to gimbal AP_HAL::UARTDriver *_uart; // uart connected to gimbal
bool _initialised; // true once the driver has been initialised bool _initialised; // true once the driver has been initialised
bool _got_firmware_version; // true once gimbal firmware version has been received 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 // buffer holding bytes from latest packet. This is only used to calculate the crc
uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX]; uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX];