Ghost Hand

/*
 * accel.c : LIS2DW12 driver (TWI0 host, polled, with timeouts)
 *
 * Register facts used (LIS2DW12 DocID029682 Rev 4):
 *   WHO_AM_I 0x0F = 0x44
 *   CTRL1 0x20: ODR[7:4] MODE[3:2] LP_MODE[1:0]; 0x20 = 12.5 Hz LP mode 1, 0x40 = 50 Hz LP mode 1
 *   CTRL2 0x21: SOFT_RESET bit 6, BDU bit 3, IF_ADD_INC bit 2
 *   CTRL3 0x22: LIR bit 4 (latched interrupts), push-pull active high by default
 *   CTRL4_INT1_PAD_CTRL 0x23: INT1_WU bit 5
 *   CTRL6 0x25: FS[5:4] = 00 -> +/-2 g, LP mode 1 is 12-bit: 0.976 mg / LSB
 *   OUT_X_L 0x28 .. OUT_Z_H 0x2D (left aligned)
 *   WAKE_UP_THS 0x34: WK_THS[5:0], 1 LSB = FS / 64 = 31.25 mg at +/-2 g
 *   WAKE_UP_DUR 0x35, WAKE_UP_SRC 0x38, ALL_INT_SRC 0x3B
 *   CTRL7 0x3F: INTERRUPTS_ENABLE bit 5
 */
#include <avr/io.h>
#include "board.h"
#include "accel.h"
#include "power.h"

#define R_WHO_AM_I    0x0F
#define R_CTRL1       0x20
#define R_CTRL2       0x21
#define R_CTRL3       0x22
#define R_CTRL4       0x23
#define R_CTRL6       0x25
#define R_OUT_X_L     0x28
#define R_WAKE_THS    0x34
#define R_WAKE_DUR    0x35
#define R_WAKE_SRC    0x38
#define R_ALL_INT_SRC 0x3B
#define R_CTRL7       0x3F

#define TWI_TIMEOUT   4000u

/* --------------------------------------------------------------- TWI host */
static void twi_on(void)
{
    /* MBAUD = f_PER / (2 f_SCL) - 5 (rise time neglected): 5 MHz, 100 kHz -> 20 */
    TWI0.MBAUD = (uint8_t)(F_CPU / (2UL * 100000UL) - 5);
    TWI0.MCTRLA = TWI_ENABLE_bm;
    TWI0.MSTATUS = TWI_BUSSTATE_IDLE_gc;
}

void twi_off(void)
{
    TWI0.MCTRLA = 0;
}

static bool twi_wait(uint8_t flag)
{
    uint16_t n = TWI_TIMEOUT;
    while (!(TWI0.MSTATUS & flag)) {
        if (TWI0.MSTATUS & (TWI_BUSERR_bm | TWI_ARBLOST_bm)) return false;
        if (!--n) return false;
    }
    return true;
}

static void twi_stop(void)
{
    TWI0.MCTRLB = TWI_MCMD_STOP_gc;
}

static bool twi_start(uint8_t addr_rw)
{
    TWI0.MADDR = addr_rw;
    if (!twi_wait(TWI_WIF_bm | TWI_RIF_bm)) { twi_stop(); TWI0.MSTATUS = TWI_BUSSTATE_IDLE_gc; return false; }
    if (TWI0.MSTATUS & TWI_RXACK_bm) { twi_stop(); return false; }      /* NACK */
    return true;
}

static bool reg_write(uint8_t reg, uint8_t val)
{
    if (!twi_start((uint8_t)(ACC_I2C_ADDR << 1))) return false;
    TWI0.MDATA = reg;
    if (!twi_wait(TWI_WIF_bm)) { twi_stop(); return false; }
    TWI0.MDATA = val;
    if (!twi_wait(TWI_WIF_bm)) { twi_stop(); return false; }
    twi_stop();
    return true;
}

static bool reg_read(uint8_t reg, uint8_t *buf, uint8_t n)
{
    if (!twi_start((uint8_t)(ACC_I2C_ADDR << 1))) return false;
    TWI0.MDATA = reg;
    if (!twi_wait(TWI_WIF_bm)) { twi_stop(); return false; }
    if (!twi_start((uint8_t)((ACC_I2C_ADDR << 1) | 1))) return false;   /* repeated start */
    for (uint8_t i = 0; i < n; i++) {
        if (i && !twi_wait(TWI_RIF_bm)) { twi_stop(); return false; }
        buf[i] = TWI0.MDATA;
        TWI0.MCTRLB = (i + 1 == n) ? (TWI_ACKACT_NACK_gc | TWI_MCMD_STOP_gc) : TWI_MCMD_RECVTRANS_gc;
    }
    return true;
}

/* ------------------------------------------------------------- LIS2DW12 */
bool acc_init(void)
{
    uint8_t id = 0;
    twi_on();
    if (!reg_read(R_WHO_AM_I, &id, 1) || id != 0x44) return false;
    reg_write(R_CTRL2, 0x40);              /* soft reset */
    delay_ms_busy(5);
    reg_write(R_CTRL2, 0x0C);              /* BDU, IF_ADD_INC */
    reg_write(R_CTRL6, 0x00);              /* +/-2 g, ODR/2 bandwidth, low-pass path */
    reg_write(R_CTRL3, 0x10);              /* LIR: INT1 stays high until the source is read */
    reg_write(R_WAKE_DUR, 0x00);           /* 1 ODR sample over threshold */
    reg_write(R_CTRL7, 0x20);              /* INTERRUPTS_ENABLE */
    reg_write(R_CTRL4, 0x20);              /* wake-up -> INT1 */
    return true;
}

/* Sleep: LP mode 1 at 12.5 Hz (~1 uA). The wake-up function compares the
 * high-pass (slope) filtered acceleration with 2 LSB = 62.5 mg, so a gentle
 * pick-up or a nudge wakes the MCU through PA2. */
void acc_mode_sleep(void)
{
    twi_on();
    reg_write(R_CTRL1, 0x20);
    reg_write(R_WAKE_THS, 2);
    reg_write(R_CTRL4, 0x20);
    acc_clear_int();
}

/* Awake / session: 50 Hz LP mode 1, wake-up threshold in mg (0.25 g in a
 * session = 8 LSB: any jolt of a pick-up raises INT1 at once). */
void acc_mode_watch(uint16_t wake_mg)
{
    uint16_t lsb = (uint16_t)((wake_mg * 64u + 1000u) / 2000u);
    if (lsb < 1) lsb = 1;
    if (lsb > 63) lsb = 63;
    twi_on();
    reg_write(R_CTRL1, 0x40);
    reg_write(R_WAKE_THS, (uint8_t)lsb);
    reg_write(R_CTRL4, 0x20);
    acc_clear_int();
}

void acc_clear_int(void)
{
    uint8_t s;
    reg_read(R_WAKE_SRC, &s, 1);
    reg_read(R_ALL_INT_SRC, &s, 1);
}

bool acc_read(vec_mg_t *v)
{
    uint8_t b[6];
    if (!reg_read(R_OUT_X_L, b, 6)) return false;
    int16_t r[3];
    for (uint8_t i = 0; i < 3; i++) {
        int16_t raw = (int16_t)((uint16_t)b[2 * i] | ((uint16_t)b[2 * i + 1] << 8));
        raw >>= 4;                                         /* 12-bit, 0.976 mg/LSB */
        r[i] = (int16_t)(((int32_t)raw * 250) >> 8);       /* * 0.977 -> mg */
    }
    v->x = r[0]; v->y = r[1]; v->z = r[2];
    return true;
}

/* ------------------------------------------------------------- geometry */
uint32_t vec_dist2(const vec_mg_t *a, const vec_mg_t *b)
{
    int32_t dx = (int32_t)a->x - b->x, dy = (int32_t)a->y - b->y, dz = (int32_t)a->z - b->z;
    return (uint32_t)(dx * dx + dy * dy + dz * dz);
}

/* angle(a, b) > 30 deg  <=>  dot < cos(30) |a||b|  <=>  dot <= 0 or 4 dot^2 < 3 |a|^2 |b|^2.
 * Vectors are scaled to 32 mg units first so everything fits in 32 bits
 * (|v| <= ~110 units at 2 g per axis). */
bool vec_tilt_over_30(const vec_mg_t *a, const vec_mg_t *b)
{
    int32_t ax = a->x / 32, ay = a->y / 32, az = a->z / 32;
    int32_t bx = b->x / 32, by = b->y / 32, bz = b->z / 32;
    int32_t dot = ax * bx + ay * by + az * bz;
    if (dot <= 0) return true;
    uint32_t na = (uint32_t)(ax * ax + ay * ay + az * az);
    uint32_t nb = (uint32_t)(bx * bx + by * by + bz * bz);
    if (!na || !nb) return true;                           /* free fall: treat as tipped */
    return 4u * (uint32_t)dot * (uint32_t)dot < 3u * na * nb;
}