Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: Enable debugging outputs and variables in debug definitions

Browse Source
This commit is contained in:
murata 2022-10-01 03:59:51 +09:00 committed by Randy Mackay
parent 49b991b92d
commit b90079bfa6
1 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -265,8 +265,10 @@ void AP_Mount_Siyi::read_incoming_packets()
if (expected_crc == _parsed_msg.crc16) { if (expected_crc == _parsed_msg.crc16) {
// successfully received a message, do something with it // successfully received a message, do something with it
process_packet(); process_packet();
#if AP_MOUNT_SIYI_DEBUG
} else { } else {
debug("crc expected:%x got:%x", (unsigned)expected_crc, (unsigned)_parsed_msg.crc16); debug("crc expected:%x got:%x", (unsigned)expected_crc, (unsigned)_parsed_msg.crc16);
#endif
} }
reset_parser = true; reset_parser = true;
break; break;
@ -283,15 +285,19 @@ void AP_Mount_Siyi::read_incoming_packets()
// process successfully decoded packets held in the _parsed_msg structure // process successfully decoded packets held in the _parsed_msg structure
void AP_Mount_Siyi::process_packet() void AP_Mount_Siyi::process_packet()
{ {
#if AP_MOUNT_SIYI_DEBUG
// flag to warn of unexpected data buffer length // flag to warn of unexpected data buffer length
bool unexpected_len = false; bool unexpected_len = false;
#endif
// process packet depending upon command id // process packet depending upon command id
switch ((SiyiCommandId)_parsed_msg.command_id) { switch ((SiyiCommandId)_parsed_msg.command_id) {
case SiyiCommandId::ACQUIRE_FIRMWARE_VERSION: { case SiyiCommandId::ACQUIRE_FIRMWARE_VERSION: {
if (_parsed_msg.data_bytes_received != 12) { if (_parsed_msg.data_bytes_received != 12) {
#if AP_MOUNT_SIYI_DEBUG
unexpected_len = true; unexpected_len = true;
#endif
break; break;
} }
_got_firmware_version = true; _got_firmware_version = true;
@ -332,7 +338,9 @@ void AP_Mount_Siyi::process_packet()
case SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS: { case SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS: {
if (_parsed_msg.data_bytes_received != 2) { if (_parsed_msg.data_bytes_received != 2) {
#if AP_MOUNT_SIYI_DEBUG
unexpected_len = true; unexpected_len = true;
#endif
break; break;
} }
const float zoom_mult = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1; const float zoom_mult = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1;
@ -372,7 +380,9 @@ void AP_Mount_Siyi::process_packet()
case SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO: { case SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO: {
if (_parsed_msg.data_bytes_received != 5) { if (_parsed_msg.data_bytes_received != 5) {
#if AP_MOUNT_SIYI_DEBUG
unexpected_len = true; unexpected_len = true;
#endif
break; break;
} }
// update recording state and warn user of mismatch // update recording state and warn user of mismatch
@ -389,7 +399,9 @@ void AP_Mount_Siyi::process_packet()
case SiyiCommandId::FUNCTION_FEEDBACK_INFO: { case SiyiCommandId::FUNCTION_FEEDBACK_INFO: {
if (_parsed_msg.data_bytes_received != 1) { if (_parsed_msg.data_bytes_received != 1) {
#if AP_MOUNT_SIYI_DEBUG
unexpected_len = true; unexpected_len = true;
#endif
break; break;
} }
const uint8_t func_feedback_info = _msg_buff[_msg_buff_data_start]; const uint8_t func_feedback_info = _msg_buff[_msg_buff_data_start];
@ -422,7 +434,9 @@ void AP_Mount_Siyi::process_packet()
case SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE: { case SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE: {
if (_parsed_msg.data_bytes_received != 12) { if (_parsed_msg.data_bytes_received != 12) {
#if AP_MOUNT_SIYI_DEBUG
unexpected_len = true; unexpected_len = true;
#endif
break; break;
} }
_last_current_angle_rad_ms = AP_HAL::millis(); _last_current_angle_rad_ms = AP_HAL::millis();
@ -440,10 +454,12 @@ void AP_Mount_Siyi::process_packet()
break; break;
} }
// handle unexpected data buffer lenth #if AP_MOUNT_SIYI_DEBUG
// handle unexpected data buffer length
if (unexpected_len) { if (unexpected_len) {
debug("CmdId:%u unexpected len:%u", (unsigned)_parsed_msg.command_id, (unsigned)_parsed_msg.data_bytes_received); debug("CmdId:%u unexpected len:%u", (unsigned)_parsed_msg.command_id, (unsigned)_parsed_msg.data_bytes_received);
} }
#endif
} }
// methods to send commands to gimbal // methods to send commands to gimbal