tools/pyopenmv.py: Implement the GET_STATE command.

This commit is contained in:
iabdalkader 2024-07-09 19:25:04 +03:00
parent 0cef6239e0
commit 6d372067a3
2 changed files with 79 additions and 31 deletions

View File

@ -37,6 +37,11 @@ __USBDBG_SYS_RESET_TO_BL= 0x0E
__USBDBG_FB_ENABLE = 0x0D
__USBDBG_TX_BUF_LEN = 0x8E
__USBDBG_TX_BUF = 0x8F
__USBDBG_GET_STATE = 0x93
__USBDBG_STATE_FLAGS_SCRIPT = (1 << 0)
__USBDBG_STATE_FLAGS_TEXT = (1 << 1)
__USBDBG_STATE_FLAGS_FRAME = (1 << 2)
ATTR_CONTRAST =0
ATTR_BRIGHTNESS =1
@ -70,6 +75,44 @@ def fb_size():
__serial.write(struct.pack("<BBI", __USBDBG_CMD, __USBDBG_FRAME_SIZE, __FB_HDR_SIZE))
return struct.unpack("III", __serial.read(12))
def read_state():
__serial.write(struct.pack("<BBI", __USBDBG_CMD, __USBDBG_GET_STATE, 64))
flags, w, h, size, res0, res1, text_buf = struct.unpack("IIIIII40s", __serial.read(64))
text = None
if flags & __USBDBG_STATE_FLAGS_TEXT:
text = text_buf.split(b'\0', 1)[0].decode()
if flags & __USBDBG_STATE_FLAGS_FRAME == 0:
return 0, 0, None, text
num_bytes = size if size > 2 else (w * h * size)
# read fb data
__serial.write(struct.pack("<BBI", __USBDBG_CMD, __USBDBG_FRAME_DUMP, num_bytes))
buff = __serial.read(num_bytes)
if size == 1: # Grayscale
y = np.fromstring(buff, dtype=np.uint8)
buff = np.column_stack((y, y, y))
elif size == 2: # RGB565
arr = np.fromstring(buff, dtype=np.uint16).newbyteorder('S')
r = (((arr & 0xF800) >>11)*255.0/31.0).astype(np.uint8)
g = (((arr & 0x07E0) >>5) *255.0/63.0).astype(np.uint8)
b = (((arr & 0x001F) >>0) *255.0/31.0).astype(np.uint8)
buff = np.column_stack((r,g,b))
else: # JPEG
try:
buff = np.asarray(Image.frombuffer("RGB", (w, h), buff, "jpeg", "RGB", ""))
except Exception as e:
raise ValueError(f"JPEG decode error (%e)")
if (buff.size != (w*h*3)):
raise ValueError(f"Unexpected frame size. Expected: {w*h*3} received: {buff.size}")
return w, h, buff.reshape((h, w, 3)), text
def fb_dump():
size = fb_size()

View File

@ -28,8 +28,10 @@ sensor.skip_frames(time = 2000) # Wait for settings take effect.
clock = time.clock() # Create a clock object to track the FPS.
while(True):
clock.tick()
img = sensor.snapshot() # Take a picture and return the image.
sensor.flush()
print(clock.fps(), " FPS")
"""
# init pygame
@ -55,10 +57,10 @@ for i in range(10):
sleep(0.100)
if not connected:
print ( "Failed to connect to OpenMV's serial port.\n"
"Please install OpenMV's udev rules first:\n"
"sudo cp openmv/udev/50-openmv.rules /etc/udev/rules.d/\n"
"sudo udevadm control --reload-rules\n\n")
print("Failed to connect to OpenMV's serial port.\n"
"Please install OpenMV's udev rules first:\n"
"sudo cp openmv/udev/50-openmv.rules /etc/udev/rules.d/\n"
"sudo udevadm control --reload-rules\n\n")
sys.exit(1)
# Set higher timeout after connecting for lengthy transfers.
@ -73,39 +75,42 @@ screen = None
IMAGE_SCALE = 4
Clock = pygame.time.Clock()
font = pygame.font.SysFont("monospace", 15)
font = pygame.font.SysFont("monospace", 25)
while running:
Clock.tick(100)
try:
while running:
Clock.tick(30)
# Read state
w, h, data, text = pyopenmv.read_state()
# read framebuffer
fb = pyopenmv.fb_dump()
if fb is not None:
fps = Clock.get_fps()
w, h, data = fb[0], fb[1], fb[2]
if text is not None:
print(text, end="")
# create image from RGB888
image = pygame.image.frombuffer(data.flat[0:], (w, h), 'RGB')
image = pygame.transform.scale(image, (w * IMAGE_SCALE, h * IMAGE_SCALE))
if data is not None:
fps = Clock.get_fps()
# Create image from RGB888
image = pygame.image.frombuffer(data.flat[0:], (w, h), 'RGB')
image = pygame.transform.scale(image, (w * IMAGE_SCALE, h * IMAGE_SCALE))
if screen is None:
screen = pygame.display.set_mode((w * IMAGE_SCALE, h * IMAGE_SCALE), pygame.DOUBLEBUF, 32)
if screen is None:
screen = pygame.display.set_mode((w * IMAGE_SCALE, h * IMAGE_SCALE), pygame.DOUBLEBUF, 32)
# blit stuff
screen.blit(image, (0, 0))
screen.blit(font.render("FPS %.2f"%(fps), 1, (255, 0, 0)), (0, 0))
# blit stuff
screen.blit(image, (0, 0))
screen.blit(font.render("FPS %.2f"%(fps), 1, (255, 0, 0)), (0, 0))
# update display
pygame.display.flip()
# update display
pygame.display.flip()
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
running = False
if event.key == pygame.K_c:
pygame.image.save(image, "capture.png")
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
running = False
if event.key == pygame.K_c:
pygame.image.save(image, "capture.png")
except KeyboardInterrupt:
pass
pygame.quit()
pyopenmv.stop_script()