mirror of
https://github.com/openglow-org/forgefirm.git
synced 2026-09-27 08:41:13 -07:00
bench: head-accel crash-detector de-risk drill (item 6, first step)
accel_crash_probe.py arms the head LIS2HH12's on-chip interrupt generator (IG_CFG1/IG_THS/IG_DUR1) and polls the latched IG_SRC1 for a strike, reporting the axes and raw magnitude. It settles the bench fact the crash detector rides on: coexist mode reaches the IG registers over i2c-dev with I2C_SLAVE_FORCE while st_accel stays bound, so it proves whether the detector can be forgectrl-only with the liveness path untouched, or whether the accel must move under glowforge.ko. It touches only the IG registers (0x30-0x35) plus the CTRL7 latch bit, never the full scale, so st_accel's raw scaling is undisturbed; no emission, no commanded motion by default. Registered on the bench page (dry, board) and in the bench README. BRINGUP item 6 now stages the drill as the committed first step, with the readout path, per-state thresholds and the two-tier wiring owed after it. Tooling only: no shipped component source changed, so no acceptance-catalog consequence (the detector feature gets its case when it is built); the bench registry test and coverage lint pass.
This commit is contained in:
ScottW514
@@ -54,6 +54,7 @@ page's takeover does that; from a host, stop them first.
|
||||
| `build-forgectrl.sh` | Cross-compiles **forgectrl** (the canonical control-daemon repo) the same way, borrowing the toolchain from the forgectrl recipe workdir (regenerate with `bitbake forgectrl` after a clean). |
|
||||
| `accel_fast.py` | Direct-I2C sampler for the two head-bus LIS2HH12s (runs on the board; unbinds/rebinds st-accel around the capture, 800 Hz ODR, ~270 Hz per device polled): optional mid-capture jogs via local grblHAL TCP. CSV to /tmp/accel.csv. The head accel is i2c-3 0x1e. |
|
||||
| `bump_seek.py` | Accelerometer bump-seek homing prototype (runs on the board): creeps toward a rail in bounded jog segments via grblHAL TCP, learns the moving-noise baseline per segment, detects the contact jolt (~530 Hz sampling, 2-sample confirm), jog-cancels (0x85) and backs off. CSV to /tmp/bump.csv. |
|
||||
| `accel_crash_probe.py` | Head-accel crash-detector de-risk drill (runs on the board; BRINGUP item 6): arms the head LIS2HH12's on-chip interrupt generator (IG_CFG1/IG_THS/IG_DUR1) and polls the latched IG_SRC1 for a strike, reporting the axes and raw magnitude. `coexist` (default) reaches the IG registers with I2C_SLAVE_FORCE while st_accel stays bound, so it proves whether the detector can be forgectrl-only with the liveness path untouched; `unbind` frees the device (stop the controller and forgectrl first). Touches only the IG registers, no full-scale change, no emission, no commanded motion by default (provoke a trip by hand or with one `--jog`). CSV to /tmp/accel_crash.csv. |
|
||||
| `build-feeder.sh` | Cross-compiles `feeder.c` the same way. |
|
||||
| `puls_profile.py` | Decodes factory `.puls` streams (raw or GF1-headered) into velocity/accel profiles: peak speeds, ramp-slope fits, per-move segments, Z cadence. Runs anywhere (stdlib only). Source of the factory-true grblHAL defaults: 700/590 mm/s² accel, 200 mm/s max rate, 28160 Hz travel tick. |
|
||||
| `cp_watchdog_timing.py` | HV charge-pump watchdog one-shot timing (runs on the board): latches every CHG_PUMP feed pulse in GPIO3's edge detector (pin 24 only, IMR untouched, ICR2 restored on exit) and polls the `!Q` (`charge_pump_alive`) and `!HV_ENABLE` (`hv_enable`) pads through /dev/mem in a tight loop while it commands short local jogs; prints per-run t_w (last pulse → Q fall), Q → HV_ENABLE delay, priming latency and the feed period, with the loop's worst gap as the resolution. Motion only, laser locked, no other Grbl client attached. |
|
||||
|
||||
@@ -0,0 +1,271 @@
|
||||
#!/usr/bin/env python3
|
||||
"""De-risk drill for the head-accelerometer crash detector (BRINGUP item 6).
|
||||
|
||||
The LIS2HH12 on the head bus (i2c-3 @0x1e) carries an on-chip interrupt
|
||||
generator: a per-axis high-event threshold (IG_THS_X1/Y1/Z1), a duration
|
||||
counter (IG_DUR1), an axis enable/AND-OR word (IG_CFG1) and a latched
|
||||
per-axis source register (IG_SRC1). The factory arms this generator per
|
||||
job and reads trips from IG_SRC1; ForgeFIRM does not. This drill arms it
|
||||
on the bench and answers the three questions the crash-detector design
|
||||
rides on, before any kernel or forgectrl work:
|
||||
|
||||
1. Does IG_SRC1 latch a real head strike at a plausible threshold, and
|
||||
which axes does it report?
|
||||
2. Can the IG registers be programmed and polled over i2c-dev while
|
||||
st_accel stays bound for the motion-liveness reads (coexist mode,
|
||||
I2C_SLAVE_FORCE)? If so the detector is forgectrl-only, no kernel
|
||||
change and the liveness path untouched. If the two collide, the
|
||||
accel must move under glowforge.ko instead.
|
||||
3. What raw magnitude does a strike produce, so the shipped threshold
|
||||
can be set in register units against the rail-contact signature
|
||||
(facts bank: 20-40x over creep within ~4 ms on a fast strike).
|
||||
|
||||
The drill touches ONLY the IG registers plus the IG-latch bit of CTRL7,
|
||||
which st_accel never writes; it does not change the full scale (CTRL4)
|
||||
or the ODR (CTRL1), so st_accel's raw scaling is undisturbed. Default is
|
||||
coexist mode with forgectrl left running: no emission, no commanded
|
||||
motion. Provoke a trip by hand (a firm tap or nudge on the head) or pass
|
||||
--jog to send one gentle grblHAL jog; a jog needs the controller free,
|
||||
so run it from the bench page's takeover or stop the controller first.
|
||||
|
||||
Usage:
|
||||
accel_crash_probe.py [seconds] [--mode coexist|unbind] [--ths N]
|
||||
[--dur N] [--axes xyz] [--jog GCODE] [--rate HZ]
|
||||
|
||||
seconds arm-and-watch window (default 20)
|
||||
--mode coexist (default): leave st_accel bound, reach the IG
|
||||
registers with I2C_SLAVE_FORCE, forgectrl stays up; unbind:
|
||||
unbind st_accel for the run and rebind after (clean access,
|
||||
but liveness is down meanwhile, so stop the controller and
|
||||
forgectrl first)
|
||||
--ths N per-axis threshold register value 0..255 (default 40); the
|
||||
LSB is full-scale dependent, so the printed CTRL4 FS fixes
|
||||
the g conversion (+/-2 g default: ~15.6 mg/LSB, 1 g ~= 64)
|
||||
--dur N IG_DUR1 duration counter, samples at the running ODR
|
||||
(default 0 = fire on the first over-threshold sample)
|
||||
--axes which axes arm high events (default xyz)
|
||||
--jog one grblHAL jog at t=2 s, e.g. "$J=G91 X5 F1000" (+X first,
|
||||
never -X: a cable lives at the end of LEFT travel)
|
||||
--rate IG_SRC1 poll rate in Hz (default 200)
|
||||
|
||||
CSV of the poll trace to /tmp/accel_crash.csv: t,ig_src,xh,yh,zh,x,y,z
|
||||
Exit 0 on a clean run whether or not a trip was seen; the summary states
|
||||
what happened. Reads/writes only; relocks nothing (no laser path).
|
||||
"""
|
||||
import fcntl
|
||||
import os
|
||||
import struct
|
||||
import sys
|
||||
import time
|
||||
|
||||
I2C_SLAVE = 0x0703
|
||||
I2C_SLAVE_FORCE = 0x0706
|
||||
BUS = '/dev/i2c-3'
|
||||
ADDR = 0x1e # head accel; 0x1d is the board accel
|
||||
|
||||
WHO_AM_I = 0x0F
|
||||
WHO_AM_I_LIS2HH12 = 0x41
|
||||
CTRL1 = 0x20
|
||||
CTRL4 = 0x23 # bits 5:4 = FS (00=+/-2g, 10=+/-4g, 11=+/-8g)
|
||||
CTRL7 = 0x26 # bit 0 = LIR1 (latch IG_SRC1, read-to-clear)
|
||||
OUT_X_L = 0x28
|
||||
IG_CFG1 = 0x30 # AOI,6D,ZHIE,ZLIE,YHIE,YLIE,XHIE,XLIE
|
||||
IG_SRC1 = 0x31 # IA(6),ZH,ZL,YH,YL,XH,XL
|
||||
IG_THS_X1 = 0x32
|
||||
IG_THS_Y1 = 0x33
|
||||
IG_THS_Z1 = 0x34
|
||||
IG_DUR1 = 0x35
|
||||
|
||||
# IG_CFG1 high-event enables and the matching IG_SRC1 source bits share
|
||||
# the same odd bit positions: X high = 1, Y high = 3, Z high = 5 (the even
|
||||
# positions 0/2/4 are the low-event bits, which this drill does not arm).
|
||||
XHIE = 1 << 1
|
||||
YHIE = 1 << 3
|
||||
ZHIE = 1 << 5
|
||||
IG_IA = 1 << 6
|
||||
IG_XH = 1 << 1
|
||||
IG_YH = 1 << 3
|
||||
IG_ZH = 1 << 5
|
||||
|
||||
DRIVER = '/sys/bus/i2c/drivers/st-accel-i2c'
|
||||
DEV = '3-%04x' % ADDR
|
||||
FS_G = {0b00: 2, 0b10: 4, 0b11: 8}
|
||||
|
||||
|
||||
def bind_ctl(op):
|
||||
try:
|
||||
with open(DRIVER + '/' + op, 'w') as f:
|
||||
f.write(DEV)
|
||||
except OSError as e:
|
||||
print('%s %s: %s' % (op, DEV, e))
|
||||
|
||||
|
||||
def rd(fd, reg, n=1):
|
||||
os.write(fd, bytes([reg]))
|
||||
d = os.read(fd, n)
|
||||
return d[0] if n == 1 else d
|
||||
|
||||
|
||||
def wr(fd, reg, val):
|
||||
os.write(fd, bytes([reg, val & 0xff]))
|
||||
|
||||
|
||||
VALUE_OPTS = ('--mode', '--ths', '--dur', '--axes', '--jog', '--rate')
|
||||
|
||||
|
||||
def parse(argv):
|
||||
"""Return (positionals, {opt: value}). Value options consume the next
|
||||
token."""
|
||||
pos, opts = [], {}
|
||||
i = 0
|
||||
while i < len(argv):
|
||||
a = argv[i]
|
||||
if a in VALUE_OPTS:
|
||||
opts[a] = argv[i + 1] if i + 1 < len(argv) else None
|
||||
i += 2
|
||||
else:
|
||||
pos.append(a)
|
||||
i += 1
|
||||
return pos, opts
|
||||
|
||||
|
||||
def main():
|
||||
pos, opts = parse(sys.argv[1:])
|
||||
dur_s = float(pos[0]) if pos else 20.0
|
||||
ths = int(opts.get('--ths', 40))
|
||||
ig_dur = int(opts.get('--dur', 0))
|
||||
axes = opts.get('--axes', 'xyz')
|
||||
rate = float(opts.get('--rate', 200))
|
||||
jog = opts.get('--jog')
|
||||
unbind = opts.get('--mode', 'coexist') == 'unbind'
|
||||
|
||||
cfg = 0
|
||||
if 'x' in axes:
|
||||
cfg |= XHIE
|
||||
if 'y' in axes:
|
||||
cfg |= YHIE
|
||||
if 'z' in axes:
|
||||
cfg |= ZHIE
|
||||
|
||||
if unbind:
|
||||
bind_ctl('unbind')
|
||||
|
||||
fd = os.open(BUS, os.O_RDWR)
|
||||
# Coexist mode leaves st_accel bound, so FORCE the address; unbind
|
||||
# mode owns it outright.
|
||||
fcntl.ioctl(fd, I2C_SLAVE if unbind else I2C_SLAVE_FORCE, ADDR)
|
||||
|
||||
who = rd(fd, WHO_AM_I)
|
||||
if who != WHO_AM_I_LIS2HH12:
|
||||
print('WHO_AM_I=0x%02x, expected 0x41 (LIS2HH12) at %s' % (who, DEV))
|
||||
os.close(fd)
|
||||
if unbind:
|
||||
bind_ctl('bind')
|
||||
return 1
|
||||
|
||||
ctrl1 = rd(fd, CTRL1)
|
||||
ctrl4 = rd(fd, CTRL4)
|
||||
fs = FS_G.get((ctrl4 >> 4) & 0b11, '?')
|
||||
if unbind and (ctrl1 & 0x07) == 0:
|
||||
wr(fd, CTRL1, 0x6F) # 800 Hz, BDU, XYZ on; st_accel does this itself
|
||||
ctrl1 = rd(fd, CTRL1)
|
||||
lsb_mg = (fs * 1000.0 / 128.0) if isinstance(fs, int) else 0
|
||||
print('WHO_AM_I=0x%02x CTRL1=0x%02x CTRL4=0x%02x FS=+/-%sg '
|
||||
'ths=%d (~%.0f mg, ~%.2f g) dur=%d axes=%s mode=%s'
|
||||
% (who, ctrl1, ctrl4, fs, ths, ths * lsb_mg, ths * lsb_mg / 1000.0,
|
||||
ig_dur, axes, 'unbind' if unbind else 'coexist(FORCE)'))
|
||||
|
||||
# Arm IG1: latch the source (CTRL7 LIR1), set the thresholds and
|
||||
# duration, then enable the axes last. Preserve CTRL7's other bits.
|
||||
ctrl7 = rd(fd, CTRL7)
|
||||
wr(fd, CTRL7, ctrl7 | 0x01)
|
||||
wr(fd, IG_THS_X1, ths)
|
||||
wr(fd, IG_THS_Y1, ths)
|
||||
wr(fd, IG_THS_Z1, ths)
|
||||
wr(fd, IG_DUR1, ig_dur & 0x7f)
|
||||
wr(fd, IG_CFG1, cfg)
|
||||
rd(fd, IG_SRC1) # clear any stale latch
|
||||
|
||||
sock = None
|
||||
if jog:
|
||||
import socket
|
||||
sock = socket.create_connection(('127.0.0.1', 23), timeout=3)
|
||||
sock.settimeout(0.1)
|
||||
|
||||
out = open('/tmp/accel_crash.csv', 'w')
|
||||
out.write('t,ig_src,xh,yh,zh,x,y,z\n')
|
||||
period = 1.0 / rate
|
||||
t0 = time.monotonic()
|
||||
jogged = False
|
||||
trips = []
|
||||
live_ok = True
|
||||
n = 0
|
||||
while True:
|
||||
t = time.monotonic() - t0
|
||||
if t > dur_s:
|
||||
break
|
||||
if sock and not jogged and t > 2.0:
|
||||
sock.sendall((jog + '\n').encode())
|
||||
jogged = True
|
||||
src = rd(fd, IG_SRC1)
|
||||
n += 1
|
||||
if src & IG_IA:
|
||||
raw = rd(fd, OUT_X_L, 6)
|
||||
x, y, z = struct.unpack('<hhh', raw)
|
||||
xh = 1 if src & IG_XH else 0
|
||||
yh = 1 if src & IG_YH else 0
|
||||
zh = 1 if src & IG_ZH else 0
|
||||
out.write('%.5f,0x%02x,%d,%d,%d,%d,%d,%d\n'
|
||||
% (t, src, xh, yh, zh, x, y, z))
|
||||
trips.append((t, xh, yh, zh, x, y, z))
|
||||
time.sleep(period)
|
||||
out.close()
|
||||
|
||||
# Coexist proof: st_accel's raw reads still work while we polled.
|
||||
if not unbind:
|
||||
try:
|
||||
base = ('/sys/bus/i2c/devices/%s/iio:device' % DEV)
|
||||
import glob
|
||||
hits = glob.glob(base + '*/in_accel_x_raw')
|
||||
if hits:
|
||||
with open(hits[0]) as f:
|
||||
_ = int(f.read())
|
||||
else:
|
||||
live_ok = False
|
||||
except (OSError, ValueError):
|
||||
live_ok = False
|
||||
|
||||
# Disarm and restore.
|
||||
wr(fd, IG_CFG1, 0x00)
|
||||
wr(fd, CTRL7, ctrl7)
|
||||
if sock:
|
||||
try:
|
||||
sock.recv(4096)
|
||||
except OSError:
|
||||
pass
|
||||
sock.close()
|
||||
os.close(fd)
|
||||
if unbind:
|
||||
bind_ctl('bind')
|
||||
|
||||
print('polled %d samples at ~%.0f Hz over %.0f s' % (n, n / dur_s, dur_s))
|
||||
if trips:
|
||||
ax = ''.join(a for a, f in zip('xyz', (
|
||||
any(t[1] for t in trips),
|
||||
any(t[2] for t in trips),
|
||||
any(t[3] for t in trips))) if f)
|
||||
first = trips[0]
|
||||
print('TRIPPED %d times, axes seen: %s' % (len(trips), ax or '-'))
|
||||
print(' first at t=%.3f s raw x=%d y=%d z=%d (FS +/-%sg, 1 g ~= %d)'
|
||||
% (first[0], first[4], first[5], first[6], fs,
|
||||
int(32768 / fs) if isinstance(fs, int) else 0))
|
||||
else:
|
||||
print('no trip in the window (raise the strike or lower --ths)')
|
||||
if not unbind:
|
||||
print('coexist liveness read after the run: %s'
|
||||
% ('OK (st_accel still serving raw)' if live_ok else 'FAILED'))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
Reference in New Issue
Block a user