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.
467 lines
16 KiB
467 lines
16 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_start_listening_with_packets,
|
|
uart_stop_listening,
|
|
uart_get_detected_packets,
|
|
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 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
|
|
|
|
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,
|
|
stop_timeout_ms: int = 5000,
|
|
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()
|
|
|
|
# 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:
|
|
# Create callback for I2C triggering
|
|
def on_uart_packet_detected(timestamp_ns: int):
|
|
"""
|
|
Called immediately when UART packet detected.
|
|
Triggers I2C read for timestamp correlation.
|
|
"""
|
|
if i2c_port:
|
|
# Read I2C angle immediately
|
|
# Note: i2c_read_block requires (handle, addr, reg, length)
|
|
# But we're using the handle's default address
|
|
status, i2c_bytes = i2c_read_block(
|
|
i2c_port,
|
|
i2c_port.config.address, # Use configured address
|
|
0xFE, # Angle register
|
|
2 # Read 2 bytes
|
|
)
|
|
|
|
if status == I2CStatus.OK:
|
|
# Store with correlated timestamp
|
|
self.i2c_readings.append({
|
|
'timestamp_ns': timestamp_ns,
|
|
'i2c_bytes': i2c_bytes
|
|
})
|
|
|
|
# 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
|
|
)
|
|
|
|
# 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)
|
|
# ================================================================
|
|
|
|
# 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}")
|
|
|
|
# Send command via COMMAND PORT
|
|
status, written = uart_write(uart_command_port, command_bytes)
|
|
|
|
if status != UARTStatus.OK:
|
|
if uart_logger_port:
|
|
uart_stop_listening(uart_logger_port)
|
|
return ("error", 0, "Failed to send UART command")
|
|
|
|
# Emit TX data (command sent)
|
|
if raw_data_callback:
|
|
hex_tx = ' '.join(f'{b:02X}' for b in command_bytes)
|
|
raw_data_callback("TX", hex_tx)
|
|
|
|
# ================================================================
|
|
# 3. Wait for stop condition
|
|
# ================================================================
|
|
|
|
# Wait for timeout
|
|
time.sleep(stop_timeout_ms / 1000.0)
|
|
|
|
# ================================================================
|
|
# 3.5. Handle raw data if packet detection disabled
|
|
# ================================================================
|
|
|
|
if not packet_config.enable:
|
|
# No packet detection - read raw buffer from COMMAND PORT (ACK/response)
|
|
status_read, raw_data = uart_read_buffer(uart_command_port)
|
|
|
|
if status_read == UARTStatus.OK and raw_data:
|
|
# Emit RX data
|
|
if raw_data_callback:
|
|
hex_rx = ' '.join(f'{b:02X}' for b in raw_data)
|
|
raw_data_callback("RX", hex_rx)
|
|
|
|
# Stop listening on logger port (if active)
|
|
if uart_logger_port:
|
|
uart_stop_listening(uart_logger_port)
|
|
|
|
# ================================================================
|
|
# 4. Get detected packets (from LOGGER PORT if exists)
|
|
# ================================================================
|
|
|
|
uart_packets = []
|
|
if uart_logger_port:
|
|
uart_packets = uart_get_detected_packets(uart_logger_port)
|
|
else:
|
|
uart_packets = []
|
|
packet_count = len(uart_packets)
|
|
|
|
if packet_count == 0 and packet_config.enable:
|
|
# Only error if packet detection was enabled
|
|
return ("error", 0, "No packets detected (timeout or no data)")
|
|
|
|
# ================================================================
|
|
# 5. Decode and save data
|
|
# ================================================================
|
|
|
|
# Decode and save UART packets
|
|
for pkt in uart_packets:
|
|
self._save_uart_telemetry(
|
|
session_id=session_id,
|
|
session_name=session_name,
|
|
run_no=run_no,
|
|
run_command_id=command_id,
|
|
packet_info=pkt,
|
|
run_start_ns=run_start_ns
|
|
)
|
|
|
|
# Decode and save I2C readings
|
|
for reading in self.i2c_readings:
|
|
self._save_i2c_telemetry(
|
|
session_id=session_id,
|
|
session_name=session_name,
|
|
run_no=run_no,
|
|
run_command_id=command_id,
|
|
timestamp_ns=reading['timestamp_ns'],
|
|
i2c_bytes=reading['i2c_bytes'],
|
|
run_start_ns=run_start_ns
|
|
)
|
|
|
|
# Commit database changes
|
|
self.db_conn.commit()
|
|
|
|
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_uart_telemetry(
|
|
self,
|
|
session_id: str,
|
|
session_name: str,
|
|
run_no: int,
|
|
run_command_id: int,
|
|
packet_info: PacketInfo,
|
|
run_start_ns: int
|
|
):
|
|
"""
|
|
Save UART telemetry to database.
|
|
|
|
Saves to both telemetry_raw and telemetry_decoded tables.
|
|
"""
|
|
# Decode packet
|
|
decoded = decode_uart_packet(packet_info.data)
|
|
|
|
# Calculate relative time from run start
|
|
time_ms = (packet_info.start_timestamp - run_start_ns) / 1_000_000.0
|
|
|
|
# Save to telemetry_raw (backup)
|
|
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
|
|
) VALUES (?, ?, ?, ?, ?, ?, ?)
|
|
""", (
|
|
session_id,
|
|
session_name,
|
|
run_no,
|
|
run_command_id,
|
|
packet_info.start_timestamp,
|
|
time_ms,
|
|
packet_info.data
|
|
))
|
|
|
|
# Save to telemetry_decoded (main data)
|
|
# For now, just save raw hex (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:
|
|
# motor_current, encoder_value, relative_encoder_value, v24_pec_diff, pwm
|
|
|
|
def _save_i2c_telemetry(
|
|
self,
|
|
session_id: str,
|
|
session_name: str,
|
|
run_no: int,
|
|
run_command_id: int,
|
|
timestamp_ns: int,
|
|
i2c_bytes: bytes,
|
|
run_start_ns: int
|
|
):
|
|
"""
|
|
Save I2C telemetry to database.
|
|
|
|
Saves to both telemetry_raw and telemetry_decoded tables.
|
|
"""
|
|
# Decode I2C sample
|
|
decoded = decode_i2c_sample(i2c_bytes)
|
|
|
|
# Calculate relative time from run start
|
|
time_ms = (timestamp_ns - run_start_ns) / 1_000_000.0
|
|
|
|
# Save to telemetry_raw (backup)
|
|
cursor = self.db_conn.cursor()
|
|
cursor.execute("""
|
|
INSERT INTO telemetry_raw (
|
|
session_id, session_name, run_no, run_command_id,
|
|
t_ns, time_ms, i2c_raw_bytes
|
|
) VALUES (?, ?, ?, ?, ?, ?, ?)
|
|
""", (
|
|
session_id,
|
|
session_name,
|
|
run_no,
|
|
run_command_id,
|
|
timestamp_ns,
|
|
time_ms,
|
|
i2c_bytes
|
|
))
|
|
|
|
# Save to telemetry_decoded (main data)
|
|
# For now, decoder is pass-through
|
|
# TODO: Update when decoder is fully implemented with angle conversion
|
|
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,
|
|
timestamp_ns,
|
|
time_ms
|
|
))
|
|
|
|
# TODO: When decoder is fully implemented, also save:
|
|
# 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,
|
|
stop_timeout_ms: int = 5000,
|
|
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
|
|
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,
|
|
stop_timeout_ms=stop_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!")
|