Solar MPPT Power System

#include "charge_control.h"
#include "board_config.h"
#include <math.h>

#define MPPT_PERTURB_INTERVAL_MS   200U     /* >> RC filter tau (1ms) and >> converter settle time */
#define MPPT_STEP_V_INITIAL        0.08f
#define MPPT_STEP_V_MIN            0.010f
#define MPPT_STEP_V_MAX            0.25f
#define MPPT_STEP_SHRINK           0.60f
#define MPPT_STEP_GROW             1.40f
#define MPPT_ICHG_DEADBAND_A       0.020f   /* below this, a change is noise, not a gradient */
#define MPPT_ICHG_STRONG_A         0.080f   /* above this, the bite was too big */

#define CC_TRIM_STEP_V             0.05f    /* per-iteration recovery step, all limiters */
#define CC_TRIM_HYSTERESIS_A       0.05f    /* dead-band around a current limit, avoids trim chatter */
#define CC_BACKOFF_V_PER_A         1.00f    /* proportional term when a limit is exceeded */
#define IIN_TRIM_HYSTERESIS_A      0.10f
#define IIN_BACKOFF_V_PER_A        1.00f

/* Fault debounce. Asymmetric on purpose: trip quickly enough to protect the
 * hardware, but require a sustained good reading before resuming, so a
 * PGOOD glitch during a load transient does not thrash the state machine. */
#define FAULT_TRIP_SAMPLES         5U       /* 5 x 20ms = 100ms of continuous badness */
#define FAULT_CLEAR_SAMPLES       25U       /* 25 x 20ms = 500ms of continuous health */

/* Sensor plausibility bands. A reading outside these is a broken divider, a
 * dead INA180 or an ADC that timed out and returned zero -- not a real
 * operating point, and acting on it is how a charger overcharges a battery. */
#define VBATT_PLAUSIBLE_MIN_V      7.0f
#define VBATT_PLAUSIBLE_MAX_V     17.0f
#define IIN_PLAUSIBLE_MAX_A       (PANEL_IIN_MAX_A * 1.6f)
#define ICHG_PLAUSIBLE_MAX_A      (BATT_CC_BULK_LIMIT_A * 3.0f)
#define I_PLAUSIBLE_MIN_A         (-1.0f)   /* a little negative is sensor offset, not a fault */

static float clampf(float v, float lo, float hi) {
    if (v < lo) return lo;
    if (v > hi) return hi;
    return v;
}

/* True when every sensor reading is physically possible. */
static int sensors_plausible(const sensor_frame_t *s) {
    if (!isfinite(s->vbatt) || !isfinite(s->iin) || !isfinite(s->ichg)) return 0;
    if (s->vbatt < VBATT_PLAUSIBLE_MIN_V || s->vbatt > VBATT_PLAUSIBLE_MAX_V) return 0;
    if (!isfinite(s->iin_fast) || !isfinite(s->ichg_fast)) return 0;
    if (s->iin  < I_PLAUSIBLE_MIN_A || s->iin  > IIN_PLAUSIBLE_MAX_A)  return 0;
    if (s->ichg < I_PLAUSIBLE_MIN_A || s->ichg > ICHG_PLAUSIBLE_MAX_A) return 0;
    return 1;
}

/* One incremental current limiter. Backing off is proportional to the
 * overshoot so it is fast; recovering is a fixed small step so it is slow.
 * That asymmetry is what keeps a limit from being overshot while still
 * letting the loop find the operating point smoothly. */
static float limiter_update(float ref, float measured, float limit,
                            float hysteresis, float backoff_gain, float hi)
{
    if (measured > limit) {
        ref -= (CC_TRIM_STEP_V + backoff_gain * (measured - limit));
    } else if (measured < limit - hysteresis) {
        /* Recover in steps proportional to the remaining headroom, so the
         * approach to the limit slows down instead of arriving at full
         * stride. A fixed step overshoots by whatever current that step is
         * worth -- and how much current a given millivolt is worth depends
         * on the battery's internal resistance, which firmware has no way to
         * know. Scaling by relative headroom needs no such knowledge: near
         * the limit the steps become small whatever the plant looks like. */
        float headroom = (limit - measured) / limit;     /* 0..1 */
        if (headroom > 1.0f) headroom = 1.0f;
        if (headroom < 0.1f) headroom = 0.1f;
        ref += CC_TRIM_STEP_V * headroom;
    }
    return clampf(ref, FB_FLOOR_V, hi);
}

/* Seat every limiter just above the present battery voltage, so current
 * ramps from zero instead of stepping. Used at init and whenever a charging
 * state is (re-)entered, including after a fault. */
static void soft_start(charge_control_t *cc, float vbatt_now) {
    float seed = clampf(vbatt_now + 0.20f, FB_FLOOR_V, BATT_CV_ABSORPTION_V);
    cc->cc_ref_v = seed;
    cc->iin_ref_v = seed;
    cc->mppt_ref_v = seed;
    cc->mppt_dir = 1;
    cc->mppt_step_v = MPPT_STEP_V_INITIAL;
    cc->mppt_last_ichg = 0.0f;
    cc->mppt_armed = 0;
    cc->mppt_ichg_sum = 0.0f;
    cc->mppt_ichg_n = 0u;
}

void charge_control_init(charge_control_t *cc, uint32_t now_ms) {
    cc->state = CHG_STATE_BULK;
    cc->state_entry_ms = now_ms;

    cc->mppt_step_v = MPPT_STEP_V_INITIAL;
    cc->mppt_last_perturb_ms = now_ms;
    cc->taper_start_ms = now_ms;
    cc->taper_running = 0;
    cc->fault_count = 0;
    cc->ok_count = 0;
    cc->fault_reason = "none";

    /* No sensor reading exists yet, so seed at the FB network's floor --
     * below any real battery voltage, hence zero charge current. The
     * limiters creep up from there once real readings arrive, which makes
     * the first control cycles a soft-start rather than a step into a
     * discharged battery. */
    soft_start(cc, FB_FLOOR_V - 1.0f);

    cc->commanded_vbatt = cc->cc_ref_v;
    cc->limiting_reason = "init";
}

/* P&O perturbation. Two differences from a textbook implementation, both
 * of which this board needs:
 *
 *  - a dead-band, because at the maximum power point the gradient is zero by
 *    definition, so without one the direction is decided by sensor noise and
 *    the tracker reverses on almost every step;
 *  - it only runs while MPPT is the constraint actually in command. When the
 *    charge-current or panel-current limiter is holding the output, MPPT's
 *    perturbations do not reach the plant, so any change it observes was
 *    caused by another loop. Integrating on that is windup: the reference
 *    walks to a rail and then takes control abruptly when the other limit
 *    releases. */
static void mppt_perturb(charge_control_t *cc, const sensor_frame_t *s, uint32_t now_ms) {
    /* Accumulate every control cycle. Judging a perturbation on ONE sample
     * taken 200ms later compares two noise draws as much as two operating
     * points; averaging the whole window instead cuts the noise by roughly
     * the square root of the sample count (10 here) and is what keeps the
     * direction decision from being made by the sensor rather than by the
     * panel. */
    cc->mppt_ichg_sum += s->ichg;
    if (cc->mppt_ichg_n < 0xFFFFu) cc->mppt_ichg_n++;

    if ((uint32_t)(now_ms - cc->mppt_last_perturb_ms) < MPPT_PERTURB_INTERVAL_MS) {
        return;
    }
    cc->mppt_last_perturb_ms = now_ms;

    float mean_ichg = (cc->mppt_ichg_n > 0u)
                    ? cc->mppt_ichg_sum / (float)cc->mppt_ichg_n : s->ichg;
    cc->mppt_ichg_sum = 0.0f;
    cc->mppt_ichg_n = 0u;

    if (!cc->mppt_armed) {
        /* first window after taking command: record, do not judge */
        cc->mppt_last_ichg = mean_ichg;
        cc->mppt_armed = 1;
        return;
    }

    /* Reverse only on a CLEAR degradation. A change inside the dead-band is
     * indistinguishable from sensor noise, so it must not flip the
     * direction -- but the perturbation itself still happens every cycle,
     * because a P&O that stops perturbing stops tracking, and at the floor
     * (where the current is zero and so is the gradient) it would never
     * start. */
    float delta = mean_ichg - cc->mppt_last_ichg;
    if (delta < -MPPT_ICHG_DEADBAND_A) {
        cc->mppt_dir = (int8_t)(-cc->mppt_dir);   /* clearly worse -- turn around */
    }

    /* Size the step so each perturbation produces a change big enough to
     * MEASURE. The step and the dead-band have to be coupled this way round:
     * an earlier version shrank the step on every reversal, which let the
     * step fall until a perturbation moved the current by less than the
     * dead-band -- at which point every delta read as "no change", the
     * direction froze, and the tracker walked steadily off the peak without
     * ever seeing that it was losing power. Growing the step whenever the
     * signal is too weak to steer by makes that failure impossible: the loop
     * cannot go blind, because going blind is itself what makes it take a
     * bigger bite. */
    float mag = delta < 0.0f ? -delta : delta;
    if (mag < MPPT_ICHG_DEADBAND_A) {
        cc->mppt_step_v *= MPPT_STEP_GROW;
    } else if (mag > MPPT_ICHG_STRONG_A) {
        cc->mppt_step_v *= MPPT_STEP_SHRINK;
    }
    cc->mppt_step_v = clampf(cc->mppt_step_v, MPPT_STEP_V_MIN, MPPT_STEP_V_MAX);

    /* Walking into a clamp is not tracking: if the reference is already
     * pinned at an end of its range and the direction pushes further into
     * it, nothing changes and no gradient is ever observed. Turn around. */
    if ((cc->mppt_dir > 0 && cc->mppt_ref_v >= BATT_CV_ABSORPTION_V - 1e-4f) ||
        (cc->mppt_dir < 0 && cc->mppt_ref_v <= FB_FLOOR_V + 1e-4f)) {
        cc->mppt_dir = (int8_t)(-cc->mppt_dir);
    }

    cc->mppt_ref_v += (float)cc->mppt_dir * cc->mppt_step_v;
    cc->mppt_ref_v = clampf(cc->mppt_ref_v, FB_FLOOR_V, BATT_CV_ABSORPTION_V);
    cc->mppt_last_ichg = mean_ichg;
}

float charge_control_update(charge_control_t *cc, const sensor_frame_t *s, uint32_t now_ms) {
    /* ---- 1. health: PGOOD and sensor plausibility, both debounced ------- */
    const char *bad = 0;
    if (!s->bias_pgood)             bad = "bias-pgood";
    else if (!s->mppt_pgood)        bad = "mppt-pgood";
    else if (!sensors_plausible(s)) bad = "sensor-range";

    if (bad) {
        cc->ok_count = 0;
        if (cc->fault_count < FAULT_TRIP_SAMPLES) cc->fault_count++;
        if (cc->fault_count >= FAULT_TRIP_SAMPLES && cc->state != CHG_STATE_FAULT) {
            cc->state = CHG_STATE_FAULT;
            cc->state_entry_ms = now_ms;
            cc->fault_reason = bad;
        }
    } else {
        cc->fault_count = 0;
        if (cc->ok_count < FAULT_CLEAR_SAMPLES) cc->ok_count++;
    }

    if (cc->state == CHG_STATE_FAULT) {
        /* Command the FB network's floor: below any real battery voltage, so
         * the converter stops delivering. The LEDs signal the state. */
        cc->commanded_vbatt = FB_FLOOR_V;
        cc->limiting_reason = "fault";
        if (!bad && cc->ok_count >= FAULT_CLEAR_SAMPLES) {
            cc->state = CHG_STATE_BULK;
            cc->state_entry_ms = now_ms;
            cc->taper_running = 0;
            soft_start(cc, s->vbatt);   /* never resume at the setpoint that faulted */
        }
        return cc->commanded_vbatt;
    }

    /* ---- 2. the state machine picks a voltage ceiling ------------------- */
    float candidate_cv;
    const char *cv_reason;

    switch (cc->state) {
    case CHG_STATE_BULK:
        candidate_cv = BATT_CV_ABSORPTION_V;
        cv_reason = "cv-ceiling";
        if (s->vbatt >= BATT_CV_ABSORPTION_V - 0.05f) {
            cc->state = CHG_STATE_ABSORPTION;
            cc->state_entry_ms = now_ms;
            cc->taper_running = 0;
        }
        break;

    case CHG_STATE_ABSORPTION:
        candidate_cv = BATT_CV_ABSORPTION_V;
        cv_reason = "absorption-cv";

        /* The taper must be SUSTAINED, not merely instantaneous: hold below
         * the taper threshold continuously for BATT_ABSORPTION_MIN_MS. A
         * single noise dip below 0.1A is not a full battery, and treating it
         * as one drops to float early and leaves the battery undercharged. */
        if (s->ichg < BATT_ABSORPTION_TAPER_A) {
            if (!cc->taper_running) {
                cc->taper_running = 1;
                cc->taper_start_ms = now_ms;
            } else if ((uint32_t)(now_ms - cc->taper_start_ms) >= BATT_ABSORPTION_MIN_MS) {
                cc->state = CHG_STATE_FLOAT;
                cc->state_entry_ms = now_ms;
                cc->taper_running = 0;
            }
        } else {
            cc->taper_running = 0;
        }

        /* A sustained sag well below the absorption setpoint means the
         * battery is no longer nearly full -- a load was added, or it was
         * swapped. Go back to bulk rather than sitting at a CV ceiling the
         * battery cannot reach. */
        if (s->vbatt < BATT_CV_FLOAT_V - 0.5f) {
            cc->state = CHG_STATE_BULK;
            cc->state_entry_ms = now_ms;
            cc->taper_running = 0;
        }
        break;

    case CHG_STATE_FLOAT:
        candidate_cv = BATT_CV_FLOAT_V;
        cv_reason = "float-cv";
        if (s->ichg > BATT_CC_BULK_LIMIT_A * 0.5f) {
            cc->state = CHG_STATE_BULK;
            cc->state_entry_ms = now_ms;
            soft_start(cc, s->vbatt);
        }
        break;

    default:
        cc->commanded_vbatt = FB_FLOOR_V;
        cc->limiting_reason = "unknown-state";
        return cc->commanded_vbatt;
    }

    /* ---- 3. current limits, applied in EVERY charging state -------------
     * These used to live inside the bulk branch, which meant that once the
     * charger reached absorption or float the charge-current limit and the
     * panel-side overcurrent ceiling simply stopped existing. Both protect
     * hardware (RS1 and the bridge FETs are sized for ~5A) and the battery,
     * so neither may depend on which charge phase happens to be running. */
    cc->cc_ref_v  = limiter_update(cc->cc_ref_v, s->ichg_fast, BATT_CC_BULK_LIMIT_A,
                                   CC_TRIM_HYSTERESIS_A, CC_BACKOFF_V_PER_A, candidate_cv);
    cc->iin_ref_v = limiter_update(cc->iin_ref_v, s->iin_fast, PANEL_IIN_MAX_A,
                                   IIN_TRIM_HYSTERESIS_A, IIN_BACKOFF_V_PER_A, candidate_cv);

    /* ---- 4. MPPT, only in bulk and only while it is in command ---------- */
    float candidate_mppt = candidate_cv;
    if (cc->state == CHG_STATE_BULK) {
        int mppt_in_command = (cc->mppt_ref_v <= cc->cc_ref_v) &&
                              (cc->mppt_ref_v <= cc->iin_ref_v) &&
                              (cc->mppt_ref_v <= candidate_cv);
        if (mppt_in_command) {
            mppt_perturb(cc, s, now_ms);
        } else {
            /* another limit is holding the output: re-seat MPPT just above
             * the binding constraint instead of letting it wind to a rail */
            float binding = cc->cc_ref_v < cc->iin_ref_v ? cc->cc_ref_v : cc->iin_ref_v;
            if (binding > candidate_cv) binding = candidate_cv;
            cc->mppt_ref_v = clampf(binding + cc->mppt_step_v, FB_FLOOR_V, BATT_CV_ABSORPTION_V);
            cc->mppt_armed = 0;
            cc->mppt_ichg_sum = 0.0f;
            cc->mppt_ichg_n = 0u;
            cc->mppt_step_v = MPPT_STEP_V_INITIAL;
            cc->mppt_last_perturb_ms = now_ms;
        }
        candidate_mppt = cc->mppt_ref_v;
    }

    /* ---- 5. arbitrate: the most restrictive constraint wins ------------- */
    float cmd = candidate_cv;
    const char *reason = cv_reason;
    if (cc->cc_ref_v  < cmd) { cmd = cc->cc_ref_v;  reason = "cc-limit"; }
    if (cc->iin_ref_v < cmd) { cmd = cc->iin_ref_v; reason = "iin-limit"; }
    if (candidate_mppt < cmd) { cmd = candidate_mppt; reason = "mppt"; }

    cc->commanded_vbatt = clampf(cmd, FB_FLOOR_V, FB_CEILING_V);
    cc->limiting_reason = reason;
    return cc->commanded_vbatt;
}