plane image fallback + seismic calibrations

sensors/main.py

@@ -151,42 +151,89 @@ def read_bme(c, sensor, alt_m=None):
         return None, None, None
 
 # ── MPU6050 ───────────────────────────────────────────────────────────────────
+# ── MPU6050 ───────────────────────────────────────────────────────────────────
 
 MPU_ADDR     = 0x68
 _mpu_peak    = None
 _mpu_lock    = threading.Lock()
-_mpu_offsets = None
+_mpu_grav    = None                  # slow-tracked gravity vector [x, y, z]
+GRAV_ALPHA   = 0.05                  # EMA rate while still
+STILL_SPREAD = 0.03                  # g, max peak-to-peak per axis to count as "still"
+_still_buf   = deque(maxlen=50)      # ~0.5s @ 100Hz
+
+def read_accel():
+    d = try_read(lambda: bus.read_i2c_block_data(MPU_ADDR, 0x3B, 6))
+    if not d: return None
+    return [
+        s16((d[0] << 8) | d[1]) / 16384.0,
+        s16((d[2] << 8) | d[3]) / 16384.0,
+        s16((d[4] << 8) | d[5]) / 16384.0,
+    ]
+
+def rotation_to_z(g):
+    """Rodrigues rotation mapping gravity vector g -> +Z. Returns (R, |g|)."""
+    mag = math.sqrt(sum(v * v for v in g))
+    u   = [v / mag for v in g]
+    kx, ky = u[1], -u[0]                     # axis = u × z
+    s      = math.sqrt(kx * kx + ky * ky)    # sin(theta)
+    c      = u[2]                            # cos(theta)
+    if s < 1e-8:
+        return [[1, 0, 0], [0, c, 0], [0, 0, c]], mag
+    kx, ky = kx / s, ky / s
+    v = 1 - c
+    return [
+        [c + kx * kx * v, kx * ky * v,      ky * s],
+        [ky * kx * v,     c + ky * ky * v, -kx * s],
+        [-ky * s,         kx * s,           c     ],
+    ], mag
+
+def is_still(buf):
+    if len(buf) < buf.maxlen: return False
+    return all(
+        max(s[i] for s in buf) - min(s[i] for s in buf) < STILL_SPREAD
+        for i in range(3)
+    )
 
 def calibrate_mpu(samples=200):
     if not bus: return None
     print("  Calibrating MPU6050, keep still...")
-    sums = [0.0] * 6
-    n    = 0
+    sums, n = [0.0, 0.0, 0.0], 0
     for _ in range(samples):
-        d = try_read(lambda: bus.read_i2c_block_data(MPU_ADDR, 0x3B, 14))
-        if d:
-            sums[0] += s16((d[0]  << 8) | d[1])  / 16384.0
-            sums[1] += s16((d[2]  << 8) | d[3])  / 16384.0
-            sums[2] += s16((d[4]  << 8) | d[5])  / 16384.0
-            sums[3] += s16((d[8]  << 8) | d[9])  / 131.0
-            sums[4] += s16((d[10] << 8) | d[11]) / 131.0
-            sums[5] += s16((d[12] << 8) | d[13]) / 131.0
+        a = read_accel()
+        if a:
+            sums = [s + v for s, v in zip(sums, a)]
             n += 1
         time.sleep(0.005)
     if n == 0: return None
-    return [sums[0]/n, sums[1]/n, sums[2]/n - 1.0, sums[3]/n, sums[4]/n, sums[5]/n]
+
+    g   = [s / n for s in sums]
+    mag = math.sqrt(sum(v * v for v in g))
+    if not 0.5 < mag < 1.5:
+        print(f"  ❌ MPU6050: bad gravity magnitude {mag:.2f}g")
+        return None
+
+    print(f"  ✅ MPU6050: gravity {mag:.3f}g on ({g[0]:.2f}, {g[1]:.2f}, {g[2]:.2f}) → mapped to Z")
+    return g
 
 def mpu_loop():
-    global _mpu_peak
+    global _mpu_peak, _mpu_grav
     while True:
-        if bus and _mpu_offsets:
-            d = try_read(lambda: bus.read_i2c_block_data(MPU_ADDR, 0x3B, 14))
-            if d:
-                o   = _mpu_offsets
-                ax  = s16((d[0] << 8) | d[1]) / 16384.0 - o[0]
-                ay  = s16((d[2] << 8) | d[3]) / 16384.0 - o[1]
-                az  = s16((d[4] << 8) | d[5]) / 16384.0 - o[2] - 1.0  # remove gravity
+        if bus and _mpu_grav:
+            a = read_accel()
+            if a:
+                _still_buf.append(a)
+
+                R, g = rotation_to_z(_mpu_grav)
+                ax = R[0][0] * a[0] + R[0][1] * a[1] + R[0][2] * a[2]
+                ay = R[1][0] * a[0] + R[1][1] * a[1] + R[1][2] * a[2]
+                az = R[2][0] * a[0] + R[2][1] * a[1] + R[2][2] * a[2] - g
                 mag = math.sqrt(ax**2 + ay**2 + az**2)
+
+                # Re-track gravity whenever the sensor is *still* —
+                # absorbs bias shifts of any size, blocks shakes
+                if is_still(_still_buf):
+                    _mpu_grav = [p + (v - p) * GRAV_ALPHA for p, v in zip(_mpu_grav, a)]
+
                 with _mpu_lock:
                     if _mpu_peak is None or mag > _mpu_peak['seismic_magnitude']:
                         _mpu_peak = {
@@ -484,9 +531,9 @@ if __name__ == '__main__':
     if bus:
         try_read(lambda: bus.write_byte_data(MPU_ADDR, 0x6B, 0))
         time.sleep(0.1)
-        _mpu_offsets = try_init(calibrate_mpu, 'MPU6050')
+        _mpu_grav = try_init(calibrate_mpu, 'MPU6050')
     else:
-        _mpu_offsets = None
+        _mpu_grav = None
         print("  ❌ MPU6050: no I2C bus")
 
     threading.Thread(target=mpu_loop, daemon=True).start()