Multi-modulator architecture: modulator slot, N-pulse patterns, live handoff, tracking observer

Assets/NodeTemplates/Actuators.CarrierAuto/inline.cpp

@@ -0,0 +1,26 @@
+/* Piecewise-linear carrier schedule: flat below the first and above the last
+ * valid breakpoint, linear between.  A breakpoint with fsw <= 0 ends the
+ * table.  The platform call deadbands small changes, so this can run every
+ * app_loop step without churning the timer. */
+const float fe = platform_get_elec_freq_hz();
+const float fes[6]  = {fe_1, fe_2, fe_3, fe_4, fe_5, fe_6};
+const float fsws[6] = {fsw_1, fsw_2, fsw_3, fsw_4, fsw_5, fsw_6};
+
+int n_pts = 0;
+while (n_pts < 6 && fsws[n_pts] > 0.0f) ++n_pts;
+
+if (n_pts > 0) {
+    float carrier = fsws[0];
+    if (fe > fes[0]) {
+        carrier = fsws[n_pts - 1];
+        for (int i = 1; i < n_pts; ++i) {
+            if (fe <= fes[i]) {
+                const float span = fes[i] - fes[i - 1];
+                const float t = (span > 0.0f) ? (fe - fes[i - 1]) / span : 1.0f;
+                carrier = fsws[i - 1] + t * (fsws[i] - fsws[i - 1]);
+                break;
+            }
+        }
+    }
+    platform_pwm_set_carrier_hz(carrier);
+}

Assets/NodeTemplates/Actuators.CarrierAuto/node.json

@@ -0,0 +1,25 @@
+{
+  "id": "Actuators.CarrierAuto",
+  "displayName": "Carrier Auto-Schedule",
+  "defaultName": "CarrierAuto",
+  "description": "Piecewise-linear PWM carrier schedule: up to 6 (fe, fsw) breakpoints; flat below the first and above the last valid point, linear between. Points with fsw <= 0 are ignored. Async-modulation experiment; the control loop stays closed throughout.",
+  "maxInstances": 1,
+  "isEntryPoint": false,
+  "domain": "app_loop",
+  "inputPorts": [],
+  "outputPorts": [],
+  "parameterTypes": {
+    "fe_1": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 1 electrical frequency (Hz)."},
+    "fsw_1": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 1 carrier (Hz). <= 0 ends the table."},
+    "fe_2": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 2 electrical frequency (Hz)."},
+    "fsw_2": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 2 carrier (Hz). <= 0 ends the table."},
+    "fe_3": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 3 electrical frequency (Hz)."},
+    "fsw_3": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 3 carrier (Hz). <= 0 ends the table."},
+    "fe_4": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 4 electrical frequency (Hz)."},
+    "fsw_4": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 4 carrier (Hz). <= 0 ends the table."},
+    "fe_5": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 5 electrical frequency (Hz)."},
+    "fsw_5": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 5 carrier (Hz). <= 0 ends the table."},
+    "fe_6": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 6 electrical frequency (Hz)."},
+    "fsw_6": {"quantity": "dimensionless", "frame": "scalar", "dtype": "f32", "description": "Breakpoint 6 carrier (Hz). <= 0 ends the table."}
+  }
+}

Assets/NodeTemplates/Actuators.ModulationAuto/inline.cpp

@@ -0,0 +1,12 @@
+/* Policy only: watch the electrical frequency (ramp command, FOC estimate,
+ * or the pattern's own) and request handoffs.  The switching itself
+ * (phase-lock, level match, timer handoff) lives in the base image behind
+ * platform_modulation_to_pattern/to_ramp. */
+const float fe = platform_get_elec_freq_hz();
+const uint8_t mod_mode = platform_modulation_mode();
+
+if (mod_mode == 0U && fe >= enter_hz) {
+    (void)platform_modulation_to_pattern((uint32_t)(pulses + 0.5f), duty);
+} else if (mod_mode == 1U && fe < exit_hz) {
+    (void)platform_modulation_to_ramp();
+}

Assets/NodeTemplates/Actuators.ModulationAuto/node.json

@@ -0,0 +1,37 @@
+{
+  "id": "Actuators.ModulationAuto",
+  "displayName": "Modulation Auto-Switch",
+  "defaultName": "ModAuto",
+  "description": "Supervises modulation mode from the open-loop ramp frequency: hands off to the N-pulse pattern above enter_hz, back to the SVPWM ramp below exit_hz. Switching is phase-locked and executed by base-image HAL primitives; this node is policy only.",
+  "maxInstances": 1,
+  "isEntryPoint": false,
+  "domain": "app_loop",
+  "inputPorts": [],
+  "outputPorts": [],
+  "parameterTypes": {
+    "enter_hz": {
+      "quantity": "dimensionless",
+      "frame": "scalar",
+      "dtype": "f32",
+      "description": "Ramp-to-pattern handoff threshold (electrical Hz)."
+    },
+    "exit_hz": {
+      "quantity": "dimensionless",
+      "frame": "scalar",
+      "dtype": "f32",
+      "description": "Pattern-to-ramp handoff threshold (electrical Hz). Set below enter_hz for hysteresis."
+    },
+    "pulses": {
+      "quantity": "dimensionless",
+      "frame": "scalar",
+      "dtype": "f32",
+      "description": "N-pulse pulses per quarter cycle (1..64). Device switching = (2N+1) x fe."
+    },
+    "duty": {
+      "quantity": "dimensionless",
+      "frame": "scalar",
+      "dtype": "f32",
+      "description": "High-time fraction per pulse cell (0..1). 1.0 = six-step; keep above ~0.7."
+    }
+  }
+}

Assets/NodeTemplates/Control.Pi/inline.cpp

@@ -1,8 +1,13 @@
 /* PI with clamping + back-calculation anti-windup.
  * Dimensionless throughout: implicit unit extraction/injection handles any
- * physical-quantity wiring at the binding sites. */
+ * physical-quantity wiring at the binding sites.
+ *
+ * dt comes from the live control rate (platform_get_control_dt) so integral
+ * gain stays correct when the carrier changes at runtime (CarrierAuto);
+ * the Dt parameter is kept for backward compatibility but ignored. */
+const float dt_s = platform_get_control_dt();
 const float error = Setpoint - Measurement;
-Integral += error * Dt;
+Integral += error * dt_s;
 
 float raw_output = Kp * error + Ki * Integral;
 
@@ -21,7 +26,7 @@ if (limited_output < min_limit) limited_output = min_limit;
  * the base-image VectorPIController). */
 if (Ki > 0.0001f && Kp > 0.0001f && AwGain > 0.0f) {
     const float excess = raw_output - limited_output;
-    Integral -= excess * Dt * AwGain / (Kp * Ki);
+    Integral -= excess * dt_s * AwGain / (Kp * Ki);
 }
 
 Output = limited_output;

Images/Gen6FW/CMakeLists.txt

@@ -79,6 +79,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     Src/Inverter/Command/Commands/CalibrationCommands.cpp
     Src/Inverter/Command/Commands/FocCommands.cpp
     Src/Inverter/Command/Commands/CanCommands.cpp
+    Src/Inverter/Command/Commands/SheCommands.cpp
     Src/Inverter/Control/ControlSupervisor.cpp
     Src/Inverter/Control/OpenLoopController.cpp
     Src/Inverter/Control/FocControlManager.cpp
@@ -108,6 +109,9 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     Src/Inverter/Drivers/Logging/ontime_logger.c
     Src/Inverter/Drivers/Logging/SupplyMonitor.cpp
     Src/Inverter/Drivers/PWM/pwm.cpp
+    Src/Inverter/Drivers/PWM/Modulator.cpp
+    Src/Inverter/Drivers/PWM/ShepwmModulator.cpp
+    Src/Inverter/Drivers/PWM/ModulationSwitch.cpp
     Src/Inverter/Drivers/Sensors/CurrentSensor.cpp
     Src/Inverter/Drivers/Sensors/CurrentSensorTest.cpp
     Src/Inverter/Drivers/Sensors/DcLinkCurrentSensor.cpp

Images/Gen6FW/Inc/Inverter/Control/FocControlManager.h

@@ -2,6 +2,7 @@
 
 #include "Inverter/Control/FocController.h"
 
+#include <cmath>
 #include <cstdint>
 
 namespace Inverter {
@@ -113,6 +114,38 @@ class FocControlManager {
     float lastIwA() const { return m_last_iw_a; }
     uint32_t missedCurrentSamples() const { return m_missed_current_samples; }
 
+    /**
+     * @brief Angle of the voltage vector FOC is currently applying [rad].
+     *
+     * Electrical Park angle + the dq voltage-vector phase; this is the angle
+     * a pattern modulator must phase-lock to for a seamless handoff.
+     */
+    float electricalVoltageAngleRad() const {
+        return m_controller.ElectricalAngle_Rad +
+               atan2f(m_controller.Vq_V, m_controller.Vd_V);
+    }
+
+    /**
+     * @brief Electrical speed magnitude [rad/s] from the controller estimate.
+     */
+    float electricalSpeedRadPerSec() const {
+        return m_controller.ElectricalSpeed_RadPerSec;
+    }
+
+    /**
+     * @brief Suspend FOC for a modulation handoff: control hook off, update
+     * ISR off, m_running cleared — but gate driver and PWM outputs stay live
+     * so a pattern modulator can take over the pins without a gap.
+     */
+    void suspendForHandoff();
+
+    /**
+     * @brief Restart FOC with the last setpoints (pattern -> FOC handoff).
+     * Runs the normal start sequence; safe into a spinning motor (the gate
+     * reset cycle briefly freewheels).
+     */
+    bool restartLastSetpoints();
+
     /**
      * @brief Per-cycle sample hook for measurement/calibration code.
      *

Images/Gen6FW/Inc/Inverter/Drivers/CAN/FdcanFault.h

@@ -5,8 +5,10 @@ namespace Inverter {
 /**
  * @brief Enable FDCAN error-status notifications and route them to FaultManager.
  *
- * Call after MX_FDCAN2_Init().
+ * Arms notifications only for buses enabled via KV (Can.A.En/Can.B.En); a
+ * disabled bus can never latch a CAN fault.  Call from CanBus::init() once
+ * the enables are known.
  */
-bool fdcanFaultInit();
+bool fdcanFaultInit(bool enable_bus_a, bool enable_bus_b);
 
 } // namespace Inverter

Images/Gen6FW/Inc/Inverter/Drivers/PWM/ModulationSwitch.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <cstdint>
+
+namespace Inverter {
+
+/**
+ * @brief Live modulator handoff primitives (ramp <-> pattern).
+ *
+ * These are the HAL-level switching operations behind both the 'handoff'
+ * shell command and the codegen-facing platform_api.  Phase-locked in both
+ * directions (angle capture + seed, level-matched pins); TIM1 owns the gate
+ * outputs throughout, so dead time, MOE and the BKIN trip stay armed.
+ */
+enum class ModulationMode : uint8_t { Ramp = 0, Pattern = 1 };
+
+ModulationMode modulationMode();
+
+/* Ramp (open-loop SPWM) -> N-pulse pattern at the same electrical frequency.
+ * Fails (returns false) if the ramp is not running or FOC is active. */
+bool modulationToPattern(uint32_t pulses_per_quarter, float duty);
+
+/* Pattern -> ramp, resuming at the pattern angle with the OL MI. */
+bool modulationToRamp();
+
+} // namespace Inverter

Images/Gen6FW/Inc/Inverter/Drivers/PWM/Modulator.h

@@ -0,0 +1,109 @@
+#pragma once
+
+/**
+ * @brief Swappable modulation strategy slot (multi-modulator architecture).
+ *
+ * A Modulator turns a voltage request (or its own internal reference, e.g.
+ * the open-loop SPWM ramp) into TIM1 output state.  It never touches GPIO or
+ * TIM1 configuration: the pwm.cpp driver owns all TIM1 registers, dead time,
+ * break/MOE and preload semantics.  On the STM32H723 the gate pins PE8-PE13
+ * are TIM1-only alternate functions, so TIM1 owns the gate outputs in every
+ * mode; future synchronous modulators (SHEPWM) will drive TIM1 forced-output
+ * modes from their own timebase instead of re-muxing pins.
+ *
+ * Two clocking styles exist:
+ *  - Externally clocked (SVPWM): the control loop calls update() + commit()
+ *    each control period.  runsInPwmIsr() == false.
+ *  - Self-clocked (SPWM): the TIM1 update ISR calls update() + commit().
+ *    runsInPwmIsr() == true.
+ *
+ * commit() writes CCR1..3 through the driver; ARPE + OC preload (enabled in
+ * MX_TIM1_Init) make the commit atomic at the next TIM1 update event.
+ */
+
+#include <cstdint>
+
+namespace Inverter {
+
+class Modulator {
+public:
+    virtual ~Modulator() = default;
+
+    /**
+     * @brief Compute this period's outputs.
+     *
+     * Voltage-driven modulators consume (valpha_v, vbeta_v, vdc_v);
+     * self-referenced ones (SPWM) ignore the arguments.
+     */
+    virtual void update(float valpha_v, float vbeta_v, float vdc_v) = 0;
+
+    /**
+     * @brief Write the outputs computed by update() to TIM1 via the driver.
+     */
+    virtual void commit() = 0;
+
+    /**
+     * @brief Take over the TIM1 outputs.
+     *
+     * theta_e_rad / modulation_index let synchronous modulators phase-lock;
+     * async modulators ignore them.  Timer/IRQ configuration (RCR, NVIC)
+     * stays with the driver call sites, not here.
+     */
+    virtual bool enter(float theta_e_rad, float modulation_index) = 0;
+
+    /**
+     * @brief Release the TIM1 outputs and reset internal pattern state.
+     */
+    virtual void exit() = 0;
+
+    /**
+     * @brief true if the TIM1 update ISR should drive update() + commit().
+     */
+    virtual bool runsInPwmIsr() const = 0;
+
+    virtual const char* name() const = 0;
+};
+
+/* The single modulation slot.  Step 1 of the multi-modulator plan: hard-wired
+ * by the existing call sites (FOC enable -> SVPWM, open-loop start -> SPWM);
+ * a future supervisor will arbitrate transitions with hysteresis and
+ * safe-point handoff. */
+Modulator* activeModulator();
+void setActiveModulator(Modulator* m);
+
+Modulator& svpwmModulator();
+Modulator& spwmModulator();
+Modulator& shepwmModulator();
+
+/* SPWM introspection / control for shell commands and calibrators
+ * (forwarded by the legacy PWM_* API in pwm.cpp). */
+bool spwmIsRunning();
+float spwmAngleRad();
+uint32_t spwmElectricalCycles();
+void spwmResetElectricalCycles();
+void spwmSetParams(float fundamental_freq_hz, float modulation_index);
+float spwmFundamentalFreqHz();
+float spwmModulationIndex();
+/* Seed the SPWM ramp angle (rad, phase-U-peak-at-+90deg convention).
+ * Used by live handoff to resume the ramp at the pattern's angle. */
+void spwmSetAngle(float angle_rad);
+
+/* SHEPWM control / introspection (shell bring-up; supervisor later).
+ * shepwmSetPattern builds the event table for (fe_hz, mi) into the inactive
+ * ping-pong buffer; while running it swaps at the next electrical-cycle wrap. */
+bool shepwmIsRunning();
+void shepwmSetPattern(float fe_hz, float mi);
+float shepwmFrequencyHz();
+float shepwmModulationIndex();
+uint32_t shepwmWrapCount();
+uint32_t shepwmEdgeCount();
+/* Runtime N-pulse mode (no tables): npq cells per quarter cycle, each HIGH
+ * for the middle `duty` fraction (centered notch of width 1-duty). */
+void shepwmSetPulsePattern(float fe_hz, uint32_t npq, float duty);
+uint32_t shepwmPulseCount();   /* 0 = SHE table mode */
+float shepwmDuty();
+/* Current pattern angle, ramp/FOC convention (rad; 0 while stopped).
+ * Derived from TIM5 CNT + the SHE-frame offset; used for live handoff. */
+float shepwmAngleRad();
+
+} // namespace Inverter

Images/Gen6FW/Inc/Inverter/Drivers/PWM/SheTables.h

@@ -0,0 +1,48 @@
+#pragma once
+
+/* Generated by Tools/she_table_gen.py -- do not edit.
+ *
+ * SHEPWM switching-angle family: 9 angles per quarter cycle,
+ * MI grid 0.50..0.90 step 0.02
+ * (per-unit of six-step fundamental; 1.0 = square wave).
+ * Angles in radians, phase-U quarter-wave; V/W derived by 120/240 deg shifts
+ * and quarter/half-wave symmetry at runtime.
+ *
+ * MI definition: 1.0 = six-step (phase fundamental peak 2*Vdc/pi).
+ * SVPWM linear max (Vdc/sqrt(3)) corresponds to MI ~ 0.907 on this scale.
+ */
+
+#include <cstdint>
+
+namespace Inverter::shetab {
+
+inline constexpr uint32_t kAnglesPerQuarter = 9;
+inline constexpr uint32_t kMiCount = 21;
+inline constexpr float kMiMin = 0.500000f;
+inline constexpr float kMiStep = 0.020000f;
+
+inline constexpr float kAngles[kMiCount][kAnglesPerQuarter] = {
+    { 0.1397518829f, 0.1718009103f, 0.2559823727f, 0.3775047358f, 0.6822995578f, 0.7817425391f, 1.1049832565f, 1.1998978524f, 1.5167039812f },  // mi = 0.50
+    { 0.1380350977f, 0.1718900186f, 0.2582415134f, 0.3760087502f, 0.6843809809f, 0.7794378823f, 1.1073220779f, 1.1976756100f, 1.5187378443f },  // mi = 0.52
+    { 0.1363127274f, 0.1719904630f, 0.2605175475f, 0.3745338810f, 0.6864517337f, 0.7771231662f, 1.1096667465f, 1.1954592448f, 1.5207715168f },  // mi = 0.54
+    { 0.1345852723f, 0.1721016605f, 0.2628111223f, 0.3730817190f, 0.6885104612f, 0.7747970095f, 1.1120182078f, 1.1932495526f, 1.5228049228f },  // mi = 0.56
+    { 0.1328531725f, 0.1722230039f, 0.2651230869f, 0.3716540189f, 0.6905555112f, 0.7724577386f, 1.1143776138f, 1.1910475350f, 1.5248379889f },  // mi = 0.58
+    { 0.1311168152f, 0.1723538611f, 0.2674545094f, 0.3702527268f, 0.6925848519f, 0.7701033056f, 1.1167463884f, 1.1888544642f, 1.5268706443f },  // mi = 0.60
+    { 0.1293765417f, 0.1724935750f, 0.2698067007f, 0.3688800116f, 0.6945959582f, 0.7677311759f, 1.1191263184f, 1.1866719727f, 1.5289028213f },  // mi = 0.62
+    { 0.1276326520f, 0.1726414640f, 0.2721812468f, 0.3675383055f, 0.6965856562f, 0.7653381736f, 1.1215196838f, 1.1845021835f, 1.5309344564f },  // mi = 0.64
+    { 0.1258854098f, 0.1727968229f, 0.2745800516f, 0.3662303529f, 0.6985499018f, 0.7629202629f, 1.1239294500f, 1.1823479008f, 1.5329654913f },  // mi = 0.66
+    { 0.1241350462f, 0.1729589254f, 0.2770053927f, 0.3649592720f, 0.7004834638f, 0.7604722339f, 1.1263595563f, 1.1802128971f, 1.5349958744f },  // mi = 0.68
+    { 0.1223817642f, 0.1731270283f, 0.2794599955f, 0.3637286348f, 0.7023794549f, 0.7579872380f, 1.1288153650f, 1.1781023611f, 1.5370255643f },  // mi = 0.70
+    { 0.1206257418f, 0.1733003799f, 0.2819471320f, 0.3625425693f, 0.7042286218f, 0.7554560838f, 1.1313043857f, 1.1760236194f, 1.5390545335f },  // mi = 0.72
+    { 0.1188671384f, 0.1734782338f, 0.2844707566f, 0.3614058979f, 0.7060182323f, 0.7528661352f, 1.1338374934f, 1.1739873517f, 1.5410827756f },  // mi = 0.74
+    { 0.1171061009f, 0.1736598742f, 0.2870356966f, 0.3603243279f, 0.7077302680f, 0.7501995182f, 1.1364310865f, 1.1720097461f, 1.5431103166f },  // mi = 0.76
+    { 0.1153427755f, 0.1738446610f, 0.2896479337f, 0.3593047271f, 0.7093383518f, 0.7474300735f, 1.1391111592f, 1.1701165671f, 1.5451372347f },  // mi = 0.78
+    { 0.1135773253f, 0.1740321115f, 0.2923150474f, 0.3583555473f, 0.7108022335f, 0.7445178829f, 1.1419216226f, 1.1683514752f, 1.5471636975f },  // mi = 0.80
+    { 0.1118099562f, 0.1742220553f, 0.2950469802f, 0.3574875408f, 0.7120571915f, 0.7413987404f, 1.1449431692f, 1.1667949024f, 1.5491900360f },  // mi = 0.82
+    { 0.1100409402f, 0.1744149343f, 0.2978575580f, 0.3567151454f, 0.7129917445f, 0.7379619777f, 1.1483425632f, 1.1656134108f, 1.5512169149f },  // mi = 0.84
+    { 0.1082705165f, 0.1746123763f, 0.3007682338f, 0.3560598052f, 0.7133943648f, 0.7339973140f, 1.1525313126f, 1.1652187276f, 1.5532457954f },  // mi = 0.86
+    { 0.1064976034f, 0.1748179453f, 0.3038215253f, 0.3555615346f, 0.7127952020f, 0.7290360727f, 1.1588885554f, 1.1669930036f, 1.5552806806f },  // mi = 0.88
+    { 0.1047008049f, 0.1750293912f, 0.3072037553f, 0.3553797554f, 0.7095982197f, 0.7214649755f, 1.1769627291f, 1.1805247564f, 1.5573433495f },  // mi = 0.90
+};
+
+} // namespace Inverter::shetab