Skip to content

Commit

Permalink
comment out debug stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
@EricPedley
EricPedley committed Jun 25, 2024
1 parent 57167fc commit 91ef3b9
Show file tree
Hide file tree
Showing 3 changed files with 40 additions and 33 deletions.
6 changes: 3 additions & 3 deletions siyi_sdk/siyi_sdk/siyi_message.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -294,7 +294,7 @@ def encodeMsg(self, data, cmd_id):
crc = crc16_str_swap(msg_front)
if crc is not None:
msg = msg_front+crc
self._logger.debug("Encoded msg: %s", msg)
# self._logger.debug("Encoded msg: %s", msg)
return msg
else:
self._logger.error("Could not encode message. crc16 is None")
Expand Down Expand Up @@ -496,7 +496,7 @@ def absoluteZoomMsg(self, zoom_level:int):
data=data1+data2

cmd_id = COMMAND.ABSOLUTE_ZOOM
self._logger.debug("Next encoded msg is absolute zoom")
# self._logger.debug("Next encoded msg is absolute zoom")
return self.encodeMsg(data, cmd_id)


Expand All @@ -523,7 +523,7 @@ def absolutePositionMsg(self, yaw, pitch):
data = data1[2:4]+data1[0:2]+data2[2:4]+data2[0:2]

cmd_id = COMMAND.ABSOLUTE_POSITION
self._logger.debug("Next encoded msg is absolute position")
# self._logger.debug("Next encoded msg is absolute position")
print(" -------------")
print(f"yaw: {yaw_data}, pitch: {pitch_data}")
print(f"data: {data}")
Expand Down
52 changes: 26 additions & 26 deletions siyi_sdk/siyi_sdk/siyi_sdk.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,10 +217,10 @@ def rcvMsg(self):
return data

def recvLoop(self):
self._logger.debug("Started data receiving thread")
# self._logger.debug("Started data receiving thread")
while(not self._stop):
self.bufferCallback()
self._logger.debug("Exiting data receiving thread")
# self._logger.debug("Exiting data receiving thread")


def bufferCallback(self):
Expand All @@ -232,7 +232,7 @@ def bufferCallback(self):
return

buff_str = buff.hex()
self._logger.debug("Buffer: %s", buff_str)
# self._logger.debug("Buffer: %s", buff_str)

# 10 bytes: STX+CTRL+Data_len+SEQ+CMD_ID+CRC16
# 2 + 1 + 2 + 2 + 1 + 2
Expand Down Expand Up @@ -606,7 +606,7 @@ def parseFirmwareMsg(self, msg:str, seq:int):
self._fw_msg.seq=seq


self._logger.debug("Firmware version: %s", self._fw_msg.gimbal_firmware_ver)
# self._logger.debug("Firmware version: %s", self._fw_msg.gimbal_firmware_ver)

return True
except Exception as e:
Expand All @@ -617,7 +617,7 @@ def parseHardwareIDMsg(self, msg:str, seq:int):
try:
self._hw_msg.seq=seq
self._hw_msg.id = msg
self._logger.debug("Hardware ID: %s", self._hw_msg.id)
# self._logger.debug("Hardware ID: %s", self._hw_msg.id)

return True
except Exception as e:
Expand All @@ -635,10 +635,10 @@ def parseAttitudeMsg(self, msg:str, seq:int):
self._att_msg.pitch_speed = toInt(msg[18:20]+msg[16:18]) /10.
self._att_msg.roll_speed = toInt(msg[22:24]+msg[20:22]) /10.

self._logger.debug("(yaw, pitch, roll= (%s, %s, %s)",
self._att_msg.yaw, self._att_msg.pitch, self._att_msg.roll)
self._logger.debug("(yaw_speed, pitch_speed, roll_speed= (%s, %s, %s)",
self._att_msg.yaw_speed, self._att_msg.pitch_speed, self._att_msg.roll_speed)
# self._logger.debug("(yaw, pitch, roll= (%s, %s, %s)",
# self._att_msg.yaw, self._att_msg.pitch, self._att_msg.roll)
# self._logger.debug("(yaw_speed, pitch_speed, roll_speed= (%s, %s, %s)",
# self._att_msg.yaw_speed, self._att_msg.pitch_speed, self._att_msg.roll_speed)
return True
except Exception as e:
self._logger.error("Error %s", e)
Expand All @@ -654,9 +654,9 @@ def parseGimbalInfoMsg(self, msg:str, seq:int):
self._motionMode_msg.mode = int('0x'+msg[8:10], base=16)
self._mountDir_msg.dir = int('0x'+msg[10:12], base=16)

self._logger.debug("Recording state %s", self._record_msg.state)
self._logger.debug("Mounting direction %s", self._mountDir_msg.dir)
self._logger.debug("Gimbal motion mode %s", self._motionMode_msg.mode)
# self._logger.debug("Recording state %s", self._record_msg.state)
# self._logger.debug("Mounting direction %s", self._mountDir_msg.dir)
# self._logger.debug("Gimbal motion mode %s", self._motionMode_msg.mode)
return True
except Exception as e:
self._logger.error("Error %s", e)
Expand All @@ -669,7 +669,7 @@ def parseAutoFocusMsg(self, msg:str, seq:int):
self._autoFocus_msg.success = bool(int('0x'+msg, base=16))


self._logger.debug("Auto focus success: %s", self._autoFocus_msg.success)
# self._logger.debug("Auto focus success: %s", self._autoFocus_msg.success)

return True
except Exception as e:
Expand All @@ -683,7 +683,7 @@ def parseAbsoluteZoomMsg(self, msg:str, seq:int):
self._absoluteZoom_msg.success = bool(int('0x'+msg, base=16))


self._logger.debug("Absolute Zoom success: %s", self._absoluteZoom_msg.success)
# self._logger.debug("Absolute Zoom success: %s", self._absoluteZoom_msg.success)

return True
except Exception as e:
Expand All @@ -695,7 +695,7 @@ def parseAbsolutePositionMsg(self, msg:str, seq:int):
try:
self._absolutePosition_msg.seq=seq
# TODO
self._logger.debug("Absolute Position success: %s", msg)
# self._logger.debug("Absolute Position success: %s", msg)

return True
except Exception as e:
Expand All @@ -709,7 +709,7 @@ def parseZoomMsg(self, msg:str, seq:int):
self._manualZoom_msg.level = int('0x'+msg[2:4]+msg[0:2], base=16) /10.


self._logger.debug("Zoom level %s", self._manualZoom_msg.level)
# self._logger.debug("Zoom level %s", self._manualZoom_msg.level)

return True
except Exception as e:
Expand All @@ -723,7 +723,7 @@ def parseManualFocusMsg(self, msg:str, seq:int):
self._manualFocus_msg.success = bool(int('0x'+msg, base=16))


self._logger.debug("Manual focus success: %s", self._manualFocus_msg.success)
# self._logger.debug("Manual focus success: %s", self._manualFocus_msg.success)

return True
except Exception as e:
Expand All @@ -737,7 +737,7 @@ def parseGimbalSpeedMsg(self, msg:str, seq:int):
self._gimbalSpeed_msg.success = bool(int('0x'+msg, base=16))


self._logger.debug("Gimbal speed success: %s", self._gimbalSpeed_msg.success)
# self._logger.debug("Gimbal speed success: %s", self._gimbalSpeed_msg.success)

return True
except Exception as e:
Expand All @@ -751,7 +751,7 @@ def parseGimbalCenterMsg(self, msg:str, seq:int):
self._center_msg.success = bool(int('0x'+msg, base=16))


self._logger.debug("Gimbal center success: %s", self._center_msg.success)
# self._logger.debug("Gimbal center success: %s", self._center_msg.success)

return True
except Exception as e:
Expand All @@ -765,7 +765,7 @@ def parseFunctionFeedbackMsg(self, msg:str, seq:int):
self._funcFeedback_msg.info_type = int('0x'+msg, base=16)


self._logger.debug("Function Feedback Code: %s", self._funcFeedback_msg.info_type)
# self._logger.debug("Function Feedback Code: %s", self._funcFeedback_msg.info_type)

return True
except Exception as e:
Expand Down Expand Up @@ -810,7 +810,7 @@ def _manual_absolute_zoom(self, target : float, timeout = 15) -> bool:
target = max(min(target, 30), 1)
# to get current zoom value
self.requestZoomHold()
self._logger.debug("Zoom level: %.1f", self.getZoomLevel())
# self._logger.debug("Zoom level: %.1f", self.getZoomLevel())
temp = time()
while self.getZoomLevel() != target:
total_sleep = 0.005
Expand All @@ -833,7 +833,7 @@ def _manual_absolute_zoom(self, target : float, timeout = 15) -> bool:
total_sleep = 0.05
sleep(total_sleep)
self.requestZoomHold()
self._logger.debug("Zoom level: %.1f", self.getZoomLevel())
# self._logger.debug("Zoom level: %.1f", self.getZoomLevel())
self.requestZoomHold()
self._logger.info("Finished absolute zoom: %.1f", self.getZoomLevel())
return True
Expand Down Expand Up @@ -893,8 +893,8 @@ def setGimbalRotation(self, yaw, pitch, err_thresh=1.0, kp=4):
yaw_err = -yaw + self._att_msg.yaw # NOTE for some reason it's reversed!!
pitch_err = pitch - self._att_msg.pitch

self._logger.debug("yaw_err= %s", yaw_err)
self._logger.debug("pitch_err= %s", pitch_err)
# self._logger.debug("yaw_err= %s", yaw_err)
# self._logger.debug("pitch_err= %s", pitch_err)

if (abs(yaw_err) <= th and abs(pitch_err)<=th):
self.requestGimbalSpeed(0, 0)
Expand All @@ -903,8 +903,8 @@ def setGimbalRotation(self, yaw, pitch, err_thresh=1.0, kp=4):

y_speed_sp = max(min(100, int(gain*yaw_err)), -100)
p_speed_sp = max(min(100, int(gain*pitch_err)), -100)
self._logger.debug("yaw speed setpoint= %s", y_speed_sp)
self._logger.debug("pitch speed setpoint= %s", p_speed_sp)
# self._logger.debug("yaw speed setpoint= %s", y_speed_sp)
# self._logger.debug("pitch speed setpoint= %s", p_speed_sp)
self.requestGimbalSpeed(y_speed_sp, p_speed_sp)

sleep(0.1) # command frequency
Expand Down
15 changes: 11 additions & 4 deletions siyi_sdk/siyi_sdk/siyi_stream.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,8 @@ def __init__(self, server_ip : str = "192.168.144.25", port : int = 8554, name :
logging.basicConfig(format=LOG_FORMAT, level=d_level)
self._logger = logging.getLogger(self.__class__.__name__)
self._logger.info("Initializing SIYISTREAM")
if self._debug:
self._logger.info("Debug mode")
self._server_ip = server_ip
self._port = port
self._name = name
Expand All @@ -47,13 +49,15 @@ def __init__(self, server_ip : str = "192.168.144.25", port : int = 8554, name :
self.lock = threading.Lock()
self.capture_thread = threading.Thread(target=self._read_stream)
self.capture_thread.daemon = True
self.bad_count=0

# grab frames as soon as they are available
def _read_stream(self):
while True:
with self.lock:
ret = self._stream.grab()
self._logger.debug("Grabbed frame")
# self._logger.debug("Grabbed frame")
pass
if not ret:
break
# Delay so the thread lock isn't hogged
Expand Down Expand Up @@ -100,13 +104,16 @@ def get_frame(self) -> np.ndarray | None:
return
ret = False
while not ret:
self._logger.debug("Waiting for lock")
# self._logger.debug("Waiting for lock")
with self.lock:
self._logger.debug("Lock acquired")
# self._logger.debug("Lock acquired")
ret, frame = self._stream.retrieve()
if ret:
self._logger.debug("Frame read")
# self._logger.debug("Frame read")
pass
else:
self._logger.warning("Unable to read frame")
return None
self.bad_count += 1
return frame

0 comments on commit 91ef3b9

Please sign in to comment.