Sensor Diagnostic
Monitor

/* diag_params.h — one parameter set (mirrors matlab/diag_params.m).
 * Monitor executes at Ts = 0.01 s (100 Hz). */
#define TS           0.01F   /* sample time [s] */

/* sensor scaling: APP1 0.5..4.5V, APP2 0.25..2.25V over 0..100% */
#define V1_LO        0.5F
#define V1_HI        4.5F
#define V2_LO        0.25F
#define V2_HI        2.25F

/* rationality thresholds */
#define V_OOR_LO     0.20F   /* electrical out-of-range low  [V] */
#define V_OOR_HI     4.80F   /* electrical out-of-range high [V] */
#define DV_MAX       1.50F   /* max plausible step / sample [V] */
#define CORR_TOL     10.0F   /* max |pct1-pct2| mismatch [%] */

/* confirmation / healing / enable */
#define CONFIRM_CNT  20U     /* failing samples to confirm (=200 ms) */
#define HEAL_CNT     300U    /* clean samples to heal (~3 cycles) */
#define WARM_MIN     60.0F   /* coolant temp enabling diagnostics [C] */

/* DTC bitfield: bit0 range, bit1 rate, bit2 correlation */

/* diag_rationality_step — range + rate + correlation with a shared
 * debounced fail counter. Counter frozen unless enable conditions met. */
void diag_rationality_step(const DiagIn *const in, DiagWork *const w)
{
    const real32_T pct1 = (in->v1 - V1_LO) / (V1_HI - V1_LO) * 100.0F;
    const real32_T pct2 = (in->v2 - V2_LO) / (V2_HI - V2_LO) * 100.0F;

    boolean_T range_bad = (in->v1 < V_OOR_LO) || (in->v1 > V_OOR_HI);   /* REQ-DIAG-001 */
    boolean_T rate_bad  = fabsf(in->v1 - w->v1_prev) > DV_MAX;          /* REQ-DIAG-002 */
    boolean_T corr_bad  = fabsf(pct1 - pct2) > CORR_TOL;               /* REQ-DIAG-003 */
    boolean_T any_bad   = range_bad || rate_bad || corr_bad;
    w->testbits = (uint8_T)range_bad | ((uint8_T)rate_bad << 1) | ((uint8_T)corr_bad << 2);

    w->enable = (in->ect >= WARM_MIN);                                /* REQ-DIAG-009 */
    if (w->enable) {
        if (any_bad) { if (w->failCnt < CONFIRM_CNT) w->failCnt++; }
        else         { if (w->failCnt > 0U)         w->failCnt--; }
    }
    w->any_bad  = any_bad;
    w->v1_prev  = in->v1;
}

/* diag_supervisor_step — NO_FAULT->PENDING->CONFIRMED->HEALING (Stateflow). */
void diag_supervisor_step(const DiagIn *const in, DiagWork *const w,
                          DiagOut *const out)
{
    switch (w->state) {
    case DIAG_NO_FAULT:
        if (w->failCnt > 0U) w->state = DIAG_PENDING;            /* REQ-DIAG-005 */
        break;
    case DIAG_PENDING:
        if (w->failCnt >= CONFIRM_CNT) {                        /* REQ-DIAG-004 */
            w->state = DIAG_CONFIRMED; w->dtc = w->testbits;
            if (!w->ff_valid) {                                 /* REQ-DIAG-006 */
                w->ff = (FreezeFrame){ in->rpm, in->load, in->ect,
                                      in->v1, in->v2, w->dtc };
                w->ff_valid = true;
            }
        } else if (w->failCnt == 0U) { w->state = DIAG_NO_FAULT; }
        break;
    case DIAG_CONFIRMED:
        if (w->enable && !w->any_bad) { w->state = DIAG_HEALING; w->healCnt = 0U; }
        break;
    case DIAG_HEALING:
        if (w->any_bad) { w->state = DIAG_CONFIRMED; w->healCnt = 0U; }   /* re-fail */
        else if (++w->healCnt >= HEAL_CNT) {                  /* REQ-DIAG-008 */
            w->dtc = 0U; w->ff_valid = false; w->failCnt = 0U;
            w->state = DIAG_NO_FAULT;
        }
        break;
    default: w->state = DIAG_NO_FAULT; break;
    }
    out->state    = w->state;
    out->dtc      = w->dtc;
    out->mil      = (w->state == DIAG_CONFIRMED) || (w->state == DIAG_HEALING); /* REQ-DIAG-007 */
    out->ff_valid = w->ff_valid;
}

/* diag_monitor_types.h — I/O contract (REQ-DIAG-010). */
typedef enum { DIAG_NO_FAULT = 0, DIAG_PENDING,
               DIAG_CONFIRMED, DIAG_HEALING } DiagState;   /* 0 … 3 */

typedef struct {
    real32_T rpm, load, ect, v1, v2;
    uint8_T  code;                /* tripping test bitfield */
} FreezeFrame;

typedef struct {
    real32_T  v1, v2;             /* redundant sensor pair [V] */
    real32_T  rpm, load, ect;      /* operating point + enable */
} DiagIn;

typedef struct {
    DiagState state;               /* supervisor state 0 … 3 */
    uint8_T   dtc;                 /* 0 = OK; bit0 range, bit1 rate, bit2 corr */
    boolean_T mil;                 /* malfunction lamp */
    boolean_T ff_valid;            /* freeze-frame captured */
} DiagOut;