Torque Safety
Monitor

/* safety_params.h — one parameter set (mirrors matlab/safety_params.m).
 * Monitor executes at Ts = 0.01 s (100 Hz). */
#define TS         0.01F   /* sample time [s] */
#define FTTI       0.05F   /* fault-tolerant time interval [s] */

/* Level 2 torque model */
#define T_MAX      300.0F  /* max engine torque [Nm] */
#define T_MARGIN   30.0F   /* allowance above requested [Nm] */
#define T_LIMP     60.0F   /* limited torque in L2 reaction [Nm] */

/* debounce / watchdog (sized so reaction <= FTTI) */
#define L2_CNT     3U      /* samples to confirm L2 fault (=30 ms) */
#define WD_CNT     5U      /* missed answers to trip L3  (=50 ms) */

/* safety goal: no unintended torque — ASIL D (illustrative) */

/* safety_monitor_step — Level 2 permissible-torque check + Level 3
 * question/answer watchdog. Both feed the safe-state supervisor. */
void safety_monitor_step(const SafIn *const in, SafWork *const w)
{
    /* L2 — independent permissible torque (REQ-SAF-001/002) */
    const real32_T requested   = in->pedal / 100.0F * T_MAX;
    w->permissible            = requested + T_MARGIN;
    boolean_T l2_bad = in->torque_actual > w->permissible;
    if (l2_bad) { if (w->l2Cnt < L2_CNT) w->l2Cnt++; } else { w->l2Cnt = 0U; }
    w->l2_confirmed = (w->l2Cnt >= L2_CNT);                 /* debounced (REQ-SAF-004) */

    /* L3 — question/answer watchdog (REQ-SAF-005/006) */
    if (!in->wd_ok) { if (w->wdFail < WD_CNT) w->wdFail++; } else { w->wdFail = 0U; }
    w->l3_trip = (w->wdFail >= WD_CNT);
}

/* safety_supervisor_step — OK->L2_LIMIT->L3_SAFE (Stateflow).
 * L3 has priority; reactions latch until reset (REQ-SAF-007/008). */
void safety_supervisor_step(const SafIn *const in, SafWork *const w,
                            SafOut *const out)
{
    if (w->l3_trip) {                                   /* highest priority */
        w->state = SAF_L3_SAFE;
    } else if (w->state == SAF_OK && w->l2_confirmed) {
        w->state = SAF_L2_LIMIT;
    }
    /* (no automatic exit — latched until a key-cycle reset) */

    switch (w->state) {
    case SAF_OK:       out->tq_cmd = fminf(in->torque_actual, w->permissible); break;
    case SAF_L2_LIMIT: out->tq_cmd = fminf(in->torque_actual, T_LIMP);         break;
    default:          out->tq_cmd = 0.0F;                                    break;
    }
    out->state      = w->state;
    out->safe_state = (w->state != SAF_OK);
}

/* safety_monitor_types.h — I/O contract (REQ-SAF-010). */
typedef enum { SAF_OK = 0, SAF_L2_LIMIT, SAF_L3_SAFE } SafState; /* 0 … 2 */

typedef struct {
    real32_T  pedal;          /* accelerator [%] — independent L2 path */
    real32_T  torque_actual;  /* L1 function-path torque [Nm] */
    boolean_T wd_ok;          /* watchdog answered correctly */
} SafIn;

typedef struct {
    SafState  state;          /* OK / L2_LIMIT / L3_SAFE */
    real32_T  tq_cmd;         /* allowed torque [Nm] (<= permissible) */
    boolean_T safe_state;     /* a safety reaction is active */
} SafOut;