Skip to content

Commit

Permalink
AP_Mount: Siyi gets improved model detection
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmackay9
rmackay9 committed Aug 24, 2023
1 parent ccaabf7 commit d74acc6
Show file tree
Hide file tree
Showing 2 changed files with 66 additions and 12 deletions.
60 changes: 50 additions & 10 deletions libraries/AP_Mount/AP_Mount_Siyi.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,16 @@ extern const AP_HAL::HAL& hal;
#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)

// hardware lookup table indexed by HardwareModel enum values
const AP_Mount_Siyi::HWInfo AP_Mount_Siyi::hardware_lookup_table[] {
{{'0','0'}, "Unknown"},
{{'7','5'}, "A2"},
{{'7','3'}, "A8"},
{{'6','B'}, "ZR10"},
{{'7','8'}, "ZR30"},
{{'7','A'}, "ZT30"},
};

// init - performs any required initialisation for this instance
void AP_Mount_Siyi::init()
{
Expand Down Expand Up @@ -54,6 +64,9 @@ void AP_Mount_Siyi::update()
if (!_got_firmware_version) {
request_firmware_version();
return;
} else if (!_got_hardware_id) {
request_hardware_id();
return;
} else {
request_configuration();
}
Expand Down Expand Up @@ -309,9 +322,6 @@ void AP_Mount_Siyi::process_packet()
}
_got_firmware_version = true;

// set hardware version based on message length
_hardware_model = (_parsed_msg.data_bytes_received <= 8) ? HardwareModel::A8 : HardwareModel::ZR10;

// consume and display camera firmware version
_cam_firmware_version = {
_msg_buff[_msg_buff_data_start+2], // firmware major version
Expand Down Expand Up @@ -342,9 +352,19 @@ void AP_Mount_Siyi::process_packet()
break;
}

case SiyiCommandId::HARDWARE_ID:
// unsupported
case SiyiCommandId::HARDWARE_ID: {
// lookup first two digits of hardware id
const uint8_t hwid0 = _msg_buff[_msg_buff_data_start];
const uint8_t hwid1 = _msg_buff[_msg_buff_data_start+1];
for (uint8_t i=1; i<ARRAY_SIZE(hardware_lookup_table); i++) {
if (hwid0 == hardware_lookup_table[i].hwid[0] && hwid1 == hardware_lookup_table[i].hwid[1]) {
_hardware_model = (HardwareModel)i;
gcs().send_text(MAV_SEVERITY_INFO, "Mount: Siyi %s", get_model_name());
}
}
_got_hardware_id = true;
break;
}

case SiyiCommandId::AUTO_FOCUS:
#if AP_MOUNT_SIYI_DEBUG
Expand Down Expand Up @@ -686,11 +706,14 @@ float AP_Mount_Siyi::get_zoom_mult_max() const
switch (_hardware_model) {
case HardwareModel::UNKNOWN:
return 0;
case HardwareModel::A2:
case HardwareModel::A8:
// a8 has 6x digital zoom
return 6;
case HardwareModel::ZR10:
// zr10 has 30x hybrid zoom (optical + digital)
case HardwareModel::ZR30:
case HardwareModel::ZT30:
// 30x hybrid zoom (optical + digital)
return 30;
}
return 0;
Expand All @@ -717,12 +740,15 @@ bool AP_Mount_Siyi::set_zoom(ZoomType zoom_type, float zoom_value)
case HardwareModel::UNKNOWN:
// unknown model
return false;
case HardwareModel::A2:
case HardwareModel::A8:
// set internal zoom control target
_zoom_type = zoom_type;
_zoom_mult_target = zoom_mult;
return true;
case HardwareModel::ZR10:
case HardwareModel::ZR30:
case HardwareModel::ZT30:
if (send_zoom_mult(zoom_mult)) {
_zoom_type = zoom_type;
_zoom_mult_target = 0;
Expand Down Expand Up @@ -819,21 +845,25 @@ void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const
}

static const uint8_t vendor_name[32] = "Siyi";
static uint8_t model_name[32] = "Unknown";
static uint8_t model_name[32] {};
const uint32_t fw_version = _cam_firmware_version.major | (_cam_firmware_version.minor << 8) | (_cam_firmware_version.patch << 16);
const char cam_definition_uri[140] {};

// copy model name
strncpy((char *)model_name, get_model_name(), sizeof(model_name)-1);

// focal length
// To-Do: check these values are correct for A2, ZR30, ZT30
float focal_length_mm = 0;
switch (_hardware_model) {
case HardwareModel::UNKNOWN:
break;
case HardwareModel::A2:
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));
case HardwareModel::ZR30:
case HardwareModel::ZT30:
// focal length range from 5.15 ~ 47.38
focal_length_mm = 5.15;
break;
Expand Down Expand Up @@ -880,4 +910,14 @@ void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const
NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown
}

// get model name string. returns "Unknown" if hardware model is not yet known
const char* AP_Mount_Siyi::get_model_name() const
{
uint8_t model_idx = (uint8_t)_hardware_model;
if (model_idx < ARRAY_SIZE(hardware_lookup_table)) {
return hardware_lookup_table[model_idx].model_name;
}
return hardware_lookup_table[0].model_name;
}

#endif // HAL_MOUNT_SIYI_ENABLED
18 changes: 16 additions & 2 deletions libraries/AP_Mount/AP_Mount_Siyi.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,9 +137,12 @@ class AP_Mount_Siyi : public AP_Mount_Backend

// hardware model enum
enum class HardwareModel : uint8_t {
UNKNOWN,
UNKNOWN = 0,
A2,
A8,
ZR10
ZR10,
ZR30,
ZT30
} _hardware_model;

// gimbal mounting method/direction
Expand Down Expand Up @@ -198,10 +201,14 @@ class AP_Mount_Siyi : public AP_Mount_Backend
// update zoom controller
void update_zoom_control();

// get model name string, returns nullptr if hardware id is unknown
const char* get_model_name() const;

// internal variables
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
bool _got_hardware_id; // true once hardware id ha been received
struct {
uint8_t major;
uint8_t minor;
Expand Down Expand Up @@ -242,6 +249,13 @@ class AP_Mount_Siyi : public AP_Mount_Backend
float _zoom_mult_target; // current zoom multiple target. 0 if no target
float _zoom_mult; // most recent actual zoom multiple received from camera
uint32_t _last_zoom_control_ms; // system time that zoom control was last run

// hardware lookup table indexed by HardwareModel enum values (see above)
struct HWInfo {
uint8_t hwid[2];
const char* model_name;
};
static const HWInfo hardware_lookup_table[];
};

#endif // HAL_MOUNT_SIYISERIAL_ENABLED

0 comments on commit d74acc6

Please sign in to comment.