mirror of
https://github.com/openmv/openmv.git
synced 2025-09-26 23:09:13 +08:00
tools/openmv: OpenMV Protocol V2 Implementation.
Signed-off-by: iabdalkader <i.abdalkader@gmail.com>
This commit is contained in:
parent
a5425c973f
commit
81f7500ba3
35
tools/openmv/__init__.py
Normal file
35
tools/openmv/__init__.py
Normal file
@ -0,0 +1,35 @@
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Protocol Package
|
||||
#
|
||||
# This package provides a Python implementation of the OpenMV Protocol
|
||||
# for communicating with OpenMV cameras.
|
||||
#
|
||||
# Main classes:
|
||||
# OMVCamera: High-level camera interface with channel operations
|
||||
#
|
||||
# Main exceptions:
|
||||
# OMVPException: Base exception for protocol errors
|
||||
# OMVPTimeoutException: Timeout during protocol operations
|
||||
# OMVPChecksumException: CRC validation failures
|
||||
# OMVPSequenceException: Sequence number validation failures
|
||||
|
||||
from .camera import OMVCamera
|
||||
from .exceptions import (
|
||||
OMVPException,
|
||||
OMVPTimeoutException,
|
||||
OMVPChecksumException,
|
||||
OMVPSequenceException
|
||||
)
|
||||
|
||||
__version__ = "2.0.0"
|
||||
|
||||
__all__ = [
|
||||
'OMVCamera',
|
||||
'OMVPException',
|
||||
'OMVPTimeoutException',
|
||||
'OMVPChecksumException',
|
||||
'OMVPSequenceException'
|
||||
]
|
85
tools/openmv/buffer.py
Normal file
85
tools/openmv/buffer.py
Normal file
@ -0,0 +1,85 @@
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Protocol Ring Buffer
|
||||
#
|
||||
# This module provides an efficient ring buffer implementation for
|
||||
# packet parsing with optimized memory operations using memoryview.
|
||||
|
||||
import struct
|
||||
|
||||
class OMVRingBuffer:
|
||||
"""Efficient ring buffer for packet parsing"""
|
||||
def __init__(self, size=4096):
|
||||
self.size = size
|
||||
self.data = memoryview(bytearray(size))
|
||||
self.start = 0 # Read position
|
||||
self.end = 0 # Write position
|
||||
self.count = 0 # Number of bytes in buffer
|
||||
|
||||
def __len__(self):
|
||||
return self.count
|
||||
|
||||
def extend(self, data):
|
||||
"""Add data to buffer - optimized with memoryview"""
|
||||
data_len = len(data)
|
||||
data_view = memoryview(data)
|
||||
if data_len > self.size - self.count:
|
||||
raise BufferError(f"Buffer overflow:")
|
||||
|
||||
# Calculate contiguous space from end to buffer boundary
|
||||
space_to_end = self.size - self.end
|
||||
|
||||
if data_len <= space_to_end:
|
||||
# All data fits without wrapping
|
||||
self.data[self.end:self.end + data_len] = data_view[:data_len]
|
||||
self.end = (self.end + data_len) % self.size
|
||||
else:
|
||||
# First part: from end to buffer boundary
|
||||
self.data[self.end:self.size] = data_view[:space_to_end]
|
||||
# Second part: from buffer start
|
||||
remaining = data_len - space_to_end
|
||||
self.data[:remaining] = data_view[space_to_end:data_len]
|
||||
self.end = remaining
|
||||
|
||||
self.count += data_len
|
||||
|
||||
def peek(self, size):
|
||||
"""Peek at data without consuming - returns memoryview when possible"""
|
||||
if size > self.count:
|
||||
return None
|
||||
|
||||
if self.start + size <= self.size:
|
||||
# No wrap-around - return memoryview (zero-copy)
|
||||
return self.data[self.start:self.start + size]
|
||||
else:
|
||||
# Wrap-around case - must create new bytes object
|
||||
first_part = self.size - self.start
|
||||
# Use tobytes() for memoryview concatenation
|
||||
return (self.data[self.start:].tobytes() +
|
||||
self.data[:size - first_part].tobytes())
|
||||
|
||||
def peek16(self):
|
||||
"""Peek at 16-bit value at start of buffer"""
|
||||
if self.count < 2:
|
||||
return None
|
||||
elif self.start + 2 <= self.size:
|
||||
# No wrap-around - use struct directly on memoryview
|
||||
return struct.unpack('<H', self.data[self.start:self.start + 2])[0]
|
||||
else:
|
||||
# Wrap-around case - need to combine bytes
|
||||
return self.data[self.start] | (self.data[(self.start + 1) % self.size] << 8)
|
||||
|
||||
def consume(self, count):
|
||||
"""Consume count bytes from buffer"""
|
||||
count = min(count, self.count)
|
||||
self.start = (self.start + count) % self.size
|
||||
self.count -= count
|
||||
|
||||
def read(self, size):
|
||||
"""Read and consume data"""
|
||||
data = self.peek(size)
|
||||
if data is not None:
|
||||
self.consume(size)
|
||||
return data
|
631
tools/openmv/camera.py
Normal file
631
tools/openmv/camera.py
Normal file
@ -0,0 +1,631 @@
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Protocol Camera Interface
|
||||
#
|
||||
# This module provides the high-level OMVCamera class that handles all
|
||||
# camera operations and channel communications using the OpenMV Protocol.
|
||||
|
||||
import struct
|
||||
import time
|
||||
import sys
|
||||
import serial
|
||||
import logging
|
||||
from functools import reduce
|
||||
from operator import mul
|
||||
from .constants import OMVPOpcode, OMVProto, OMVPEventType, OMVPChannelIOCTL
|
||||
from .exceptions import OMVPException, OMVPTimeoutException, OMVPResyncException
|
||||
from .transport import OMVTransport
|
||||
from . import image as omv_image
|
||||
|
||||
class OMVCamera:
|
||||
"""OpenMV Camera Protocol Implementation"""
|
||||
|
||||
def __init__(self, port, baudrate=921600,
|
||||
crc=True, seq=True, ack=True, events=True,
|
||||
timeout=1.0, max_retry=3, max_payload=4096,
|
||||
drop_rate=0.0):
|
||||
# Serial connection
|
||||
self._serial = None
|
||||
self.port = port
|
||||
self.baudrate = baudrate
|
||||
self.timeout = timeout
|
||||
self.max_retry = max_retry
|
||||
self.drop_rate = drop_rate
|
||||
|
||||
# Configuration stored in dictionary
|
||||
self.caps = {
|
||||
'crc': crc,
|
||||
'seq': seq,
|
||||
'ack': ack,
|
||||
'events': events,
|
||||
'max_payload': max_payload,
|
||||
}
|
||||
|
||||
# Protocol components
|
||||
self.channels_by_name = {}
|
||||
self.channels_by_id = {}
|
||||
self.pending_channel_events = 0
|
||||
self.sysinfo = {}
|
||||
self.transport = None
|
||||
self.frame_event = False
|
||||
|
||||
def __enter__(self):
|
||||
"""Context manager entry"""
|
||||
self.connect()
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_val, exc_tb):
|
||||
"""Context manager exit"""
|
||||
self.disconnect()
|
||||
|
||||
@staticmethod
|
||||
def retry_if_failed(func):
|
||||
"""Decorator to automatically retry on resync"""
|
||||
# Note all held locks are released on resync in the firmware.
|
||||
def wrapper(self, *args, **kwargs):
|
||||
try:
|
||||
return func(self, *args, **kwargs)
|
||||
except OMVPResyncException:
|
||||
return func(self, *args, **kwargs)
|
||||
wrapper.__name__ = func.__name__
|
||||
wrapper.__doc__ = func.__doc__
|
||||
return wrapper
|
||||
|
||||
def poll_events(self):
|
||||
self.transport.recv_packet(poll_events=True)
|
||||
|
||||
def _send_cmd_wait_resp(self, opcode, channel=0, data=b''):
|
||||
"""Send a command and wait for response (ACK/NAK or data)"""
|
||||
if not self.is_connected():
|
||||
raise OMVPException("Not connected")
|
||||
|
||||
# Special handling for reset commands - they never return
|
||||
if opcode in [OMVPOpcode.SYS_RESET, OMVPOpcode.SYS_BOOT]:
|
||||
self.transport.send_packet(opcode, channel, 0, data)
|
||||
self.disconnect() # Device will reset, connection lost
|
||||
return None
|
||||
|
||||
try:
|
||||
self.transport.send_packet(opcode, channel, 0, data)
|
||||
# Returns payload/None or raises exception
|
||||
return self.transport.recv_packet()
|
||||
except KeyboardInterrupt:
|
||||
sys.exit(0)
|
||||
except:
|
||||
self._resync()
|
||||
raise OMVPResyncException()
|
||||
|
||||
def _handle_event(self, channel_id, event):
|
||||
"""Handle events from the device"""
|
||||
if channel_id == 0:
|
||||
# System events
|
||||
event_name = OMVPEventType(event).name if event in OMVPEventType else f"0x{event:04X}"
|
||||
logging.info(f"🔔 System Event: channel=system, event={event_name}")
|
||||
|
||||
# Handle system events
|
||||
if event == OMVPEventType.SOFT_REBOOT:
|
||||
logging.info("🔥 Soft Reboot triggered")
|
||||
elif event == OMVPEventType.CHANNEL_REGISTERED:
|
||||
self.pending_channel_events += 1
|
||||
elif channel_id in self.channels_by_id:
|
||||
# Channel events
|
||||
event_type = ""
|
||||
channel = self.channels_by_id[channel_id]["name"]
|
||||
|
||||
if channel == "stream":
|
||||
self.frame_event = True
|
||||
event_type = " (Frame Ready)"
|
||||
elif channel == "stdin":
|
||||
event_type = " (Script Started)" if event == 1 else " (Script Stopped)"
|
||||
|
||||
logging.info(f"🔔 Channel Event: channel={channel}, event=0x{event:04X}{event_type}")
|
||||
else:
|
||||
# Unknown channel
|
||||
logging.warning(f"⚠️ Unknown Event: channel={channel_id}, event=0x{event:04X}")
|
||||
|
||||
def _resync(self):
|
||||
logging.info("🔁 Resynchronizing")
|
||||
|
||||
# Perform resync sequence on timeout
|
||||
for attempt in range(self.max_retry):
|
||||
try:
|
||||
# Send SYNC command and wait for response
|
||||
self.transport.reset_sequence()
|
||||
self.transport.send_packet(OMVPOpcode.PROTO_SYNC, 0, 0)
|
||||
if self.transport.recv_packet():
|
||||
self.transport.reset_sequence()
|
||||
break
|
||||
except OMVPException:
|
||||
if attempt < self.max_retry - 1:
|
||||
logging.warning(f"⚠️ Sync attempt {attempt + 1} failed, retrying...")
|
||||
continue
|
||||
else:
|
||||
logging.error("❌ Failed to resync after maximum attempts")
|
||||
raise OMVPTimeoutException("Resync failed - unable to synchronize with device")
|
||||
|
||||
def _channel_lock(self, channel_id):
|
||||
"""Lock a data channel"""
|
||||
return self._send_cmd_wait_resp(OMVPOpcode.CHANNEL_LOCK, channel_id)
|
||||
|
||||
def _channel_unlock(self, channel_id):
|
||||
"""Lock a data channel"""
|
||||
return self._send_cmd_wait_resp(OMVPOpcode.CHANNEL_UNLOCK, channel_id)
|
||||
|
||||
def _channel_size(self, channel_id):
|
||||
"""Get available data size for a channel"""
|
||||
payload = self._send_cmd_wait_resp(OMVPOpcode.CHANNEL_SIZE, channel_id)
|
||||
return struct.unpack('<I', payload[:4])[0]
|
||||
|
||||
def _channel_shape(self, channel_id):
|
||||
"""Get available data size for a channel"""
|
||||
payload = self._send_cmd_wait_resp(OMVPOpcode.CHANNEL_SHAPE, channel_id)
|
||||
return struct.unpack(f'<{len(payload)//4}I', payload)
|
||||
|
||||
def _channel_read(self, channel_id, offset, length):
|
||||
"""Read data from a channel (protocol handles fragmentation automatically)"""
|
||||
payload = struct.pack('<II', offset, length)
|
||||
data = self._send_cmd_wait_resp(OMVPOpcode.CHANNEL_READ, channel_id, payload)
|
||||
return bytes(data)
|
||||
|
||||
def _channel_write(self, channel_id, data, offset=0):
|
||||
"""Write data to a channel with automatic packet splitting"""
|
||||
# Maximum payload size - 8-byte header (offset + length)
|
||||
chunk_size = self.caps['max_payload'] - 8
|
||||
|
||||
for start in range(0, len(data), chunk_size):
|
||||
chunk = data[start:start + chunk_size]
|
||||
# Payload: offset + length + data
|
||||
payload = struct.pack('<II', offset + start, len(chunk)) + chunk
|
||||
self._send_cmd_wait_resp(OMVPOpcode.CHANNEL_WRITE, channel_id, payload)
|
||||
|
||||
def _channel_ioctl(self, channel_id, cmd, fmt=None, *args):
|
||||
"""Perform ioctl operation on a channel"""
|
||||
payload = struct.pack('<I', cmd)
|
||||
if fmt and args:
|
||||
payload += struct.pack('<' + fmt, *args)
|
||||
return self._send_cmd_wait_resp(OMVPOpcode.CHANNEL_IOCTL, channel_id, payload)
|
||||
|
||||
def _channel_list(self):
|
||||
"""List registered channels on the device"""
|
||||
channels = {}
|
||||
entry_size = 16 # bytes: 1 (id) + 1 (flags) + 14 (name)
|
||||
|
||||
payload = self._send_cmd_wait_resp(OMVPOpcode.CHANNEL_LIST)
|
||||
num_channels = len(payload) // entry_size
|
||||
|
||||
for i in range(num_channels):
|
||||
offset = i * entry_size
|
||||
# Unpack id, flags, and raw name bytes in one go
|
||||
cid, flags, raw_name = struct.unpack_from("<BB14s", payload, offset)
|
||||
# Strip at first null byte, then decode
|
||||
name = raw_name.split(b"\x00", 1)[0].decode("utf-8", errors="ignore")
|
||||
channels[cid] = {"name": name, "flags": flags}
|
||||
|
||||
return channels
|
||||
|
||||
def update_channels(self):
|
||||
"""Update channel list from device"""
|
||||
if self.pending_channel_events > 0:
|
||||
self.pending_channel_events -= 1
|
||||
self.channels_by_id = self._channel_list()
|
||||
self.channels_by_name = {ch['name']: cid for cid, ch in self.channels_by_id.items()}
|
||||
logging.info(f"Registered channels ({len(self.channels_by_id)}):")
|
||||
for cid, ch in self.channels_by_id.items():
|
||||
logging.info(f" ID: {cid}, Flags: 0x{ch['flags']:02X}, Name: {ch['name']}")
|
||||
|
||||
def get_channel(self, name=None, channel_id=None):
|
||||
"""Get channel ID by name or channel name by ID with lazy loading"""
|
||||
if self.pending_channel_events > 0:
|
||||
self.update_channels()
|
||||
|
||||
if name is not None:
|
||||
# Return channel ID for given name
|
||||
return self.channels_by_name.get(name)
|
||||
elif channel_id is not None:
|
||||
# Return channel name given ID
|
||||
return self.channels_by_id.get(channel_id)["name"]
|
||||
else:
|
||||
raise ValueError("Must specify either name or channel_id")
|
||||
|
||||
def connect(self):
|
||||
"""Establish connection to the OpenMV camera"""
|
||||
try:
|
||||
self._serial = serial.Serial(self.port, self.baudrate, timeout=self.timeout)
|
||||
|
||||
# Use the protocol defaults for the initial connection
|
||||
self.transport = OMVTransport(self._serial, crc=True, seq=True,
|
||||
max_payload=OMVProto.MIN_PAYLOAD_SIZE, timeout=self.timeout,
|
||||
event_callback=self._handle_event, drop_rate=self.drop_rate)
|
||||
|
||||
# Perform resync
|
||||
self._resync()
|
||||
|
||||
# Set protocol configuration to user-requested values
|
||||
self.update_capabilities()
|
||||
|
||||
# Update transport with final negotiated capabilities
|
||||
self.transport.update_caps(self.caps['crc'], self.caps['seq'],
|
||||
self.caps['ack'], self.caps['max_payload'])
|
||||
|
||||
# Cache channel list
|
||||
self.update_channels()
|
||||
|
||||
# Cache system info
|
||||
self.sysinfo = self.system_info()
|
||||
|
||||
# Print system information
|
||||
self.print_system_info()
|
||||
except Exception as e:
|
||||
self.disconnect()
|
||||
raise OMVPException(f"Failed to connect: {e}")
|
||||
|
||||
def disconnect(self):
|
||||
"""Close connection to the OpenMV camera"""
|
||||
if self._serial:
|
||||
self._serial.close()
|
||||
self._serial = None
|
||||
self.transport = None
|
||||
|
||||
def is_connected(self):
|
||||
"""Check if connected to camera"""
|
||||
return self._serial is not None and self._serial.is_open
|
||||
|
||||
def host_stats(self):
|
||||
"""Get transport statistics"""
|
||||
return self.transport.stats
|
||||
|
||||
@retry_if_failed
|
||||
def device_stats(self):
|
||||
"""Get protocol statistics"""
|
||||
payload = self._send_cmd_wait_resp(OMVPOpcode.PROTO_STATS)
|
||||
|
||||
if len(payload) < 32:
|
||||
raise OMVPException(f"Invalid PROTO_STATS payload size: {len(payload)}")
|
||||
|
||||
# Unpack the structure: 8 uint32_t fields (32 bytes total)
|
||||
data = struct.unpack('<8I', payload)
|
||||
|
||||
return {
|
||||
'sent': data[0],
|
||||
'received': data[1],
|
||||
'checksum': data[2],
|
||||
'sequence': data[3],
|
||||
'retransmit': data[4],
|
||||
'transport': data[5],
|
||||
'sent_events': data[6],
|
||||
'max_ack_queue_depth': data[7]
|
||||
}
|
||||
|
||||
@retry_if_failed
|
||||
def reset(self):
|
||||
"""Reset the camera"""
|
||||
self._send_cmd_wait_resp(OMVPOpcode.SYS_RESET)
|
||||
|
||||
@retry_if_failed
|
||||
def boot(self):
|
||||
"""Jump to bootloader"""
|
||||
self._send_cmd_wait_resp(OMVPOpcode.SYS_BOOT)
|
||||
|
||||
@retry_if_failed
|
||||
def update_capabilities(self):
|
||||
"""Set device capabilities"""
|
||||
payload = self._send_cmd_wait_resp(OMVPOpcode.PROTO_GET_CAPS)
|
||||
flags, max_payload = struct.unpack('<IH', payload[:6])
|
||||
|
||||
flags = (self.caps['crc'] << 0 |
|
||||
self.caps['seq'] << 1 |
|
||||
self.caps['ack'] << 2 |
|
||||
self.caps['events'] << 3)
|
||||
|
||||
self.caps['max_payload'] = min(max_payload, self.caps['max_payload'])
|
||||
|
||||
payload = struct.pack('<IH10x', flags, self.caps['max_payload'])
|
||||
response = self._send_cmd_wait_resp(OMVPOpcode.PROTO_SET_CAPS, 0, payload)
|
||||
|
||||
@retry_if_failed
|
||||
def stop(self):
|
||||
"""Stop running script"""
|
||||
# Stop running script
|
||||
stdin_id = self.get_channel(name="stdin")
|
||||
self._channel_ioctl(stdin_id, OMVPChannelIOCTL.STDIN_STOP)
|
||||
|
||||
@retry_if_failed
|
||||
def exec(self, script):
|
||||
"""Write and execute a script"""
|
||||
stdin_id = self.get_channel(name="stdin")
|
||||
# Reset script buffer
|
||||
self._channel_ioctl(stdin_id, OMVPChannelIOCTL.STDIN_RESET)
|
||||
# Upload script data
|
||||
self._channel_write(stdin_id, memoryview(script.encode('utf-8')))
|
||||
# Execute the script
|
||||
self._channel_ioctl(stdin_id, OMVPChannelIOCTL.STDIN_EXEC)
|
||||
|
||||
@retry_if_failed
|
||||
def streaming(self, enable, raw=False, res=None):
|
||||
"""Enable or disable streaming"""
|
||||
stream_id = self.get_channel(name="stream")
|
||||
if raw:
|
||||
self._channel_ioctl(stream_id, OMVPChannelIOCTL.STREAM_RAW_CFG, 'II', *res)
|
||||
self._channel_ioctl(stream_id, OMVPChannelIOCTL.STREAM_RAW_CTRL, 'I', raw)
|
||||
self._channel_ioctl(stream_id, OMVPChannelIOCTL.STREAM_CTRL, 'I', enable)
|
||||
|
||||
@retry_if_failed
|
||||
def read_status(self):
|
||||
"""Poll channels status and return a dictionary of channel readiness"""
|
||||
payload = self._send_cmd_wait_resp(OMVPOpcode.CHANNEL_POLL)
|
||||
flags = struct.unpack('<I', payload[:4])[0]
|
||||
|
||||
# Build a dictionary mapping channel names to their poll status
|
||||
result = {}
|
||||
if self.pending_channel_events > 0:
|
||||
self.update_channels()
|
||||
for name, channel_id in self.channels_by_name.items():
|
||||
result[name] = bool(flags & (1 << channel_id))
|
||||
|
||||
return result
|
||||
|
||||
@retry_if_failed
|
||||
def profiler_reset(self):
|
||||
"""Reset the profiler data"""
|
||||
if profile_id := self.get_channel(name="profile"):
|
||||
self._channel_ioctl(profile_id, OMVPChannelIOCTL.PROFILE_RESET)
|
||||
logging.debug("Profiler reset")
|
||||
|
||||
@retry_if_failed
|
||||
def profiler_mode(self, exclusive=False):
|
||||
"""Set profiler mode (exclusive=True for exclusive, False for inclusive)"""
|
||||
if profile_id := self.get_channel(name="profile"):
|
||||
mode = 1 if exclusive else 0
|
||||
self._channel_ioctl(profile_id, OMVPChannelIOCTL.PROFILE_MODE, 'I', mode)
|
||||
logging.debug(f"Profile mode set to {'exclusive' if exclusive else 'inclusive'}")
|
||||
|
||||
@retry_if_failed
|
||||
def profiler_event_type(self, counter_num, event_id):
|
||||
"""Configure an event counter to monitor a specific event"""
|
||||
if profile_id := self.get_channel(name="profile"):
|
||||
self._channel_ioctl(profile_id, OMVPChannelIOCTL.PROFILE_SET_EVENT, 'II', counter_num, event_id)
|
||||
logging.debug(f"Event counter {counter_num} set to event 0x{event_id:04X}")
|
||||
|
||||
@retry_if_failed
|
||||
def read_profile(self):
|
||||
"""Read profiler data from the profile channel"""
|
||||
# Check if profile channel is available (replaces profile_enabled check)
|
||||
profile_id = self.get_channel(name="profile")
|
||||
if not profile_id:
|
||||
return None
|
||||
|
||||
# TODO just subtract the known record size
|
||||
# Get event count from cached system info (pmu_eventcnt field)
|
||||
event_count = self.sysinfo['pmu_eventcnt']
|
||||
|
||||
# Lock the profile channel
|
||||
if not self._channel_lock(profile_id):
|
||||
return None
|
||||
|
||||
try:
|
||||
# Get profile data shape and calculate size
|
||||
shape = self._channel_shape(profile_id)
|
||||
if len(shape) < 2:
|
||||
return None
|
||||
|
||||
profile_size = reduce(mul, shape)
|
||||
if profile_size == 0:
|
||||
return None
|
||||
|
||||
record_count, record_size = shape[0], shape[1]
|
||||
|
||||
# Read raw profile data using calculated size
|
||||
data = self._channel_read(profile_id, 0, profile_size)
|
||||
if len(data) == 0:
|
||||
return None
|
||||
|
||||
# Parse profile records
|
||||
records = []
|
||||
record_format = f"<5I2Q{event_count}QI"
|
||||
|
||||
for i in range(record_count):
|
||||
offset = i * record_size
|
||||
if offset + record_size > len(data):
|
||||
break
|
||||
|
||||
# Unpack the record
|
||||
profile = struct.unpack(record_format, data[offset:offset + record_size])
|
||||
|
||||
# Parse the profile data
|
||||
records.append({
|
||||
'address': profile[0],
|
||||
'caller': profile[1],
|
||||
'call_count': profile[2],
|
||||
'min_ticks': profile[3],
|
||||
'max_ticks': profile[4],
|
||||
'total_ticks': profile[5],
|
||||
'total_cycles': profile[6],
|
||||
'events': profile[7:7 + event_count]
|
||||
})
|
||||
|
||||
return records
|
||||
finally:
|
||||
self._channel_unlock(profile_id)
|
||||
|
||||
@retry_if_failed
|
||||
def read_stdout(self):
|
||||
"""Read text output buffer"""
|
||||
stdout_id = self.get_channel(name="stdout")
|
||||
if size := self._channel_size(stdout_id):
|
||||
data = self._channel_read(stdout_id, 0, size)
|
||||
return bytes(data).decode('utf-8', errors='ignore')
|
||||
|
||||
@retry_if_failed
|
||||
def read_frame(self):
|
||||
"""Read stream buffer data with header at the beginning and convert to RGB888"""
|
||||
stream_id = self.get_channel(name="stream")
|
||||
|
||||
# Lock the stream buffer
|
||||
if not self._channel_lock(stream_id):
|
||||
return None
|
||||
|
||||
self.frame_event = False
|
||||
try:
|
||||
# Get total size (16-byte header + stream data)
|
||||
if (size := self._channel_size(stream_id)) <= 16:
|
||||
return None
|
||||
|
||||
# Read all data (header + stream data)
|
||||
data = self._channel_read(stream_id, 0, size)
|
||||
if len(data) < 16:
|
||||
return None
|
||||
|
||||
# Parse stream header: width(4), height(4), pixformat(4), depth/size(4)
|
||||
width, height, pixformat, depth = struct.unpack('<IIII', data[:16])
|
||||
|
||||
# Extract raw frame data
|
||||
raw_data = data[16:]
|
||||
|
||||
# Convert to RGB888 using image module
|
||||
rgb_data, fmt_str = omv_image.convert_to_rgb888(raw_data, width, height, pixformat)
|
||||
if rgb_data is None:
|
||||
return None
|
||||
|
||||
return {
|
||||
'width': width,
|
||||
'height': height,
|
||||
'format': pixformat,
|
||||
'depth': depth,
|
||||
'data': rgb_data,
|
||||
'raw_size': len(raw_data)
|
||||
}
|
||||
finally:
|
||||
self._channel_unlock(stream_id)
|
||||
|
||||
@retry_if_failed
|
||||
def channel_size(self, channel):
|
||||
"""Get size of data available in a custom channel"""
|
||||
channel_id = self.get_channel(name=channel)
|
||||
return 0 if channel_id is None else self._channel_size(channel_id)
|
||||
|
||||
@retry_if_failed
|
||||
def channel_read(self, channel, size=None):
|
||||
"""Read data from a custom channel"""
|
||||
if channel_id := self.get_channel(name=channel):
|
||||
if size is None:
|
||||
size = self._channel_size(channel_id)
|
||||
return self._channel_read(channel_id, 0, size)
|
||||
return None
|
||||
|
||||
@retry_if_failed
|
||||
def channel_write(self, channel, data):
|
||||
"""Write data to a custom channel"""
|
||||
channel_id = self.get_channel(name=channel)
|
||||
if channel_id:
|
||||
self._channel_write(channel_id, data)
|
||||
return channel_id is not None
|
||||
|
||||
def has_channel(self, channel):
|
||||
"""Check if a channel exists"""
|
||||
return self.get_channel(name=channel) is not None
|
||||
|
||||
@retry_if_failed
|
||||
def system_info(self):
|
||||
"""Get system information"""
|
||||
payload = self._send_cmd_wait_resp(OMVPOpcode.SYS_INFO)
|
||||
|
||||
if len(payload) < 80:
|
||||
raise OMVPException(f"Invalid SYS_INFO payload size: {len(payload)}")
|
||||
|
||||
# Unpack the structure:
|
||||
# cpu_id[1] + dev_id[3] + chip_id[3] + id_reserved[2] + hw_caps[2] + memory[6] + versions[9] + padding[3]
|
||||
data = struct.unpack('<I 3I 3I 2I 2I 6I 3s3s3s 3x', payload)
|
||||
|
||||
# Extract capability bitfield (always 2 words)
|
||||
capabilities = data[9] # First hw_caps word
|
||||
capabilities2 = data[10] # Second hw_caps word
|
||||
|
||||
return {
|
||||
'cpu_id': data[0],
|
||||
'device_id': data[1:4], # Now 3 words
|
||||
'sensor_chip_id': data[4:7], # Now 3 words
|
||||
'gpu_present': bool(capabilities & (1 << 0)),
|
||||
'npu_present': bool(capabilities & (1 << 1)),
|
||||
'isp_present': bool(capabilities & (1 << 2)),
|
||||
'venc_present': bool(capabilities & (1 << 3)),
|
||||
'jpeg_present': bool(capabilities & (1 << 4)),
|
||||
'dram_present': bool(capabilities & (1 << 5)),
|
||||
'crc_present': bool(capabilities & (1 << 6)),
|
||||
'pmu_present': bool(capabilities & (1 << 7)),
|
||||
'pmu_eventcnt': (capabilities >> 8) & 0xFF, # 8 bits starting at bit 8
|
||||
'wifi_present': bool(capabilities & (1 << 16)),
|
||||
'bt_present': bool(capabilities & (1 << 17)),
|
||||
'sd_present': bool(capabilities & (1 << 18)),
|
||||
'eth_present': bool(capabilities & (1 << 19)),
|
||||
'usb_highspeed': bool(capabilities & (1 << 20)),
|
||||
'multicore_present': bool(capabilities & (1 << 21)),
|
||||
'flash_size_kb': data[11],
|
||||
'ram_size_kb': data[12],
|
||||
'framebuffer_size_kb': data[13],
|
||||
'stream_buffer_size_kb': data[14],
|
||||
'firmware_version': data[17],
|
||||
'protocol_version': data[18],
|
||||
'bootloader_version': data[19]
|
||||
}
|
||||
|
||||
def print_system_info(self):
|
||||
"""Print formatted system information"""
|
||||
logging.info("=== OpenMV System Information ===")
|
||||
|
||||
# Print registered channels
|
||||
logging.info(f"CPU ID: 0x{self.sysinfo['cpu_id']:08X}")
|
||||
|
||||
# Device ID is now an array of 3 words
|
||||
dev_id_hex = ''.join(f"{word:08X}" for word in self.sysinfo['device_id'])
|
||||
logging.info(f"Device ID: {dev_id_hex}")
|
||||
|
||||
# Sensor Chip IDs are now an array of 3 words
|
||||
for i, chip_id in enumerate(self.sysinfo['sensor_chip_id']):
|
||||
if chip_id != 0: # Only show non-zero chip IDs
|
||||
logging.info(f"CSI{i}: 0x{chip_id:08X}")
|
||||
|
||||
# Memory info
|
||||
if self.sysinfo['flash_size_kb'] > 0:
|
||||
logging.info(f"Flash: {self.sysinfo['flash_size_kb']} KB")
|
||||
if self.sysinfo['ram_size_kb'] > 0:
|
||||
logging.info(f"RAM: {self.sysinfo['ram_size_kb']} KB")
|
||||
if self.sysinfo['framebuffer_size_kb'] > 0:
|
||||
logging.info(f"Framebuffer: {self.sysinfo['framebuffer_size_kb']} KB")
|
||||
if self.sysinfo['stream_buffer_size_kb'] > 0:
|
||||
logging.info(f"Stream Buffer: {self.sysinfo['stream_buffer_size_kb']} KB")
|
||||
|
||||
# Hardware capabilities
|
||||
logging.info("Hardware capabilities:")
|
||||
logging.info(f" GPU: {'Yes' if self.sysinfo['gpu_present'] else 'No'}")
|
||||
logging.info(f" NPU: {'Yes' if self.sysinfo['npu_present'] else 'No'}")
|
||||
logging.info(f" ISP: {'Yes' if self.sysinfo['isp_present'] else 'No'}")
|
||||
logging.info(f" Video Encoder: {'Yes' if self.sysinfo['venc_present'] else 'No'}")
|
||||
logging.info(f" JPEG Encoder: {'Yes' if self.sysinfo['jpeg_present'] else 'No'}")
|
||||
logging.info(f" DRAM: {'Yes' if self.sysinfo['dram_present'] else 'No'}")
|
||||
logging.info(f" CRC Hardware: {'Yes' if self.sysinfo['crc_present'] else 'No'}")
|
||||
logging.info(f" PMU: {'Yes' if self.sysinfo['pmu_present'] else 'No'} "
|
||||
f"({self.sysinfo['pmu_eventcnt']} counters)")
|
||||
logging.info(f" Multi-core: {'Yes' if self.sysinfo['multicore_present'] else 'No'}")
|
||||
logging.info(f" WiFi: {'Yes' if self.sysinfo['wifi_present'] else 'No'}")
|
||||
logging.info(f" Bluetooth: {'Yes' if self.sysinfo['bt_present'] else 'No'}")
|
||||
logging.info(f" SD Card: {'Yes' if self.sysinfo['sd_present'] else 'No'}")
|
||||
logging.info(f" Ethernet: {'Yes' if self.sysinfo['eth_present'] else 'No'}")
|
||||
logging.info(f" USB High-Speed: {'Yes' if self.sysinfo['usb_highspeed'] else 'No'}")
|
||||
|
||||
# Profiler info - check if profile channel is available
|
||||
profile_available = self.get_channel(name="profile") is not None
|
||||
logging.info(f"Profiler: {'Available' if profile_available else 'Not available'}")
|
||||
|
||||
# Version info
|
||||
fw = self.sysinfo['firmware_version']
|
||||
proto = self.sysinfo['protocol_version']
|
||||
boot = self.sysinfo['bootloader_version']
|
||||
logging.info(f"Firmware version: {fw[0]}.{fw[1]}.{fw[2]}")
|
||||
logging.info(f"Protocol version: {proto[0]}.{proto[1]}.{proto[2]}")
|
||||
logging.info(f"Bootloader version: {boot[0]}.{boot[1]}.{boot[2]}")
|
||||
logging.info(f"Protocol capabilities: CRC={self.caps['crc']}, SEQ={self.caps['seq']}, "
|
||||
f"ACK={self.caps['ack']}, EVENTS={self.caps['events']}, "
|
||||
f"PAYLOAD={self.caps['max_payload']}")
|
||||
logging.info("=================================")
|
101
tools/openmv/constants.py
Normal file
101
tools/openmv/constants.py
Normal file
@ -0,0 +1,101 @@
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Protocol Constants
|
||||
#
|
||||
# This module defines all the constants used in the OpenMV Protocol
|
||||
# including opcodes, status codes, flags, and other definitions.
|
||||
|
||||
from enum import IntEnum
|
||||
|
||||
class OMVPFlags(IntEnum):
|
||||
"""OpenMV Protocol Packet Flags"""
|
||||
ACK = (1 << 0)
|
||||
NAK = (1 << 1)
|
||||
RTX = (1 << 2)
|
||||
ACK_REQ = (1 << 3)
|
||||
FRAGMENT = (1 << 4)
|
||||
EVENT = (1 << 5)
|
||||
|
||||
|
||||
class OMVPState(IntEnum):
|
||||
"""OpenMV Protocol Parser State Machine States"""
|
||||
SYNC = 0
|
||||
HEADER = 1
|
||||
PAYLOAD = 2
|
||||
|
||||
|
||||
class OMVProto(IntEnum):
|
||||
"""OpenMV Protocol Constants"""
|
||||
SYNC_WORD = 0xD5AA
|
||||
HEADER_SIZE = 10
|
||||
CRC_SIZE = 2
|
||||
MIN_PAYLOAD_SIZE = 64 - 10 - 2 # 64 - OMV_PROTOCOL_HEADER_SIZE - 2 = 52
|
||||
|
||||
|
||||
class OMVPStatus(IntEnum):
|
||||
"""OpenMV Protocol Status Codes"""
|
||||
SUCCESS = 0x00
|
||||
FAILED = 0x01
|
||||
INVALID = 0x02
|
||||
TIMEOUT = 0x03
|
||||
BUSY = 0x04
|
||||
CHECKSUM = 0x05
|
||||
SEQUENCE = 0x06
|
||||
OVERFLOW = 0x07
|
||||
FRAGMENT = 0x08
|
||||
UNKNOWN = 0x09
|
||||
|
||||
class OMVPOpcode(IntEnum):
|
||||
"""OpenMV Protocol Operation Codes"""
|
||||
# Protocol commands
|
||||
PROTO_SYNC = 0x00
|
||||
PROTO_GET_CAPS = 0x01
|
||||
PROTO_SET_CAPS = 0x02
|
||||
PROTO_STATS = 0x03
|
||||
|
||||
# System commands
|
||||
SYS_RESET = 0x10
|
||||
SYS_BOOT = 0x11
|
||||
SYS_INFO = 0x12
|
||||
SYS_EVENT = 0x13
|
||||
|
||||
# Channel commands
|
||||
CHANNEL_LIST = 0x20
|
||||
CHANNEL_POLL = 0x21
|
||||
CHANNEL_LOCK = 0x22
|
||||
CHANNEL_UNLOCK = 0x23
|
||||
CHANNEL_SHAPE = 0x24
|
||||
CHANNEL_SIZE = 0x25
|
||||
CHANNEL_READ = 0x26
|
||||
CHANNEL_WRITE = 0x27
|
||||
CHANNEL_IOCTL = 0x28
|
||||
CHANNEL_EVENT = 0x29
|
||||
|
||||
|
||||
class OMVPEventType(IntEnum):
|
||||
"""OpenMV Protocol Event Types"""
|
||||
CHANNEL_REGISTERED = 0x00
|
||||
CHANNEL_UNREGISTERED = 0x01
|
||||
SOFT_REBOOT = 0x02
|
||||
|
||||
|
||||
class OMVPChannelIOCTL(IntEnum):
|
||||
"""OpenMV Protocol Channel IOCTL Commands"""
|
||||
# Stdin channel IOCTLs
|
||||
STDIN_STOP = 0x01 # Stop running script
|
||||
STDIN_EXEC = 0x02 # Execute script
|
||||
STDIN_RESET = 0x03 # Reseet script buffer
|
||||
|
||||
# Stream channel IOCTLs
|
||||
STREAM_CTRL = 0x00 # Enable/disable streaming
|
||||
STREAM_RAW_CTRL = 0x01 # Enable/disable raw streaming
|
||||
STREAM_RAW_CFG = 0x02 # Set raw stream resolution
|
||||
|
||||
# Profile channel IOCTLs
|
||||
PROFILE_MODE = 0x00 # Set profiling mode
|
||||
PROFILE_SET_EVENT = 0x01 # Set event type to profile
|
||||
PROFILE_RESET = 0x02 # Reset profiler data
|
||||
|
||||
|
62
tools/openmv/crc.py
Normal file
62
tools/openmv/crc.py
Normal file
@ -0,0 +1,62 @@
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# CRC-16 implementation for OpenMV Protocol
|
||||
#
|
||||
# This module provides CRC-16 calculation using polynomial 0xBAAD.
|
||||
|
||||
# CRC-16 lookup table for polynomial 0xBAAD
|
||||
_CRC16_TABLE = [
|
||||
0x0000, 0xBAAD, 0xCFF7, 0x755A, 0x2543, 0x9FEE, 0xEAB4, 0x5019,
|
||||
0x4A86, 0xF02B, 0x8571, 0x3FDC, 0x6FC5, 0xD568, 0xA032, 0x1A9F,
|
||||
0x950C, 0x2FA1, 0x5AFB, 0xE056, 0xB04F, 0x0AE2, 0x7FB8, 0xC515,
|
||||
0xDF8A, 0x6527, 0x107D, 0xAAD0, 0xFAC9, 0x4064, 0x353E, 0x8F93,
|
||||
0x90B5, 0x2A18, 0x5F42, 0xE5EF, 0xB5F6, 0x0F5B, 0x7A01, 0xC0AC,
|
||||
0xDA33, 0x609E, 0x15C4, 0xAF69, 0xFF70, 0x45DD, 0x3087, 0x8A2A,
|
||||
0x05B9, 0xBF14, 0xCA4E, 0x70E3, 0x20FA, 0x9A57, 0xEF0D, 0x55A0,
|
||||
0x4F3F, 0xF592, 0x80C8, 0x3A65, 0x6A7C, 0xD0D1, 0xA58B, 0x1F26,
|
||||
0x9BC7, 0x216A, 0x5430, 0xEE9D, 0xBE84, 0x0429, 0x7173, 0xCBDE,
|
||||
0xD141, 0x6BEC, 0x1EB6, 0xA41B, 0xF402, 0x4EAF, 0x3BF5, 0x8158,
|
||||
0x0ECB, 0xB466, 0xC13C, 0x7B91, 0x2B88, 0x9125, 0xE47F, 0x5ED2,
|
||||
0x444D, 0xFEE0, 0x8BBA, 0x3117, 0x610E, 0xDBA3, 0xAEF9, 0x1454,
|
||||
0x0B72, 0xB1DF, 0xC485, 0x7E28, 0x2E31, 0x949C, 0xE1C6, 0x5B6B,
|
||||
0x41F4, 0xFB59, 0x8E03, 0x34AE, 0x64B7, 0xDE1A, 0xAB40, 0x11ED,
|
||||
0x9E7E, 0x24D3, 0x5189, 0xEB24, 0xBB3D, 0x0190, 0x74CA, 0xCE67,
|
||||
0xD4F8, 0x6E55, 0x1B0F, 0xA1A2, 0xF1BB, 0x4B16, 0x3E4C, 0x84E1,
|
||||
0x8D23, 0x378E, 0x42D4, 0xF879, 0xA860, 0x12CD, 0x6797, 0xDD3A,
|
||||
0xC7A5, 0x7D08, 0x0852, 0xB2FF, 0xE2E6, 0x584B, 0x2D11, 0x97BC,
|
||||
0x182F, 0xA282, 0xD7D8, 0x6D75, 0x3D6C, 0x87C1, 0xF29B, 0x4836,
|
||||
0x52A9, 0xE804, 0x9D5E, 0x27F3, 0x77EA, 0xCD47, 0xB81D, 0x02B0,
|
||||
0x1D96, 0xA73B, 0xD261, 0x68CC, 0x38D5, 0x8278, 0xF722, 0x4D8F,
|
||||
0x5710, 0xEDBD, 0x98E7, 0x224A, 0x7253, 0xC8FE, 0xBDA4, 0x0709,
|
||||
0x889A, 0x3237, 0x476D, 0xFDC0, 0xADD9, 0x1774, 0x622E, 0xD883,
|
||||
0xC21C, 0x78B1, 0x0DEB, 0xB746, 0xE75F, 0x5DF2, 0x28A8, 0x9205,
|
||||
0x16E4, 0xAC49, 0xD913, 0x63BE, 0x33A7, 0x890A, 0xFC50, 0x46FD,
|
||||
0x5C62, 0xE6CF, 0x9395, 0x2938, 0x7921, 0xC38C, 0xB6D6, 0x0C7B,
|
||||
0x83E8, 0x3945, 0x4C1F, 0xF6B2, 0xA6AB, 0x1C06, 0x695C, 0xD3F1,
|
||||
0xC96E, 0x73C3, 0x0699, 0xBC34, 0xEC2D, 0x5680, 0x23DA, 0x9977,
|
||||
0x8651, 0x3CFC, 0x49A6, 0xF30B, 0xA312, 0x19BF, 0x6CE5, 0xD648,
|
||||
0xCCD7, 0x767A, 0x0320, 0xB98D, 0xE994, 0x5339, 0x2663, 0x9CCE,
|
||||
0x135D, 0xA9F0, 0xDCAA, 0x6607, 0x361E, 0x8CB3, 0xF9E9, 0x4344,
|
||||
0x59DB, 0xE376, 0x962C, 0x2C81, 0x7C98, 0xC635, 0xB36F, 0x09C2
|
||||
]
|
||||
|
||||
def crc16(data, init_value=0xFFFF):
|
||||
"""
|
||||
Calculate CRC-16 with polynomial 0xBAAD using lookup table.
|
||||
|
||||
Args:
|
||||
data: bytes-like object to calculate CRC for
|
||||
init_value: initial CRC value (default: 0xFFFF)
|
||||
|
||||
Returns:
|
||||
16-bit CRC value as integer
|
||||
"""
|
||||
crc = init_value
|
||||
for byte in data:
|
||||
index = (crc >> 8) ^ byte
|
||||
crc = (crc << 8) ^ _CRC16_TABLE[index]
|
||||
crc &= 0xFFFF # Keep it 16-bit
|
||||
|
||||
return crc
|
192
tools/openmv/examples/basic.py
Normal file
192
tools/openmv/examples/basic.py
Normal file
@ -0,0 +1,192 @@
|
||||
#!/usr/bin/env python
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Protocol Test Script
|
||||
#
|
||||
# This module provides a test script for the OpenMV Protocol implementation.
|
||||
# It demonstrates basic camera operations including script execution and data
|
||||
# channel communication.
|
||||
|
||||
import sys
|
||||
import os
|
||||
import argparse
|
||||
import time
|
||||
import logging
|
||||
import random
|
||||
import signal
|
||||
|
||||
# Add parent directories to path for openmv module
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '../..'))
|
||||
|
||||
from openmv.camera import OMVCamera
|
||||
|
||||
# Test script -
|
||||
test_script = """
|
||||
import time
|
||||
import csi
|
||||
import image
|
||||
|
||||
csi0 = csi.CSI()
|
||||
csi0.reset()
|
||||
csi0.pixformat(csi.RGB565)
|
||||
csi0.framesize(csi.VGA)
|
||||
|
||||
clock = time.clock()
|
||||
|
||||
while(True):
|
||||
clock.tick()
|
||||
img = csi0.snapshot()
|
||||
print(clock.fps(), " FPS")
|
||||
"""
|
||||
|
||||
|
||||
def str2bool(v):
|
||||
"""Convert string to boolean for argparse"""
|
||||
if isinstance(v, bool):
|
||||
return v
|
||||
if v.lower() in ('yes', 'true', 't', 'y', '1'):
|
||||
return True
|
||||
elif v.lower() in ('no', 'false', 'f', 'n', '0'):
|
||||
return False
|
||||
else:
|
||||
raise argparse.ArgumentTypeError('Boolean value expected.')
|
||||
|
||||
def signal_handler(sig, frame):
|
||||
"""Handle interrupt signals"""
|
||||
logging.info("Interrupt signal received, stopping...")
|
||||
sys.exit(0)
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='OpenMV Protocol Test')
|
||||
parser.add_argument('--port', action='store', help='Serial port (dev/ttyACM0)', default='/dev/ttyACM0')
|
||||
parser.add_argument("--script", action="store", default=None, help="Script file")
|
||||
parser.add_argument('--timeout', action='store', type=float, default=1.0, help='Protocol timeout in seconds')
|
||||
parser.add_argument('--debug', action='store_true', help='Enable debug logging')
|
||||
|
||||
# Camera configuration options
|
||||
parser.add_argument('--baudrate', type=int, default=921600, help='Serial baudrate (default: 921600)')
|
||||
parser.add_argument('--crc', type=str2bool, nargs='?', const=True, default=True, help='Enable CRC validation (default: true)')
|
||||
parser.add_argument('--seq', type=str2bool, nargs='?', const=True, default=True, help='Enable sequence number validation (default: true)')
|
||||
parser.add_argument('--ack', type=str2bool, nargs='?', const=True, default=False, help='Enable packet acknowledgment (default: false)')
|
||||
parser.add_argument('--events', type=str2bool, nargs='?', const=True, default=True, help='Enable event notifications (default: true)')
|
||||
parser.add_argument('--max-retry', type=int, default=3, help='Maximum number of retries (default: 3)')
|
||||
parser.add_argument('--max-payload', type=int, default=4096, help='Maximum payload size in bytes (default: 4096)')
|
||||
parser.add_argument('--drop-rate', type=float, default=0.0, help='Packet drop simulation rate (0.0-1.0, default: 0.0)')
|
||||
parser.add_argument('--quiet', action='store_true', help='Suppress script output text')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# Set up signal handler for graceful shutdown
|
||||
signal.signal(signal.SIGINT, signal_handler)
|
||||
signal.signal(signal.SIGTERM, signal_handler)
|
||||
|
||||
# Configure logging
|
||||
if args.quiet:
|
||||
log_level = logging.WARNING
|
||||
elif args.debug:
|
||||
log_level = logging.DEBUG
|
||||
else:
|
||||
log_level = logging.INFO
|
||||
|
||||
logging.basicConfig(
|
||||
format="%(relativeCreated)010.3f - %(message)s",
|
||||
level=log_level,
|
||||
)
|
||||
|
||||
if args.script is None:
|
||||
args.script = test_script
|
||||
logging.info("Using built-in test script...")
|
||||
else:
|
||||
with open(args.script, 'r') as f:
|
||||
args.script = f.read()
|
||||
logging.info("Loaded script from file")
|
||||
|
||||
try:
|
||||
with OMVCamera(args.port, baudrate=args.baudrate, crc=args.crc, seq=args.seq,
|
||||
ack=args.ack, events=args.events, timeout=args.timeout, max_retry=args.max_retry,
|
||||
max_payload=args.max_payload, drop_rate=args.drop_rate) as camera:
|
||||
logging.critical(f"Connected to OpenMV camera on {args.port}")
|
||||
|
||||
data = camera.read_profile()
|
||||
if data:
|
||||
logging.info(f"Profile data ({len(data)} entries):")
|
||||
# Print first 5 entries to avoid overwhelming output
|
||||
for i, entry in enumerate(data[:min(5, len(data))]):
|
||||
logging.info(f"Entry {i}: address=0x{entry['address']:08X}, "
|
||||
f"caller=0x{entry['caller']:08X}, calls={entry['call_count']}, "
|
||||
f"ticks={entry['total_ticks']}")
|
||||
if len(data) > 5:
|
||||
logging.info(f"... and {len(data) - min(5, len(data))} more entries")
|
||||
|
||||
# Execute script
|
||||
logging.info("Executing script...")
|
||||
camera.streaming(True, raw=False, res=(256, 256))
|
||||
camera.exec(args.script)
|
||||
logging.info("Script executed successfully")
|
||||
|
||||
# Let script run for a few seconds
|
||||
test_time = 1
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < test_time:
|
||||
if not (status := camera.read_status()):
|
||||
continue
|
||||
|
||||
if status['stdout'] and (text := camera.read_stdout()):
|
||||
if not args.quiet:
|
||||
print(text, end='')
|
||||
|
||||
if status['stream'] and (frame := camera.read_frame()):
|
||||
logging.info(f"Frame: {frame['width']}x{frame['height']} depth={frame['depth']} "
|
||||
f"format=0x{frame['format']:08X} length: {len(frame['data'])} bytes")
|
||||
|
||||
# Test custom channel
|
||||
if data := camera.channel_read("time"):
|
||||
logging.info("Channel output: " + bytes(data).decode('utf-8', errors='ignore'))
|
||||
|
||||
if camera.has_channel("buffer"):
|
||||
random_data = bytes([random.randint(0, 255) for _ in range(10)])
|
||||
logging.info(f"Writing random data: {random_data.hex()}")
|
||||
camera.channel_write("buffer", random_data)
|
||||
if data := camera.channel_read("buffer"):
|
||||
logging.info(f"Read back data: {bytes(data).hex()}")
|
||||
if bytes(data) == random_data:
|
||||
logging.info("✓ Data verification successful!")
|
||||
else:
|
||||
logging.warning("✗ Data mismatch!")
|
||||
|
||||
# Stop the script
|
||||
logging.info("Stopping script...")
|
||||
camera.stop()
|
||||
|
||||
# Read output for a while
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < 1:
|
||||
if status['stdout'] and (text := camera.read_stdout()):
|
||||
if not args.quiet:
|
||||
print(text, end='')
|
||||
|
||||
print("")
|
||||
host_stats = camera.host_stats()
|
||||
logging.critical("=========== Host Statistics ===========")
|
||||
logging.critical(f"{host_stats}")
|
||||
logging.critical(f"==========================================")
|
||||
|
||||
device_stats = camera.device_stats()
|
||||
logging.critical("=========== Device Statistics ===========")
|
||||
logging.critical(f"{device_stats}")
|
||||
logging.critical(f"==========================================")
|
||||
|
||||
except KeyboardInterrupt:
|
||||
logging.info("Interrupted by user")
|
||||
sys.exit(0)
|
||||
except Exception as e:
|
||||
logging.error(f"Error: {e}")
|
||||
import traceback
|
||||
logging.error(f"{traceback.format_exc()}")
|
||||
sys.exit(1)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
211
tools/openmv/examples/framebuffer.py
Normal file
211
tools/openmv/examples/framebuffer.py
Normal file
@ -0,0 +1,211 @@
|
||||
#!/usr/bin/env python
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Framebuffer Display Example
|
||||
#
|
||||
# This example demonstrates how to display live video from an OpenMV camera using pygame.
|
||||
# It shows the basic functionality of connecting to a camera, executing a script, and
|
||||
# displaying the framebuffer data in real-time with FPS counter.
|
||||
#
|
||||
# Controls:
|
||||
# - C key: Capture screenshot to 'capture.png'
|
||||
# - ESC key: Exit
|
||||
#
|
||||
# Dependencies:
|
||||
# - pygame
|
||||
# - numpy
|
||||
|
||||
import sys
|
||||
import os
|
||||
import argparse
|
||||
import time
|
||||
import logging
|
||||
import pygame
|
||||
import numpy as np
|
||||
|
||||
# Add parent directories to path for openmv module
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '../..'))
|
||||
|
||||
from openmv.camera import OMVCamera
|
||||
|
||||
def str2bool(v):
|
||||
"""Convert string to boolean for argparse"""
|
||||
if isinstance(v, bool):
|
||||
return v
|
||||
if v.lower() in ('yes', 'true', 't', 'y', '1'):
|
||||
return True
|
||||
elif v.lower() in ('no', 'false', 'f', 'n', '0'):
|
||||
return False
|
||||
else:
|
||||
raise argparse.ArgumentTypeError('Boolean value expected.')
|
||||
|
||||
# Benchmark script for throughput testing
|
||||
bench_script = """
|
||||
import sensor, image, time
|
||||
sensor.reset()
|
||||
sensor.set_pixformat(sensor.RGB565)
|
||||
sensor.set_framesize(sensor.QVGA)
|
||||
img = sensor.snapshot().compress()
|
||||
while(True):
|
||||
img.flush()
|
||||
"""
|
||||
|
||||
# Default test script for sensor-based cameras
|
||||
test_script = """
|
||||
import sensor, image, time
|
||||
sensor.reset()
|
||||
sensor.set_pixformat(sensor.RGB565)
|
||||
sensor.set_framesize(sensor.QVGA)
|
||||
sensor.skip_frames(time = 2000)
|
||||
clock = time.clock()
|
||||
|
||||
while(True):
|
||||
clock.tick()
|
||||
img = sensor.snapshot()
|
||||
print(clock.fps(), " FPS")
|
||||
"""
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='OpenMV Framebuffer Display')
|
||||
parser.add_argument('--port', action='store', help='Serial port (default: /dev/ttyACM0)', default='/dev/ttyACM0')
|
||||
parser.add_argument("--script", action="store", default=None, help="Script file")
|
||||
parser.add_argument('--poll', action='store', help='Poll rate in ms (default: 4)', default=4, type=int)
|
||||
parser.add_argument('--scale', action='store', help='Display scaling factor (default: 4)', default=4, type=int)
|
||||
parser.add_argument('--bench', action='store_true', help='Run throughput benchmark', default=False)
|
||||
parser.add_argument('--timeout', action='store', type=float, default=2.0, help='Protocol timeout in seconds')
|
||||
parser.add_argument('--debug', action='store_true', help='Enable debug logging')
|
||||
|
||||
# Camera configuration options
|
||||
parser.add_argument('--baudrate', type=int, default=921600, help='Serial baudrate (default: 921600)')
|
||||
parser.add_argument('--crc', type=str2bool, nargs='?', const=True, default=True, help='Enable CRC validation (default: true)')
|
||||
parser.add_argument('--seq', type=str2bool, nargs='?', const=True, default=True, help='Enable sequence number validation (default: true)')
|
||||
parser.add_argument('--ack', type=str2bool, nargs='?', const=True, default=True, help='Enable packet acknowledgment (default: false)')
|
||||
parser.add_argument('--events', type=str2bool, nargs='?', const=True, default=True, help='Enable event notifications (default: true)')
|
||||
parser.add_argument('--max-retry', type=int, default=3, help='Maximum number of retries (default: 3)')
|
||||
parser.add_argument('--max-payload', type=int, default=4096, help='Maximum payload size in bytes (default: 4096)')
|
||||
parser.add_argument('--drop-rate', type=float, default=0.0, help='Packet drop simulation rate (0.0-1.0, default: 0.0)')
|
||||
parser.add_argument('--quiet', action='store_true', help='Suppress script output text')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# Configure logging
|
||||
if args.debug:
|
||||
log_level = logging.DEBUG
|
||||
else:
|
||||
log_level = logging.INFO
|
||||
|
||||
logging.basicConfig(
|
||||
format="%(relativeCreated)010.3f - %(message)s",
|
||||
level=log_level,
|
||||
)
|
||||
|
||||
# Load script
|
||||
if args.script is not None:
|
||||
with open(args.script, 'r') as f:
|
||||
script = f.read()
|
||||
logging.info(f"Loaded script from {args.script}")
|
||||
else:
|
||||
script = bench_script if args.bench else test_script
|
||||
logging.info("Using built-in script")
|
||||
|
||||
# Initialize pygame
|
||||
pygame.init()
|
||||
|
||||
screen = None
|
||||
clock = pygame.time.Clock()
|
||||
fps_clock = pygame.time.Clock()
|
||||
font = pygame.font.SysFont("monospace", 30)
|
||||
|
||||
if not args.bench:
|
||||
pygame.display.set_caption("OpenMV Camera")
|
||||
else:
|
||||
pygame.display.set_caption("OpenMV Camera (Benchmark)")
|
||||
screen = pygame.display.set_mode((640, 120), pygame.DOUBLEBUF, 32)
|
||||
|
||||
try:
|
||||
with OMVCamera(args.port, baudrate=args.baudrate, crc=args.crc, seq=args.seq,
|
||||
ack=args.ack, events=args.events,
|
||||
timeout=args.timeout, max_retry=args.max_retry,
|
||||
max_payload=args.max_payload, drop_rate=args.drop_rate) as camera:
|
||||
logging.info(f"Connected to OpenMV camera on {args.port}")
|
||||
|
||||
camera.stop()
|
||||
time.sleep(0.500) # Wait for soft-reboot (if script is running)
|
||||
camera.exec(script)
|
||||
camera.streaming(True, raw=False, res=(512, 512))
|
||||
logging.info("Script executed, starting display...")
|
||||
|
||||
while True:
|
||||
# Handle pygame events first to keep UI responsive
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
raise KeyboardInterrupt
|
||||
elif event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_ESCAPE:
|
||||
raise KeyboardInterrupt
|
||||
elif event.key == pygame.K_c and not args.bench:
|
||||
if 'image' in locals():
|
||||
pygame.image.save(image, "capture.png")
|
||||
logging.info("Screenshot saved as capture.png")
|
||||
|
||||
# Read camera status
|
||||
status = camera.read_status()
|
||||
|
||||
# Handle text output
|
||||
if not args.bench and status and status.get('stdout'):
|
||||
text = camera.read_stdout()
|
||||
if text and not args.quiet:
|
||||
print(text, end='')
|
||||
|
||||
# Handle frame data
|
||||
if frame := camera.read_frame():
|
||||
fps = fps_clock.get_fps()
|
||||
w, h, data = frame['width'], frame['height'], frame['data']
|
||||
|
||||
# Create image from RGB888 data (always converted by camera module)
|
||||
if not args.bench:
|
||||
image = pygame.image.frombuffer(data, (w, h), 'RGB')
|
||||
image = pygame.transform.smoothscale(image, (w * args.scale, h * args.scale))
|
||||
|
||||
# Create/resize screen if needed
|
||||
if screen is None:
|
||||
screen = pygame.display.set_mode((w * args.scale, h * args.scale), pygame.DOUBLEBUF, 32)
|
||||
|
||||
# Draw frame
|
||||
if args.bench:
|
||||
screen.fill((0, 0, 0))
|
||||
else:
|
||||
screen.blit(image, (0, 0))
|
||||
|
||||
# Draw FPS info with accurate data rate
|
||||
current_mbps = (fps * frame['raw_size']) / 1024**2
|
||||
if current_mbps < 1.0:
|
||||
rate_text = f"{current_mbps * 1024:.2f} KB/s"
|
||||
else:
|
||||
rate_text = f"{current_mbps:.2f} MB/s"
|
||||
fps_text = f"{fps:.2f} FPS {rate_text} {w}x{h} RGB888"
|
||||
screen.blit(font.render(fps_text, True, (255, 0, 0)), (0, 0))
|
||||
|
||||
# Update display
|
||||
pygame.display.flip()
|
||||
fps_clock.tick()
|
||||
|
||||
# Control main loop timing
|
||||
clock.tick(1000//args.poll)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
logging.info("Interrupted by user")
|
||||
sys.exit(0)
|
||||
except Exception as e:
|
||||
logging.error(f"Error: {e}")
|
||||
import traceback
|
||||
logging.error(f"{traceback.format_exc()}")
|
||||
sys.exit(1)
|
||||
finally:
|
||||
pygame.quit()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
625
tools/openmv/examples/profiler.py
Normal file
625
tools/openmv/examples/profiler.py
Normal file
@ -0,0 +1,625 @@
|
||||
#!/usr/bin/env python
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Framebuffer Display with Advanced Profiling
|
||||
#
|
||||
# This example demonstrates advanced profiling features of OpenMV cameras, including:
|
||||
# - Live framebuffer display with pygame
|
||||
# - Performance profiling with function timing
|
||||
# - Event counter profiling for hardware performance monitoring
|
||||
# - Symbol loading from ELF firmware files for function name resolution
|
||||
# - Interactive profiling controls and colorized performance visualization
|
||||
#
|
||||
# Controls:
|
||||
# - P key: Toggle profiling overlay (off/profile/events)
|
||||
# - M key: Toggle profiling mode (inclusive/exclusive)
|
||||
# - R key: Reset profiler data
|
||||
# - C key: Capture screenshot to 'capture.png'
|
||||
# - ESC key: Exit
|
||||
#
|
||||
# Dependencies:
|
||||
# - pygame
|
||||
# - numpy
|
||||
# - elftools (for symbol loading)
|
||||
|
||||
import sys
|
||||
import os
|
||||
import argparse
|
||||
import time
|
||||
import logging
|
||||
import pygame
|
||||
import numpy as np
|
||||
import struct
|
||||
from functools import reduce
|
||||
from operator import mul
|
||||
|
||||
# Add parent directories to path for openmv module
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '../..'))
|
||||
|
||||
from openmv.camera import OMVCamera
|
||||
|
||||
def str2bool(v):
|
||||
"""Convert string to boolean for argparse"""
|
||||
if isinstance(v, bool):
|
||||
return v
|
||||
if v.lower() in ('yes', 'true', 't', 'y', '1'):
|
||||
return True
|
||||
elif v.lower() in ('no', 'false', 'f', 'n', '0'):
|
||||
return False
|
||||
else:
|
||||
raise argparse.ArgumentTypeError('Boolean value expected.')
|
||||
|
||||
# Default test script for sensor-based cameras
|
||||
test_script = """
|
||||
import sensor, image, time
|
||||
sensor.reset()
|
||||
sensor.set_pixformat(sensor.RGB565)
|
||||
sensor.set_framesize(sensor.QVGA)
|
||||
sensor.skip_frames(time = 2000)
|
||||
clock = time.clock()
|
||||
|
||||
while(True):
|
||||
clock.tick()
|
||||
img = sensor.snapshot()
|
||||
print(clock.fps(), " FPS")
|
||||
"""
|
||||
|
||||
# Benchmark script for throughput testing
|
||||
bench_script = """
|
||||
import sensor, image, time
|
||||
sensor.reset()
|
||||
sensor.set_pixformat(sensor.RGB565)
|
||||
sensor.set_framesize(sensor.VGA)
|
||||
img = sensor.snapshot().compress()
|
||||
while(True):
|
||||
img.flush()
|
||||
"""
|
||||
|
||||
def addr_to_symbol(symbols, address):
|
||||
"""Binary search for speed"""
|
||||
lo, hi = 0, len(symbols) - 1
|
||||
while lo <= hi:
|
||||
mid = (lo + hi) // 2
|
||||
start, end, name = symbols[mid]
|
||||
if start <= address < end:
|
||||
return name
|
||||
elif address < start:
|
||||
hi = mid - 1
|
||||
else:
|
||||
lo = mid + 1
|
||||
return None
|
||||
|
||||
def get_color_by_percentage(percentage, base_color=(220, 220, 220)):
|
||||
"""Return a color based on percentage with fine-grained intensity levels."""
|
||||
def clamp(value):
|
||||
return max(0, min(255, int(value)))
|
||||
|
||||
if percentage >= 50:
|
||||
# Very high - bright red
|
||||
intensity = min(1.0, (percentage - 50) / 50)
|
||||
return (255, clamp(120 - 120 * intensity), clamp(120 - 120 * intensity))
|
||||
elif percentage >= 30:
|
||||
# High - red-orange
|
||||
intensity = (percentage - 30) / 20
|
||||
return (255, clamp(160 + 40 * intensity), clamp(160 - 40 * intensity))
|
||||
elif percentage >= 20:
|
||||
# Medium-high - orange
|
||||
intensity = (percentage - 20) / 10
|
||||
return (255, clamp(200 + 55 * intensity), clamp(180 - 20 * intensity))
|
||||
elif percentage >= 15:
|
||||
# Medium - yellow-orange
|
||||
intensity = (percentage - 15) / 5
|
||||
return (255, clamp(220 + 35 * intensity), clamp(180 + 20 * intensity))
|
||||
elif percentage >= 10:
|
||||
# Medium-low - yellow
|
||||
intensity = (percentage - 10) / 5
|
||||
return (clamp(255 - 75 * intensity), 255, clamp(180 + 75 * intensity))
|
||||
elif percentage >= 5:
|
||||
# Low - light green
|
||||
intensity = (percentage - 5) / 5
|
||||
return (clamp(180 + 75 * intensity), 255, clamp(180 + 75 * intensity))
|
||||
elif percentage >= 2:
|
||||
# Very low - green
|
||||
intensity = (percentage - 2) / 3
|
||||
return (clamp(160 + 95 * intensity), clamp(255 - 55 * intensity), clamp(160 + 95 * intensity))
|
||||
elif percentage >= 1:
|
||||
# Minimal - light blue-green
|
||||
intensity = (percentage - 1) / 1
|
||||
return (clamp(140 + 120 * intensity), clamp(200 + 55 * intensity), clamp(255 - 95 * intensity))
|
||||
else:
|
||||
# Zero or negligible - base color
|
||||
return base_color
|
||||
|
||||
def draw_rounded_rect(surface, color, rect, radius=5):
|
||||
x, y, w, h = rect
|
||||
if w <= 0 or h <= 0:
|
||||
return
|
||||
pygame.draw.rect(surface, color, (x + radius, y, w - 2*radius, h))
|
||||
pygame.draw.rect(surface, color, (x, y + radius, w, h - 2*radius))
|
||||
pygame.draw.circle(surface, color, (x + radius, y + radius), radius)
|
||||
pygame.draw.circle(surface, color, (x + w - radius, y + radius), radius)
|
||||
pygame.draw.circle(surface, color, (x + radius, y + h - radius), radius)
|
||||
pygame.draw.circle(surface, color, (x + w - radius, y + h - radius), radius)
|
||||
|
||||
|
||||
def draw_table(overlay_surface, config, title, headers, col_widths):
|
||||
"""Draw the common table background, title, and header."""
|
||||
# Draw main table background
|
||||
table_rect = (0, 0, config['width'], config['height'])
|
||||
draw_rounded_rect(overlay_surface, config['colors']['bg'], table_rect, int(8 * config['scale_factor']))
|
||||
pygame.draw.rect(overlay_surface, config['colors']['border'], table_rect, max(1, int(2 * config['scale_factor'])))
|
||||
|
||||
# Table title
|
||||
title_text = config['fonts']['title'].render(title, True, config['colors']['header_text'])
|
||||
title_rect = title_text.get_rect()
|
||||
title_x = (config['width'] - title_rect.width) // 2
|
||||
overlay_surface.blit(title_text, (title_x, int(12 * config['scale_factor'])))
|
||||
|
||||
# Header
|
||||
header_y = int(50 * config['scale_factor'])
|
||||
header_height = int(40 * config['scale_factor'])
|
||||
|
||||
# Draw header background
|
||||
header_rect = (int(5 * config['scale_factor']), header_y,
|
||||
config['width'] - int(10 * config['scale_factor']), header_height)
|
||||
draw_rounded_rect(overlay_surface, config['colors']['header_bg'], header_rect, int(4 * config['scale_factor']))
|
||||
|
||||
# Draw header text and separators
|
||||
current_x = int(10 * config['scale_factor'])
|
||||
for i, (header, width) in enumerate(zip(headers, col_widths)):
|
||||
header_surface = config['fonts']['header'].render(header, True, config['colors']['header_text'])
|
||||
overlay_surface.blit(header_surface, (current_x, header_y + int(6 * config['scale_factor'])))
|
||||
|
||||
if i < len(headers) - 1:
|
||||
sep_x = current_x + width - int(5 * config['scale_factor'])
|
||||
pygame.draw.line(overlay_surface, config['colors']['border'],
|
||||
(sep_x, header_y + 2), (sep_x, header_y + header_height - 2), 1)
|
||||
current_x += width
|
||||
|
||||
|
||||
def draw_event_table(overlay_surface, config, profile_data, profile_mode, symbols):
|
||||
"""Draw the event counter mode table."""
|
||||
|
||||
# Prepare data
|
||||
num_events = len(profile_data[0]['events']) if profile_data else 0
|
||||
if not num_events:
|
||||
sorted_data = sorted(profile_data, key=lambda x: x['address'])
|
||||
else:
|
||||
sort_func = lambda x: x['events'][0] // max(1, x['call_count'])
|
||||
sorted_data = sorted(profile_data, key=sort_func, reverse=True)
|
||||
|
||||
headers = ["Function"] + [f"E{i}" for i in range(num_events)]
|
||||
proportions = [0.30] + [0.70/num_events] * num_events
|
||||
col_widths = [config['width'] * prop for prop in proportions]
|
||||
profile_mode = "Exclusive" if profile_mode else "Inclusive"
|
||||
|
||||
# Calculate event totals for percentage calculation
|
||||
event_totals = [0] * num_events
|
||||
for record in sorted_data:
|
||||
for i, event_count in enumerate(record['events']):
|
||||
event_totals[i] += event_count // max(1, record['call_count'])
|
||||
|
||||
# Draw table structure
|
||||
draw_table(overlay_surface, config, f"Event Counters ({profile_mode})", headers, col_widths)
|
||||
|
||||
# Draw data rows
|
||||
row_height = int(30 * config['scale_factor'])
|
||||
data_start_y = int(50 * config['scale_factor'] + 40 * config['scale_factor'] + 8 * config['scale_factor'])
|
||||
available_height = config['height'] - data_start_y - int(60 * config['scale_factor'])
|
||||
visible_rows = min(len(sorted_data), available_height // row_height)
|
||||
|
||||
for i in range(visible_rows):
|
||||
record = sorted_data[i]
|
||||
row_y = data_start_y + i * row_height
|
||||
|
||||
# Draw row background
|
||||
row_color = config['colors']['row_alt'] if i % 2 == 0 else config['colors']['row_normal']
|
||||
row_rect = (int(5 * config['scale_factor']), row_y,
|
||||
config['width'] - int(10 * config['scale_factor']), row_height)
|
||||
pygame.draw.rect(overlay_surface, row_color, row_rect)
|
||||
|
||||
# Function name
|
||||
name = addr_to_symbol(symbols, record['address']) if symbols else "<no symbols>"
|
||||
max_name_chars = int(col_widths[0] // (11 * config['scale_factor']))
|
||||
display_name = name if len(name) <= max_name_chars else name[:max_name_chars - 3] + "..."
|
||||
|
||||
row_data = [display_name]
|
||||
|
||||
# Event data
|
||||
for j, event_count in enumerate(record['events']):
|
||||
event_scale = ""
|
||||
event_count //= max(1, record['call_count'])
|
||||
if event_count > 1_000_000_000:
|
||||
event_count //= 1_000_000_000
|
||||
event_scale = "B"
|
||||
elif event_count > 1_000_000:
|
||||
event_count //= 1_000_000
|
||||
event_scale = "M"
|
||||
row_data.append(f"{event_count:,}{event_scale}")
|
||||
|
||||
# Determine row color based on sorting key (event 0)
|
||||
if len(record['events']) > 0 and event_totals[0] > 0:
|
||||
sort_key_value = record['events'][0] // max(1, record['call_count'])
|
||||
percentage = (sort_key_value / event_totals[0] * 100)
|
||||
row_text_color = get_color_by_percentage(percentage, config['colors']['content_text'])
|
||||
else:
|
||||
row_text_color = config['colors']['content_text']
|
||||
|
||||
# Draw row data with uniform color
|
||||
current_x = 10
|
||||
for j, (data, width) in enumerate(zip(row_data, col_widths)):
|
||||
text_surface = config['fonts']['content'].render(str(data), True, row_text_color)
|
||||
overlay_surface.blit(text_surface, (current_x, row_y + int(8 * config['scale_factor'])))
|
||||
|
||||
if j < len(row_data) - 1:
|
||||
sep_x = current_x + width - 8
|
||||
pygame.draw.line(overlay_surface, (60, 70, 85),
|
||||
(sep_x, row_y), (sep_x, row_y + row_height), 1)
|
||||
current_x += width
|
||||
|
||||
# Draw summary
|
||||
summary_y = config['height'] - int(50 * config['scale_factor'])
|
||||
total_functions = len(profile_data)
|
||||
grand_total = sum(event_totals)
|
||||
summary_text = (
|
||||
f"Profiles: {total_functions} | "
|
||||
f"Events: {num_events} | "
|
||||
f"Total Events: {grand_total:,}"
|
||||
)
|
||||
|
||||
summary_surface = config['fonts']['summary'].render(summary_text, True, config['colors']['content_text'])
|
||||
summary_rect = summary_surface.get_rect()
|
||||
summary_x = (config['width'] - summary_rect.width) // 2
|
||||
overlay_surface.blit(summary_surface, (summary_x, summary_y))
|
||||
|
||||
# Instructions
|
||||
instruction_str = "Press 'P' to toggle event counter overlay"
|
||||
instruction_text = config['fonts']['instruction'].render(instruction_str, True, (180, 180, 180))
|
||||
overlay_surface.blit(instruction_text, (0, summary_y + int(20 * config['scale_factor'])))
|
||||
|
||||
|
||||
def draw_profile_table(overlay_surface, config, profile_data, profile_mode, symbols):
|
||||
"""Draw the profile mode table."""
|
||||
|
||||
# Prepare data
|
||||
sort_func = lambda x: x['total_ticks']
|
||||
sorted_data = sorted(profile_data, key=sort_func, reverse=True)
|
||||
total_ticks_all = sum(record['total_ticks'] for record in profile_data)
|
||||
profile_mode = "Exclusive" if profile_mode else "Inclusive"
|
||||
|
||||
headers = ["Function", "Calls", "Min", "Max", "Total", "Avg", "Cycles", "%"]
|
||||
proportions = [0.30, 0.08, 0.10, 0.10, 0.13, 0.10, 0.13, 0.05]
|
||||
col_widths = [config['width'] * prop for prop in proportions]
|
||||
|
||||
# Draw table structure
|
||||
draw_table(overlay_surface, config, f"Performance Profile ({profile_mode})", headers, col_widths)
|
||||
|
||||
# Draw data rows
|
||||
row_height = int(30 * config['scale_factor'])
|
||||
data_start_y = int(50 * config['scale_factor'] + 40 * config['scale_factor'] + 8 * config['scale_factor'])
|
||||
available_height = config['height'] - data_start_y - int(60 * config['scale_factor'])
|
||||
visible_rows = min(len(sorted_data), available_height // row_height)
|
||||
|
||||
for i in range(visible_rows):
|
||||
record = sorted_data[i]
|
||||
row_y = data_start_y + i * row_height
|
||||
|
||||
# Draw row background
|
||||
row_color = config['colors']['row_alt'] if i % 2 == 0 else config['colors']['row_normal']
|
||||
row_rect = (int(5 * config['scale_factor']), row_y,
|
||||
config['width'] - int(10 * config['scale_factor']), row_height)
|
||||
pygame.draw.rect(overlay_surface, row_color, row_rect)
|
||||
|
||||
# Function name
|
||||
name = addr_to_symbol(symbols, record['address']) if symbols else "<no symbols>"
|
||||
max_name_chars = int(col_widths[0] // (11 * config['scale_factor']))
|
||||
display_name = name if len(name) <= max_name_chars else name[:max_name_chars - 3] + "..."
|
||||
|
||||
# Calculate values
|
||||
call_count = record['call_count']
|
||||
min_ticks = record['min_ticks'] if call_count else 0
|
||||
max_ticks = record['max_ticks'] if call_count else 0
|
||||
total_ticks = record['total_ticks']
|
||||
avg_cycles = record['total_cycles'] // max(1, call_count)
|
||||
avg_ticks = total_ticks // max(1, call_count)
|
||||
percentage = (total_ticks / total_ticks_all * 100) if total_ticks_all else 0
|
||||
|
||||
ticks_scale = ""
|
||||
if total_ticks > 1_000_000_000:
|
||||
total_ticks //= 1_000_000
|
||||
ticks_scale = "M"
|
||||
|
||||
row_data = [
|
||||
display_name,
|
||||
f"{call_count:,}",
|
||||
f"{min_ticks:,}",
|
||||
f"{max_ticks:,}",
|
||||
f"{total_ticks:,}{ticks_scale}",
|
||||
f"{avg_ticks:,}",
|
||||
f"{avg_cycles:,}",
|
||||
f"{percentage:.1f}%"
|
||||
]
|
||||
|
||||
# Determine row color based on percentage
|
||||
text_color = get_color_by_percentage(percentage, config['colors']['content_text'])
|
||||
|
||||
# Draw row data
|
||||
current_x = int(10 * config['scale_factor'])
|
||||
for j, (data, width) in enumerate(zip(row_data, col_widths)):
|
||||
text_surface = config['fonts']['content'].render(str(data), True, text_color)
|
||||
overlay_surface.blit(text_surface, (current_x, row_y + int(8 * config['scale_factor'])))
|
||||
|
||||
if j < len(row_data) - 1:
|
||||
sep_x = current_x + width - int(8 * config['scale_factor'])
|
||||
pygame.draw.line(overlay_surface, (60, 70, 85),
|
||||
(sep_x, row_y), (sep_x, row_y + row_height), 1)
|
||||
current_x += width
|
||||
|
||||
# Draw summary
|
||||
summary_y = config['height'] - int(50 * config['scale_factor'])
|
||||
total_calls = sum(record['call_count'] for record in profile_data)
|
||||
total_cycles = sum(record['total_cycles'] for record in profile_data)
|
||||
total_ticks_summary = sum(record['total_ticks'] for record in profile_data)
|
||||
|
||||
summary_text = (
|
||||
f"Profiles: {len(profile_data)} | "
|
||||
f"Total Calls: {total_calls:,} | "
|
||||
f"Total Ticks: {total_ticks_summary:,} | "
|
||||
f"Total Cycles: {total_cycles:,}"
|
||||
)
|
||||
|
||||
summary_surface = config['fonts']['summary'].render(summary_text, True, config['colors']['content_text'])
|
||||
summary_rect = summary_surface.get_rect()
|
||||
summary_x = (config['width'] - summary_rect.width) // 2
|
||||
overlay_surface.blit(summary_surface, (summary_x, summary_y))
|
||||
|
||||
# Instructions
|
||||
instruction_str = "Press 'P' to toggle event counter overlay"
|
||||
instruction_text = config['fonts']['instruction'].render(instruction_str, True, (180, 180, 180))
|
||||
overlay_surface.blit(instruction_text, (0, summary_y + int(20 * config['scale_factor'])))
|
||||
|
||||
def handle_key_events(event, camera, profile_type, profile_mode, zoom_factor, base_width, base_height, screen, args):
|
||||
"""Handle keyboard events and return updated values"""
|
||||
if event.key == pygame.K_ESCAPE:
|
||||
raise KeyboardInterrupt
|
||||
elif event.key == pygame.K_c and not args.bench:
|
||||
if 'image' in locals():
|
||||
pygame.image.save(image, "capture.png")
|
||||
logging.info("Screenshot saved as capture.png")
|
||||
elif event.key == pygame.K_p:
|
||||
profile_type = not profile_type
|
||||
logging.info(f"Profile type: {'Performance' if profile_type else 'Events'}")
|
||||
elif event.key == pygame.K_m:
|
||||
profile_mode = not profile_mode
|
||||
camera.profiler_mode(exclusive=profile_mode)
|
||||
logging.info(f"Profile mode: {'Exclusive' if profile_mode else 'Inclusive'}")
|
||||
elif event.key == pygame.K_r:
|
||||
camera.profiler_reset()
|
||||
logging.info("Profiler reset")
|
||||
elif event.key == pygame.K_EQUALS and (pygame.key.get_pressed()[pygame.K_LCTRL] or pygame.key.get_pressed()[pygame.K_RCTRL]):
|
||||
# Ctrl+ to zoom in
|
||||
zoom_factor = min(zoom_factor * 1.25, 3.0)
|
||||
new_width, new_height = int(base_width * zoom_factor), int(base_height * zoom_factor)
|
||||
screen = pygame.display.set_mode((new_width, new_height), pygame.DOUBLEBUF, 32)
|
||||
logging.info(f"Zoom: {zoom_factor:.2f}x")
|
||||
elif event.key == pygame.K_MINUS and (pygame.key.get_pressed()[pygame.K_LCTRL] or pygame.key.get_pressed()[pygame.K_RCTRL]):
|
||||
# Ctrl- to zoom out
|
||||
zoom_factor = max(zoom_factor / 1.25, 0.5)
|
||||
new_width, new_height = int(base_width * zoom_factor), int(base_height * zoom_factor)
|
||||
screen = pygame.display.set_mode((new_width, new_height), pygame.DOUBLEBUF, 32)
|
||||
logging.info(f"Zoom: {zoom_factor:.2f}x")
|
||||
|
||||
return profile_type, profile_mode, zoom_factor, screen
|
||||
|
||||
def draw_profile_overlay(screen, screen_width, screen_height, profile_data,
|
||||
profile_mode, profile_type, scale, symbols, alpha=250):
|
||||
"""Main entry point for drawing the profile overlay."""
|
||||
# Calculate dimensions and create surface
|
||||
base_width, base_height = 800, 800
|
||||
screen_width *= scale
|
||||
screen_height *= scale
|
||||
scale_factor = min(screen_width / base_width, screen_height / base_height)
|
||||
|
||||
overlay_surface = pygame.Surface((screen_width, screen_height), pygame.SRCALPHA)
|
||||
overlay_surface.set_alpha(alpha)
|
||||
|
||||
# Setup common configuration
|
||||
config = {
|
||||
'width': screen_width,
|
||||
'height': screen_height,
|
||||
'scale_factor': scale_factor,
|
||||
'colors': {
|
||||
'bg': (40, 50, 65),
|
||||
'border': (70, 80, 100),
|
||||
'header_bg': (60, 80, 120),
|
||||
'header_text': (255, 255, 255),
|
||||
'content_text': (220, 220, 220),
|
||||
'row_alt': (35, 45, 60),
|
||||
'row_normal': (45, 55, 70)
|
||||
},
|
||||
'fonts': {
|
||||
'title': pygame.font.SysFont("arial", int(28 * scale_factor), bold=True),
|
||||
'header': pygame.font.SysFont("monospace", int(20 * scale_factor), bold=True),
|
||||
'content': pygame.font.SysFont("monospace", int(18 * scale_factor)),
|
||||
'summary': pygame.font.SysFont("arial", int(20 * scale_factor)),
|
||||
'instruction': pygame.font.SysFont("arial", int(22 * scale_factor))
|
||||
}
|
||||
}
|
||||
|
||||
# Draw based on mode
|
||||
if profile_type == 1:
|
||||
draw_profile_table(overlay_surface, config, profile_data, profile_mode, symbols)
|
||||
elif profile_type == 2:
|
||||
draw_event_table(overlay_surface, config, profile_data, profile_mode, symbols)
|
||||
|
||||
screen.blit(overlay_surface, (0, 0))
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='OpenMV Framebuffer Display with Profiling')
|
||||
parser.add_argument('--port', action='store', help='Serial port (default: /dev/ttyACM0)', default='/dev/ttyACM0')
|
||||
parser.add_argument("--script", action="store", default=None, help="Script file")
|
||||
parser.add_argument('--poll', action='store', help='Poll rate in ms (default: 4)', default=4, type=int)
|
||||
parser.add_argument('--bench', action='store_true', help='Run throughput benchmark', default=False)
|
||||
parser.add_argument('--firmware', action='store', help='Firmware ELF file for symbol resolution', default=None)
|
||||
parser.add_argument('--timeout', action='store', type=float, default=2.0, help='Protocol timeout in seconds')
|
||||
parser.add_argument('--debug', action='store_true', help='Enable debug logging')
|
||||
|
||||
# Camera configuration options
|
||||
parser.add_argument('--baudrate', type=int, default=921600, help='Serial baudrate (default: 921600)')
|
||||
parser.add_argument('--crc', type=str2bool, nargs='?', const=True, default=True, help='Enable CRC validation (default: true)')
|
||||
parser.add_argument('--seq', type=str2bool, nargs='?', const=True, default=True, help='Enable sequence number validation (default: true)')
|
||||
parser.add_argument('--ack', type=str2bool, nargs='?', const=True, default=False, help='Enable packet acknowledgment (default: false)')
|
||||
parser.add_argument('--events', type=str2bool, nargs='?', const=True, default=True, help='Enable event notifications (default: true)')
|
||||
parser.add_argument('--max-retry', type=int, default=3, help='Maximum number of retries (default: 3)')
|
||||
parser.add_argument('--max-payload', type=int, default=4096, help='Maximum payload size in bytes (default: 4096)')
|
||||
parser.add_argument('--quiet', action='store_true', help='Suppress script output text')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# Configure logging
|
||||
if args.quiet:
|
||||
log_level = logging.WARNING
|
||||
elif args.debug:
|
||||
log_level = logging.DEBUG
|
||||
else:
|
||||
log_level = logging.INFO
|
||||
|
||||
logging.basicConfig(
|
||||
format="%(relativeCreated)010.3f - %(message)s",
|
||||
level=log_level,
|
||||
)
|
||||
|
||||
# Load script
|
||||
if args.script is not None:
|
||||
with open(args.script, 'r') as f:
|
||||
script = f.read()
|
||||
logging.info(f"Loaded script from {args.script}")
|
||||
else:
|
||||
script = bench_script if args.bench else test_script
|
||||
logging.info("Using built-in script")
|
||||
|
||||
# Load symbols from firmware ELF file
|
||||
symbols = []
|
||||
if args.firmware:
|
||||
try:
|
||||
from elftools.elf.elffile import ELFFile
|
||||
with open(args.firmware, 'rb') as f:
|
||||
elf = ELFFile(f)
|
||||
symtab = elf.get_section_by_name('.symtab')
|
||||
if not symtab:
|
||||
logging.warning("No symbol table found in ELF file")
|
||||
else:
|
||||
for sym in symtab.iter_symbols():
|
||||
addr = sym['st_value']
|
||||
size = sym['st_size']
|
||||
name = sym.name
|
||||
if name and size > 0: # ignore empty symbols
|
||||
symbols.append((addr, addr + size, name))
|
||||
symbols.sort()
|
||||
logging.info(f"Loaded {len(symbols)} symbols from {args.firmware}")
|
||||
except ImportError:
|
||||
logging.error("elftools package not installed. Install with: pip install pyelftools")
|
||||
sys.exit(1)
|
||||
except Exception as e:
|
||||
logging.error(f"Failed to load symbols from {args.firmware}: {e}")
|
||||
|
||||
# Initialize pygame with centered window
|
||||
import os
|
||||
os.environ['SDL_VIDEO_CENTERED'] = '1'
|
||||
pygame.init()
|
||||
|
||||
running = True
|
||||
screen = None
|
||||
clock = pygame.time.Clock()
|
||||
fps_clock = pygame.time.Clock()
|
||||
font = pygame.font.SysFont("monospace", 30)
|
||||
|
||||
# Profiling control - start with performance profiling enabled
|
||||
profile_type = True # True = performance profiling, False = event profiling
|
||||
profile_mode = False # False = inclusive, True = exclusive
|
||||
profile_data = []
|
||||
last_profile_read = 0
|
||||
|
||||
# Zoom control
|
||||
zoom_factor = 1.0
|
||||
base_width, base_height = 1600, 1200
|
||||
|
||||
# Create window for profiling display
|
||||
pygame.display.set_caption("OpenMV Profiler")
|
||||
screen = pygame.display.set_mode((int(base_width * zoom_factor),
|
||||
int(base_height * zoom_factor)), pygame.DOUBLEBUF, 32)
|
||||
|
||||
|
||||
try:
|
||||
with OMVCamera(args.port, baudrate=args.baudrate, crc=args.crc, seq=args.seq,
|
||||
ack=args.ack, events=args.events, timeout=args.timeout, max_retry=args.max_retry,
|
||||
max_payload=args.max_payload) as camera:
|
||||
logging.info(f"Connected to OpenMV camera on {args.port}")
|
||||
|
||||
# Configure event counters for profiling
|
||||
camera.profiler_event_type(0, 0x0039) # CPU cycles
|
||||
camera.profiler_event_type(1, 0x0023) # L1I cache access
|
||||
camera.profiler_event_type(2, 0x0024) # L1I cache refill
|
||||
camera.profiler_event_type(3, 0x0001) # L1I cache TLB refill
|
||||
camera.profiler_event_type(4, 0x0003) # L1D cache access
|
||||
camera.profiler_event_type(5, 0xC102) # L1D cache refill
|
||||
camera.profiler_event_type(6, 0x02CC) # L2D cache access
|
||||
camera.profiler_event_type(7, 0xC303) # L2D cache refill
|
||||
|
||||
camera.profiler_reset()
|
||||
|
||||
camera.stop()
|
||||
camera.exec(script)
|
||||
camera.streaming(True)
|
||||
logging.info("Script executed, starting display...")
|
||||
|
||||
while True:
|
||||
# Handle pygame events first to keep UI responsive
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
raise KeyboardInterrupt
|
||||
elif event.type == pygame.KEYDOWN:
|
||||
profile_type, profile_mode, zoom_factor, screen = handle_key_events(
|
||||
event, camera, profile_type, profile_mode, zoom_factor,
|
||||
base_width, base_height, screen, args)
|
||||
|
||||
# Read profiling data (maximum 10Hz)
|
||||
current_time = time.time()
|
||||
if current_time - last_profile_read >= 0.1: # 10Hz = 0.1s interval
|
||||
tmp_data = camera.read_profile()
|
||||
if tmp_data:
|
||||
profile_data = tmp_data
|
||||
last_profile_read = current_time
|
||||
|
||||
# Clear screen
|
||||
screen.fill((0, 0, 0))
|
||||
|
||||
# Draw profile overlay if enabled
|
||||
if profile_data:
|
||||
screen_width, screen_height = screen.get_size()
|
||||
# Convert boolean to expected integer: True=1 (performance), False=2 (events)
|
||||
overlay_type = 1 if profile_type else 2
|
||||
draw_profile_overlay(screen, screen_width, screen_height,
|
||||
profile_data, profile_mode, overlay_type, 1, symbols)
|
||||
|
||||
# Update display
|
||||
pygame.display.flip()
|
||||
|
||||
# Control main loop timing
|
||||
clock.tick(1000//args.poll)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
logging.info("Interrupted by user")
|
||||
except Exception as e:
|
||||
logging.error(f"Error: {e}")
|
||||
import traceback
|
||||
logging.error(f"{traceback.format_exc()}")
|
||||
sys.exit(1)
|
||||
finally:
|
||||
pygame.quit()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
36
tools/openmv/exceptions.py
Normal file
36
tools/openmv/exceptions.py
Normal file
@ -0,0 +1,36 @@
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Protocol Exceptions
|
||||
#
|
||||
# This module defines all the custom exceptions used in the OpenMV
|
||||
# Protocol implementation for proper error handling and debugging.
|
||||
|
||||
import traceback
|
||||
|
||||
class OMVPException(Exception):
|
||||
"""Base exception for OpenMV protocol errors"""
|
||||
def __init__(self, message):
|
||||
super().__init__(message)
|
||||
self.traceback = traceback.format_exc()
|
||||
|
||||
class OMVPTimeoutException(OMVPException):
|
||||
"""Raised when a protocol operation times out"""
|
||||
def __init__(self, message):
|
||||
super().__init__(message)
|
||||
|
||||
class OMVPChecksumException(OMVPException):
|
||||
"""Raised when CRC validation fails"""
|
||||
def __init__(self, message):
|
||||
super().__init__(message)
|
||||
|
||||
class OMVPSequenceException(OMVPException):
|
||||
"""Raised when sequence number validation fails"""
|
||||
def __init__(self, message):
|
||||
super().__init__(message)
|
||||
|
||||
class OMVPResyncException(OMVPException):
|
||||
"""Raised to indicate that a resync was performed and operation should be retried"""
|
||||
def __init__(self, message="Resync performed, retry operation"):
|
||||
super().__init__(message)
|
123
tools/openmv/image.py
Normal file
123
tools/openmv/image.py
Normal file
@ -0,0 +1,123 @@
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Image Utilities
|
||||
#
|
||||
# This module provides image format conversion utilities for OpenMV camera data.
|
||||
# Handles conversion from various pixel formats to RGB888 for display purposes.
|
||||
|
||||
import logging
|
||||
try:
|
||||
import numpy as np
|
||||
except ImportError:
|
||||
np = None
|
||||
try:
|
||||
from PIL import Image
|
||||
except ImportError:
|
||||
Image = None
|
||||
|
||||
# Pixel format constants
|
||||
PIXFORMAT_GRAYSCALE = 0x08020001 # 1 byte per pixel
|
||||
PIXFORMAT_RGB565 = 0x0C030002 # 2 bytes per pixel
|
||||
PIXFORMAT_JPEG = 0x06060000 # Variable size JPEG
|
||||
|
||||
def convert_to_rgb888(raw_data, width, height, pixformat):
|
||||
"""
|
||||
Convert various pixel formats to RGB888.
|
||||
|
||||
Args:
|
||||
raw_data (bytes): Raw image data
|
||||
width (int): Image width
|
||||
height (int): Image height
|
||||
pixformat (int): Pixel format identifier
|
||||
|
||||
Returns:
|
||||
tuple: (rgb_data, format_string) where rgb_data is bytes or None on error
|
||||
"""
|
||||
|
||||
if pixformat == PIXFORMAT_GRAYSCALE:
|
||||
return _convert_grayscale(raw_data, width, height)
|
||||
elif pixformat == PIXFORMAT_RGB565:
|
||||
return _convert_rgb565(raw_data, width, height)
|
||||
elif pixformat == PIXFORMAT_JPEG:
|
||||
return _convert_jpeg(raw_data, width, height)
|
||||
else:
|
||||
# Unknown format - return raw data and let caller handle it
|
||||
fmt_str = f"0x{pixformat:08X}"
|
||||
logging.warning(f"Unknown pixel format: {fmt_str}")
|
||||
return raw_data, fmt_str
|
||||
|
||||
def _convert_grayscale(raw_data, width, height):
|
||||
"""Convert grayscale to RGB888"""
|
||||
fmt_str = "GRAY"
|
||||
|
||||
if np is None:
|
||||
logging.error("numpy required for grayscale conversion")
|
||||
return None, fmt_str
|
||||
|
||||
# Convert grayscale to RGB by duplicating the gray value
|
||||
gray_array = np.frombuffer(raw_data, dtype=np.uint8)
|
||||
if len(gray_array) != width * height:
|
||||
logging.error(f"Grayscale data size mismatch: expected {width * height}, got {len(gray_array)}")
|
||||
return None, fmt_str
|
||||
|
||||
rgb_array = np.column_stack((gray_array, gray_array, gray_array))
|
||||
return rgb_array.tobytes(), fmt_str
|
||||
|
||||
def _convert_rgb565(raw_data, width, height):
|
||||
"""Convert RGB565 to RGB888"""
|
||||
fmt_str = "RGB565"
|
||||
|
||||
if np is None:
|
||||
logging.error("numpy required for RGB565 conversion")
|
||||
return None, fmt_str
|
||||
|
||||
# Convert RGB565 to RGB888
|
||||
rgb565_array = np.frombuffer(raw_data, dtype=np.uint16)
|
||||
if len(rgb565_array) != width * height:
|
||||
logging.error(f"RGB565 data size mismatch: expected {width * height}, got {len(rgb565_array)}")
|
||||
return None, fmt_str
|
||||
|
||||
# Extract RGB components from 16-bit RGB565
|
||||
r = (((rgb565_array & 0xF800) >> 11) * 255.0 / 31.0).astype(np.uint8)
|
||||
g = (((rgb565_array & 0x07E0) >> 5) * 255.0 / 63.0).astype(np.uint8)
|
||||
b = (((rgb565_array & 0x001F) >> 0) * 255.0 / 31.0).astype(np.uint8)
|
||||
|
||||
rgb_array = np.column_stack((r, g, b))
|
||||
return rgb_array.tobytes(), fmt_str
|
||||
|
||||
def _convert_jpeg(raw_data, width, height):
|
||||
"""Convert JPEG to RGB888"""
|
||||
fmt_str = "JPEG"
|
||||
|
||||
if Image is None:
|
||||
logging.error("PIL/Pillow required for JPEG conversion")
|
||||
return None, fmt_str
|
||||
|
||||
try:
|
||||
# Decode JPEG to RGB
|
||||
image = Image.frombuffer("RGB", (width, height), raw_data, "jpeg", "RGB", "")
|
||||
rgb_array = np.asarray(image) if np else None
|
||||
|
||||
if rgb_array is not None:
|
||||
if rgb_array.size != (width * height * 3):
|
||||
logging.error(f"JPEG decode size mismatch: expected {width * height * 3}, got {rgb_array.size}")
|
||||
return None, fmt_str
|
||||
return rgb_array.tobytes(), fmt_str
|
||||
else:
|
||||
# Fallback without numpy
|
||||
return image.tobytes(), fmt_str
|
||||
|
||||
except Exception as e:
|
||||
logging.error(f"JPEG decode error: {e}")
|
||||
return None, fmt_str
|
||||
|
||||
def get_format_string(pixformat):
|
||||
"""Get a human-readable format string from pixel format code"""
|
||||
format_map = {
|
||||
PIXFORMAT_GRAYSCALE: "GRAY",
|
||||
PIXFORMAT_RGB565: "RGB565",
|
||||
PIXFORMAT_JPEG: "JPEG"
|
||||
}
|
||||
return format_map.get(pixformat, f"0x{pixformat:08X}")
|
321
tools/openmv/transport.py
Normal file
321
tools/openmv/transport.py
Normal file
@ -0,0 +1,321 @@
|
||||
# SPDX-License-Identifier: MIT
|
||||
#
|
||||
# Copyright (c) 2025 OpenMV, LLC.
|
||||
#
|
||||
# OpenMV Protocol Transport Layer
|
||||
#
|
||||
# This module provides the low-level transport layer with the protocol state
|
||||
# machine for packet parsing and communication management.
|
||||
|
||||
import time
|
||||
import logging
|
||||
import struct
|
||||
import random
|
||||
from .constants import *
|
||||
from .exceptions import *
|
||||
from .crc import crc16
|
||||
from .buffer import OMVRingBuffer
|
||||
|
||||
# Precompiled struct objects for efficiency
|
||||
_crc_struct = struct.Struct('<H')
|
||||
_hdr_struct = struct.Struct('<HBBBBHH')
|
||||
|
||||
class OMVTransport:
|
||||
"""Low-level transport layer with state machine"""
|
||||
|
||||
def __init__(self, serial, crc, seq, max_payload, timeout, event_callback, drop_rate=0.0):
|
||||
self.serial = serial
|
||||
self.timeout = timeout
|
||||
self.max_payload = max_payload
|
||||
|
||||
# Protocol state
|
||||
self.sequence = 0
|
||||
self.state = OMVPState.SYNC
|
||||
self.crc_enabled = crc
|
||||
self.seq_enabled = seq
|
||||
|
||||
# Event callback
|
||||
self.event_callback = event_callback
|
||||
|
||||
# Packet simulation
|
||||
self.drop_rate = drop_rate
|
||||
|
||||
# Packet buffers for send/recv
|
||||
self.buf = OMVRingBuffer(max(max_payload * 4, 4 * 1024 * 1024))
|
||||
self.pbuf = memoryview(bytearray(max_payload + OMVProto.HEADER_SIZE + OMVProto.CRC_SIZE))
|
||||
|
||||
# Statistics
|
||||
self.stats = {
|
||||
'sent': 0,
|
||||
'received': 0,
|
||||
'checksum': 0,
|
||||
'sequence': 0,
|
||||
}
|
||||
|
||||
def reset_sequence(self):
|
||||
"""Reset sequence counter to 0"""
|
||||
self.sequence = 0
|
||||
|
||||
def update_caps(self, crc, seq, ack, max_payload):
|
||||
"""Update transport capabilities"""
|
||||
self.crc_enabled = crc
|
||||
self.seq_enabled = seq
|
||||
self.max_payload = max_payload
|
||||
# Reallocate buffers to accommodate new max_payload
|
||||
self.buf = OMVRingBuffer(max(max_payload * 4, 4 * 1024 * 1024))
|
||||
self.pbuf = memoryview(bytearray(max_payload + OMVProto.HEADER_SIZE + OMVProto.CRC_SIZE))
|
||||
|
||||
def _crc(self, data):
|
||||
"""Calculate CRC-16 with polynomial 0xBAAD"""
|
||||
return 0 if not self.crc_enabled else crc16(data)
|
||||
|
||||
def _check_crc(self, crc, buffer):
|
||||
"""Check if CRC matches the calculated value or CRC is disabled"""
|
||||
return not self.crc_enabled or crc == self._crc(buffer)
|
||||
|
||||
def _check_seq(self, sequence, expected_sequence, opcode, flags):
|
||||
"""Check if sequence is valid or sequence checking is disabled"""
|
||||
return (not self.seq_enabled or
|
||||
(flags & OMVPFlags.EVENT) or
|
||||
(flags & OMVPFlags.RTX) or
|
||||
sequence == expected_sequence or
|
||||
opcode == OMVPOpcode.PROTO_SYNC)
|
||||
|
||||
def _format_flags(self, flags):
|
||||
"""Get human-readable name for packet flags"""
|
||||
if flags == 0:
|
||||
return "0x00"
|
||||
|
||||
flag_parts = []
|
||||
if flags & OMVPFlags.ACK:
|
||||
flag_parts.append("ACK")
|
||||
if flags & OMVPFlags.NAK:
|
||||
flag_parts.append("NAK")
|
||||
if flags & OMVPFlags.RTX:
|
||||
flag_parts.append("RTX")
|
||||
if flags & OMVPFlags.FRAGMENT:
|
||||
flag_parts.append("FRAG")
|
||||
if flags & OMVPFlags.EVENT:
|
||||
flag_parts.append("EVT")
|
||||
if flags & OMVPFlags.ACK_REQ:
|
||||
flag_parts.append("ACK_REQ")
|
||||
|
||||
return "|".join(flag_parts) if flag_parts else f"0x{flags:02X}"
|
||||
|
||||
def log(self, seq=None, ch=None, opcode=None, flags=None, length=None, direction=None, packet=None):
|
||||
"""Log packet information for debugging"""
|
||||
if packet is not None:
|
||||
seq = packet['sequence']
|
||||
ch = packet['channel']
|
||||
opcode = packet['opcode']
|
||||
flags = packet['flags']
|
||||
length = packet['length']
|
||||
|
||||
opcode_str = OMVPOpcode(opcode).name if opcode in OMVPOpcode else f"0x{opcode:02X}"
|
||||
flags_str = self._format_flags(flags)
|
||||
|
||||
# Add emoji based on packet type and direction
|
||||
if direction == "Drop":
|
||||
emoji = "🎲"
|
||||
elif direction == "Rjct":
|
||||
emoji = "🚫"
|
||||
elif flags & OMVPFlags.ACK:
|
||||
emoji = "✅"
|
||||
elif flags & OMVPFlags.NAK:
|
||||
emoji = "❌"
|
||||
elif direction == "Send":
|
||||
emoji = "➡️"
|
||||
else:
|
||||
emoji = "⬅️"
|
||||
|
||||
logging.debug(f"{emoji} {direction}: seq={seq:03d},"
|
||||
f" chan={ch}, opcode={opcode_str},"
|
||||
f" flags={flags_str}, length={length}")
|
||||
|
||||
def send_packet(self, opcode, channel, flags, data=None, sequence=None):
|
||||
"""Send a packet to the camera"""
|
||||
if not self.serial or not self.serial.is_open:
|
||||
raise OMVPTimeoutException("Serial connection not open")
|
||||
|
||||
sequence = self.sequence if sequence is None else sequence
|
||||
length = 0 if data is None else len(data)
|
||||
|
||||
if length > self.max_payload:
|
||||
raise OMVPException(f"Payload too large: {length} > {self.max_payload}")
|
||||
|
||||
# Pack header without CRC first (10 bytes)
|
||||
struct.pack_into('<HBBBBH', self.pbuf, 0, OMVProto.SYNC_WORD,
|
||||
sequence, channel, flags, opcode, length)
|
||||
struct.pack_into('<H', self.pbuf, OMVProto.HEADER_SIZE - 2,
|
||||
self._crc(self.pbuf[:OMVProto.HEADER_SIZE - 2]))
|
||||
|
||||
# Pack data if present
|
||||
if length > 0:
|
||||
self.pbuf[OMVProto.HEADER_SIZE:OMVProto.HEADER_SIZE + length] = data
|
||||
struct.pack_into('<H', self.pbuf, OMVProto.HEADER_SIZE + length, self._crc(data))
|
||||
|
||||
packet_size = OMVProto.HEADER_SIZE + length + (OMVProto.CRC_SIZE if length else 0)
|
||||
|
||||
self.log(sequence, channel, opcode, flags, length, "Send")
|
||||
self.serial.write(self.pbuf[:packet_size])
|
||||
self.stats['sent'] += 1
|
||||
|
||||
def recv_packet(self, poll_events=False):
|
||||
"""Receive and parse a packet from the camera with NAK handling"""
|
||||
if not self.serial or not self.serial.is_open:
|
||||
raise OMVPException("Serial connection not open")
|
||||
|
||||
fragments = bytearray() # Collect fragment payloads
|
||||
start_time = time.time()
|
||||
|
||||
while time.time() - start_time < self.timeout:
|
||||
if self.serial.in_waiting > 0:
|
||||
data = self.serial.read(self.serial.in_waiting)
|
||||
self.buf.extend(data)
|
||||
|
||||
# Process state machine
|
||||
if not (packet := self._process()):
|
||||
if poll_events:
|
||||
return
|
||||
time.sleep(0.001)
|
||||
continue
|
||||
|
||||
# Simulate packet drops by randomly dropping parsed packets
|
||||
if self.drop_rate > 0.0 and random.random() < self.drop_rate:
|
||||
self.log(packet=packet, direction="Drop")
|
||||
continue
|
||||
|
||||
# Handle retransmission
|
||||
if (packet['flags'] & OMVPFlags.RTX) and (self.sequence != packet['sequence']):
|
||||
if packet['flags'] & OMVPFlags.ACK_REQ:
|
||||
self.send_packet(packet['opcode'], packet['channel'],
|
||||
OMVPFlags.ACK, sequence=packet['sequence'])
|
||||
continue # Skip further processing of duplicate packet
|
||||
|
||||
self.stats['received'] += 1
|
||||
self.log(packet=packet, direction="Recv")
|
||||
|
||||
# ACK the received packet
|
||||
if packet['flags'] & OMVPFlags.ACK_REQ:
|
||||
if self.drop_rate > 0.0 and random.random() < self.drop_rate:
|
||||
self.log(packet['sequence'], packet['channel'], packet['opcode'], OMVPFlags.ACK, 0, "Drop")
|
||||
else:
|
||||
self.send_packet(packet['opcode'], packet['channel'], OMVPFlags.ACK)
|
||||
|
||||
# Handle event packets
|
||||
if packet['flags'] & OMVPFlags.EVENT:
|
||||
self.event_callback(packet['channel'], 0xFFFF if not packet['length']
|
||||
else struct.unpack('<H', packet['payload'])[0])
|
||||
start_time = time.time()
|
||||
continue # Not ACK'd
|
||||
|
||||
# Update sequence after each packet (including fragments)
|
||||
self.sequence = (self.sequence + 1) & 0xFF
|
||||
|
||||
# Check if this is a fragmented packet
|
||||
if packet['flags'] & OMVPFlags.FRAGMENT:
|
||||
fragments.extend(packet['payload'])
|
||||
start_time = time.time()
|
||||
continue # Continue collecting fragments
|
||||
|
||||
# Either last fragment or non-fragmented packet
|
||||
if fragments:
|
||||
# This is the last fragment - combine all
|
||||
fragments.extend(packet['payload'])
|
||||
packet['payload'] = bytes(fragments)
|
||||
packet['length'] = len(fragments)
|
||||
|
||||
# Handle NAK flags
|
||||
if packet['flags'] & OMVPFlags.NAK:
|
||||
status = struct.unpack('<H', packet['payload'][:2])[0]
|
||||
logging.debug(f"Command failed with status: {OMVPStatus(status).name}")
|
||||
# Raise specific exception for all NAK statuses except BUSY
|
||||
if status == OMVPStatus.CHECKSUM:
|
||||
raise OMVPChecksumException("")
|
||||
elif status == OMVPStatus.SEQUENCE:
|
||||
raise OMVPSequenceException("")
|
||||
elif status == OMVPStatus.TIMEOUT:
|
||||
raise OMVPTimeoutException("")
|
||||
elif status != OMVPStatus.BUSY:
|
||||
raise OMVPException(f"Command failed with status: {OMVPStatus(status).name}")
|
||||
return False
|
||||
|
||||
# Return payload or True for ACK
|
||||
return True if not packet['length'] else bytes(packet['payload'])
|
||||
|
||||
if not poll_events:
|
||||
raise OMVPTimeoutException("Packet receive timeout")
|
||||
|
||||
def _process(self):
|
||||
"""Process the protocol state machine"""
|
||||
while len(self.buf) > 2:
|
||||
if self.state == OMVPState.SYNC:
|
||||
# Find sync pattern
|
||||
while len(self.buf) > 2:
|
||||
sync = self.buf.peek16()
|
||||
if sync == OMVProto.SYNC_WORD:
|
||||
self.state = OMVPState.HEADER
|
||||
break
|
||||
self.buf.consume(1)
|
||||
|
||||
elif self.state == OMVPState.HEADER:
|
||||
# Wait for complete header
|
||||
if len(self.buf) < OMVProto.HEADER_SIZE:
|
||||
return None
|
||||
|
||||
# Parse header
|
||||
header = self.buf.peek(OMVProto.HEADER_SIZE)
|
||||
sync, seq, chan, flags, opcode, length, crc = _hdr_struct.unpack(header[:OMVProto.HEADER_SIZE])
|
||||
|
||||
self.state = OMVPState.SYNC
|
||||
if length > self.max_payload:
|
||||
self.log(seq, chan, opcode, flags, length, "Rjct")
|
||||
self.buf.consume(1)
|
||||
elif not self._check_seq(seq, self.sequence, opcode, flags):
|
||||
self.log(seq, chan, opcode, flags, length, "Rjct")
|
||||
self.buf.consume(1)
|
||||
elif not self._check_crc(crc, header[:OMVProto.HEADER_SIZE-2]):
|
||||
self.log(seq, chan, opcode, flags, length, "Rjct")
|
||||
self.buf.consume(1)
|
||||
else:
|
||||
self.state = OMVPState.PAYLOAD
|
||||
self.plength = OMVProto.HEADER_SIZE + length
|
||||
self.plength += OMVProto.CRC_SIZE if length else 0
|
||||
|
||||
elif self.state == OMVPState.PAYLOAD:
|
||||
# Wait for a complete packet
|
||||
if len(self.buf) < self.plength:
|
||||
return None
|
||||
|
||||
payload = None
|
||||
self.state = OMVPState.SYNC
|
||||
|
||||
# Parse packet
|
||||
packet = self.buf.peek(self.plength)
|
||||
sync, seq, chan, flags, opcode, length, crc = _hdr_struct.unpack(packet[:OMVProto.HEADER_SIZE])
|
||||
|
||||
# Parse payload
|
||||
if length > 0:
|
||||
payload = packet[OMVProto.HEADER_SIZE:-OMVProto.CRC_SIZE]
|
||||
payload_crc = _crc_struct.unpack(packet[-OMVProto.CRC_SIZE:])[0]
|
||||
|
||||
if not self._check_crc(payload_crc, payload):
|
||||
self.stats['checksum'] += 1
|
||||
self.log(seq, chan, opcode, flags, length, "Rjct")
|
||||
self.buf.consume(1) # Try next byte
|
||||
continue
|
||||
|
||||
self.buf.consume(self.plength)
|
||||
|
||||
return {
|
||||
'sync': sync,
|
||||
'sequence': seq,
|
||||
'channel': chan,
|
||||
'flags': flags,
|
||||
'opcode': opcode,
|
||||
'length': length,
|
||||
'header_crc': crc,
|
||||
'payload': payload
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user