openmv/usr/openmv-fb.py
2014-02-09 22:24:18 +02:00

77 lines
1.8 KiB
Python
Executable File

#!/usr/bin/env python
import sys
import usb.core
import usb.util
import numpy as np
import pygame
from time import sleep
interface = 2;
altsetting= 1;
def rgb_to_surface(buff):
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)
arr = np.column_stack((r,g,b)).flat[0:]
return pygame.image.frombuffer(arr, (160, 120), 'RGB')
# find USB device
dev = usb.core.find(idVendor=0x0483, idProduct=0x5740)
if dev is None:
raise ValueError('Device not found')
# detach kernel driver
if dev.is_kernel_driver_active(interface):
dev.detach_kernel_driver(interface)
# claim interface
usb.util.claim_interface(dev, interface)
# set FB debug alt setting
dev.set_interface_altsetting(interface, altsetting)
# init pygame
pygame.init()
# init screen
screen = pygame.display.set_mode((160, 120), pygame.DOUBLEBUF, 32)
running = True
img_size = 160*120*2
Clock = pygame.time.Clock()
font = pygame.font.SysFont("monospace", 15)
while running:
Clock.tick(60)
# request snapshot
dev.ctrl_transfer(0xC1, 8, 0, 2, None, 2000)
# read framebuffer
buf = dev.read(0x83, img_size, interface, 5000)
if len(buf) <img_size:
print(len(buf))
continue
# convert to RGB888 and blit
image = rgb_to_surface(buf)
# blit stuff
screen.blit(image, (0, 0))
screen.blit(font.render("FPS %.2f"%(Clock.get_fps()), 1, (255, 0, 0)), (0, 0))
# update display
pygame.display.flip()
event = pygame.event.poll()
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
running = False
pygame.quit()