You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
549 lines
22 KiB
549 lines
22 KiB
#!/usr/bin/env python3
|
|
"""
|
|
Run Module - vzug-e-hinge
|
|
==========================
|
|
Executes a single RUN (one UART command with data collection).
|
|
|
|
Flow:
|
|
1. Configure UART packet detection with callback
|
|
2. Callback triggers I2C read (real-time correlation)
|
|
3. Send UART command
|
|
4. Wait for stop condition
|
|
5. Decode packets (call decoder.py)
|
|
6. Save to database (telemetry_decoded + telemetry_raw)
|
|
|
|
Author: Kynsight
|
|
Version: 1.0.0
|
|
Date: 2025-11-09
|
|
"""
|
|
|
|
import time
|
|
from typing import Tuple, Optional, List
|
|
import sqlite3
|
|
|
|
# Import UART core
|
|
from uart.uart_kit.uart_core import (
|
|
UARTPort,
|
|
PacketConfig,
|
|
PacketInfo,
|
|
uart_write,
|
|
uart_send_and_read_pgkomm2,
|
|
uart_start_listening_with_packets,
|
|
uart_stop_listening,
|
|
uart_get_detected_packets,
|
|
uart_get_packet_errors,
|
|
uart_clear_detected_packets,
|
|
uart_read_buffer,
|
|
Status as UARTStatus
|
|
)
|
|
|
|
# Import I2C core
|
|
from i2c.i2c_kit.i2c_core import (
|
|
I2CHandle,
|
|
i2c_read_block,
|
|
Status as I2CStatus
|
|
)
|
|
|
|
# Import buffer utilities
|
|
from buffer_kit.circular_buffer import cb_fill_bytes, cb_capacity
|
|
|
|
# Import decoder
|
|
from decoder import decode_uart_packet, decode_i2c_sample
|
|
|
|
|
|
class RunExecutor:
|
|
"""
|
|
Executes a single RUN.
|
|
|
|
A RUN consists of:
|
|
- Send UART command
|
|
- Collect UART packets (with timestamps)
|
|
- Trigger I2C reads via callback (correlated timestamps)
|
|
- Wait for stop condition
|
|
- Decode all data
|
|
- Save to database
|
|
"""
|
|
|
|
def __init__(self, db_connection: sqlite3.Connection):
|
|
"""
|
|
Initialize run executor.
|
|
|
|
Args:
|
|
db_connection: Database connection
|
|
"""
|
|
self.db_conn = db_connection
|
|
self.i2c_readings = [] # Storage for I2C readings from callback
|
|
self.i2c_failures = 0 # Counter for I2C read failures
|
|
|
|
def execute_run(
|
|
self,
|
|
session_id: str,
|
|
session_name: str,
|
|
run_no: int,
|
|
command_id: int,
|
|
command_hex: str,
|
|
uart_command_port: UARTPort,
|
|
uart_logger_port: Optional[UARTPort],
|
|
i2c_port: Optional[I2CHandle],
|
|
packet_config: PacketConfig,
|
|
i2c_address: int = 0x40,
|
|
i2c_register: int = 0xFE,
|
|
stop_timeout_ms: int = 5000,
|
|
grace_timeout_ms: int = 1500,
|
|
raw_data_callback = None
|
|
) -> Tuple[str, int, str]:
|
|
"""
|
|
Execute a single RUN.
|
|
|
|
Args:
|
|
session_id: Session ID
|
|
session_name: Session name
|
|
run_no: Run number (1, 2, 3, ...)
|
|
command_id: UART command ID from database
|
|
command_hex: Command hex string (e.g., "DD 22 50 48...")
|
|
uart_command_port: UART command port (TX/RX for commands)
|
|
uart_logger_port: UART logger port (RX for telemetry, None if disabled)
|
|
i2c_port: I2C port (optional, for angle readings)
|
|
packet_config: Packet detection configuration
|
|
stop_timeout_ms: Maximum wait time for stop condition
|
|
|
|
Returns:
|
|
(status, packet_count, error_msg)
|
|
- status: "success" or "error"
|
|
- packet_count: Number of packets detected
|
|
- error_msg: Error message if status="error", empty otherwise
|
|
"""
|
|
try:
|
|
# Clear previous packets (only if logger port exists)
|
|
if uart_logger_port:
|
|
uart_clear_detected_packets(uart_logger_port)
|
|
self.i2c_readings.clear()
|
|
self.i2c_failures = 0 # Reset error counter
|
|
|
|
# Record run start time
|
|
run_start_ns = time.time_ns()
|
|
|
|
# ================================================================
|
|
# 1. Configure packet detection with callback (LOGGER PORT)
|
|
# ================================================================
|
|
|
|
if uart_logger_port and packet_config.enable:
|
|
# Debug: Check if I2C is available
|
|
if raw_data_callback:
|
|
if i2c_port:
|
|
raw_data_callback("INFO", f"I2C enabled: will trigger reads on packet detection")
|
|
else:
|
|
raw_data_callback("INFO", f"I2C disabled: no I2C port available")
|
|
|
|
# Create callback for I2C triggering
|
|
callback_count = [0] # Use list for mutable counter in nested function
|
|
|
|
def on_uart_packet_detected(timestamp_ns: int):
|
|
"""
|
|
Called immediately when UART packet detected.
|
|
Triggers I2C read for timestamp correlation.
|
|
"""
|
|
callback_count[0] += 1
|
|
|
|
# Debug first few callbacks
|
|
if callback_count[0] <= 3:
|
|
print(f"[DEBUG] I2C callback triggered #{callback_count[0]} at {timestamp_ns}")
|
|
|
|
if i2c_port:
|
|
# Read I2C angle immediately
|
|
status, i2c_bytes = i2c_read_block(
|
|
i2c_port,
|
|
i2c_address, # Device address from session config
|
|
i2c_register, # Register address from session config
|
|
2 # Read 2 bytes
|
|
)
|
|
|
|
if status == I2CStatus.OK:
|
|
# Store with correlated timestamp
|
|
self.i2c_readings.append({
|
|
'timestamp_ns': timestamp_ns,
|
|
'i2c_bytes': i2c_bytes
|
|
})
|
|
else:
|
|
# I2C read failed - count the failure
|
|
self.i2c_failures += 1
|
|
if callback_count[0] <= 3:
|
|
print(f"[DEBUG] I2C read failed: {status}")
|
|
|
|
# Create packet config with callback
|
|
packet_config_with_callback = PacketConfig(
|
|
enable=packet_config.enable,
|
|
start_marker=packet_config.start_marker,
|
|
packet_length=packet_config.packet_length,
|
|
end_marker=packet_config.end_marker,
|
|
on_packet_callback=on_uart_packet_detected if i2c_port else None
|
|
)
|
|
|
|
# Debug: Verify callback is attached
|
|
if raw_data_callback:
|
|
has_callback = packet_config_with_callback.on_packet_callback is not None
|
|
raw_data_callback("INFO", f"Packet config: callback={'attached' if has_callback else 'None'}")
|
|
|
|
# Start listening with packet detection on LOGGER PORT
|
|
status = uart_start_listening_with_packets(uart_logger_port, packet_config_with_callback)
|
|
|
|
if status != UARTStatus.OK:
|
|
return ("error", 0, "Failed to start UART packet detection")
|
|
|
|
# ================================================================
|
|
# 2. Send UART command (COMMAND PORT) - Using PGKomm2
|
|
# ================================================================
|
|
|
|
# Parse hex string to bytes
|
|
command_bytes = self._parse_hex_string(command_hex)
|
|
if not command_bytes:
|
|
if uart_logger_port:
|
|
uart_stop_listening(uart_logger_port)
|
|
return ("error", 0, f"Invalid command hex string: {command_hex}")
|
|
|
|
# Emit TX data (command to be sent)
|
|
if raw_data_callback:
|
|
hex_tx = ' '.join(f'{b:02X}' for b in command_bytes)
|
|
# Add ASCII (skip DD 22 magic bytes)
|
|
ascii_data = command_bytes[2:] if len(command_bytes) >= 2 else command_bytes
|
|
ascii_tx = ''.join(chr(b) if 32 <= b <= 126 else '.' for b in ascii_data)
|
|
raw_data_callback("TX", f"{hex_tx} | '{ascii_tx}'")
|
|
|
|
# Send command via PGKomm2 (always use this mode for sessions)
|
|
status, frames = uart_send_and_read_pgkomm2(
|
|
uart_command_port,
|
|
command_bytes,
|
|
capture_max_ms=30, # Default PGKomm2 timeout
|
|
log_callback=raw_data_callback # Pass callback for logging
|
|
)
|
|
|
|
if status != UARTStatus.OK:
|
|
if uart_logger_port:
|
|
uart_stop_listening(uart_logger_port)
|
|
return ("error", 0, f"PGKomm2 command failed: {status}")
|
|
|
|
# Emit RX data (frames received) - only show Echo and Response, skip SB broadcasts
|
|
if raw_data_callback and frames:
|
|
for frame in frames:
|
|
if len(frame) >= 5:
|
|
adr1, adr2 = frame[2], frame[3]
|
|
|
|
# Skip SB status broadcasts (background noise from device)
|
|
if adr1 == 0x53 and adr2 == 0x42: # SB
|
|
continue
|
|
|
|
hex_rx = ' '.join(f'{b:02X}' for b in frame)
|
|
# Add ASCII (skip DD 22 magic bytes)
|
|
ascii_data = frame[2:] if len(frame) >= 2 else frame
|
|
ascii_rx = ''.join(chr(b) if 32 <= b <= 126 else '.' for b in ascii_data)
|
|
|
|
if adr1 == 0x50 and adr2 == 0x48: # PH echo
|
|
raw_data_callback("RX", f"{hex_rx} (Echo) | '{ascii_rx}'")
|
|
elif adr1 == 0x48 and adr2 == 0x50: # HP response
|
|
raw_data_callback("RX", f"{hex_rx} (Response) | '{ascii_rx}'")
|
|
else:
|
|
raw_data_callback("RX", f"{hex_rx} | '{ascii_rx}'")
|
|
else:
|
|
# Unknown frame format
|
|
hex_rx = ' '.join(f'{b:02X}' for b in frame)
|
|
ascii_data = frame[2:] if len(frame) >= 2 else frame
|
|
ascii_rx = ''.join(chr(b) if 32 <= b <= 126 else '.' for b in ascii_data)
|
|
raw_data_callback("RX", f"{hex_rx} | '{ascii_rx}'")
|
|
|
|
# ================================================================
|
|
# 3. Wait for logger packets (polling mode with stop condition)
|
|
# ================================================================
|
|
|
|
uart_packets = []
|
|
|
|
if uart_logger_port and packet_config.enable:
|
|
# Polling mode: wait for packets with grace period and timeout
|
|
# Use defaults if None from database
|
|
grace_ms = grace_timeout_ms if grace_timeout_ms is not None else 1500
|
|
stop_ms = stop_timeout_ms if stop_timeout_ms is not None else 150
|
|
|
|
grace_timeout_s = grace_ms / 1000.0 # Wait for first packet
|
|
stop_timeout_s = stop_ms / 1000.0 # Silence between packets
|
|
|
|
last_packet_count = 0
|
|
last_packet_time = 0.0
|
|
start_time = time.time()
|
|
first_packet_received = False
|
|
|
|
if raw_data_callback:
|
|
raw_data_callback("INFO", f"Waiting for logger packets (grace: {grace_timeout_s*1000:.0f}ms, timeout: {stop_timeout_s*1000:.0f}ms)...")
|
|
|
|
# Polling loop
|
|
while True:
|
|
time.sleep(0.05) # Poll every 50ms
|
|
current_time = time.time()
|
|
|
|
# Get current packet count
|
|
current_packets = uart_get_detected_packets(uart_logger_port)
|
|
current_count = len(current_packets)
|
|
|
|
# Check if new packets arrived
|
|
if current_count > last_packet_count:
|
|
last_packet_count = current_count
|
|
last_packet_time = current_time
|
|
if not first_packet_received:
|
|
first_packet_received = True
|
|
if raw_data_callback:
|
|
raw_data_callback("INFO", f"First logger packet received, monitoring for stop condition...")
|
|
|
|
# Grace period check (only if no packets yet)
|
|
if not first_packet_received:
|
|
elapsed = current_time - start_time
|
|
if elapsed >= grace_timeout_s:
|
|
# Grace period expired, no packets
|
|
uart_stop_listening(uart_logger_port)
|
|
return ("error", 0, f"Logger not responding (grace timeout: {grace_timeout_s*1000:.0f}ms)")
|
|
|
|
# Stop timeout check (only after first packet received)
|
|
if first_packet_received:
|
|
silence = current_time - last_packet_time
|
|
if silence >= stop_timeout_s:
|
|
# Stop condition met!
|
|
if raw_data_callback:
|
|
# Report buffer status
|
|
if uart_logger_port._rx_buffer:
|
|
fill = cb_fill_bytes(uart_logger_port._rx_buffer)
|
|
cap = cb_capacity(uart_logger_port._rx_buffer)
|
|
fill_mb = fill / (1024 * 1024)
|
|
cap_mb = cap / (1024 * 1024)
|
|
raw_data_callback("INFO", f"Buffer: {fill_mb:.2f}MB / {cap_mb:.1f}MB")
|
|
|
|
# Report packet statistics
|
|
packet_errors = uart_get_packet_errors(uart_logger_port)
|
|
if packet_errors > 0:
|
|
raw_data_callback("ERROR", f"⚠ Packet errors: {packet_errors} packets with end marker mismatch")
|
|
raw_data_callback("INFO", f"✓ Valid packets: {current_count}")
|
|
raw_data_callback("INFO", f"Stop condition: {stop_timeout_s*1000:.0f}ms silence detected")
|
|
|
|
# Stop listening (but keep port open for next command)
|
|
uart_stop_listening(uart_logger_port)
|
|
uart_packets = current_packets
|
|
break
|
|
|
|
packet_count = len(uart_packets)
|
|
|
|
elif uart_logger_port:
|
|
# Logger enabled but packet detection disabled - just stop listening
|
|
uart_stop_listening(uart_logger_port)
|
|
packet_count = 0
|
|
|
|
else:
|
|
# No logger port
|
|
packet_count = 0
|
|
|
|
# ================================================================
|
|
# 4. Decode and save data
|
|
# ================================================================
|
|
|
|
if raw_data_callback:
|
|
i2c_count = len(self.i2c_readings)
|
|
if i2c_count == 0 and i2c_port and packet_config.enable:
|
|
# Expected I2C but got none - report
|
|
raw_data_callback("ERROR", f"⚠ No I2C readings captured (expected ~{packet_count})")
|
|
if self.i2c_failures > 0:
|
|
raw_data_callback("ERROR", f"I2C failures: {self.i2c_failures}")
|
|
raw_data_callback("INFO", f"Decoding and saving {packet_count} UART packets + {i2c_count} I2C readings...")
|
|
|
|
# Create timestamp → I2C reading map for matching
|
|
i2c_by_timestamp = {}
|
|
for reading in self.i2c_readings:
|
|
i2c_by_timestamp[reading['timestamp_ns']] = reading['i2c_bytes']
|
|
|
|
# Decode and save UART packets WITH correlated I2C data
|
|
for pkt in uart_packets:
|
|
# Look up matching I2C reading by timestamp
|
|
i2c_bytes = i2c_by_timestamp.get(pkt.start_timestamp, None)
|
|
|
|
self._save_combined_telemetry(
|
|
session_id=session_id,
|
|
session_name=session_name,
|
|
run_no=run_no,
|
|
run_command_id=command_id,
|
|
packet_info=pkt,
|
|
i2c_bytes=i2c_bytes,
|
|
run_start_ns=run_start_ns
|
|
)
|
|
|
|
# Commit database changes
|
|
self.db_conn.commit()
|
|
|
|
if raw_data_callback:
|
|
raw_data_callback("INFO", f"✓ Database saved: {packet_count} UART packets, {len(self.i2c_readings)} I2C readings")
|
|
|
|
# Report errors if any via callback
|
|
if self.i2c_failures > 0 and raw_data_callback:
|
|
raw_data_callback("ERROR", f"I2C read failures: {self.i2c_failures}")
|
|
|
|
return ("success", packet_count, "")
|
|
|
|
except Exception as e:
|
|
# Stop listening if still active (logger port)
|
|
try:
|
|
if uart_logger_port:
|
|
uart_stop_listening(uart_logger_port)
|
|
except:
|
|
pass
|
|
|
|
return ("error", 0, f"Exception during run: {str(e)}")
|
|
|
|
def _parse_hex_string(self, hex_str: str) -> Optional[bytes]:
|
|
"""
|
|
Parse hex string to bytes.
|
|
|
|
Args:
|
|
hex_str: Hex string (e.g., "DD 22 50 48" or "DD225048")
|
|
|
|
Returns:
|
|
Bytes or None if invalid
|
|
"""
|
|
try:
|
|
# Remove spaces and convert
|
|
hex_clean = hex_str.replace(' ', '')
|
|
return bytes.fromhex(hex_clean)
|
|
except:
|
|
return None
|
|
|
|
def _save_combined_telemetry(
|
|
self,
|
|
session_id: str,
|
|
session_name: str,
|
|
run_no: int,
|
|
run_command_id: int,
|
|
packet_info: PacketInfo,
|
|
i2c_bytes: Optional[bytes],
|
|
run_start_ns: int
|
|
):
|
|
"""
|
|
Save combined UART + I2C telemetry to database (single row).
|
|
|
|
Saves to both telemetry_raw and telemetry_decoded tables.
|
|
UART and I2C data are correlated by timestamp and saved together.
|
|
"""
|
|
# Decode packets
|
|
decoded_uart = decode_uart_packet(packet_info.data)
|
|
decoded_i2c = decode_i2c_sample(i2c_bytes) if i2c_bytes else None
|
|
|
|
# Calculate relative time from run start
|
|
time_ms = (packet_info.start_timestamp - run_start_ns) / 1_000_000.0
|
|
|
|
# Save to telemetry_raw (backup) - BOTH uart_raw_packet AND i2c_raw_bytes in ONE row
|
|
cursor = self.db_conn.cursor()
|
|
cursor.execute("""
|
|
INSERT INTO telemetry_raw (
|
|
session_id, session_name, run_no, run_command_id,
|
|
t_ns, time_ms, uart_raw_packet, i2c_raw_bytes
|
|
) VALUES (?, ?, ?, ?, ?, ?, ?, ?)
|
|
""", (
|
|
session_id,
|
|
session_name,
|
|
run_no,
|
|
run_command_id,
|
|
packet_info.start_timestamp,
|
|
time_ms,
|
|
packet_info.data,
|
|
i2c_bytes # Can be None if no I2C
|
|
))
|
|
|
|
# Save to telemetry_decoded (main data)
|
|
# For now, just save timestamps (decoder is pass-through)
|
|
# TODO: Update when decoder is fully implemented
|
|
cursor.execute("""
|
|
INSERT INTO telemetry_decoded (
|
|
session_id, session_name, run_no, run_command_id,
|
|
t_ns, time_ms
|
|
) VALUES (?, ?, ?, ?, ?, ?)
|
|
""", (
|
|
session_id,
|
|
session_name,
|
|
run_no,
|
|
run_command_id,
|
|
packet_info.start_timestamp,
|
|
time_ms
|
|
))
|
|
|
|
# TODO: When decoder is fully implemented, also save:
|
|
# UART: motor_current, encoder_value, relative_encoder_value, v24_pec_diff, pwm
|
|
# I2C: i2c_raw14, i2c_angle_deg, i2c_zero_raw14, etc.
|
|
|
|
|
|
# =============================================================================
|
|
# Convenience function for external use
|
|
# =============================================================================
|
|
|
|
def execute_run(
|
|
db_connection: sqlite3.Connection,
|
|
session_id: str,
|
|
session_name: str,
|
|
run_no: int,
|
|
command_id: int,
|
|
command_hex: str,
|
|
uart_command_port: UARTPort,
|
|
uart_logger_port: Optional[UARTPort],
|
|
i2c_port: Optional[I2CHandle],
|
|
packet_config: PacketConfig,
|
|
i2c_address: int = 0x40,
|
|
i2c_register: int = 0xFE,
|
|
stop_timeout_ms: int = 5000,
|
|
grace_timeout_ms: int = 1500,
|
|
raw_data_callback = None
|
|
) -> Tuple[str, int, str]:
|
|
"""
|
|
Execute a single RUN (convenience function).
|
|
|
|
Args:
|
|
db_connection: Database connection
|
|
session_id: Session ID
|
|
session_name: Session name
|
|
run_no: Run number
|
|
command_id: UART command ID
|
|
command_hex: Command hex string
|
|
uart_command_port: UART command port (TX/RX for commands)
|
|
uart_logger_port: UART logger port (RX for telemetry, optional)
|
|
i2c_port: I2C port (optional)
|
|
packet_config: Packet detection configuration
|
|
stop_timeout_ms: Stop condition timeout
|
|
grace_timeout_ms: Grace period before first packet
|
|
raw_data_callback: Callback for raw data display (direction, hex_string)
|
|
|
|
Returns:
|
|
(status, packet_count, error_msg)
|
|
"""
|
|
executor = RunExecutor(db_connection)
|
|
return executor.execute_run(
|
|
session_id=session_id,
|
|
session_name=session_name,
|
|
run_no=run_no,
|
|
command_id=command_id,
|
|
command_hex=command_hex,
|
|
uart_command_port=uart_command_port,
|
|
uart_logger_port=uart_logger_port,
|
|
i2c_port=i2c_port,
|
|
packet_config=packet_config,
|
|
i2c_address=i2c_address,
|
|
i2c_register=i2c_register,
|
|
stop_timeout_ms=stop_timeout_ms,
|
|
grace_timeout_ms=grace_timeout_ms,
|
|
raw_data_callback=raw_data_callback
|
|
)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
print("Run Module")
|
|
print("=" * 60)
|
|
print("This module executes a single RUN.")
|
|
print("It should be called by session.py, not run directly.")
|
|
print()
|
|
print("Features:")
|
|
print("✓ UART packet detection with callback")
|
|
print("✓ Real-time I2C triggering")
|
|
print("✓ Decoder integration")
|
|
print("✓ Database storage (telemetry_raw + telemetry_decoded)")
|
|
print("✓ Error handling")
|
|
print()
|
|
print("Ready to be used by session.py!")
|