SelfRising/calibration/remote_runner.py

300 lines
13 KiB
Python

"""Bounded X5 calibration runner. Status is read-only; run requires stdin lease."""
import fcntl
import importlib.util
import json
import os
from pathlib import Path
import select
import signal
import sys
import threading
import time
import uuid
BOARD = 'D-Robotics RDK X5 V1.0'
LEASE_SECONDS = 2.5
MAX_SECONDS = 15
LOCK_FILE = Path('/tmp/x5-calibration-web.lock')
CALIBRATION_PULSES = (550, 1450, 1550, 2450) + tuple(p for p in range(900, 2101, 100) if p != 1500)
class Cancelled(RuntimeError):
pass
def validate(payload):
if not isinstance(payload, dict):
raise ValueError('Request must be an object')
try:
request_id = str(uuid.UUID(payload.get('request_id', '')))
except (ValueError, TypeError, AttributeError):
raise ValueError('request_id must be a UUID')
if payload.get('type') == 'imu':
if payload.get('pose') not in ('stationary', 'front-up', 'left-up'):
raise ValueError('Unknown IMU pose')
return dict(type='imu', request_id=request_id, pose=payload['pose'])
if payload.get('type') not in ('servo', 'recovery'):
raise ValueError('Only imu, servo calibration and one recovery attempt are supported')
if payload['type'] == 'servo' and (type(payload.get('pin')) is not int or payload['pin'] not in (32, 33)):
raise ValueError('Only physical pin 32 or 33 is permitted')
if payload['type'] == 'servo' and (type(payload.get('pulse_us')) is not int or payload['pulse_us'] not in CALIBRATION_PULSES):
raise ValueError('Target must be one of the fixed calibration pulse values')
if payload.get('observation_mode') == 'user_feedback':
# Explicit bench-only alternative to camera observation; never enables recovery or sweeps.
if (payload['type'] != 'servo' or payload['pulse_us'] not in (1450, 1550)
or any(payload.get(k) is not True for k in
('mechanically_unloaded', 'operator_observing', 'clearance_confirmed', 'reference_confirmed'))):
raise ValueError('User-observed testing requires an unloaded servo and a 50us step')
return {k: payload[k] for k in ('type', 'pin', 'pulse_us', 'observation_mode',
'mechanically_unloaded', 'operator_observing',
'clearance_confirmed', 'reference_confirmed')} | {'request_id': request_id}
if payload.get('observation_mode') is not None:
raise ValueError('Unknown observation mode')
if any(payload.get(k) is not True for k in
('clearance_confirmed', 'reference_confirmed', 'camera_confirmed')):
raise ValueError('Current camera, clearance and reference posture must be confirmed')
frame = payload.get('before_frame_id')
if not isinstance(frame, str) or not frame or len(frame) > 150:
raise ValueError('A current before_frame_id is required')
if payload['type'] == 'recovery':
if payload.get('known_fallen_back') is not True:
raise ValueError('This controller requires the verified fallen-back side')
return {k: payload[k] for k in ('type', 'clearance_confirmed', 'reference_confirmed',
'camera_confirmed', 'before_frame_id', 'known_fallen_back')} | {
'request_id': request_id, 'initial_targets_deg': [0, 0]}
return {k: payload[k] for k in ('type', 'pin', 'pulse_us', 'clearance_confirmed',
'reference_confirmed', 'camera_confirmed', 'before_frame_id')} | {'request_id': request_id}
def load_sampler():
spec = importlib.util.spec_from_file_location('qmi_sample', Path(__file__).with_name('sample_qmi8658.py'))
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
return module
def channels(root=Path('/sys/class/pwm')):
return [p for chip in root.glob('pwmchip*') for p in chip.glob('pwm[0-9]*')]
def controller(root=Path('/sys/class/pwm')):
matches = [p for p in root.glob('pwmchip*')
if (p / 'device/of_node').resolve().name == 'pwm@34170000']
if len(matches) != 1:
raise RuntimeError('PWM controller pwm@34170000 not uniquely identified')
return matches[0]
def read_pwm(path):
return {k: int((path / k).read_text()) if (path / k).exists() else None
for k in ('enable', 'period', 'duty_cycle')}
def status():
identity = Path('/proc/device-tree/model').read_text().strip('\x00\n')
if identity != BOARD:
raise RuntimeError('Unexpected board identity: ' + identity)
imu = {'address': '0x6b', 'bus': 5, 'who_am_i': None}
try:
imu['who_am_i'] = hex(load_sampler().I2C().read(0)[0])
except Exception as exc:
imu['error'] = str(exc)
recovery_ready, recovery_error = False, 'Target-angle calibration is not available'
if Path(__file__).with_name('calibration_profile.json').exists():
try:
from recovery_hardware import load_configuration
load_configuration()
recovery_ready, recovery_error = True, None
except Exception as exc:
recovery_error = str(exc)
return {'connected': True, 'identity': identity, 'imu': imu,
'recovery_ready': recovery_ready, 'recovery_error': recovery_error,
'pwm': [{'path': str(p), **read_pwm(p)} for p in channels()],
'pwm_controller': str(controller()), 'motion_started': False}
def servo(payload, gpio, emit, chip=None, sleep=time.sleep):
pin = payload['pin']
chip = chip or controller()
path = chip / ('pwm0' if pin == 32 else 'pwm1')
pwm = None
started = time.monotonic()
result = {'pin': pin, 'pulses_us': [1500, payload['pulse_us'], 1500],
'holds_s': [2, 3, 2], 'readbacks': [], 'cleanup_verified': False,
'mechanical_success': 'unknown', 'angle_mapping': 'not_calibrated'}
extended = abs(payload['pulse_us'] - 1500) > 50
result['ramp_us_per_s'] = 400 if extended else None
gpio.setmode(gpio.BOARD)
try:
# Reuse the tested startup order; start() alone skips enable at zero duty.
pwm = gpio.PWM(pin, 50)
pwm.ChangeDutyCycle(1500 / 200)
pwm.start(1500 / 200)
previous = 1500
for index, (pulse, hold) in enumerate(zip(result['pulses_us'], result['holds_s'])):
if index:
if extended:
emit('ramp', from_us=previous, target_us=pulse, rate_us_per_s=400)
while previous != pulse:
step = max(-20, min(20, pulse - previous))
sleep(abs(step) / 400)
previous += step
pwm.ChangeDutyCycle(previous / 200)
else:
pwm.ChangeDutyCycle(pulse / 200)
actual = read_pwm(path)
if actual != {'enable': 1, 'period': 20000000, 'duty_cycle': pulse * 1000}:
raise RuntimeError('PWM readback mismatch: ' + repr(actual))
phase = ('initial_command', 'increment', 'return_command')[index]
record = {'phase': phase, 'pulse_us': pulse, 'hold_s': hold,
'elapsed_s': time.monotonic() - started, 'pwm': actual}
result['readbacks'].append(record)
emit('phase', **record)
sleep(hold)
finally:
errors = []
try:
if pwm is not None:
pwm.stop()
except Exception as exc:
errors.append('stop: ' + str(exc))
if pwm is not None:
try:
gpio.cleanup(pin)
except Exception as exc:
errors.append('cleanup: ' + str(exc))
# Only this task's channel is eligible for fallback cleanup, never another pin.
if path.exists() and pwm is not None:
errors.append('Normal cleanup left the channel exported')
try:
(path / 'enable').write_text('0')
(chip / 'unexport').write_text(path.name[3:])
result['emergency_cleanup'] = True
except Exception as exc:
errors.append('fallback: ' + str(exc))
result['cleanup_verified'] = not path.exists()
result['cleanup_errors'] = errors
emit('cleanup', **result)
if not result['cleanup_verified'] or errors:
raise RuntimeError('PWM cleanup needs inspection: ' + repr(result))
return result
def imu(emit):
sampler = load_sampler()
original_decode = sampler.decode
started = time.monotonic()
def decode(data):
sample = original_decode(data)
emit('sample', sample={**sample, 'elapsed_s': time.monotonic() - started})
return sample
sampler.decode = decode
result = sampler.capture(sampler.I2C(), 8)
result['cleanup_verified'] = result.get('configuration_restored') is True
result['parameters_applied'] = False
return result
def run(payload, emit):
payload = validate(payload)
# O_EXCL is a durable replay barrier, including retries after SSH failure.
ledger = Path(__file__).with_name('requests')
ledger.mkdir(exist_ok=True)
with (ledger / (payload['request_id'] + '.json')).open('x') as f:
json.dump(payload, f)
lock = LOCK_FILE.open('a')
fcntl.flock(lock, fcntl.LOCK_EX | fcntl.LOCK_NB)
done = threading.Event()
reason = {'message': None}
cancellation_raised = False
def abort(signum, frame):
nonlocal cancellation_raised
# A second signal must not interrupt restoration already running in finally.
if cancellation_raised:
return
cancellation_raised = True
raise Cancelled(reason['message'] or ('Board timeout' if signum == signal.SIGALRM else 'Cancelled'))
def lease():
last = time.monotonic()
while not done.wait(0.05):
ready, _, _ = select.select([sys.stdin], [], [], 0)
if ready:
line = sys.stdin.readline()
if not line:
reason['message'] = 'SSH input disconnected'
break
try:
message = json.loads(line)
except ValueError:
reason['message'] = 'Invalid lease message'
break
if message == {'action': 'heartbeat'}:
last = time.monotonic()
else:
reason['message'] = 'User requested stop'
break
if time.monotonic() - last > LEASE_SECONDS:
reason['message'] = 'Browser lease expired'
break
if not done.is_set():
os.kill(os.getpid(), signal.SIGUSR1)
for sig in (signal.SIGTERM, signal.SIGHUP, signal.SIGUSR1, signal.SIGALRM):
signal.signal(sig, abort)
signal.alarm(MAX_SECONDS)
threading.Thread(target=lease, daemon=True).start()
result = None
try:
current = status()
if current['pwm']:
raise RuntimeError('PWM already exported; another controller may own it')
emit('started', device=current, phase='preflight')
if payload['type'] == 'servo':
import Hobot.GPIO as GPIO
result = servo(payload, GPIO, emit)
elif payload['type'] == 'recovery':
from recovery_hardware import recovery
result = recovery(payload, emit)
else:
result = imu(emit)
terminal = ('stopped' if reason['message'] else
'failed' if result.get('status') == 'failed' else 'completed')
emit('result', status=terminal, result=result,
cleanup_verified=result.get('cleanup_verified', False), error=reason['message'])
except Exception as exc:
clean = not channels() if payload['type'] == 'servo' else bool(result and result.get('cleanup_verified'))
emit('result', status='stopped' if isinstance(exc, Cancelled) else 'failed',
error=str(exc), cleanup_verified=clean, result=result)
finally:
done.set()
signal.alarm(0)
fcntl.flock(lock, fcntl.LOCK_UN)
lock.close()
def main():
start = time.monotonic()
def emit(event, **fields):
print(json.dumps({'event': event, 'elapsed_s': time.monotonic() - start, **fields}), flush=True)
try:
if len(sys.argv) == 2 and sys.argv[1] == 'status':
print(json.dumps(status()), flush=True)
elif len(sys.argv) == 2 and sys.argv[1] == 'run':
payload = json.loads(sys.stdin.readline(8192))
run(payload, emit)
else:
raise ValueError('Use status or run')
except Exception as exc:
emit('result', status='failed', error=str(exc), cleanup_verified=False)
raise SystemExit(1)
if __name__ == '__main__':
main()