mirror of https://github.com/ArduPilot/ardupilot
AP_Mount: fix for Siyi A8
This commit is contained in:
parent
34bb5e10cd
commit
345629cdad
|
@ -294,7 +294,8 @@ void AP_Mount_Siyi::process_packet()
|
|||
switch ((SiyiCommandId)_parsed_msg.command_id) {
|
||||
|
||||
case SiyiCommandId::ACQUIRE_FIRMWARE_VERSION: {
|
||||
if (_parsed_msg.data_bytes_received != 12) {
|
||||
if (_parsed_msg.data_bytes_received != 12 && // ZR10 firmware version reply is 12bytes
|
||||
_parsed_msg.data_bytes_received != 8) { // A8 firmware version reply is 8 bytes
|
||||
#if AP_MOUNT_SIYI_DEBUG
|
||||
unexpected_len = true;
|
||||
#endif
|
||||
|
@ -315,10 +316,14 @@ void AP_Mount_Siyi::process_packet()
|
|||
(unsigned)_msg_buff[_msg_buff_data_start+7]); // firmware revision
|
||||
|
||||
// display zoom firmware version
|
||||
debug("Mount: SiyiZoom fw:%u.%u.%u",
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+9], // firmware major version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+10], // firmware minor version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+11]); // firmware revision
|
||||
#if AP_MOUNT_SIYI_DEBUG
|
||||
if (_parsed_msg.data_bytes_received >= 12) {
|
||||
debug("Mount: SiyiZoom fw:%u.%u.%u",
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+9], // firmware major version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+10], // firmware minor version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+11]); // firmware revision
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue