diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 299169d160..bfaacaaafa 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -21,6 +21,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() { @@ -52,6 +62,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(); } @@ -307,9 +320,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 @@ -340,9 +350,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