diff --git a/Assets/Examples/foc_demo.json b/Assets/Examples/foc_demo.json index 91d737ea..427225cd 100644 --- a/Assets/Examples/foc_demo.json +++ b/Assets/Examples/foc_demo.json @@ -1,3541 +1,3695 @@ { - "bridges": [ - { - "consumer": { - "nodeId": "Clarke", - "portName": "I_A" - }, - "id": "i_a_bridge", - "producer": { - "nodeId": "Currents", - "portName": "I_A" - }, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "consumer": { - "nodeId": "Clarke", - "portName": "I_B" - }, - "id": "i_b_bridge", - "producer": { - "nodeId": "Currents", - "portName": "I_B" - }, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "consumer": { - "nodeId": "Clarke", - "portName": "I_C" - }, - "id": "i_c_bridge", - "producer": { - "nodeId": "Currents", - "portName": "I_C" - }, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "consumer": { - "nodeId": "LogU", - "portName": "Value" - }, - "id": "i_a_app_bridge", - "producer": { - "nodeId": "Currents", - "portName": "I_A" - }, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "consumer": { - "nodeId": "LogV", - "portName": "Value" - }, - "id": "i_b_app_bridge", - "producer": { - "nodeId": "Currents", - "portName": "I_B" - }, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "consumer": { - "nodeId": "LogW", - "portName": "Value" - }, - "id": "i_c_app_bridge", - "producer": { - "nodeId": "Currents", - "portName": "I_C" - }, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "connections": [ - { - "from": { - "nodeId": "Encoder", - "portName": "Theta" - }, - "id": "c1", - "to": { - "nodeId": "ElecAngle", - "portName": "ThetaMech" - } - }, - { - "from": { - "nodeId": "ElecAngle", - "portName": "ThetaElec" - }, - "id": "c2", - "to": { - "nodeId": "Park", - "portName": "Theta" - } - }, - { - "from": { - "nodeId": "ElecAngle", - "portName": "ThetaElec" - }, - "id": "c3", - "to": { - "nodeId": "InvPark", - "portName": "Theta" - } - }, - { - "from": { - "nodeId": "Clarke", - "portName": "I_Alpha" - }, - "id": "c4", - "to": { - "nodeId": "Park", - "portName": "I_Alpha" - } - }, - { - "from": { - "nodeId": "IdVar", - "portName": "Value" - }, - "id": "c6", - "to": { - "nodeId": "PiD", - "portName": "Setpoint" - } - }, - { - "from": { - "nodeId": "Park", - "portName": "I_D" - }, - "id": "c7", - "to": { - "nodeId": "PiD", - "portName": "Measurement" - } - }, - { - "from": { - "nodeId": "Park", - "portName": "I_Q" - }, - "id": "c9", - "to": { - "nodeId": "PiQ", - "portName": "Measurement" - } - }, - { - "from": { - "nodeId": "PiD", - "portName": "Output" - }, - "id": "c10", - "to": { - "nodeId": "InvPark", - "portName": "V_D" - } - }, - { - "from": { - "nodeId": "PiQ", - "portName": "Output" - }, - "id": "c11", - "to": { - "nodeId": "InvPark", - "portName": "V_Q" - } - }, - { - "from": { - "nodeId": "InvPark", - "portName": "V_Alpha" - }, - "id": "c13", - "to": { - "nodeId": "Svpwm", - "portName": "V_Alpha" - } - }, - { - "from": { - "nodeId": "VdcSense", - "portName": "V_Dc" - }, - "id": "c14", - "to": { - "nodeId": "Svpwm", - "portName": "V_Dc" - } - }, - { - "from": { - "nodeId": "Svpwm", - "portName": "Duty_A" - }, - "id": "c15", - "to": { - "nodeId": "PwmOut", - "portName": "Duty_A" - } - }, - { - "from": { - "nodeId": "Svpwm", - "portName": "Duty_B" - }, - "id": "c16", - "to": { - "nodeId": "PwmOut", - "portName": "Duty_B" - } - }, - { - "from": { - "nodeId": "Svpwm", - "portName": "Duty_C" - }, - "id": "c17", - "to": { - "nodeId": "PwmOut", - "portName": "Duty_C" - } - }, - { - "from": { - "nodeId": "EncoderApp", - "portName": "Theta" - }, - "id": "c24", - "to": { - "nodeId": "LogTheta", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Svpwm", - "portName": "Duty_A" - }, - "id": "c35", - "to": { - "nodeId": "LogDu", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Svpwm", - "portName": "Duty_B" - }, - "id": "c36", - "to": { - "nodeId": "LogDv", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Svpwm", - "portName": "Duty_C" - }, - "id": "c37", - "to": { - "nodeId": "LogDw", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Clarke", - "portName": "I_Beta" - }, - "id": "c38", - "to": { - "nodeId": "Park", - "portName": "I_Beta" - } - }, - { - "from": { - "nodeId": "InvPark", - "portName": "V_Beta" - }, - "id": "c39", - "to": { - "nodeId": "Svpwm", - "portName": "V_Beta" - } - }, - { - "from": { - "nodeId": "CfgEncSign", - "portName": "Value" - }, - "id": "c40", - "to": { - "nodeId": "ElecAngle", - "portName": "EncoderSign" - } - }, - { - "from": { - "nodeId": "CfgEncOffset", - "portName": "Value" - }, - "id": "c41", - "to": { - "nodeId": "ElecAngle", - "portName": "OffsetDeg" - } - }, - { - "from": { - "nodeId": "CfgPoles", - "portName": "Value" - }, - "id": "c42", - "to": { - "nodeId": "ElecAngle", - "portName": "Poles" - } - }, - { - "from": { - "nodeId": "Park", - "portName": "I_D" - }, - "id": "c43", - "to": { - "nodeId": "LogId", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Park", - "portName": "I_Q" - }, - "id": "c44", - "to": { - "nodeId": "LogIq", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "PiD", - "portName": "Output" - }, - "id": "c45", - "to": { - "nodeId": "LogVd", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "PiQ", - "portName": "Output" - }, - "id": "c46", - "to": { - "nodeId": "LogVq", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "VdcApp", - "portName": "V_Dc" - }, - "id": "c47", - "to": { - "nodeId": "LogVdc", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "VoltSense", - "portName": "V_U" - }, - "id": "c48", - "to": { - "nodeId": "LogVu", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "VoltSense", - "portName": "V_V" - }, - "id": "c49", - "to": { - "nodeId": "LogVv", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "VoltSense", - "portName": "V_W" - }, - "id": "c50", - "to": { - "nodeId": "LogVw", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "CfgKpQ", - "portName": "Value" - }, - "id": "c51", - "to": { - "nodeId": "PiQ", - "portName": "Kp" - } - }, - { - "from": { - "nodeId": "CfgEnable", - "portName": "Value" - }, - "id": "c52", - "to": { - "nodeId": "EnableCmp", - "portName": "A" - } - }, - { - "from": { - "nodeId": "EnableCmp", - "portName": "Out" - }, - "id": "c53", - "to": { - "nodeId": "ThrottleEnable", - "portName": "In" - } - }, - { - "from": { - "nodeId": "ThrottleEnable", - "portName": "Value" - }, - "id": "c54", - "to": { - "nodeId": "IqGate", - "portName": "Sel" - } - }, - { - "from": { - "nodeId": "IqGate", - "portName": "Out" - }, - "id": "c55", - "to": { - "nodeId": "PiQ", - "portName": "Setpoint" - } - }, - { - "from": { - "nodeId": "IqVar", - "portName": "Value" - }, - "id": "c56", - "to": { - "nodeId": "SlewIq", - "portName": "In" - } - }, - { - "from": { - "nodeId": "Din1", - "portName": "Out" - }, - "id": "c_din1_led", - "to": { - "nodeId": "LedGreen", - "portName": "In" - } - }, - { - "from": { - "nodeId": "Temps", - "portName": "T_Motor" - }, - "id": "c_t_motor", - "to": { - "nodeId": "LogT_Motor", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Temps", - "portName": "T_Inv1" - }, - "id": "c_t_inv1", - "to": { - "nodeId": "LogT_Inv1", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Temps", - "portName": "T_Inv2" - }, - "id": "c_t_inv2", - "to": { - "nodeId": "LogT_Inv2", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Temps", - "portName": "T_Inv3" - }, - "id": "c_t_inv3", - "to": { - "nodeId": "LogT_Inv3", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Throttle", - "portName": "A" - }, - "id": "c_thrA", - "to": { - "nodeId": "LogThrA", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "Throttle", - "portName": "B" - }, - "id": "c_thrB", - "to": { - "nodeId": "LogThrB", - "portName": "Value" - } - }, - { - "from": { - "nodeId": "SlewIq", - "portName": "Out" - }, - "id": "c_slew_iq_out", - "to": { - "nodeId": "IqGate", - "portName": "Val" - } - }, - { - "from": { - "nodeId": "CfgIqSlew", - "portName": "Value" - }, - "id": "c_slew_rate", - "to": { - "nodeId": "SlewIq", - "portName": "Rate" - } - }, - { - "from": { - "nodeId": "CfgKpD", - "portName": "Value" - }, - "id": "c_kp_d", - "to": { - "nodeId": "PiD", - "portName": "Kp" - } - }, - { - "from": { - "nodeId": "CfgKiD", - "portName": "Value" - }, - "id": "c_ki_d", - "to": { - "nodeId": "PiD", - "portName": "Ki" - } - }, - { - "from": { - "nodeId": "CfgKiQ", - "portName": "Value" - }, - "id": "c_ki_q", - "to": { - "nodeId": "PiQ", - "portName": "Ki" - } - }, - { - "from": { - "nodeId": "CanRx1", - "portName": "D0" - }, - "id": "c_can_d0", - "to": { - "nodeId": "LogCanD0", - "portName": "Value" - } - } - ], - "name": "foc_demo", - "nodeTypes": [ - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Const", - "description": "Provides a constant scalar value, optionally replaced by a wired input.", - "displayName": "Constant", - "domain": "", - "id": "Values.Constant", - "inlineCode": "/* Compile-time constant (or wired value via optional In). */\nOut = Value;\n", - "inputPorts": [ - { - "description": "Optional value that replaces the configured constant when wired.", - "direction": "input", - "name": "In", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Configured constant or the value supplied through In.", - "direction": "output", - "name": "Out", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "parameterTypes": { - "In": { - "description": "Fallback storage for the optional In port.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Value": { - "description": "Constant value emitted when no input overrides it.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Log", - "description": "Publishes a scalar value to runtime telemetry under a configurable key.", - "displayName": "Telemetry Log", - "domain": "", - "id": "Debug.TelemetryLog", - "inlineCode": "platform_telemetry_log_f32(Key, Value);\n", - "inputPorts": [ - { - "description": "Scalar value to publish.", - "direction": "input", - "name": "Value", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [], - "parameterTypes": { - "Key": { - "description": "Telemetry channel name used when publishing the value.", - "dtype": "f32", - "frame": "scalar", - "quantity": "string" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "PwmOut", - "description": "Applies three phase-duty commands to the platform PWM peripheral.", - "displayName": "PWM Out", - "domain": "tim_isr", - "id": "Actuators.PwmOut", - "inlineCode": "platform_pwm_set(Duty_A, Duty_B, Duty_C);\n", - "inputPorts": [ - { - "description": "Phase A PWM duty command in percent.", - "direction": "input", - "name": "Duty_A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Phase B PWM duty command in percent.", - "direction": "input", - "name": "Duty_B", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Phase C PWM duty command in percent.", - "direction": "input", - "name": "Duty_C", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 1, - "outputPorts": [] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "CanRx", - "description": "Reads the latest frame from a configured CAN receive mailbox and exposes its payload.", - "displayName": "CAN RX", - "domain": "", - "id": "Sensors.CanRx", - "inlineCode": "/* CAN receive mailbox for Id on Bus (1=A/FDCAN1, 2=B/FDCAN2).\n * Fresh is true for one step when a new frame arrived; Dlc/bytes hold the\n * latest payload otherwise. */\nuint8_t buf[8] = {};\nuint32_t seq = 0;\nconst int dlc = platform_can_rx(static_cast(Bus),\n static_cast(Id), buf, &seq);\nFresh = (seq != LastSeq);\nLastSeq = static_cast(seq);\nif (dlc > 0) {\n Dlc = static_cast(dlc);\n D0 = buf[0]; D1 = buf[1]; D2 = buf[2]; D3 = buf[3];\n D4 = buf[4]; D5 = buf[5]; D6 = buf[6]; D7 = buf[7];\n} else {\n Dlc = 0.0f;\n}\n", - "inputPorts": [], - "isEntryPoint": true, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Most recently received payload byte 0.", - "direction": "output", - "name": "D0", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Most recently received payload byte 1.", - "direction": "output", - "name": "D1", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Most recently received payload byte 2.", - "direction": "output", - "name": "D2", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Most recently received payload byte 3.", - "direction": "output", - "name": "D3", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Most recently received payload byte 4.", - "direction": "output", - "name": "D4", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Most recently received payload byte 5.", - "direction": "output", - "name": "D5", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Most recently received payload byte 6.", - "direction": "output", - "name": "D6", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Most recently received payload byte 7.", - "direction": "output", - "name": "D7", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Data length of the latest received frame.", - "direction": "output", - "name": "Dlc", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "True for one execution step when a new frame has arrived.", - "direction": "output", - "name": "Fresh", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - ], - "parameterTypes": { - "Bus": { - "description": "CAN controller number: 1 selects bus A/FDCAN1 and 2 selects bus B/FDCAN2.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Id": { - "description": "CAN arbitration identifier accepted by the receive mailbox.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "LastSeq": { - "description": "Internal sequence counter used to detect newly received frames.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "PhaseVoltages", - "description": "Reads the filtered three-phase and DC-link voltage sensor channels.", - "displayName": "Phase Voltages", - "domain": "", - "id": "Sensors.PhaseVoltages", - "inlineCode": "/* All voltage sense channels from the MAX22530 isolated ADC, filtered. */\nV_U = rte::Volts(platform_phase_voltage_u());\nV_V = rte::Volts(platform_phase_voltage_v());\nV_W = rte::Volts(platform_phase_voltage_w());\nV_Dc = rte::Volts(platform_get_dc_link_voltage());\n", - "inputPorts": [], - "isEntryPoint": false, - "maxInstances": 1, - "outputPorts": [ - { - "description": "Measured phase U voltage in volts.", - "direction": "output", - "name": "V_U", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Measured phase V voltage in volts.", - "direction": "output", - "name": "V_V", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Measured phase W voltage in volts.", - "direction": "output", - "name": "V_W", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Measured DC-link voltage in volts.", - "direction": "output", - "name": "V_Dc", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "InvPark", - "description": "Rotates d-q voltage commands into stationary alpha-beta coordinates.", - "displayName": "Inverse Park Transform", - "domain": "", - "id": "Transforms.InversePark", - "inlineCode": "const float cos_theta = cosf(Theta);\nconst float sin_theta = sinf(Theta);\nV_Alpha = V_D * cos_theta - V_Q * sin_theta;\nV_Beta = V_D * sin_theta + V_Q * cos_theta;\n", - "inputPorts": [ - { - "description": "Rotating-frame direct-axis voltage.", - "direction": "input", - "name": "V_D", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Rotating-frame quadrature-axis voltage.", - "direction": "input", - "name": "V_Q", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Electrical rotation angle in radians.", - "direction": "input", - "name": "Theta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Stationary-frame alpha-axis voltage.", - "direction": "output", - "name": "V_Alpha", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Stationary-frame beta-axis voltage.", - "direction": "output", - "name": "V_Beta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Din", - "description": "Reads a boolean level from one of the platform user digital inputs.", - "displayName": "Digital In", - "domain": "", - "id": "Sensors.DigitalIn", - "inlineCode": "/* User digital input. Pin 1..8 -> USER_DIN_1..8 (see platform_api.h). */\nOut = platform_digital_read(static_cast(Pin));\n", - "inputPorts": [], - "isEntryPoint": true, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Current logic level of the selected digital input.", - "direction": "output", - "name": "Out", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - ], - "parameterTypes": { - "Pin": { - "description": "User digital input selector in the range 1 through 8.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Dout", - "description": "Writes a boolean signal to a user digital output or onboard status LED.", - "displayName": "Digital Out", - "domain": "", - "id": "Actuators.DigitalOut", - "inlineCode": "/* User digital output. Pin 1..4 -> USER_DOUT_1..4, 5 -> green LED,\n * 6 -> orange LED (see platform_api.h). */\nplatform_digital_write(static_cast(Pin), In);\n", - "inputPorts": [ - { - "description": "Logic level to write to the selected output.", - "direction": "input", - "name": "In", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [], - "parameterTypes": { - "Pin": { - "description": "Output selector: 1-4 are user digital outputs, 5 is the green LED, and 6 is the orange LED.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "ForcedAngle", - "description": "Generates a continuously advancing electrical angle for open-loop control.", - "displayName": "Forced Angle", - "domain": "", - "id": "Transforms.ForcedAngle", - "inlineCode": "Angle += RateRadPerS * Dt;\nif (Angle >= 6.28318530718f) Angle -= 6.28318530718f;\nThetaElec = Angle;\n", - "inputPorts": [], - "isEntryPoint": false, - "maxInstances": 1, - "outputPorts": [ - { - "description": "Generated electrical angle wrapped to one revolution.", - "direction": "output", - "name": "ThetaElec", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "parameterTypes": { - "Angle": { - "description": "Persistent generated angle state in radians.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Dt": { - "description": "Update period in seconds.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "RateRadPerS": { - "description": "Electrical angular velocity in radians per second.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Pi", - "description": "Proportional-integral controller with output clamping and back-calculation anti-windup.", - "displayName": "PI Controller", - "domain": "", - "id": "Control.Pi", - "inlineCode": "/* PI with clamping + back-calculation anti-windup.\n * Dimensionless throughout: implicit unit extraction/injection handles any\n * physical-quantity wiring at the binding sites. */\nconst float error = Setpoint - Measurement;\nIntegral += error * Dt;\n\nfloat raw_output = Kp * error + Ki * Integral;\n\n/* Dynamic limit derived from DC-link voltage, matching the base image's\n * VectorPIController convention: max = Vdc/sqrt(3) * 0.95. */\nconst float vdc = platform_get_dc_link_voltage();\nconst float dynamic_max = (vdc / 1.7320508075688772f) * 0.95f;\nconst float max_limit = (dynamic_max < OutputMax) ? dynamic_max : OutputMax;\nconst float min_limit = (-dynamic_max > OutputMin) ? -dynamic_max : OutputMin;\n\nfloat limited_output = raw_output;\nif (limited_output > max_limit) limited_output = max_limit;\nif (limited_output < min_limit) limited_output = min_limit;\n\n/* Back-calculation anti-windup, scaled by AwGain (0 disables; 1.0 matches\n * the base-image VectorPIController). */\nif (Ki > 0.0001f && Kp > 0.0001f && AwGain > 0.0f) {\n const float excess = raw_output - limited_output;\n Integral -= excess * Dt * AwGain / (Kp * Ki);\n}\n\nOutput = limited_output;\n", - "inputPorts": [ - { - "description": "Desired target value.", - "direction": "input", - "name": "Setpoint", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Measured process value subtracted from the setpoint.", - "direction": "input", - "name": "Measurement", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Clamped PI control effort.", - "direction": "output", - "name": "Output", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "parameterTypes": { - "AwGain": { - "description": "Back-calculation anti-windup gain. Zero disables anti-windup.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Dt": { - "description": "Controller step period in seconds.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Integral": { - "description": "Persistent accumulated-error state.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Ki": { - "description": "Integral gain applied to the accumulated error.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Kp": { - "description": "Proportional gain applied to the current error.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "OutputMax": { - "description": "Upper output clamp, additionally limited by the measured DC-link voltage.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "OutputMin": { - "description": "Lower output clamp, additionally limited by the measured DC-link voltage.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Throttle", - "description": "Reads the redundant throttle channels and their plausibility status.", - "displayName": "Throttle", - "domain": "", - "id": "Sensors.Throttle", - "inlineCode": "/* Dual throttle channels, normalized [0..1] by the base-image driver\n * (KV calibration Hw.ThrA/B.MinV/MaxV). Both read 0 and Valid is false\n * while the channels disagree beyond the plausibility tolerance. */\nA = platform_get_throttle_a();\nB = platform_get_throttle_b();\nValid = platform_get_throttle_valid();\n", - "inputPorts": [], - "isEntryPoint": true, - "maxInstances": 1, - "outputPorts": [ - { - "description": "Normalized throttle channel A in the range 0 to 1.", - "direction": "output", - "name": "A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Normalized throttle channel B in the range 0 to 1.", - "direction": "output", - "name": "B", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "True when both throttle channels pass the plausibility check.", - "direction": "output", - "name": "Valid", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Temps", - "description": "Reads the motor and three inverter temperature channels from the application sensors.", - "displayName": "Temperatures", - "domain": "", - "id": "Sensors.Temperatures", - "inlineCode": "/* Motor + board temperatures from the base-image ApplicationSensors driver.\n * NAN while a channel is disabled or out of range (open/short). */\nT_Motor = rte::Celsius(platform_get_motor_temperature());\nT_Inv1 = rte::Celsius(platform_get_inverter_temperature(0));\nT_Inv2 = rte::Celsius(platform_get_inverter_temperature(1));\nT_Inv3 = rte::Celsius(platform_get_inverter_temperature(2));\n", - "inputPorts": [], - "isEntryPoint": false, - "maxInstances": 1, - "outputPorts": [ - { - "description": "Motor temperature in degrees Celsius; NaN indicates a disabled or invalid sensor.", - "direction": "output", - "name": "T_Motor", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "temperature" - } - }, - { - "description": "Inverter temperature channel 1 in degrees Celsius; NaN indicates an invalid sensor.", - "direction": "output", - "name": "T_Inv1", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "temperature" - } - }, - { - "description": "Inverter temperature channel 2 in degrees Celsius; NaN indicates an invalid sensor.", - "direction": "output", - "name": "T_Inv2", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "temperature" - } - }, - { - "description": "Inverter temperature channel 3 in degrees Celsius; NaN indicates an invalid sensor.", - "direction": "output", - "name": "T_Inv3", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "temperature" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Greater", - "description": "Compares two scalar values and reports whether A is greater than B.", - "displayName": "Greater Than", - "domain": "", - "id": "Logic.Greater", - "inlineCode": "Out = (A > B);\n", - "inputPorts": [ - { - "description": "Left-hand value in the comparison.", - "direction": "input", - "name": "A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Right-hand value in the comparison.", - "direction": "input", - "name": "B", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "True when A is greater than B.", - "direction": "output", - "name": "Out", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - ], - "parameterTypes": { - "B": { - "description": "Fallback comparison threshold used when the B input is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "VdcSense", - "description": "Reads the inverter DC-link voltage from the platform sensor interface.", - "displayName": "DC Link Voltage", - "domain": "", - "id": "Sensors.DcLinkVoltage", - "inlineCode": "V_Dc = rte::Volts(platform_get_dc_link_voltage());\n", - "inputPorts": [], - "isEntryPoint": true, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Measured DC-link voltage in volts.", - "direction": "output", - "name": "V_Dc", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Deriv", - "description": "Estimates a signal derivative with an optional first-order low-pass filter.", - "displayName": "Derivative", - "domain": "", - "id": "Transforms.Derivative", - "inlineCode": "/* Filtered derivative: Out ~= d(In)/dt, low-passed at Fc [Hz] to keep\n * sampling noise out of the estimate (Fc <= 0 disables filtering).\n * Prev/OutState are persistent state (leave at graph defaults). */\nconst float raw = (In - Prev) / Dt;\nPrev = In;\nif (Fc > 0.0f) {\n const float alpha = 1.0f / (1.0f + 1.0f / (6.28318530718f * Fc * Dt));\n OutState += alpha * (raw - OutState);\n} else {\n OutState = raw;\n}\nOut = OutState;\n", - "inputPorts": [ - { - "description": "Signal whose time derivative is estimated.", - "direction": "input", - "name": "In", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Filtered derivative estimate.", - "direction": "output", - "name": "Out", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "parameterTypes": { - "Dt": { - "description": "Sample period in seconds.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Fc": { - "description": "Low-pass cutoff frequency in hertz. Values at or below zero disable filtering.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "OutState": { - "description": "Persistent filtered derivative state.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Prev": { - "description": "Persistent previous input sample used by the finite difference.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Encoder", - "description": "Reads the latest mechanical rotor angle and motor speed from the encoder driver.", - "displayName": "Encoder", - "domain": "", - "id": "Sensors.Encoder", - "inlineCode": "/* Mechanical angle [rad] and speed [rpm] from the base-image encoder driver. */\nTheta = platform_get_encoder_angle_latest() * 0.01745329251f; // deg -> rad\nOmega = platform_get_motor_rpm();\n", - "inputPorts": [], - "isEntryPoint": true, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Mechanical rotor angle in radians.", - "direction": "output", - "name": "Theta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Mechanical motor speed in revolutions per minute.", - "direction": "output", - "name": "Omega", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Svpwm", - "description": "Converts an alpha-beta voltage vector into centered three-phase PWM duty commands.", - "displayName": "Space Vector PWM", - "domain": "", - "id": "Transforms.Svpwm", - "inlineCode": "/* Clamp the alpha/beta voltage vector to the linear SVM limit.\n * The maximum line-to-neutral voltage magnitude for linear modulation is\n * Vdc / sqrt(3). Use a small margin to stay away from the overmodulation\n * boundary. */\nconst float sqrt3 = 1.7320508075688772f;\nconst float v_max_linear = (V_Dc.in(au::volts) / sqrt3) * 0.95f;\nfloat valpha = V_Alpha.in(au::volts);\nfloat vbeta = V_Beta.in(au::volts);\nconst float v_albe_sq = valpha * valpha + vbeta * vbeta;\nif (v_albe_sq > v_max_linear * v_max_linear && v_albe_sq > 1e-12f) {\n const float scale = v_max_linear / sqrtf(v_albe_sq);\n valpha *= scale;\n vbeta *= scale;\n}\n\n/* Inverse Clarke: alpha/beta -> A/B/C. */\nconst float v_a = valpha / V_Dc.in(au::volts);\nconst float v_b = (-0.5f * valpha + 0.86602540378f * vbeta) / V_Dc.in(au::volts);\nconst float v_c = (-0.5f * valpha - 0.86602540378f * vbeta) / V_Dc.in(au::volts);\n\nfloat v_min = v_a;\nif (v_b < v_min) v_min = v_b;\nif (v_c < v_min) v_min = v_c;\n\nfloat v_max = v_a;\nif (v_b > v_max) v_max = v_b;\nif (v_c > v_max) v_max = v_c;\n\nconst float v_offset = 0.5f * (v_min + v_max);\n\n/* Convert to percent duty and clamp. Linear SVM stays roughly in\n * [21%, 79%]; clamping to [0,100] only catches numerical edge cases. */\nfloat duty_a_pct = 50.0f + 50.0f * (v_a - v_offset);\nfloat duty_b_pct = 50.0f + 50.0f * (v_b - v_offset);\nfloat duty_c_pct = 50.0f + 50.0f * (v_c - v_offset);\n\nif (duty_a_pct < 0.0f) duty_a_pct = 0.0f; else if (duty_a_pct > 100.0f) duty_a_pct = 100.0f;\nif (duty_b_pct < 0.0f) duty_b_pct = 0.0f; else if (duty_b_pct > 100.0f) duty_b_pct = 100.0f;\nif (duty_c_pct < 0.0f) duty_c_pct = 0.0f; else if (duty_c_pct > 100.0f) duty_c_pct = 100.0f;\n\nDuty_A = duty_a_pct;\nDuty_B = duty_b_pct;\nDuty_C = duty_c_pct;\n", - "inputPorts": [ - { - "description": "Requested stationary-frame alpha-axis voltage.", - "direction": "input", - "name": "V_Alpha", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Requested stationary-frame beta-axis voltage.", - "direction": "input", - "name": "V_Beta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Measured DC-link voltage used for normalization and modulation limiting.", - "direction": "input", - "name": "V_Dc", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Phase A PWM duty command in percent.", - "direction": "output", - "name": "Duty_A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Phase B PWM duty command in percent.", - "direction": "output", - "name": "Duty_B", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Phase C PWM duty command in percent.", - "direction": "output", - "name": "Duty_C", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Var", - "description": "Stores a scalar value and updates it when the Set input is true.", - "displayName": "Var", - "domain": "", - "id": "Values.Var", - "inlineCode": "if (Set) Stored = In;\nValue = Stored;\n", - "inputPorts": [ - { - "description": "New scalar value to store when Set is true.", - "direction": "input", - "name": "In", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Write enable. When true, copies In into the stored value.", - "direction": "input", - "name": "Set", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Current stored scalar value.", - "direction": "output", - "name": "Value", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "parameterTypes": { - "In": { - "description": "Fallback new value when the In input is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Set": { - "description": "Fallback write-enable value when the Set input is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - }, - "Stored": { - "description": "Persistent scalar storage state.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Gate", - "description": "Passes a value when enabled and outputs zero when disabled.", - "displayName": "Gate", - "domain": "", - "id": "Logic.Gate", - "inlineCode": "/* Pass Val when Sel is true, else 0. */\nOut = Sel ? Val : 0.0f;\n", - "inputPorts": [ - { - "description": "Enable signal. True passes Val; false selects zero.", - "direction": "input", - "name": "Sel", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - }, - { - "description": "Value passed through while Sel is true.", - "direction": "input", - "name": "Val", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Val when selected, otherwise zero.", - "direction": "output", - "name": "Out", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Var", - "description": "Stores a boolean value and updates it when the Set input is true.", - "displayName": "Var (Bool)", - "domain": "", - "id": "Values.VarBool", - "inlineCode": "if (Set) Stored = In;\nValue = Stored;\n", - "inputPorts": [ - { - "description": "New boolean value to store when Set is true.", - "direction": "input", - "name": "In", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - }, - { - "description": "Write enable. When true, copies In into the stored value.", - "direction": "input", - "name": "Set", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Current stored boolean value.", - "direction": "output", - "name": "Value", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - ], - "parameterTypes": { - "In": { - "description": "Fallback new boolean value when the In input is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - }, - "Set": { - "description": "Fallback write-enable value when the Set input is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - }, - "Stored": { - "description": "Persistent boolean storage state.", - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Slew", - "description": "Limits how quickly a signal can rise or fall over time.", - "displayName": "Slew Rate Limiter", - "domain": "", - "id": "Control.Slew", - "inlineCode": "/* Rate-limited reference follower: Out tracks In at max Rate units/sec.\n * On control start Value resets to the graph default, so references ramp\n * smoothly from zero instead of stepping. */\nconst float target = In;\nconst float step = Rate * Dt;\nfloat v = Value;\nif (target > v + step) {\n v += step;\n} else if (target < v - step) {\n v -= step;\n} else {\n v = target;\n}\nValue = v;\nOut = Value;\n", - "inputPorts": [ - { - "description": "Target value for the limiter to follow.", - "direction": "input", - "name": "In", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Rate-limited output value.", - "direction": "output", - "name": "Out", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "parameterTypes": { - "Dt": { - "description": "Update period in seconds.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Rate": { - "description": "Maximum absolute rate of change in units per second.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Value": { - "description": "Persistent current output state.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "ElecAngle", - "description": "Converts mechanical rotor angle to wrapped electrical angle using pole count, direction, and offset.", - "displayName": "Electrical Angle", - "domain": "", - "id": "Transforms.ElecAngle", - "inlineCode": "/* Wrap the mechanical encoder angle to [0, 2*pi). */\nconst float two_pi = 6.28318530718f;\nfloat theta = fmodf(ThetaMech, two_pi);\nif (theta < 0.0f) theta += two_pi;\n\n/* Electrical angle = offset (elec deg) + sign * encoder_angle * (Poles / 2).\n * Matches the base-image FocController convention. */\nconstexpr float DEG_TO_RAD = 0.01745329251f;\nfloat elec = OffsetDeg * DEG_TO_RAD + EncoderSign * theta * Poles * 0.5f;\n\n/* Wrap the electrical angle to [0, 2*pi). */\nelec = fmodf(elec, two_pi);\nif (elec < 0.0f) elec += two_pi;\n\nThetaElec = elec;\n", - "inputPorts": [ - { - "description": "Mechanical rotor angle in radians.", - "direction": "input", - "name": "ThetaMech", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Electrical-angle calibration offset in degrees.", - "direction": "input", - "name": "OffsetDeg", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Encoder direction multiplier, normally 1 or -1.", - "direction": "input", - "name": "EncoderSign", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Motor pole count used to convert mechanical to electrical angle.", - "direction": "input", - "name": "Poles", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 1, - "outputPorts": [ - { - "description": "Electrical rotor angle wrapped to the range 0 through 2π radians.", - "direction": "output", - "name": "ThetaElec", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "parameterTypes": { - "EncoderSign": { - "description": "Fallback encoder direction multiplier when the input is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "OffsetDeg": { - "description": "Fallback electrical-angle calibration offset in degrees when the input is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Poles": { - "description": "Fallback motor pole count when the input is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "PhaseCurrents", - "description": "Samples the three inverter phase-current channels from the platform current sensors.", - "displayName": "Phase Currents", - "domain": "", - "id": "Sensors.PhaseCurrents", - "inlineCode": "float iu_f = 0.0f;\nfloat iv_f = 0.0f;\nfloat iw_f = 0.0f;\nif (platform_get_phase_currents(&iu_f, &iv_f, &iw_f)) {\n /* Keep raw sensor polarity available for general telemetry, while FOC\n * graphs enable inversion for this hardware's current-sensor wiring. */\n const float polarity = InvertPolarity ? -1.0f : 1.0f;\n I_A = rte::Amperes(polarity * iu_f);\n I_B = rte::Amperes(polarity * iv_f);\n I_C = rte::Amperes(polarity * iw_f);\n}\n", - "inputPorts": [], - "isEntryPoint": true, - "maxInstances": 1, - "outputPorts": [ - { - "description": "Measured phase A current in amperes.", - "direction": "output", - "name": "I_A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Measured phase B current in amperes.", - "direction": "output", - "name": "I_B", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Measured phase C current in amperes.", - "direction": "output", - "name": "I_C", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - } - ], - "parameterTypes": { - "InvertPolarity": { - "description": "Negate all three phase currents to match the FOC sign convention on hardware with inverted current sensors.", - "quantity": "boolean", - "frame": "scalar", - "dtype": "f32" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "SinCos", - "description": "Computes the sine and cosine of an input angle.", - "displayName": "Sin/Cos", - "domain": "", - "id": "Transforms.SinCos", - "inlineCode": "SinTheta = sinf(Theta);\nCosTheta = cosf(Theta);\n", - "inputPorts": [ - { - "description": "Input angle in radians.", - "direction": "input", - "name": "Theta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Sine of the input angle.", - "direction": "output", - "name": "SinTheta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Cosine of the input angle.", - "direction": "output", - "name": "CosTheta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Park", - "description": "Rotates stationary alpha-beta currents into rotor-aligned d-q coordinates.", - "displayName": "Park Transform", - "domain": "", - "id": "Transforms.Park", - "inlineCode": "const float cos_theta = cosf(Theta);\nconst float sin_theta = sinf(Theta);\nI_D = I_Alpha * cos_theta + I_Beta * sin_theta;\nI_Q = -I_Alpha * sin_theta + I_Beta * cos_theta;\n", - "inputPorts": [ - { - "description": "Stationary-frame alpha-axis current.", - "direction": "input", - "name": "I_Alpha", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Stationary-frame beta-axis current.", - "direction": "input", - "name": "I_Beta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Electrical rotation angle in radians.", - "direction": "input", - "name": "Theta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Rotating-frame direct-axis current.", - "direction": "output", - "name": "I_D", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Rotating-frame quadrature-axis current.", - "direction": "output", - "name": "I_Q", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "LogI", - "description": "Publishes three phase-current signals to the standard current telemetry channels.", - "displayName": "Telemetry Current Sink", - "domain": "", - "id": "Debug.TelemetryCurrentSink", - "inlineCode": "platform_telemetry_log_f32(\"cg_iu_a\", I_A.in(au::amperes));\nplatform_telemetry_log_f32(\"cg_iv_a\", I_B.in(au::amperes));\nplatform_telemetry_log_f32(\"cg_iw_a\", I_C.in(au::amperes));\n", - "inputPorts": [ - { - "description": "Phase A current to publish in amperes.", - "direction": "input", - "name": "I_A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Phase B current to publish in amperes.", - "direction": "input", - "name": "I_B", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Phase C current to publish in amperes.", - "direction": "input", - "name": "I_C", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Clarke", - "description": "Transforms three-phase currents into stationary alpha-beta coordinates.", - "displayName": "Clarke Transform", - "domain": "", - "id": "Transforms.Clarke", - "inlineCode": "I_Alpha = I_A;\nI_Beta = (I_B - I_C) * 0.57735026919f;\n", - "inputPorts": [ - { - "description": "Phase A current.", - "direction": "input", - "name": "I_A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Phase B current.", - "direction": "input", - "name": "I_B", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Phase C current.", - "direction": "input", - "name": "I_C", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Stationary-frame alpha-axis current.", - "direction": "output", - "name": "I_Alpha", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - }, - { - "description": "Stationary-frame beta-axis current.", - "direction": "output", - "name": "I_Beta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "current" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Less", - "description": "Compares two scalar values and reports whether A is less than B.", - "displayName": "Less Than", - "domain": "", - "id": "Logic.Less", - "inlineCode": "Out = (A < B);\n", - "inputPorts": [ - { - "description": "Left-hand value in the comparison.", - "direction": "input", - "name": "A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Right-hand value in the comparison.", - "direction": "input", - "name": "B", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "True when A is less than B.", - "direction": "output", - "name": "Out", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - } - ], - "parameterTypes": { - "B": { - "description": "Fallback comparison threshold used when the B input is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "InvClarke", - "description": "Transforms stationary alpha-beta voltages into three-phase voltage commands.", - "displayName": "Inverse Clarke Transform", - "domain": "", - "id": "Transforms.InverseClarke", - "inlineCode": "V_A = V_Alpha;\nV_B = -0.5f * V_Alpha + 0.86602540378f * V_Beta;\nV_C = -0.5f * V_Alpha - 0.86602540378f * V_Beta;\n", - "inputPorts": [ - { - "description": "Stationary-frame alpha-axis voltage.", - "direction": "input", - "name": "V_Alpha", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Stationary-frame beta-axis voltage.", - "direction": "input", - "name": "V_Beta", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Reconstructed phase A voltage.", - "direction": "output", - "name": "V_A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Reconstructed phase B voltage.", - "direction": "output", - "name": "V_B", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - }, - { - "description": "Reconstructed phase C voltage.", - "direction": "output", - "name": "V_C", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "voltage" - } - } - ] - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "Cached = platform_config_load(Key, DefaultValue);\n", - "defaultName": "Cfg", - "description": "Exposes a named configuration value, using a default when no stored value is available.", - "displayName": "Config Value", - "domain": "", - "id": "Values.Config", - "inlineCode": "Value = Cached;\n", - "inputPorts": [], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Cached configuration value supplied to the graph.", - "direction": "output", - "name": "Value", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "parameterTypes": { - "Cached": { - "description": "Persistent cached configuration value exposed by the output.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "DefaultValue": { - "description": "Value used when the configuration key has not been stored.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Key": { - "description": "Name used to look up the value in platform configuration storage.", - "dtype": "f32", - "frame": "scalar", - "quantity": "string" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "CanTx", - "description": "Transmits up to eight data bytes on a selected CAN bus at a configurable maximum rate.", - "displayName": "CAN TX", - "domain": "", - "id": "Actuators.CanTx", - "inlineCode": "/* CAN transmit, rate-limited by wall clock: sends when 1000/Rate ms have\n * elapsed since the previous send (Rate <= 0 sends every step).\n * D0..D7 = payload bytes (unconnected = 0), Dlc = how many of the 8 to\n * send, Ext: 0 std / 1 ext. */\nconst uint32_t period_ms = (Rate > 0.0f) ? static_cast(1000.0f / Rate) : 0U;\nconst uint32_t now_ms = platform_millis();\nif (period_ms == 0U || (now_ms - static_cast(LastMs)) >= period_ms) {\n LastMs = static_cast(now_ms);\n const uint8_t b[8] = {\n static_cast(D0), static_cast(D1),\n static_cast(D2), static_cast(D3),\n static_cast(D4), static_cast(D5),\n static_cast(D6), static_cast(D7),\n };\n uint8_t n = static_cast(Dlc);\n if (n > 8) n = 8;\n platform_can_send(static_cast(Bus), static_cast(Id),\n Ext > 0.5f, b, n);\n}\n", - "inputPorts": [ - { - "description": "Payload byte 0. Values are converted to an unsigned 8-bit integer.", - "direction": "input", - "name": "D0", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Payload byte 1. Values are converted to an unsigned 8-bit integer.", - "direction": "input", - "name": "D1", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Payload byte 2. Values are converted to an unsigned 8-bit integer.", - "direction": "input", - "name": "D2", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Payload byte 3. Values are converted to an unsigned 8-bit integer.", - "direction": "input", - "name": "D3", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Payload byte 4. Values are converted to an unsigned 8-bit integer.", - "direction": "input", - "name": "D4", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Payload byte 5. Values are converted to an unsigned 8-bit integer.", - "direction": "input", - "name": "D5", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Payload byte 6. Values are converted to an unsigned 8-bit integer.", - "direction": "input", - "name": "D6", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Payload byte 7. Values are converted to an unsigned 8-bit integer.", - "direction": "input", - "name": "D7", - "optional": true, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [], - "parameterTypes": { - "Bus": { - "description": "CAN controller number: 1 selects bus A/FDCAN1 and 2 selects bus B/FDCAN2.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "D0": { - "description": "Fallback value for payload byte 0 when its input port is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "D1": { - "description": "Fallback value for payload byte 1 when its input port is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "D2": { - "description": "Fallback value for payload byte 2 when its input port is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "D3": { - "description": "Fallback value for payload byte 3 when its input port is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "D4": { - "description": "Fallback value for payload byte 4 when its input port is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "D5": { - "description": "Fallback value for payload byte 5 when its input port is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "D6": { - "description": "Fallback value for payload byte 6 when its input port is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "D7": { - "description": "Fallback value for payload byte 7 when its input port is not wired.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Dlc": { - "description": "Number of payload bytes to transmit, clamped to the range 0 through 8.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Ext": { - "description": "Use an extended 29-bit identifier when greater than 0.5; otherwise use a standard identifier.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Id": { - "description": "CAN arbitration identifier to transmit.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "LastMs": { - "description": "Internal timestamp of the previous transmission in milliseconds.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - }, - "Rate": { - "description": "Maximum transmission rate in frames per second. Values at or below zero send every step.", - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - }, - { - "classDefinition": "", - "classHeader": "", - "constructorCode": "", - "defaultName": "Mux", - "description": "Selects one of two scalar inputs using a boolean control signal.", - "displayName": "Mux", - "domain": "", - "id": "Logic.Mux", - "inlineCode": "Out = Sel ? A : B;\n", - "inputPorts": [ - { - "description": "Selection signal. True selects A; false selects B.", - "direction": "input", - "name": "Sel", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "boolean" - } - }, - { - "description": "Value selected when Sel is true.", - "direction": "input", - "name": "A", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - }, - { - "description": "Value selected when Sel is false.", - "direction": "input", - "name": "B", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ], - "isEntryPoint": false, - "maxInstances": 0, - "outputPorts": [ - { - "description": "Currently selected input value.", - "direction": "output", - "name": "Out", - "optional": false, - "type": { - "dtype": "f32", - "frame": "scalar", - "quantity": "dimensionless" - } - } - ] - } - ], - "nodes": [ - { - "displayName": "", - "domain": "tim_isr", - "id": "Clarke", - "parameters": {}, - "position": { - "x": 618.0, - "y": 2396.0 - }, - "type": "Transforms.Clarke" - }, - { - "displayName": "", - "domain": "adc_isr", - "id": "Currents", - "parameters": { - "OffsetU": "0.0", - "OffsetV": "0.0", - "InvertPolarity": "1.0" - }, - "position": { - "x": 150.0, - "y": 80.0 - }, - "type": "Sensors.PhaseCurrents" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "ElecAngle", - "parameters": { - "EncoderSign": "1.0", - "OffsetDeg": "6.445", - "Poles": "10.0" - }, - "position": { - "x": 618.0, - "y": 2577.0 - }, - "type": "Transforms.ElecAngle" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "Encoder", - "parameters": {}, - "position": { - "x": 150.0, - "y": 3302.0 - }, - "type": "Sensors.Encoder" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "EncoderApp", - "parameters": {}, - "position": { - "x": 522.0, - "y": 1498.0 - }, - "type": "Sensors.Encoder" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "IdRef", - "parameters": { - "Value": "0.0" - }, - "position": { - "x": 150.0, - "y": 2246.0 - }, - "type": "Values.Constant" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "InvPark", - "parameters": {}, - "position": { - "x": 1848.0, - "y": 2521.0 - }, - "type": "Transforms.InversePark" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "IqRef", - "parameters": { - "Value": "0.0" - }, - "position": { - "x": 150.0, - "y": 2396.0 - }, - "type": "Values.Constant" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "LogDu", - "parameters": { - "Key": "cg_du" - }, - "position": { - "x": 2473.0, - "y": 2246.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "LogDv", - "parameters": { - "Key": "cg_dv" - }, - "position": { - "x": 2473.0, - "y": 2396.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "LogDw", - "parameters": { - "Key": "cg_dw" - }, - "position": { - "x": 2473.0, - "y": 2546.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "LogId", - "parameters": { - "Key": "cg_id_a" - }, - "position": { - "x": 1236.0, - "y": 2624.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "LogIq", - "parameters": { - "Key": "cg_iq_a" - }, - "position": { - "x": 1236.0, - "y": 2774.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogTheta", - "parameters": { - "Key": "cg_theta_rad" - }, - "position": { - "x": 856.0, - "y": 380.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogU", - "parameters": { - "Key": "cg_iu_a" - }, - "position": { - "x": 522.0, - "y": 457.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogV", - "parameters": { - "Key": "cg_iv_a" - }, - "position": { - "x": 522.0, - "y": 607.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "LogVd", - "parameters": { - "Key": "cg_vd_v" - }, - "position": { - "x": 1542.0, - "y": 2559.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogVdc", - "parameters": { - "Key": "cg_vdc_v" - }, - "position": { - "x": 856.0, - "y": 1430.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "LogVq", - "parameters": { - "Key": "cg_vq_v" - }, - "position": { - "x": 1848.0, - "y": 2246.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogW", - "parameters": { - "Key": "cg_iw_a" - }, - "position": { - "x": 522.0, - "y": 757.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "Park", - "parameters": {}, - "position": { - "x": 931.0, - "y": 2435.0 - }, - "type": "Transforms.Park" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "PiD", - "parameterInputs": [ - "Kp", - "Ki" - ], - "parameters": { - "AwGain": "1.0", - "Dt": "0.0002", - "Integral": "0.0", - "Ki": "10.0", - "Kp": "0.03", - "OutputMax": "200.0", - "OutputMin": "-200.0" - }, - "position": { - "x": 1236.0, - "y": 3077.0 - }, - "type": "Control.Pi" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "PiQ", - "parameterInputs": [ - "Kp", - "Ki" - ], - "parameters": { - "AwGain": "1.0", - "Dt": "0.0002", - "Integral": "0.0", - "Ki": "10.0", - "Kp": "0.03", - "OutputMax": "200.0", - "OutputMin": "-200.0" - }, - "position": { - "x": 1542.0, - "y": 2246.0 - }, - "type": "Control.Pi" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "PwmOut", - "parameters": {}, - "position": { - "x": 2473.0, - "y": 2696.0 - }, - "type": "Actuators.PwmOut" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "Svpwm", - "parameters": {}, - "position": { - "x": 2177.0, - "y": 2246.0 - }, - "type": "Transforms.Svpwm" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "VdcApp", - "parameters": {}, - "position": { - "x": 522.0, - "y": 2041.0 - }, - "type": "Sensors.DcLinkVoltage" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "VdcSense", - "parameters": {}, - "position": { - "x": 1848.0, - "y": 2396.0 - }, - "type": "Sensors.DcLinkVoltage" - }, - { - "displayName": "CfgEncSign", - "domain": "tim_isr", - "id": "CfgEncSign", - "parameters": { - "Cached": "0.0", - "DefaultValue": "1.0", - "Key": "Motor.Encoder.SinCos.Sign" - }, - "position": { - "x": 150.0, - "y": 2924.0 - }, - "type": "Values.Config" - }, - { - "displayName": "CfgEncOffset", - "domain": "tim_isr", - "id": "CfgEncOffset", - "parameters": { - "Cached": "0.0", - "DefaultValue": "6.445", - "Key": "Motor.Encoder.SinCos.OffsetDeg" - }, - "position": { - "x": 150.0, - "y": 2735.0 - }, - "type": "Values.Config" - }, - { - "displayName": "CfgPoles", - "domain": "tim_isr", - "id": "CfgPoles", - "parameters": { - "Cached": "0.0", - "DefaultValue": "10.0", - "Key": "Motor.Poles" - }, - "position": { - "x": 150.0, - "y": 3113.0 - }, - "type": "Values.Config" - }, - { - "displayName": "VoltSense", - "domain": "vsense", - "id": "VoltSense", - "parameters": {}, - "position": { - "x": 2829.0, - "y": 2246.0 - }, - "type": "Sensors.PhaseVoltages" - }, - { - "displayName": "LogVu", - "domain": "vsense", - "id": "LogVu", - "parameters": { - "Key": "cg_vu_v" - }, - "position": { - "x": 3151.0, - "y": 2246.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "LogVv", - "domain": "vsense", - "id": "LogVv", - "parameters": { - "Key": "cg_vv_v" - }, - "position": { - "x": 3151.0, - "y": 2396.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "LogVw", - "domain": "vsense", - "id": "LogVw", - "parameters": { - "Key": "cg_vw_v" - }, - "position": { - "x": 3151.0, - "y": 2546.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "CfgKpQ", - "domain": "tim_isr", - "id": "CfgKpQ", - "parameters": { - "Cached": "0.0", - "DefaultValue": "0.16", - "Key": "Ctrl.PiQ.Kp" - }, - "position": { - "x": 1236.0, - "y": 2435.0 - }, - "type": "Values.Config" - }, - { - "displayName": "CfgEnable", - "domain": "tim_isr", - "id": "CfgEnable", - "parameters": { - "Cached": "0.0", - "DefaultValue": "1.0", - "Key": "Ctrl.ThrottleEnable" - }, - "position": { - "x": 150.0, - "y": 2546.0 - }, - "type": "Values.Config" - }, - { - "displayName": "EnableCmp", - "domain": "tim_isr", - "id": "EnableCmp", - "parameters": { - "B": "0.5" - }, - "position": { - "x": 618.0, - "y": 2246.0 - }, - "type": "Logic.Greater" - }, - { - "displayName": "ThrottleEnable", - "domain": "tim_isr", - "id": "ThrottleEnable", - "parameters": { - "In": "0.0", - "Set": "1.0", - "Stored": "1.0" - }, - "position": { - "x": 931.0, - "y": 2246.0 - }, - "type": "Values.VarBool" - }, - { - "displayName": "IqGate", - "domain": "tim_isr", - "id": "IqGate", - "parameters": {}, - "position": { - "x": 1236.0, - "y": 2924.0 - }, - "type": "Logic.Gate" - }, - { - "displayName": "IqVar", - "domain": "tim_isr", - "id": "IqVar", - "parameters": { - "In": "0.0", - "Set": "0.0", - "Stored": "0.0" - }, - "position": { - "x": 150.0, - "y": 3644.0 - }, - "type": "Values.Var" - }, - { - "displayName": "IdVar", - "domain": "tim_isr", - "id": "IdVar", - "parameters": { - "In": "0.0", - "Set": "0.0", - "Stored": "0.0" - }, - "position": { - "x": 931.0, - "y": 2994.0 - }, - "type": "Values.Var" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "Din1", - "parameters": { - "Pin": "1.0" - }, - "position": { - "x": 522.0, - "y": 1348.0 - }, - "type": "Sensors.DigitalIn" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LedGreen", - "parameters": { - "Pin": "5.0" - }, - "position": { - "x": 856.0, - "y": 230.0 - }, - "type": "Actuators.DigitalOut" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "Temps", - "parameters": {}, - "position": { - "x": 522.0, - "y": 1651.0 - }, - "type": "Sensors.Temperatures" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "Throttle", - "parameters": {}, - "position": { - "x": 522.0, - "y": 1860.0 - }, - "type": "Sensors.Throttle" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogT_Motor", - "parameters": { - "Key": "temp_motor_c" - }, - "position": { - "x": 856.0, - "y": 980.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogT_Inv1", - "parameters": { - "Key": "temp_inv1_c" - }, - "position": { - "x": 856.0, - "y": 530.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogT_Inv2", - "parameters": { - "Key": "temp_inv2_c" - }, - "position": { - "x": 856.0, - "y": 680.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogT_Inv3", - "parameters": { - "Key": "temp_inv3_c" - }, - "position": { - "x": 856.0, - "y": 830.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogThrA", - "parameters": { - "Key": "thr_a_norm" - }, - "position": { - "x": 856.0, - "y": 1130.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogThrB", - "parameters": { - "Key": "thr_b_norm" - }, - "position": { - "x": 856.0, - "y": 1280.0 - }, - "type": "Debug.TelemetryLog" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "SlewIq", - "parameterInputs": [ - "Rate" - ], - "parameters": { - "Dt": "0.0002", - "Rate": "50.0", - "Value": "0.0" - }, - "position": { - "x": 618.0, - "y": 2766.0 - }, - "type": "Control.Slew" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "CfgIqSlew", - "parameters": { - "Cached": "0.0", - "DefaultValue": "50.0", - "Key": "Ctrl.Iq.SlewAps" - }, - "position": { - "x": 150.0, - "y": 3455.0 - }, - "type": "Values.Config" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "CfgKpD", - "parameters": { - "Cached": "0.0", - "DefaultValue": "0.08", - "Key": "Ctrl.PiD.Kp" - }, - "position": { - "x": 931.0, - "y": 2805.0 - }, - "type": "Values.Config" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "CfgKiD", - "parameters": { - "Cached": "0.0", - "DefaultValue": "10.0", - "Key": "Ctrl.PiD.Ki" - }, - "position": { - "x": 931.0, - "y": 2616.0 - }, - "type": "Values.Config" - }, - { - "displayName": "", - "domain": "tim_isr", - "id": "CfgKiQ", - "parameters": { - "Cached": "0.0", - "DefaultValue": "10.0", - "Key": "Ctrl.PiQ.Ki" - }, - "position": { - "x": 1236.0, - "y": 2246.0 - }, - "type": "Values.Config" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "CanTx1", - "parameters": { - "Bus": "2.0", - "Id": "291.0", - "Ext": "0.0", - "Dlc": "3.0", - "Rate": "10.0", - "LastMs": "0.0", - "D0": "222.0", - "D1": "173.0", - "D2": "190.0", - "D3": "0.0", - "D4": "0.0", - "D5": "0.0", - "D6": "0.0", - "D7": "0.0" - }, - "position": { - "x": 0.0, - "y": 1550.0 - }, - "type": "Actuators.CanTx" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "CanRx1", - "parameters": { - "Bus": "2.0", - "Id": "801.0", - "LastSeq": "0.0" - }, - "position": { - "x": 0.0, - "y": 1650.0 - }, - "type": "Sensors.CanRx" - }, - { - "displayName": "", - "domain": "app_loop", - "id": "LogCanD0", - "parameters": { - "Key": "can_rx_d0" - }, - "position": { - "x": 300.0, - "y": 1650.0 - }, - "type": "Debug.TelemetryLog" - } - ] + "bridges": [ + { + "consumer": { + "nodeId": "Clarke", + "portName": "I_A" + }, + "id": "i_a_bridge", + "producer": { + "nodeId": "Currents", + "portName": "I_A" + }, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "consumer": { + "nodeId": "Clarke", + "portName": "I_B" + }, + "id": "i_b_bridge", + "producer": { + "nodeId": "Currents", + "portName": "I_B" + }, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "consumer": { + "nodeId": "Clarke", + "portName": "I_C" + }, + "id": "i_c_bridge", + "producer": { + "nodeId": "Currents", + "portName": "I_C" + }, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "consumer": { + "nodeId": "LogU", + "portName": "Value" + }, + "id": "i_a_app_bridge", + "producer": { + "nodeId": "Currents", + "portName": "I_A" + }, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "consumer": { + "nodeId": "LogV", + "portName": "Value" + }, + "id": "i_b_app_bridge", + "producer": { + "nodeId": "Currents", + "portName": "I_B" + }, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "consumer": { + "nodeId": "LogW", + "portName": "Value" + }, + "id": "i_c_app_bridge", + "producer": { + "nodeId": "Currents", + "portName": "I_C" + }, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "connections": [ + { + "from": { + "nodeId": "Encoder", + "portName": "Theta" + }, + "id": "c1", + "to": { + "nodeId": "ElecAngle", + "portName": "ThetaMech" + } + }, + { + "from": { + "nodeId": "ElecAngle", + "portName": "ThetaElec" + }, + "id": "c2", + "to": { + "nodeId": "Park", + "portName": "Theta" + } + }, + { + "from": { + "nodeId": "ElecAngle", + "portName": "ThetaElec" + }, + "id": "c3", + "to": { + "nodeId": "InvPark", + "portName": "Theta" + } + }, + { + "from": { + "nodeId": "Clarke", + "portName": "I_Alpha" + }, + "id": "c4", + "to": { + "nodeId": "Park", + "portName": "I_Alpha" + } + }, + { + "from": { + "nodeId": "IdVar", + "portName": "Value" + }, + "id": "c6", + "to": { + "nodeId": "PiD", + "portName": "Setpoint" + } + }, + { + "from": { + "nodeId": "Park", + "portName": "I_D" + }, + "id": "c7", + "to": { + "nodeId": "PiD", + "portName": "Measurement" + } + }, + { + "from": { + "nodeId": "Park", + "portName": "I_Q" + }, + "id": "c9", + "to": { + "nodeId": "PiQ", + "portName": "Measurement" + } + }, + { + "from": { + "nodeId": "PiD", + "portName": "Output" + }, + "id": "c10", + "to": { + "nodeId": "InvPark", + "portName": "V_D" + } + }, + { + "from": { + "nodeId": "PiQ", + "portName": "Output" + }, + "id": "c11", + "to": { + "nodeId": "InvPark", + "portName": "V_Q" + } + }, + { + "from": { + "nodeId": "InvPark", + "portName": "V_Alpha" + }, + "id": "c13", + "to": { + "nodeId": "Svpwm", + "portName": "V_Alpha" + } + }, + { + "from": { + "nodeId": "VdcSense", + "portName": "V_Dc" + }, + "id": "c14", + "to": { + "nodeId": "Svpwm", + "portName": "V_Dc" + } + }, + { + "from": { + "nodeId": "Svpwm", + "portName": "Duty_A" + }, + "id": "c15", + "to": { + "nodeId": "PwmOut", + "portName": "Duty_A" + } + }, + { + "from": { + "nodeId": "Svpwm", + "portName": "Duty_B" + }, + "id": "c16", + "to": { + "nodeId": "PwmOut", + "portName": "Duty_B" + } + }, + { + "from": { + "nodeId": "Svpwm", + "portName": "Duty_C" + }, + "id": "c17", + "to": { + "nodeId": "PwmOut", + "portName": "Duty_C" + } + }, + { + "from": { + "nodeId": "EncoderApp", + "portName": "Theta" + }, + "id": "c24", + "to": { + "nodeId": "LogTheta", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Svpwm", + "portName": "Duty_A" + }, + "id": "c35", + "to": { + "nodeId": "LogDu", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Svpwm", + "portName": "Duty_B" + }, + "id": "c36", + "to": { + "nodeId": "LogDv", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Svpwm", + "portName": "Duty_C" + }, + "id": "c37", + "to": { + "nodeId": "LogDw", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Clarke", + "portName": "I_Beta" + }, + "id": "c38", + "to": { + "nodeId": "Park", + "portName": "I_Beta" + } + }, + { + "from": { + "nodeId": "InvPark", + "portName": "V_Beta" + }, + "id": "c39", + "to": { + "nodeId": "Svpwm", + "portName": "V_Beta" + } + }, + { + "from": { + "nodeId": "CfgEncSign", + "portName": "Value" + }, + "id": "c40", + "to": { + "nodeId": "ElecAngle", + "portName": "EncoderSign" + } + }, + { + "from": { + "nodeId": "CfgEncOffset", + "portName": "Value" + }, + "id": "c41", + "to": { + "nodeId": "ElecAngle", + "portName": "OffsetDeg" + } + }, + { + "from": { + "nodeId": "CfgPoles", + "portName": "Value" + }, + "id": "c42", + "to": { + "nodeId": "ElecAngle", + "portName": "Poles" + } + }, + { + "from": { + "nodeId": "Park", + "portName": "I_D" + }, + "id": "c43", + "to": { + "nodeId": "LogId", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Park", + "portName": "I_Q" + }, + "id": "c44", + "to": { + "nodeId": "LogIq", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "PiD", + "portName": "Output" + }, + "id": "c45", + "to": { + "nodeId": "LogVd", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "PiQ", + "portName": "Output" + }, + "id": "c46", + "to": { + "nodeId": "LogVq", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "VdcApp", + "portName": "V_Dc" + }, + "id": "c47", + "to": { + "nodeId": "LogVdc", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "VoltSense", + "portName": "V_U" + }, + "id": "c48", + "to": { + "nodeId": "LogVu", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "VoltSense", + "portName": "V_V" + }, + "id": "c49", + "to": { + "nodeId": "LogVv", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "VoltSense", + "portName": "V_W" + }, + "id": "c50", + "to": { + "nodeId": "LogVw", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "CfgKpQ", + "portName": "Value" + }, + "id": "c51", + "to": { + "nodeId": "PiQ", + "portName": "Kp" + } + }, + { + "from": { + "nodeId": "CfgEnable", + "portName": "Value" + }, + "id": "c52", + "to": { + "nodeId": "EnableCmp", + "portName": "A" + } + }, + { + "from": { + "nodeId": "EnableCmp", + "portName": "Out" + }, + "id": "c53", + "to": { + "nodeId": "ThrottleEnable", + "portName": "In" + } + }, + { + "from": { + "nodeId": "ThrottleEnable", + "portName": "Value" + }, + "id": "c54", + "to": { + "nodeId": "IqGate", + "portName": "Sel" + } + }, + { + "from": { + "nodeId": "IqGate", + "portName": "Out" + }, + "id": "c55", + "to": { + "nodeId": "PiQ", + "portName": "Setpoint" + } + }, + { + "from": { + "nodeId": "IqVar", + "portName": "Value" + }, + "id": "c56", + "to": { + "nodeId": "SlewIq", + "portName": "In" + } + }, + { + "from": { + "nodeId": "Din1", + "portName": "Out" + }, + "id": "c_din1_led", + "to": { + "nodeId": "LedGreen", + "portName": "In" + } + }, + { + "from": { + "nodeId": "Temps", + "portName": "T_Motor" + }, + "id": "c_t_motor", + "to": { + "nodeId": "LogT_Motor", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Temps", + "portName": "T_Inv1" + }, + "id": "c_t_inv1", + "to": { + "nodeId": "LogT_Inv1", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Temps", + "portName": "T_Inv2" + }, + "id": "c_t_inv2", + "to": { + "nodeId": "LogT_Inv2", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Temps", + "portName": "T_Inv3" + }, + "id": "c_t_inv3", + "to": { + "nodeId": "LogT_Inv3", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Throttle", + "portName": "A" + }, + "id": "c_thrA", + "to": { + "nodeId": "LogThrA", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "Throttle", + "portName": "B" + }, + "id": "c_thrB", + "to": { + "nodeId": "LogThrB", + "portName": "Value" + } + }, + { + "from": { + "nodeId": "SlewIq", + "portName": "Out" + }, + "id": "c_slew_iq_out", + "to": { + "nodeId": "IqGate", + "portName": "Val" + } + }, + { + "from": { + "nodeId": "CfgIqSlew", + "portName": "Value" + }, + "id": "c_slew_rate", + "to": { + "nodeId": "SlewIq", + "portName": "Rate" + } + }, + { + "from": { + "nodeId": "CfgKpD", + "portName": "Value" + }, + "id": "c_kp_d", + "to": { + "nodeId": "PiD", + "portName": "Kp" + } + }, + { + "from": { + "nodeId": "CfgKiD", + "portName": "Value" + }, + "id": "c_ki_d", + "to": { + "nodeId": "PiD", + "portName": "Ki" + } + }, + { + "from": { + "nodeId": "CfgKiQ", + "portName": "Value" + }, + "id": "c_ki_q", + "to": { + "nodeId": "PiQ", + "portName": "Ki" + } + }, + { + "from": { + "nodeId": "CanRx1", + "portName": "D0" + }, + "id": "c_can_d0", + "to": { + "nodeId": "LogCanD0", + "portName": "Value" + } + } + ], + "name": "foc_demo", + "nodeTypes": [ + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Const", + "description": "Provides a constant scalar value, optionally replaced by a wired input.", + "displayName": "Constant", + "domain": "", + "id": "Values.Constant", + "inlineCode": "/* Compile-time constant (or wired value via optional In). */\nOut = Value;\n", + "inputPorts": [ + { + "description": "Optional value that replaces the configured constant when wired.", + "direction": "input", + "name": "In", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Configured constant or the value supplied through In.", + "direction": "output", + "name": "Out", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "parameterTypes": { + "In": { + "description": "Fallback storage for the optional In port.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Value": { + "description": "Constant value emitted when no input overrides it.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Log", + "description": "Publishes a scalar value to runtime telemetry under a configurable key.", + "displayName": "Telemetry Log", + "domain": "", + "id": "Debug.TelemetryLog", + "inlineCode": "platform_telemetry_log_f32(Key, Value);\n", + "inputPorts": [ + { + "description": "Scalar value to publish.", + "direction": "input", + "name": "Value", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [], + "parameterTypes": { + "Key": { + "description": "Telemetry channel name used when publishing the value.", + "dtype": "f32", + "frame": "scalar", + "quantity": "string" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "PwmOut", + "description": "Applies three phase-duty commands to the platform PWM peripheral.", + "displayName": "PWM Out", + "domain": "tim_isr", + "id": "Actuators.PwmOut", + "inlineCode": "platform_pwm_set(Duty_A, Duty_B, Duty_C);\n", + "inputPorts": [ + { + "description": "Phase A PWM duty command in percent.", + "direction": "input", + "name": "Duty_A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Phase B PWM duty command in percent.", + "direction": "input", + "name": "Duty_B", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Phase C PWM duty command in percent.", + "direction": "input", + "name": "Duty_C", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 1, + "outputPorts": [] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "CanRx", + "description": "Reads the latest frame from a configured CAN receive mailbox and exposes its payload.", + "displayName": "CAN RX", + "domain": "", + "id": "Sensors.CanRx", + "inlineCode": "/* CAN receive mailbox for Id on Bus (1=A/FDCAN1, 2=B/FDCAN2).\n * Fresh is true for one step when a new frame arrived; Dlc/bytes hold the\n * latest payload otherwise. */\nuint8_t buf[8] = {};\nuint32_t seq = 0;\nconst int dlc = platform_can_rx(static_cast(Bus),\n static_cast(Id), buf, &seq);\nFresh = (seq != LastSeq);\nLastSeq = static_cast(seq);\nif (dlc > 0) {\n Dlc = static_cast(dlc);\n D0 = buf[0]; D1 = buf[1]; D2 = buf[2]; D3 = buf[3];\n D4 = buf[4]; D5 = buf[5]; D6 = buf[6]; D7 = buf[7];\n} else {\n Dlc = 0.0f;\n}\n", + "inputPorts": [], + "isEntryPoint": true, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Most recently received payload byte 0.", + "direction": "output", + "name": "D0", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Most recently received payload byte 1.", + "direction": "output", + "name": "D1", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Most recently received payload byte 2.", + "direction": "output", + "name": "D2", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Most recently received payload byte 3.", + "direction": "output", + "name": "D3", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Most recently received payload byte 4.", + "direction": "output", + "name": "D4", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Most recently received payload byte 5.", + "direction": "output", + "name": "D5", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Most recently received payload byte 6.", + "direction": "output", + "name": "D6", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Most recently received payload byte 7.", + "direction": "output", + "name": "D7", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Data length of the latest received frame.", + "direction": "output", + "name": "Dlc", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "True for one execution step when a new frame has arrived.", + "direction": "output", + "name": "Fresh", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + ], + "parameterTypes": { + "Bus": { + "description": "CAN controller number: 1 selects bus A/FDCAN1 and 2 selects bus B/FDCAN2.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Id": { + "description": "CAN arbitration identifier accepted by the receive mailbox.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "LastSeq": { + "description": "Internal sequence counter used to detect newly received frames.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "PhaseVoltages", + "description": "Reads the filtered three-phase and DC-link voltage sensor channels.", + "displayName": "Phase Voltages", + "domain": "", + "id": "Sensors.PhaseVoltages", + "inlineCode": "/* All voltage sense channels from the MAX22530 isolated ADC, filtered. */\nV_U = rte::Volts(platform_phase_voltage_u());\nV_V = rte::Volts(platform_phase_voltage_v());\nV_W = rte::Volts(platform_phase_voltage_w());\nV_Dc = rte::Volts(platform_get_dc_link_voltage());\n", + "inputPorts": [], + "isEntryPoint": false, + "maxInstances": 1, + "outputPorts": [ + { + "description": "Measured phase U voltage in volts.", + "direction": "output", + "name": "V_U", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Measured phase V voltage in volts.", + "direction": "output", + "name": "V_V", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Measured phase W voltage in volts.", + "direction": "output", + "name": "V_W", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Measured DC-link voltage in volts.", + "direction": "output", + "name": "V_Dc", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "InvPark", + "description": "Rotates d-q voltage commands into stationary alpha-beta coordinates.", + "displayName": "Inverse Park Transform", + "domain": "", + "id": "Transforms.InversePark", + "inlineCode": "const float cos_theta = cosf(Theta);\nconst float sin_theta = sinf(Theta);\nV_Alpha = V_D * cos_theta - V_Q * sin_theta;\nV_Beta = V_D * sin_theta + V_Q * cos_theta;\n", + "inputPorts": [ + { + "description": "Rotating-frame direct-axis voltage.", + "direction": "input", + "name": "V_D", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Rotating-frame quadrature-axis voltage.", + "direction": "input", + "name": "V_Q", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Electrical rotation angle in radians.", + "direction": "input", + "name": "Theta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Stationary-frame alpha-axis voltage.", + "direction": "output", + "name": "V_Alpha", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Stationary-frame beta-axis voltage.", + "direction": "output", + "name": "V_Beta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Din", + "description": "Reads a boolean level from one of the platform user digital inputs.", + "displayName": "Digital In", + "domain": "", + "id": "Sensors.DigitalIn", + "inlineCode": "/* User digital input. Pin 1..8 -> USER_DIN_1..8 (see platform_api.h). */\nOut = platform_digital_read(static_cast(Pin));\n", + "inputPorts": [], + "isEntryPoint": true, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Current logic level of the selected digital input.", + "direction": "output", + "name": "Out", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + ], + "parameterTypes": { + "Pin": { + "description": "User digital input selector in the range 1 through 8.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Dout", + "description": "Writes a boolean signal to a user digital output or onboard status LED.", + "displayName": "Digital Out", + "domain": "", + "id": "Actuators.DigitalOut", + "inlineCode": "/* User digital output. Pin 1..4 -> USER_DOUT_1..4, 5 -> green LED,\n * 6 -> orange LED (see platform_api.h). */\nplatform_digital_write(static_cast(Pin), In);\n", + "inputPorts": [ + { + "description": "Logic level to write to the selected output.", + "direction": "input", + "name": "In", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [], + "parameterTypes": { + "Pin": { + "description": "Output selector: 1-4 are user digital outputs, 5 is the green LED, and 6 is the orange LED.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "ForcedAngle", + "description": "Generates a continuously advancing electrical angle for open-loop control.", + "displayName": "Forced Angle", + "domain": "", + "id": "Transforms.ForcedAngle", + "inlineCode": "Angle += RateRadPerS * Dt;\nif (Angle >= 6.28318530718f) Angle -= 6.28318530718f;\nThetaElec = Angle;\n", + "inputPorts": [], + "isEntryPoint": false, + "maxInstances": 1, + "outputPorts": [ + { + "description": "Generated electrical angle wrapped to one revolution.", + "direction": "output", + "name": "ThetaElec", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "parameterTypes": { + "Angle": { + "description": "Persistent generated angle state in radians.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Dt": { + "description": "Update period in seconds.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "RateRadPerS": { + "description": "Electrical angular velocity in radians per second.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Pi", + "description": "Proportional-integral controller with output clamping and back-calculation anti-windup. Dt follows the live control rate.", + "displayName": "PI Controller", + "domain": "", + "id": "Control.Pi", + "inlineCode": "/* PI with clamping + back-calculation anti-windup.\n * Dimensionless throughout: implicit unit extraction/injection handles any\n * physical-quantity wiring at the binding sites.\n *\n * dt comes from the live control rate (platform_get_control_dt) so integral\n * gain stays correct when the carrier changes at runtime (CarrierAuto);\n * the Dt parameter is kept for backward compatibility but ignored. */\nconst float dt_s = platform_get_control_dt();\nconst float error = Setpoint - Measurement;\nIntegral += error * dt_s;\n\nfloat raw_output = Kp * error + Ki * Integral;\n\n/* Dynamic limit derived from DC-link voltage, matching the base image's\n * VectorPIController convention: max = Vdc/sqrt(3) * 0.95. */\nconst float vdc = platform_get_dc_link_voltage();\nconst float dynamic_max = (vdc / 1.7320508075688772f) * 0.95f;\nconst float max_limit = (dynamic_max < OutputMax) ? dynamic_max : OutputMax;\nconst float min_limit = (-dynamic_max > OutputMin) ? -dynamic_max : OutputMin;\n\nfloat limited_output = raw_output;\nif (limited_output > max_limit) limited_output = max_limit;\nif (limited_output < min_limit) limited_output = min_limit;\n\n/* Back-calculation anti-windup, scaled by AwGain (0 disables; 1.0 matches\n * the base-image VectorPIController). */\nif (Ki > 0.0001f && Kp > 0.0001f && AwGain > 0.0f) {\n const float excess = raw_output - limited_output;\n Integral -= excess * dt_s * AwGain / (Kp * Ki);\n}\n\nOutput = limited_output;\n", + "inputPorts": [ + { + "description": "Desired target value.", + "direction": "input", + "name": "Setpoint", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Measured process value subtracted from the setpoint.", + "direction": "input", + "name": "Measurement", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Clamped PI control effort.", + "direction": "output", + "name": "Output", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "parameterTypes": { + "AwGain": { + "description": "Back-calculation anti-windup gain. Zero disables anti-windup.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Dt": { + "description": "Controller step period in seconds.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Integral": { + "description": "Persistent accumulated-error state.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Ki": { + "description": "Integral gain applied to the accumulated error.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Kp": { + "description": "Proportional gain applied to the current error.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "OutputMax": { + "description": "Upper output clamp, additionally limited by the measured DC-link voltage.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "OutputMin": { + "description": "Lower output clamp, additionally limited by the measured DC-link voltage.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Throttle", + "description": "Reads the redundant throttle channels and their plausibility status.", + "displayName": "Throttle", + "domain": "", + "id": "Sensors.Throttle", + "inlineCode": "/* Dual throttle channels, normalized [0..1] by the base-image driver\n * (KV calibration Hw.ThrA/B.MinV/MaxV). Both read 0 and Valid is false\n * while the channels disagree beyond the plausibility tolerance. */\nA = platform_get_throttle_a();\nB = platform_get_throttle_b();\nValid = platform_get_throttle_valid();\n", + "inputPorts": [], + "isEntryPoint": true, + "maxInstances": 1, + "outputPorts": [ + { + "description": "Normalized throttle channel A in the range 0 to 1.", + "direction": "output", + "name": "A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Normalized throttle channel B in the range 0 to 1.", + "direction": "output", + "name": "B", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "True when both throttle channels pass the plausibility check.", + "direction": "output", + "name": "Valid", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Temps", + "description": "Reads the motor and three inverter temperature channels from the application sensors.", + "displayName": "Temperatures", + "domain": "", + "id": "Sensors.Temperatures", + "inlineCode": "/* Motor + board temperatures from the base-image ApplicationSensors driver.\n * NAN while a channel is disabled or out of range (open/short). */\nT_Motor = rte::Celsius(platform_get_motor_temperature());\nT_Inv1 = rte::Celsius(platform_get_inverter_temperature(0));\nT_Inv2 = rte::Celsius(platform_get_inverter_temperature(1));\nT_Inv3 = rte::Celsius(platform_get_inverter_temperature(2));\n", + "inputPorts": [], + "isEntryPoint": false, + "maxInstances": 1, + "outputPorts": [ + { + "description": "Motor temperature in degrees Celsius; NaN indicates a disabled or invalid sensor.", + "direction": "output", + "name": "T_Motor", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "temperature" + } + }, + { + "description": "Inverter temperature channel 1 in degrees Celsius; NaN indicates an invalid sensor.", + "direction": "output", + "name": "T_Inv1", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "temperature" + } + }, + { + "description": "Inverter temperature channel 2 in degrees Celsius; NaN indicates an invalid sensor.", + "direction": "output", + "name": "T_Inv2", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "temperature" + } + }, + { + "description": "Inverter temperature channel 3 in degrees Celsius; NaN indicates an invalid sensor.", + "direction": "output", + "name": "T_Inv3", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "temperature" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Greater", + "description": "Compares two scalar values and reports whether A is greater than B.", + "displayName": "Greater Than", + "domain": "", + "id": "Logic.Greater", + "inlineCode": "Out = (A > B);\n", + "inputPorts": [ + { + "description": "Left-hand value in the comparison.", + "direction": "input", + "name": "A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Right-hand value in the comparison.", + "direction": "input", + "name": "B", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "True when A is greater than B.", + "direction": "output", + "name": "Out", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + ], + "parameterTypes": { + "B": { + "description": "Fallback comparison threshold used when the B input is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "VdcSense", + "description": "Reads the inverter DC-link voltage from the platform sensor interface.", + "displayName": "DC Link Voltage", + "domain": "", + "id": "Sensors.DcLinkVoltage", + "inlineCode": "V_Dc = rte::Volts(platform_get_dc_link_voltage());\n", + "inputPorts": [], + "isEntryPoint": true, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Measured DC-link voltage in volts.", + "direction": "output", + "name": "V_Dc", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Deriv", + "description": "Estimates a signal derivative with an optional first-order low-pass filter.", + "displayName": "Derivative", + "domain": "", + "id": "Transforms.Derivative", + "inlineCode": "/* Filtered derivative: Out ~= d(In)/dt, low-passed at Fc [Hz] to keep\n * sampling noise out of the estimate (Fc <= 0 disables filtering).\n * Prev/OutState are persistent state (leave at graph defaults). */\nconst float raw = (In - Prev) / Dt;\nPrev = In;\nif (Fc > 0.0f) {\n const float alpha = 1.0f / (1.0f + 1.0f / (6.28318530718f * Fc * Dt));\n OutState += alpha * (raw - OutState);\n} else {\n OutState = raw;\n}\nOut = OutState;\n", + "inputPorts": [ + { + "description": "Signal whose time derivative is estimated.", + "direction": "input", + "name": "In", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Filtered derivative estimate.", + "direction": "output", + "name": "Out", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "parameterTypes": { + "Dt": { + "description": "Sample period in seconds.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Fc": { + "description": "Low-pass cutoff frequency in hertz. Values at or below zero disable filtering.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "OutState": { + "description": "Persistent filtered derivative state.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Prev": { + "description": "Persistent previous input sample used by the finite difference.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Encoder", + "description": "Reads the latest mechanical rotor angle and motor speed from the encoder driver.", + "displayName": "Encoder", + "domain": "", + "id": "Sensors.Encoder", + "inlineCode": "/* Mechanical angle [rad] and speed [rpm] from the base-image encoder driver. */\nTheta = platform_get_encoder_angle_latest() * 0.01745329251f; // deg -> rad\nOmega = platform_get_motor_rpm();\n", + "inputPorts": [], + "isEntryPoint": true, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Mechanical rotor angle in radians.", + "direction": "output", + "name": "Theta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Mechanical motor speed in revolutions per minute.", + "direction": "output", + "name": "Omega", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Svpwm", + "description": "Converts an alpha-beta voltage vector into centered three-phase PWM duty commands.", + "displayName": "Space Vector PWM", + "domain": "", + "id": "Transforms.Svpwm", + "inlineCode": "/* Clamp the alpha/beta voltage vector to the linear SVM limit.\n * The maximum line-to-neutral voltage magnitude for linear modulation is\n * Vdc / sqrt(3). Use a small margin to stay away from the overmodulation\n * boundary. */\nconst float sqrt3 = 1.7320508075688772f;\nconst float v_max_linear = (V_Dc.in(au::volts) / sqrt3) * 0.95f;\nfloat valpha = V_Alpha.in(au::volts);\nfloat vbeta = V_Beta.in(au::volts);\nconst float v_albe_sq = valpha * valpha + vbeta * vbeta;\nif (v_albe_sq > v_max_linear * v_max_linear && v_albe_sq > 1e-12f) {\n const float scale = v_max_linear / sqrtf(v_albe_sq);\n valpha *= scale;\n vbeta *= scale;\n}\n\n/* Inverse Clarke: alpha/beta -> A/B/C. */\nconst float v_a = valpha / V_Dc.in(au::volts);\nconst float v_b = (-0.5f * valpha + 0.86602540378f * vbeta) / V_Dc.in(au::volts);\nconst float v_c = (-0.5f * valpha - 0.86602540378f * vbeta) / V_Dc.in(au::volts);\n\nfloat v_min = v_a;\nif (v_b < v_min) v_min = v_b;\nif (v_c < v_min) v_min = v_c;\n\nfloat v_max = v_a;\nif (v_b > v_max) v_max = v_b;\nif (v_c > v_max) v_max = v_c;\n\nconst float v_offset = 0.5f * (v_min + v_max);\n\n/* Convert to percent duty and clamp. Linear SVM stays roughly in\n * [21%, 79%]; clamping to [0,100] only catches numerical edge cases. */\nfloat duty_a_pct = 50.0f + 50.0f * (v_a - v_offset);\nfloat duty_b_pct = 50.0f + 50.0f * (v_b - v_offset);\nfloat duty_c_pct = 50.0f + 50.0f * (v_c - v_offset);\n\nif (duty_a_pct < 0.0f) duty_a_pct = 0.0f; else if (duty_a_pct > 100.0f) duty_a_pct = 100.0f;\nif (duty_b_pct < 0.0f) duty_b_pct = 0.0f; else if (duty_b_pct > 100.0f) duty_b_pct = 100.0f;\nif (duty_c_pct < 0.0f) duty_c_pct = 0.0f; else if (duty_c_pct > 100.0f) duty_c_pct = 100.0f;\n\nDuty_A = duty_a_pct;\nDuty_B = duty_b_pct;\nDuty_C = duty_c_pct;\n", + "inputPorts": [ + { + "description": "Requested stationary-frame alpha-axis voltage.", + "direction": "input", + "name": "V_Alpha", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Requested stationary-frame beta-axis voltage.", + "direction": "input", + "name": "V_Beta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Measured DC-link voltage used for normalization and modulation limiting.", + "direction": "input", + "name": "V_Dc", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Phase A PWM duty command in percent.", + "direction": "output", + "name": "Duty_A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Phase B PWM duty command in percent.", + "direction": "output", + "name": "Duty_B", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Phase C PWM duty command in percent.", + "direction": "output", + "name": "Duty_C", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Var", + "description": "Stores a scalar value and updates it when the Set input is true.", + "displayName": "Var", + "domain": "", + "id": "Values.Var", + "inlineCode": "if (Set) Stored = In;\nValue = Stored;\n", + "inputPorts": [ + { + "description": "New scalar value to store when Set is true.", + "direction": "input", + "name": "In", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Write enable. When true, copies In into the stored value.", + "direction": "input", + "name": "Set", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Current stored scalar value.", + "direction": "output", + "name": "Value", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "parameterTypes": { + "In": { + "description": "Fallback new value when the In input is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Set": { + "description": "Fallback write-enable value when the Set input is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + }, + "Stored": { + "description": "Persistent scalar storage state.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Gate", + "description": "Passes a value when enabled and outputs zero when disabled.", + "displayName": "Gate", + "domain": "", + "id": "Logic.Gate", + "inlineCode": "/* Pass Val when Sel is true, else 0. */\nOut = Sel ? Val : 0.0f;\n", + "inputPorts": [ + { + "description": "Enable signal. True passes Val; false selects zero.", + "direction": "input", + "name": "Sel", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + }, + { + "description": "Value passed through while Sel is true.", + "direction": "input", + "name": "Val", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Val when selected, otherwise zero.", + "direction": "output", + "name": "Out", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Var", + "description": "Stores a boolean value and updates it when the Set input is true.", + "displayName": "Var (Bool)", + "domain": "", + "id": "Values.VarBool", + "inlineCode": "if (Set) Stored = In;\nValue = Stored;\n", + "inputPorts": [ + { + "description": "New boolean value to store when Set is true.", + "direction": "input", + "name": "In", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + }, + { + "description": "Write enable. When true, copies In into the stored value.", + "direction": "input", + "name": "Set", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Current stored boolean value.", + "direction": "output", + "name": "Value", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + ], + "parameterTypes": { + "In": { + "description": "Fallback new boolean value when the In input is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + }, + "Set": { + "description": "Fallback write-enable value when the Set input is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + }, + "Stored": { + "description": "Persistent boolean storage state.", + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Slew", + "description": "Limits how quickly a signal can rise or fall over time.", + "displayName": "Slew Rate Limiter", + "domain": "", + "id": "Control.Slew", + "inlineCode": "/* Rate-limited reference follower: Out tracks In at max Rate units/sec.\n * On control start Value resets to the graph default, so references ramp\n * smoothly from zero instead of stepping. */\nconst float target = In;\nconst float step = Rate * Dt;\nfloat v = Value;\nif (target > v + step) {\n v += step;\n} else if (target < v - step) {\n v -= step;\n} else {\n v = target;\n}\nValue = v;\nOut = Value;\n", + "inputPorts": [ + { + "description": "Target value for the limiter to follow.", + "direction": "input", + "name": "In", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Rate-limited output value.", + "direction": "output", + "name": "Out", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "parameterTypes": { + "Dt": { + "description": "Update period in seconds.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Rate": { + "description": "Maximum absolute rate of change in units per second.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Value": { + "description": "Persistent current output state.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "ElecAngle", + "description": "Converts mechanical rotor angle to wrapped electrical angle using pole count, direction, and offset.", + "displayName": "Electrical Angle", + "domain": "", + "id": "Transforms.ElecAngle", + "inlineCode": "/* Wrap the mechanical encoder angle to [0, 2*pi). */\nconst float two_pi = 6.28318530718f;\nfloat theta = fmodf(ThetaMech, two_pi);\nif (theta < 0.0f) theta += two_pi;\n\n/* Electrical angle = offset (elec deg) + sign * encoder_angle * (Poles / 2).\n * Matches the base-image FocController convention. */\nconstexpr float DEG_TO_RAD = 0.01745329251f;\nfloat elec = OffsetDeg * DEG_TO_RAD + EncoderSign * theta * Poles * 0.5f;\n\n/* Wrap the electrical angle to [0, 2*pi). */\nelec = fmodf(elec, two_pi);\nif (elec < 0.0f) elec += two_pi;\n\nThetaElec = elec;\n", + "inputPorts": [ + { + "description": "Mechanical rotor angle in radians.", + "direction": "input", + "name": "ThetaMech", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Electrical-angle calibration offset in degrees.", + "direction": "input", + "name": "OffsetDeg", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Encoder direction multiplier, normally 1 or -1.", + "direction": "input", + "name": "EncoderSign", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Motor pole count used to convert mechanical to electrical angle.", + "direction": "input", + "name": "Poles", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 1, + "outputPorts": [ + { + "description": "Electrical rotor angle wrapped to the range 0 through 2\u03c0 radians.", + "direction": "output", + "name": "ThetaElec", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "parameterTypes": { + "EncoderSign": { + "description": "Fallback encoder direction multiplier when the input is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "OffsetDeg": { + "description": "Fallback electrical-angle calibration offset in degrees when the input is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Poles": { + "description": "Fallback motor pole count when the input is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "PhaseCurrents", + "description": "Samples the three inverter phase-current channels from the platform current sensors.", + "displayName": "Phase Currents", + "domain": "", + "id": "Sensors.PhaseCurrents", + "inlineCode": "float iu_f = 0.0f;\nfloat iv_f = 0.0f;\nfloat iw_f = 0.0f;\nif (platform_get_phase_currents(&iu_f, &iv_f, &iw_f)) {\n /* Keep raw sensor polarity available for general telemetry, while FOC\n * graphs enable inversion for this hardware's current-sensor wiring. */\n const float polarity = InvertPolarity ? -1.0f : 1.0f;\n I_A = rte::Amperes(polarity * iu_f);\n I_B = rte::Amperes(polarity * iv_f);\n I_C = rte::Amperes(polarity * iw_f);\n}\n", + "inputPorts": [], + "isEntryPoint": true, + "maxInstances": 1, + "outputPorts": [ + { + "description": "Measured phase A current in amperes.", + "direction": "output", + "name": "I_A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Measured phase B current in amperes.", + "direction": "output", + "name": "I_B", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Measured phase C current in amperes.", + "direction": "output", + "name": "I_C", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + } + ], + "parameterTypes": { + "InvertPolarity": { + "description": "Negate all three phase currents to match the FOC sign convention on hardware with inverted current sensors.", + "quantity": "boolean", + "frame": "scalar", + "dtype": "f32" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "SinCos", + "description": "Computes the sine and cosine of an input angle.", + "displayName": "Sin/Cos", + "domain": "", + "id": "Transforms.SinCos", + "inlineCode": "SinTheta = sinf(Theta);\nCosTheta = cosf(Theta);\n", + "inputPorts": [ + { + "description": "Input angle in radians.", + "direction": "input", + "name": "Theta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Sine of the input angle.", + "direction": "output", + "name": "SinTheta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Cosine of the input angle.", + "direction": "output", + "name": "CosTheta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Park", + "description": "Rotates stationary alpha-beta currents into rotor-aligned d-q coordinates.", + "displayName": "Park Transform", + "domain": "", + "id": "Transforms.Park", + "inlineCode": "const float cos_theta = cosf(Theta);\nconst float sin_theta = sinf(Theta);\nI_D = I_Alpha * cos_theta + I_Beta * sin_theta;\nI_Q = -I_Alpha * sin_theta + I_Beta * cos_theta;\n", + "inputPorts": [ + { + "description": "Stationary-frame alpha-axis current.", + "direction": "input", + "name": "I_Alpha", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Stationary-frame beta-axis current.", + "direction": "input", + "name": "I_Beta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Electrical rotation angle in radians.", + "direction": "input", + "name": "Theta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Rotating-frame direct-axis current.", + "direction": "output", + "name": "I_D", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Rotating-frame quadrature-axis current.", + "direction": "output", + "name": "I_Q", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "LogI", + "description": "Publishes three phase-current signals to the standard current telemetry channels.", + "displayName": "Telemetry Current Sink", + "domain": "", + "id": "Debug.TelemetryCurrentSink", + "inlineCode": "platform_telemetry_log_f32(\"cg_iu_a\", I_A.in(au::amperes));\nplatform_telemetry_log_f32(\"cg_iv_a\", I_B.in(au::amperes));\nplatform_telemetry_log_f32(\"cg_iw_a\", I_C.in(au::amperes));\n", + "inputPorts": [ + { + "description": "Phase A current to publish in amperes.", + "direction": "input", + "name": "I_A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Phase B current to publish in amperes.", + "direction": "input", + "name": "I_B", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Phase C current to publish in amperes.", + "direction": "input", + "name": "I_C", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Clarke", + "description": "Transforms three-phase currents into stationary alpha-beta coordinates.", + "displayName": "Clarke Transform", + "domain": "", + "id": "Transforms.Clarke", + "inlineCode": "I_Alpha = I_A;\nI_Beta = (I_B - I_C) * 0.57735026919f;\n", + "inputPorts": [ + { + "description": "Phase A current.", + "direction": "input", + "name": "I_A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Phase B current.", + "direction": "input", + "name": "I_B", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Phase C current.", + "direction": "input", + "name": "I_C", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Stationary-frame alpha-axis current.", + "direction": "output", + "name": "I_Alpha", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + }, + { + "description": "Stationary-frame beta-axis current.", + "direction": "output", + "name": "I_Beta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "current" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Less", + "description": "Compares two scalar values and reports whether A is less than B.", + "displayName": "Less Than", + "domain": "", + "id": "Logic.Less", + "inlineCode": "Out = (A < B);\n", + "inputPorts": [ + { + "description": "Left-hand value in the comparison.", + "direction": "input", + "name": "A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Right-hand value in the comparison.", + "direction": "input", + "name": "B", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "True when A is less than B.", + "direction": "output", + "name": "Out", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + } + ], + "parameterTypes": { + "B": { + "description": "Fallback comparison threshold used when the B input is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "InvClarke", + "description": "Transforms stationary alpha-beta voltages into three-phase voltage commands.", + "displayName": "Inverse Clarke Transform", + "domain": "", + "id": "Transforms.InverseClarke", + "inlineCode": "V_A = V_Alpha;\nV_B = -0.5f * V_Alpha + 0.86602540378f * V_Beta;\nV_C = -0.5f * V_Alpha - 0.86602540378f * V_Beta;\n", + "inputPorts": [ + { + "description": "Stationary-frame alpha-axis voltage.", + "direction": "input", + "name": "V_Alpha", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Stationary-frame beta-axis voltage.", + "direction": "input", + "name": "V_Beta", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Reconstructed phase A voltage.", + "direction": "output", + "name": "V_A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Reconstructed phase B voltage.", + "direction": "output", + "name": "V_B", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + }, + { + "description": "Reconstructed phase C voltage.", + "direction": "output", + "name": "V_C", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "voltage" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "Cached = platform_config_load(Key, DefaultValue);\n", + "defaultName": "Cfg", + "description": "Exposes a named configuration value, using a default when no stored value is available.", + "displayName": "Config Value", + "domain": "", + "id": "Values.Config", + "inlineCode": "Value = Cached;\n", + "inputPorts": [], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Cached configuration value supplied to the graph.", + "direction": "output", + "name": "Value", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "parameterTypes": { + "Cached": { + "description": "Persistent cached configuration value exposed by the output.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "DefaultValue": { + "description": "Value used when the configuration key has not been stored.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Key": { + "description": "Name used to look up the value in platform configuration storage.", + "dtype": "f32", + "frame": "scalar", + "quantity": "string" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "CanTx", + "description": "Transmits up to eight data bytes on a selected CAN bus at a configurable maximum rate.", + "displayName": "CAN TX", + "domain": "", + "id": "Actuators.CanTx", + "inlineCode": "/* CAN transmit, rate-limited by wall clock: sends when 1000/Rate ms have\n * elapsed since the previous send (Rate <= 0 sends every step).\n * D0..D7 = payload bytes (unconnected = 0), Dlc = how many of the 8 to\n * send, Ext: 0 std / 1 ext. */\nconst uint32_t period_ms = (Rate > 0.0f) ? static_cast(1000.0f / Rate) : 0U;\nconst uint32_t now_ms = platform_millis();\nif (period_ms == 0U || (now_ms - static_cast(LastMs)) >= period_ms) {\n LastMs = static_cast(now_ms);\n const uint8_t b[8] = {\n static_cast(D0), static_cast(D1),\n static_cast(D2), static_cast(D3),\n static_cast(D4), static_cast(D5),\n static_cast(D6), static_cast(D7),\n };\n uint8_t n = static_cast(Dlc);\n if (n > 8) n = 8;\n platform_can_send(static_cast(Bus), static_cast(Id),\n Ext > 0.5f, b, n);\n}\n", + "inputPorts": [ + { + "description": "Payload byte 0. Values are converted to an unsigned 8-bit integer.", + "direction": "input", + "name": "D0", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Payload byte 1. Values are converted to an unsigned 8-bit integer.", + "direction": "input", + "name": "D1", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Payload byte 2. Values are converted to an unsigned 8-bit integer.", + "direction": "input", + "name": "D2", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Payload byte 3. Values are converted to an unsigned 8-bit integer.", + "direction": "input", + "name": "D3", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Payload byte 4. Values are converted to an unsigned 8-bit integer.", + "direction": "input", + "name": "D4", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Payload byte 5. Values are converted to an unsigned 8-bit integer.", + "direction": "input", + "name": "D5", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Payload byte 6. Values are converted to an unsigned 8-bit integer.", + "direction": "input", + "name": "D6", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Payload byte 7. Values are converted to an unsigned 8-bit integer.", + "direction": "input", + "name": "D7", + "optional": true, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [], + "parameterTypes": { + "Bus": { + "description": "CAN controller number: 1 selects bus A/FDCAN1 and 2 selects bus B/FDCAN2.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "D0": { + "description": "Fallback value for payload byte 0 when its input port is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "D1": { + "description": "Fallback value for payload byte 1 when its input port is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "D2": { + "description": "Fallback value for payload byte 2 when its input port is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "D3": { + "description": "Fallback value for payload byte 3 when its input port is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "D4": { + "description": "Fallback value for payload byte 4 when its input port is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "D5": { + "description": "Fallback value for payload byte 5 when its input port is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "D6": { + "description": "Fallback value for payload byte 6 when its input port is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "D7": { + "description": "Fallback value for payload byte 7 when its input port is not wired.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Dlc": { + "description": "Number of payload bytes to transmit, clamped to the range 0 through 8.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Ext": { + "description": "Use an extended 29-bit identifier when greater than 0.5; otherwise use a standard identifier.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Id": { + "description": "CAN arbitration identifier to transmit.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "LastMs": { + "description": "Internal timestamp of the previous transmission in milliseconds.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "Rate": { + "description": "Maximum transmission rate in frames per second. Values at or below zero send every step.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "defaultName": "Mux", + "description": "Selects one of two scalar inputs using a boolean control signal.", + "displayName": "Mux", + "domain": "", + "id": "Logic.Mux", + "inlineCode": "Out = Sel ? A : B;\n", + "inputPorts": [ + { + "description": "Selection signal. True selects A; false selects B.", + "direction": "input", + "name": "Sel", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "boolean" + } + }, + { + "description": "Value selected when Sel is true.", + "direction": "input", + "name": "A", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + }, + { + "description": "Value selected when Sel is false.", + "direction": "input", + "name": "B", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ], + "isEntryPoint": false, + "maxInstances": 0, + "outputPorts": [ + { + "description": "Currently selected input value.", + "direction": "output", + "name": "Out", + "optional": false, + "type": { + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + ] + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "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.", + "displayName": "Modulation Auto-Switch", + "domain": "app_loop", + "id": "Actuators.ModulationAuto", + "inlineCode": "/* Policy only: watch the commanded ramp frequency and request handoffs.\n * The switching itself (phase-lock, level match, timer handoff) lives in the\n * base image behind platform_modulation_to_pattern/to_ramp. */\nconst float fe = platform_get_elec_freq_hz();\nconst uint8_t mod_mode = platform_modulation_mode();\n\nif (mod_mode == 0U && fe >= enter_hz) {\n (void)platform_modulation_to_pattern((uint32_t)(pulses + 0.5f), duty);\n} else if (mod_mode == 1U && fe < exit_hz) {\n (void)platform_modulation_to_ramp();\n}\n", + "inputPorts": [], + "isEntryPoint": false, + "maxInstances": 1, + "outputPorts": [], + "parameterTypes": { + "enter_hz": { + "description": "Ramp-to-pattern handoff threshold (electrical Hz).", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "exit_hz": { + "description": "Pattern-to-ramp handoff threshold (electrical Hz). Set below enter_hz for hysteresis.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "pulses": { + "description": "N-pulse pulses per quarter cycle (1..64). Device switching = (2N+1) x fe.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + }, + "duty": { + "description": "High-time fraction per pulse cell (0..1). 1.0 = six-step; keep above ~0.7.", + "dtype": "f32", + "frame": "scalar", + "quantity": "dimensionless" + } + } + }, + { + "classDefinition": "", + "classHeader": "", + "constructorCode": "", + "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.", + "displayName": "Carrier Auto-Schedule", + "domain": "app_loop", + "id": "Actuators.CarrierAuto", + "inlineCode": "/* Piecewise-linear carrier schedule: flat below the first and above the last\n * valid breakpoint, linear between. A breakpoint with fsw <= 0 ends the\n * table. The platform call deadbands small changes, so this can run every\n * app_loop step without churning the timer. */\nconst float fe = platform_get_elec_freq_hz();\nconst float fes[6] = {fe_1, fe_2, fe_3, fe_4, fe_5, fe_6};\nconst float fsws[6] = {fsw_1, fsw_2, fsw_3, fsw_4, fsw_5, fsw_6};\n\nint n_pts = 0;\nwhile (n_pts < 6 && fsws[n_pts] > 0.0f) ++n_pts;\n\nif (n_pts > 0) {\n float carrier = fsws[0];\n if (fe > fes[0]) {\n carrier = fsws[n_pts - 1];\n for (int i = 1; i < n_pts; ++i) {\n if (fe <= fes[i]) {\n const float span = fes[i] - fes[i - 1];\n const float t = (span > 0.0f) ? (fe - fes[i - 1]) / span : 1.0f;\n carrier = fsws[i - 1] + t * (fsws[i] - fsws[i - 1]);\n break;\n }\n }\n }\n platform_pwm_set_carrier_hz(carrier);\n}\n", + "inputPorts": [], + "isEntryPoint": false, + "maxInstances": 1, + "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." + } + } + } + ], + "nodes": [ + { + "displayName": "", + "domain": "tim_isr", + "id": "Clarke", + "parameters": {}, + "position": { + "x": 618.0, + "y": 2396.0 + }, + "type": "Transforms.Clarke" + }, + { + "displayName": "", + "domain": "adc_isr", + "id": "Currents", + "parameters": { + "OffsetU": "0.0", + "OffsetV": "0.0", + "InvertPolarity": "1.0" + }, + "position": { + "x": 150.0, + "y": 80.0 + }, + "type": "Sensors.PhaseCurrents" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "ElecAngle", + "parameters": { + "EncoderSign": "1.0", + "OffsetDeg": "6.445", + "Poles": "10.0" + }, + "position": { + "x": 618.0, + "y": 2577.0 + }, + "type": "Transforms.ElecAngle" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "Encoder", + "parameters": {}, + "position": { + "x": 150.0, + "y": 3302.0 + }, + "type": "Sensors.Encoder" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "EncoderApp", + "parameters": {}, + "position": { + "x": 522.0, + "y": 1498.0 + }, + "type": "Sensors.Encoder" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "IdRef", + "parameters": { + "Value": "0.0" + }, + "position": { + "x": 150.0, + "y": 2246.0 + }, + "type": "Values.Constant" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "InvPark", + "parameters": {}, + "position": { + "x": 1848.0, + "y": 2521.0 + }, + "type": "Transforms.InversePark" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "IqRef", + "parameters": { + "Value": "0.0" + }, + "position": { + "x": 150.0, + "y": 2396.0 + }, + "type": "Values.Constant" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "LogDu", + "parameters": { + "Key": "cg_du" + }, + "position": { + "x": 2473.0, + "y": 2246.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "LogDv", + "parameters": { + "Key": "cg_dv" + }, + "position": { + "x": 2473.0, + "y": 2396.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "LogDw", + "parameters": { + "Key": "cg_dw" + }, + "position": { + "x": 2473.0, + "y": 2546.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "LogId", + "parameters": { + "Key": "cg_id_a" + }, + "position": { + "x": 1236.0, + "y": 2624.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "LogIq", + "parameters": { + "Key": "cg_iq_a" + }, + "position": { + "x": 1236.0, + "y": 2774.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogTheta", + "parameters": { + "Key": "cg_theta_rad" + }, + "position": { + "x": 856.0, + "y": 380.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogU", + "parameters": { + "Key": "cg_iu_a" + }, + "position": { + "x": 522.0, + "y": 457.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogV", + "parameters": { + "Key": "cg_iv_a" + }, + "position": { + "x": 522.0, + "y": 607.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "LogVd", + "parameters": { + "Key": "cg_vd_v" + }, + "position": { + "x": 1542.0, + "y": 2559.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogVdc", + "parameters": { + "Key": "cg_vdc_v" + }, + "position": { + "x": 856.0, + "y": 1430.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "LogVq", + "parameters": { + "Key": "cg_vq_v" + }, + "position": { + "x": 1848.0, + "y": 2246.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogW", + "parameters": { + "Key": "cg_iw_a" + }, + "position": { + "x": 522.0, + "y": 757.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "Park", + "parameters": {}, + "position": { + "x": 931.0, + "y": 2435.0 + }, + "type": "Transforms.Park" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "PiD", + "parameterInputs": [ + "Kp", + "Ki" + ], + "parameters": { + "AwGain": "1.0", + "Dt": "0.0002", + "Integral": "0.0", + "Ki": "10.0", + "Kp": "0.03", + "OutputMax": "200.0", + "OutputMin": "-200.0" + }, + "position": { + "x": 1236.0, + "y": 3077.0 + }, + "type": "Control.Pi" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "PiQ", + "parameterInputs": [ + "Kp", + "Ki" + ], + "parameters": { + "AwGain": "1.0", + "Dt": "0.0002", + "Integral": "0.0", + "Ki": "10.0", + "Kp": "0.03", + "OutputMax": "200.0", + "OutputMin": "-200.0" + }, + "position": { + "x": 1542.0, + "y": 2246.0 + }, + "type": "Control.Pi" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "PwmOut", + "parameters": {}, + "position": { + "x": 2473.0, + "y": 2696.0 + }, + "type": "Actuators.PwmOut" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "Svpwm", + "parameters": {}, + "position": { + "x": 2177.0, + "y": 2246.0 + }, + "type": "Transforms.Svpwm" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "VdcApp", + "parameters": {}, + "position": { + "x": 522.0, + "y": 2041.0 + }, + "type": "Sensors.DcLinkVoltage" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "VdcSense", + "parameters": {}, + "position": { + "x": 1848.0, + "y": 2396.0 + }, + "type": "Sensors.DcLinkVoltage" + }, + { + "displayName": "CfgEncSign", + "domain": "tim_isr", + "id": "CfgEncSign", + "parameters": { + "Cached": "0.0", + "DefaultValue": "1.0", + "Key": "Motor.Encoder.SinCos.Sign" + }, + "position": { + "x": 150.0, + "y": 2924.0 + }, + "type": "Values.Config" + }, + { + "displayName": "CfgEncOffset", + "domain": "tim_isr", + "id": "CfgEncOffset", + "parameters": { + "Cached": "0.0", + "DefaultValue": "6.445", + "Key": "Motor.Encoder.SinCos.OffsetDeg" + }, + "position": { + "x": 150.0, + "y": 2735.0 + }, + "type": "Values.Config" + }, + { + "displayName": "CfgPoles", + "domain": "tim_isr", + "id": "CfgPoles", + "parameters": { + "Cached": "0.0", + "DefaultValue": "10.0", + "Key": "Motor.Poles" + }, + "position": { + "x": 150.0, + "y": 3113.0 + }, + "type": "Values.Config" + }, + { + "displayName": "VoltSense", + "domain": "vsense", + "id": "VoltSense", + "parameters": {}, + "position": { + "x": 2829.0, + "y": 2246.0 + }, + "type": "Sensors.PhaseVoltages" + }, + { + "displayName": "LogVu", + "domain": "vsense", + "id": "LogVu", + "parameters": { + "Key": "cg_vu_v" + }, + "position": { + "x": 3151.0, + "y": 2246.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "LogVv", + "domain": "vsense", + "id": "LogVv", + "parameters": { + "Key": "cg_vv_v" + }, + "position": { + "x": 3151.0, + "y": 2396.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "LogVw", + "domain": "vsense", + "id": "LogVw", + "parameters": { + "Key": "cg_vw_v" + }, + "position": { + "x": 3151.0, + "y": 2546.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "CfgKpQ", + "domain": "tim_isr", + "id": "CfgKpQ", + "parameters": { + "Cached": "0.0", + "DefaultValue": "0.16", + "Key": "Ctrl.PiQ.Kp" + }, + "position": { + "x": 1236.0, + "y": 2435.0 + }, + "type": "Values.Config" + }, + { + "displayName": "CfgEnable", + "domain": "tim_isr", + "id": "CfgEnable", + "parameters": { + "Cached": "0.0", + "DefaultValue": "1.0", + "Key": "Ctrl.ThrottleEnable" + }, + "position": { + "x": 150.0, + "y": 2546.0 + }, + "type": "Values.Config" + }, + { + "displayName": "EnableCmp", + "domain": "tim_isr", + "id": "EnableCmp", + "parameters": { + "B": "0.5" + }, + "position": { + "x": 618.0, + "y": 2246.0 + }, + "type": "Logic.Greater" + }, + { + "displayName": "ThrottleEnable", + "domain": "tim_isr", + "id": "ThrottleEnable", + "parameters": { + "In": "0.0", + "Set": "1.0", + "Stored": "1.0" + }, + "position": { + "x": 931.0, + "y": 2246.0 + }, + "type": "Values.VarBool" + }, + { + "displayName": "IqGate", + "domain": "tim_isr", + "id": "IqGate", + "parameters": {}, + "position": { + "x": 1236.0, + "y": 2924.0 + }, + "type": "Logic.Gate" + }, + { + "displayName": "IqVar", + "domain": "tim_isr", + "id": "IqVar", + "parameters": { + "In": "0.0", + "Set": "0.0", + "Stored": "0.0" + }, + "position": { + "x": 150.0, + "y": 3644.0 + }, + "type": "Values.Var" + }, + { + "displayName": "IdVar", + "domain": "tim_isr", + "id": "IdVar", + "parameters": { + "In": "0.0", + "Set": "0.0", + "Stored": "0.0" + }, + "position": { + "x": 931.0, + "y": 2994.0 + }, + "type": "Values.Var" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "Din1", + "parameters": { + "Pin": "1.0" + }, + "position": { + "x": 522.0, + "y": 1348.0 + }, + "type": "Sensors.DigitalIn" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LedGreen", + "parameters": { + "Pin": "5.0" + }, + "position": { + "x": 856.0, + "y": 230.0 + }, + "type": "Actuators.DigitalOut" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "Temps", + "parameters": {}, + "position": { + "x": 522.0, + "y": 1651.0 + }, + "type": "Sensors.Temperatures" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "Throttle", + "parameters": {}, + "position": { + "x": 522.0, + "y": 1860.0 + }, + "type": "Sensors.Throttle" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogT_Motor", + "parameters": { + "Key": "temp_motor_c" + }, + "position": { + "x": 856.0, + "y": 980.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogT_Inv1", + "parameters": { + "Key": "temp_inv1_c" + }, + "position": { + "x": 856.0, + "y": 530.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogT_Inv2", + "parameters": { + "Key": "temp_inv2_c" + }, + "position": { + "x": 856.0, + "y": 680.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogT_Inv3", + "parameters": { + "Key": "temp_inv3_c" + }, + "position": { + "x": 856.0, + "y": 830.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogThrA", + "parameters": { + "Key": "thr_a_norm" + }, + "position": { + "x": 856.0, + "y": 1130.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogThrB", + "parameters": { + "Key": "thr_b_norm" + }, + "position": { + "x": 856.0, + "y": 1280.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "SlewIq", + "parameterInputs": [ + "Rate" + ], + "parameters": { + "Dt": "0.0002", + "Rate": "50.0", + "Value": "0.0" + }, + "position": { + "x": 618.0, + "y": 2766.0 + }, + "type": "Control.Slew" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "CfgIqSlew", + "parameters": { + "Cached": "0.0", + "DefaultValue": "50.0", + "Key": "Ctrl.Iq.SlewAps" + }, + "position": { + "x": 150.0, + "y": 3455.0 + }, + "type": "Values.Config" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "CfgKpD", + "parameters": { + "Cached": "0.0", + "DefaultValue": "0.08", + "Key": "Ctrl.PiD.Kp" + }, + "position": { + "x": 931.0, + "y": 2805.0 + }, + "type": "Values.Config" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "CfgKiD", + "parameters": { + "Cached": "0.0", + "DefaultValue": "10.0", + "Key": "Ctrl.PiD.Ki" + }, + "position": { + "x": 931.0, + "y": 2616.0 + }, + "type": "Values.Config" + }, + { + "displayName": "", + "domain": "tim_isr", + "id": "CfgKiQ", + "parameters": { + "Cached": "0.0", + "DefaultValue": "10.0", + "Key": "Ctrl.PiQ.Ki" + }, + "position": { + "x": 1236.0, + "y": 2246.0 + }, + "type": "Values.Config" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "CanTx1", + "parameters": { + "Bus": "2.0", + "Id": "291.0", + "Ext": "0.0", + "Dlc": "3.0", + "Rate": "10.0", + "LastMs": "0.0", + "D0": "222.0", + "D1": "173.0", + "D2": "190.0", + "D3": "0.0", + "D4": "0.0", + "D5": "0.0", + "D6": "0.0", + "D7": "0.0" + }, + "position": { + "x": 0.0, + "y": 1550.0 + }, + "type": "Actuators.CanTx" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "CanRx1", + "parameters": { + "Bus": "2.0", + "Id": "801.0", + "LastSeq": "0.0" + }, + "position": { + "x": 0.0, + "y": 1650.0 + }, + "type": "Sensors.CanRx" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "LogCanD0", + "parameters": { + "Key": "can_rx_d0" + }, + "position": { + "x": 300.0, + "y": 1650.0 + }, + "type": "Debug.TelemetryLog" + }, + { + "displayName": "", + "domain": "app_loop", + "id": "CarrierAuto", + "parameters": { + "fe_1": "0.0", + "fsw_1": "2000.0", + "fe_2": "15.0", + "fsw_2": "2000.0", + "fe_3": "20.0", + "fsw_3": "3000.0", + "fe_4": "30.0", + "fsw_4": "3000.0", + "fe_5": "40.0", + "fsw_5": "4000.0", + "fe_6": "0.0", + "fsw_6": "0.0" + }, + "position": { + "x": 760.0, + "y": 1620.0 + }, + "type": "Actuators.CarrierAuto" + } + ] } diff --git a/Assets/NodeTemplates/Actuators.CarrierAuto/inline.cpp b/Assets/NodeTemplates/Actuators.CarrierAuto/inline.cpp new file mode 100644 index 00000000..9980ef97 --- /dev/null +++ b/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); +} diff --git a/Assets/NodeTemplates/Actuators.CarrierAuto/node.json b/Assets/NodeTemplates/Actuators.CarrierAuto/node.json new file mode 100644 index 00000000..6b24f449 --- /dev/null +++ b/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."} + } +} diff --git a/Assets/NodeTemplates/Actuators.ModulationAuto/inline.cpp b/Assets/NodeTemplates/Actuators.ModulationAuto/inline.cpp new file mode 100644 index 00000000..1efe9386 --- /dev/null +++ b/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(); +} diff --git a/Assets/NodeTemplates/Actuators.ModulationAuto/node.json b/Assets/NodeTemplates/Actuators.ModulationAuto/node.json new file mode 100644 index 00000000..36c8bbfa --- /dev/null +++ b/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." + } + } +} diff --git a/Assets/NodeTemplates/Control.Pi/inline.cpp b/Assets/NodeTemplates/Control.Pi/inline.cpp index 869343c0..0d290dbd 100644 --- a/Assets/NodeTemplates/Control.Pi/inline.cpp +++ b/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; diff --git a/Images/Gen6FW/CMakeLists.txt b/Images/Gen6FW/CMakeLists.txt index 305a6db8..109659f5 100644 --- a/Images/Gen6FW/CMakeLists.txt +++ b/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 diff --git a/Images/Gen6FW/Inc/Inverter/Control/FocControlManager.h b/Images/Gen6FW/Inc/Inverter/Control/FocControlManager.h index caf11d2a..4cf51b6d 100644 --- a/Images/Gen6FW/Inc/Inverter/Control/FocControlManager.h +++ b/Images/Gen6FW/Inc/Inverter/Control/FocControlManager.h @@ -2,6 +2,7 @@ #include "Inverter/Control/FocController.h" +#include #include 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. * diff --git a/Images/Gen6FW/Inc/Inverter/Drivers/CAN/FdcanFault.h b/Images/Gen6FW/Inc/Inverter/Drivers/CAN/FdcanFault.h index 08d1ab0f..23309b8d 100644 --- a/Images/Gen6FW/Inc/Inverter/Drivers/CAN/FdcanFault.h +++ b/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 diff --git a/Images/Gen6FW/Inc/Inverter/Drivers/PWM/ModulationSwitch.h b/Images/Gen6FW/Inc/Inverter/Drivers/PWM/ModulationSwitch.h new file mode 100644 index 00000000..cb82eca6 --- /dev/null +++ b/Images/Gen6FW/Inc/Inverter/Drivers/PWM/ModulationSwitch.h @@ -0,0 +1,26 @@ +#pragma once + +#include + +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 diff --git a/Images/Gen6FW/Inc/Inverter/Drivers/PWM/Modulator.h b/Images/Gen6FW/Inc/Inverter/Drivers/PWM/Modulator.h new file mode 100644 index 00000000..40f8d092 --- /dev/null +++ b/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 + +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 diff --git a/Images/Gen6FW/Inc/Inverter/Drivers/PWM/SheTables.h b/Images/Gen6FW/Inc/Inverter/Drivers/PWM/SheTables.h new file mode 100644 index 00000000..11641daf --- /dev/null +++ b/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 + +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 diff --git a/Images/Gen6FW/Inc/Inverter/Drivers/PWM/pwm.h b/Images/Gen6FW/Inc/Inverter/Drivers/PWM/pwm.h index 5ffd028e..5dee2094 100644 --- a/Images/Gen6FW/Inc/Inverter/Drivers/PWM/pwm.h +++ b/Images/Gen6FW/Inc/Inverter/Drivers/PWM/pwm.h @@ -75,6 +75,25 @@ void PWM_ResetSPWMElectricalCycles(void); */ float PWM_GetSPWMAngle(void); +/** + * @brief Force one phase leg's output level immediately (sync modulators). + * + * high=true forces OCxREF active (high-side on after dead-time insertion); + * false forces inactive (low-side on). The write is immediate (OCxM forced + * mode has no preload); dead-time insertion and the BKIN break path stay in + * effect, and TIM1 keeps running so the ADC trigger cadence is unchanged. + * CH4 (the ADC trigger) is never touched. + */ +void PWM_ForcePhaseLevel(uint8_t phase, bool high); + +/** + * @brief Restore all three phase channels to PWM1 output-compare mode. + * + * The CCRs keep their last written values; preloaded duties apply from the + * next update event, so an async modulator (SVPWM) resumes glitch-free. + */ +void PWM_ReleaseForcedOutputs(void); + void PWM_StartPhase(uint8_t phase); void PWM_StopPhase(uint8_t phase); void PWM_Start(void); diff --git a/Images/Gen6FW/Inc/Inverter/Drivers/Sensors/EncoderADC.h b/Images/Gen6FW/Inc/Inverter/Drivers/Sensors/EncoderADC.h index 0bf614cd..f6a8dd38 100644 --- a/Images/Gen6FW/Inc/Inverter/Drivers/Sensors/EncoderADC.h +++ b/Images/Gen6FW/Inc/Inverter/Drivers/Sensors/EncoderADC.h @@ -110,8 +110,9 @@ class EncoderADC { /** * @brief Mechanical speed in RPM, signed by direction. * - * Derived in the DMA ISR from unwrapped angle deltas at the 10 kHz - * sample rate, low-passed with an EMA (~100 ms settling). + * Tracking-observer (2nd-order PLL) estimate, updated per DMA sample; + * ~60 Hz bandwidth, ~4 ms tracking lag, per-sample angle noise + * strongly attenuated. */ float rpmMech() const { return m_rpm_ema; } @@ -276,17 +277,18 @@ class EncoderADC { bool m_mag_ema_init = false; volatile uint32_t m_isr_count = 0; - /* Mechanical speed estimation (main loop, time-based window). - * m_sample_hz is measured from the actual trigger rate in diagnose(). */ - static constexpr float RPM_ALPHA = 0.005f; - static constexpr uint32_t RPM_WINDOW_MS = 40U; /* long enough to average - away per-sample angle noise (EMI) that a 1-sample delta amplifies. */ + /* Mechanical speed estimation: tracking observer (2nd-order PLL) on the + * angle stream, updated per DMA sample. ~60 Hz bandwidth tracks in + * ~4 ms while crushing per-sample angle noise. dt comes from the DWT + * cycle counter so it stays correct when the sample rate follows the + * carrier (TRGO2 sync during control). */ + static constexpr float OBS_BW_HZ = 60.0f; /**< observer bandwidth */ + static constexpr float OBS_ZETA = 1.0f; /**< damping ratio */ float m_sample_hz = 10000.0f; - float m_rpm_prev_angle = 0.0f; - float m_rpm_filt_angle = 0.0f; - float m_unwrapped_angle = 0.0f; - float m_window_ref_angle = 0.0f; - uint32_t m_rpm_window_ms = 0; + float m_obs_theta = 0.0f; /**< mech angle estimate [rad] */ + float m_obs_omega = 0.0f; /**< mech speed estimate [rad/s] */ + uint32_t m_obs_last_cycles = 0; + uint32_t m_obs_rejects = 0; /**< glitch-rejected samples (diagnostic) */ bool m_rpm_init = false; volatile float m_rpm_ema = 0.0f; }; diff --git a/Images/Gen6FW/Inc/Inverter/platform_api.h b/Images/Gen6FW/Inc/Inverter/platform_api.h index 79027129..647b9b9f 100644 --- a/Images/Gen6FW/Inc/Inverter/platform_api.h +++ b/Images/Gen6FW/Inc/Inverter/platform_api.h @@ -32,6 +32,59 @@ void platform_pwm_set(float du, float dv, float dw); */ void platform_pwm_set_voltage_vector(float valpha, float vbeta, float vdc); +/* -------------------------------------------------------------------------- + * Modulation mode switching (open-loop ramp <-> N-pulse pattern) + * + * HAL-level live handoff primitives: phase-locked both directions, TIM1 + * always owns the gate pins (dead time, MOE, BKIN stay armed). A graph + * supervisor (e.g. Actuators.ModulationAuto) drives these from thresholds; + * the switching machinery itself stays in the base image. + * -------------------------------------------------------------------------- */ + +/** + * @brief Commanded open-loop ramp frequency [Hz] (0 when not ramping). + */ +float platform_get_ol_freq_hz(void); + +/** + * @brief Best available electrical frequency [Hz]: the pattern's own + * frequency in pattern mode, the FOC speed estimate under FOC, otherwise + * the commanded open-loop ramp frequency. Drives mode supervisors. + */ +float platform_get_elec_freq_hz(void); + +/** + * @brief Request a PWM carrier (switching) frequency change [Hz]. + * + * Async-modulation knob: safe while running (ARR preload; the driver keeps + * RCR consistent with the active control mode). Changes smaller than 5 Hz + * are ignored to avoid prescaler churn from a continuously-lerped request. + */ +void platform_pwm_set_carrier_hz(float freq_hz); + +/** + * @brief Live control-loop period [s] (1 / TIM1 update frequency). PI + * nodes should prefer this over a baked Dt when the carrier is + * runtime-variable. + */ +float platform_get_control_dt(void); + +/** + * @brief Active modulation mode: 0 = ramp (SVPWM), 1 = N-pulse pattern. + */ +uint8_t platform_modulation_mode(void); + +/** + * @brief Phase-locked handoff ramp -> N-pulse pattern at the same frequency. + * @return false if the ramp is not running (or FOC is active). + */ +bool platform_modulation_to_pattern(uint32_t pulses_per_quarter, float duty); + +/** + * @brief Phase-locked handoff pattern -> ramp (resumes at the pattern angle). + */ +bool platform_modulation_to_ramp(void); + /* -------------------------------------------------------------------------- * Sensor inputs * -------------------------------------------------------------------------- */ diff --git a/Images/Gen6FW/Src/Inverter/Command/CommandInitializer.cpp b/Images/Gen6FW/Src/Inverter/Command/CommandInitializer.cpp index 59db54e7..9fb66e62 100644 --- a/Images/Gen6FW/Src/Inverter/Command/CommandInitializer.cpp +++ b/Images/Gen6FW/Src/Inverter/Command/CommandInitializer.cpp @@ -9,6 +9,7 @@ void registerControlCommands(CommandManager& mgr); void registerCalibrationCommands(CommandManager& mgr); void registerFocCommands(CommandManager& mgr); void registerCanCommands(CommandManager& mgr); +void registerSheCommands(CommandManager& mgr); /* TIME_DOMAIN: APPLICATION_COMMAND_REGISTRATION * Registers all shell commands at boot. Commands execute in main-loop context. @@ -26,4 +27,5 @@ void initializeCommands() { registerCalibrationCommands(mgr); registerFocCommands(mgr); registerCanCommands(mgr); + registerSheCommands(mgr); } diff --git a/Images/Gen6FW/Src/Inverter/Command/Commands/SheCommands.cpp b/Images/Gen6FW/Src/Inverter/Command/Commands/SheCommands.cpp new file mode 100644 index 00000000..04a9c7e5 --- /dev/null +++ b/Images/Gen6FW/Src/Inverter/Command/Commands/SheCommands.cpp @@ -0,0 +1,232 @@ +#include "Inverter/Command/CommandInterface.h" +#include "Inverter/Command/CommandContext.h" +#include "Inverter/Control/FocControlManager.h" +#include "Inverter/Control/OpenLoopController.h" +#include "Inverter/Control/FaultManager.h" +#include "Inverter/Drivers/GateDriver/gate_driver.h" +#include "Inverter/Drivers/PWM/Modulator.h" +#include "Inverter/Drivers/PWM/ModulationSwitch.h" +#include "Inverter/Drivers/PWM/pwm.h" +#include "Inverter/Telemetry.h" + +#include "main.h" + +using Inverter::FaultManager; +using Inverter::FaultSeverity; +using Inverter::focControlManager; +using Inverter::openLoopController; + +namespace { + +bool otherControlActive() { + if (focControlManager().isRunning() || openLoopController().isRunning()) { + Telemetry::printf("[SHE] ERROR: FOC or open-loop is running; stop it first"); + return true; + } + if (FaultManager::instance().isSeverityActive(FaultSeverity::Critical) || + FaultManager::instance().isSeverityActive(FaultSeverity::High)) { + Telemetry::printf("[SHE] ERROR: active Critical/High faults"); + FaultManager::instance().printSummary(); + return true; + } + return false; +} + +/* Blocking safe-start: park at zero vector, cycle gate-driver reset, verify + * ready, start TIM1 outputs, hand the modulation slot to the SHEPWM engine. + * The caller stages the pattern (SHE table or N-pulse) before calling. */ +bool sheSafeStart() { + if (otherControlActive()) return false; + if (Inverter::shepwmIsRunning()) { + Telemetry::printf("[SHE] already running; use sheset/npset or shestop"); + return false; + } + + PWM_SetThreePhaseDuty(50.0f, 50.0f, 50.0f); + GateDriver_DisableOutputs(); + HAL_Delay(10); + GateDriver_EnableOutputs(); + HAL_Delay(10); + + if (!GateDriver_IsReady() || GateDriver_IsFault()) { + Telemetry::printf("[SHE] ERROR: gate driver not ready or fault latched"); + GateDriver_DisableOutputs(); + return false; + } + + PWM_ClearFault(); + PWM_Start(); + + if ((TIM1->BDTR & TIM_BDTR_MOE) == 0U) { + Telemetry::printf("[SHE] ERROR: TIM1 MOE not active after PWM start"); + GateDriver_DisableOutputs(); + return false; + } + + if (!Inverter::shepwmModulator().enter(0.0f, 0.0f)) { + Telemetry::printf("[SHE] ERROR: enter failed"); + PWM_Stop(); + GateDriver_DisableOutputs(); + return false; + } + Inverter::setActiveModulator(&Inverter::shepwmModulator()); + return true; +} + +class SheStartCommand : public CommandInterface { +public: + SheStartCommand() + : CommandInterface("shestart", "Start pattern output: shestart [pulses_per_qtr]", + {ArgSpec{"freq_hz", "Hz", 0.1f, 1000.0f, 0.0f, true, ArgSpec::FLOAT}, + ArgSpec{"duty_or_mi", "", 0.0f, 1.0f, 0.0f, true, ArgSpec::FLOAT}, + ArgSpec{"pulses", "/qtr", 0.0f, 128.0f, 0.0f, false, ArgSpec::FLOAT}}) {} + + void execute(const ArgValue* args, CommandContext&) override { + if (args[2].present) { + /* N-pulse mode: arg1 = duty (high fraction per cell). */ + const uint32_t npq = static_cast(args[2].f_val + 0.5f); + Inverter::shepwmSetPulsePattern(args[0].f_val, npq, args[1].f_val); + if (!sheSafeStart()) return; + Telemetry::printf("[SHE] STARTED N-pulse fe=%.2f Hz pulses/qtr=%lu duty=%.3f", + static_cast(args[0].f_val), + static_cast(npq), + static_cast(args[1].f_val)); + } else { + /* SHE table mode: arg1 = modulation index. */ + Inverter::shepwmSetPattern(args[0].f_val, args[1].f_val); + if (!sheSafeStart()) return; + Telemetry::printf("[SHE] STARTED fe=%.2f Hz mi=%.3f", + static_cast(args[0].f_val), + static_cast(args[1].f_val)); + } + } +}; + +class SheStopCommand : public CommandInterface { +public: + SheStopCommand() : CommandInterface("shestop", "Stop SHEPWM/N-pulse output") {} + + void execute(const ArgValue*, CommandContext&) override { + if (!Inverter::shepwmIsRunning()) { + Telemetry::printf("[SHE] not running"); + return; + } + Inverter::shepwmModulator().exit(); + if (Inverter::activeModulator() == &Inverter::shepwmModulator()) { + Inverter::setActiveModulator(nullptr); + } + PWM_Stop(); + GateDriver_DisableOutputs(); + PWM_SetThreePhaseDuty(50.0f, 50.0f, 50.0f); + Telemetry::printf("[SHE] STOPPED"); + } +}; + +class SheSetCommand : public CommandInterface { +public: + SheSetCommand() + : CommandInterface("sheset", "Set pattern: sheset [pulses_per_qtr] (swaps at wrap)", + {ArgSpec{"freq_hz", "Hz", 0.1f, 1000.0f, 0.0f, true, ArgSpec::FLOAT}, + ArgSpec{"duty_or_mi", "", 0.0f, 1.0f, 0.0f, true, ArgSpec::FLOAT}, + ArgSpec{"pulses", "/qtr", 0.0f, 128.0f, 0.0f, false, ArgSpec::FLOAT}}) {} + + void execute(const ArgValue* args, CommandContext&) override { + if (!Inverter::shepwmIsRunning()) { + Telemetry::printf("[SHE] not running; use shestart"); + return; + } + if (args[2].present) { + const uint32_t npq = static_cast(args[2].f_val + 0.5f); + Inverter::shepwmSetPulsePattern(args[0].f_val, npq, args[1].f_val); + Telemetry::printf("[SHE] pattern staged: N-pulse fe=%.2f Hz pulses/qtr=%lu duty=%.3f (swaps at wrap)", + static_cast(args[0].f_val), + static_cast(npq), + static_cast(args[1].f_val)); + } else { + Inverter::shepwmSetPattern(args[0].f_val, args[1].f_val); + Telemetry::printf("[SHE] pattern staged: fe=%.2f Hz mi=%.3f (swaps at wrap)", + static_cast(args[0].f_val), + static_cast(args[1].f_val)); + } + } +}; + +class SheStatusCommand : public CommandInterface { +public: + SheStatusCommand() : CommandInterface("shestatus", "Show SHEPWM/N-pulse status") {} + + void execute(const ArgValue*, CommandContext&) override { + const uint32_t npq = Inverter::shepwmPulseCount(); + if (npq > 0) { + Telemetry::printf("[SHE] run=%s mode=N-pulse fe=%.2f Hz pulses/qtr=%lu duty=%.3f wraps=%lu edges=%lu", + Inverter::shepwmIsRunning() ? "Y" : "N", + static_cast(Inverter::shepwmFrequencyHz()), + static_cast(npq), + static_cast(Inverter::shepwmDuty()), + static_cast(Inverter::shepwmWrapCount()), + static_cast(Inverter::shepwmEdgeCount())); + } else { + Telemetry::printf("[SHE] run=%s mode=SHE-table fe=%.2f Hz mi=%.3f wraps=%lu edges=%lu", + Inverter::shepwmIsRunning() ? "Y" : "N", + static_cast(Inverter::shepwmFrequencyHz()), + static_cast(Inverter::shepwmModulationIndex()), + static_cast(Inverter::shepwmWrapCount()), + static_cast(Inverter::shepwmEdgeCount())); + } + } +}; + +/** + * @brief Live modulator handoff at speed (step 4): open-loop ramp <-> pattern. + * + * handoff 1 ramp (SPWM) -> N-pulse pattern + * handoff 0 pattern -> ramp (same fe/MI) + * + * Thin shell wrapper over Inverter::modulationToPattern/ToRamp (which the + * graph-facing platform_api also calls). Phase-locked both directions. + */ +class HandoffCommand : public CommandInterface { +public: + HandoffCommand() + : CommandInterface("handoff", "Live handoff: 'handoff 1 ' to pattern, 'handoff 0' back to ramp", + {ArgSpec{"to_pattern", "0/1", 0.0f, 1.0f, 0.0f, true, ArgSpec::FLOAT}, + ArgSpec{"pulses", "/qtr", 0.0f, 128.0f, 0.0f, false, ArgSpec::FLOAT}, + ArgSpec{"duty", "", 0.0f, 1.0f, 0.0f, false, ArgSpec::FLOAT}}) {} + + void execute(const ArgValue* args, CommandContext&) override { + if (args[0].f_val >= 0.5f) { + if (!args[1].present || !args[2].present) { + Telemetry::printf("[HO] usage: handoff 1 "); + return; + } + const uint32_t npq = static_cast(args[1].f_val + 0.5f); + if (!Inverter::modulationToPattern(npq, args[2].f_val)) { + Telemetry::printf("[HO] failed: need open-loop ramp running (not FOC)"); + } + } else { + if (!Inverter::modulationToRamp()) { + Telemetry::printf("[HO] failed"); + } + } + } +}; + +SheStartCommand sSheStartCmd; +SheStopCommand sSheStopCmd; +SheSetCommand sSheSetCmd; +SheStatusCommand sSheStatusCmd; +/* DTCMRAM is full; command objects are main-loop-only, so this one lives in + * AXI SRAM (runtime-constructed like all statics; NOLOAD is fine). */ +HandoffCommand sHandoffCmd __attribute__((section(".dma_buffers"))); + +} // namespace + +#include "Inverter/Command/CommandManager.h" + +void registerSheCommands(CommandManager& mgr) { + mgr.registerCommand(&sSheStartCmd); + mgr.registerCommand(&sSheStopCmd); + mgr.registerCommand(&sSheSetCmd); + mgr.registerCommand(&sSheStatusCmd); + mgr.registerCommand(&sHandoffCmd); +} diff --git a/Images/Gen6FW/Src/Inverter/Control/ControlSupervisor.cpp b/Images/Gen6FW/Src/Inverter/Control/ControlSupervisor.cpp index 4e0be2ea..ab720196 100644 --- a/Images/Gen6FW/Src/Inverter/Control/ControlSupervisor.cpp +++ b/Images/Gen6FW/Src/Inverter/Control/ControlSupervisor.cpp @@ -4,6 +4,7 @@ #include "Inverter/Control/FaultManager.h" #include "Inverter/Drivers/GateDriver/gate_driver.h" #include "Inverter/Drivers/PWM/pwm.h" +#include "Inverter/Drivers/PWM/Modulator.h" #include "Inverter/Drivers/Sensors/EncoderADC.h" #include "Inverter/Drivers/Sensors/PhaseCurrentADC.h" #include "Inverter/Telemetry.h" @@ -65,6 +66,11 @@ bool ControlSupervisor::start() { return false; } + if (activeModulator() == &shepwmModulator()) { + Telemetry::printf("[SUP] ERROR: SHEPWM is running; stop it first (shestop)"); + return false; + } + m_state = State::Starting; if (!gateDriverStartup()) { @@ -109,6 +115,15 @@ void ControlSupervisor::stop() { m_state = State::Stopping; + /* If the pattern modulator owns the slot (handoff mode), release it + * first so the outputs actually go quiet. */ + if (Inverter::shepwmIsRunning()) { + Inverter::shepwmModulator().exit(); + if (Inverter::activeModulator() == &Inverter::shepwmModulator()) { + Inverter::setActiveModulator(nullptr); + } + } + /* Zero generated outputs before stopping the ISR. */ app::TimIsrStop(appState.tim_isr); diff --git a/Images/Gen6FW/Src/Inverter/Control/FocControlManager.cpp b/Images/Gen6FW/Src/Inverter/Control/FocControlManager.cpp index af854992..7ddaab3c 100644 --- a/Images/Gen6FW/Src/Inverter/Control/FocControlManager.cpp +++ b/Images/Gen6FW/Src/Inverter/Control/FocControlManager.cpp @@ -12,6 +12,7 @@ #include "Inverter/Control/OpenLoopController.h" #include "Inverter/Drivers/GateDriver/gate_driver.h" #include "Inverter/Drivers/PWM/pwm.h" +#include "Inverter/Drivers/PWM/Modulator.h" #include "Inverter/Drivers/Sensors/DcLinkVoltageSensor.h" #include "Inverter/Drivers/Sensors/EncoderADC.h" #include "Inverter/Drivers/Sensors/PhaseCurrentADC.h" @@ -159,6 +160,11 @@ bool FocControlManager::start(float iq_a, float id_a, bool allow_during_cal) { openLoopController().stop(); } + if (activeModulator() == &shepwmModulator()) { + Telemetry::printf("[FOC] ERROR: SHEPWM is running; stop it first (shestop)"); + return false; + } + if (!allow_during_cal && isAnyCalibrationActive()) { Telemetry::printf("[FOC] ERROR: calibration is active"); return false; @@ -495,7 +501,11 @@ void FocControlManager::onPwmPeriod() { return; } - float angle_deg = encoderADC().lastAngle(); + /* Extrapolated to this control instant from the DWT-timestamped snapshot: + * smooths the sample staircase between the encoder stream and the 10 kHz + * FOC steps (same angle the generated tim_isr domain consumes via + * platform_get_encoder_angle_latest()). */ + float angle_deg = encoderADC().extrapolatedAngleDeg(); float angle_rad = angle_deg * (3.14159265358979323846f / 180.0f); /* Forced-angle diagnostic: ignore the encoder and drive the Park angle @@ -548,6 +558,20 @@ void FocControlManager::onPwmPeriod() { } } +void FocControlManager::suspendForHandoff() { + if (!m_running) { + return; + } + PWM_DisableFocMode(); + PWM_StopUpdateInterrupt(); + m_running = false; + Telemetry::printf("[FOC] suspended for modulation handoff"); +} + +bool FocControlManager::restartLastSetpoints() { + return start(m_setpoints.iq_a, m_setpoints.id_a, false); +} + void FocControlManager::requestSafeStopFromIsr() { /* Never call the full stop() routine from an ISR: it uses HAL delays and * UART printfs. Park at zero vector and let the main loop perform the diff --git a/Images/Gen6FW/Src/Inverter/Control/OpenLoopController.cpp b/Images/Gen6FW/Src/Inverter/Control/OpenLoopController.cpp index e354867c..5bfb86b2 100644 --- a/Images/Gen6FW/Src/Inverter/Control/OpenLoopController.cpp +++ b/Images/Gen6FW/Src/Inverter/Control/OpenLoopController.cpp @@ -3,6 +3,7 @@ #include "Inverter/Control/FocControlManager.h" #include "Inverter/Drivers/PWM/pwm.h" +#include "Inverter/Drivers/PWM/Modulator.h" #include "Inverter/Drivers/GateDriver/gate_driver.h" #include "Inverter/Drivers/Sensors/PhaseCurrentADC.h" #include "Inverter/Drivers/Sensors/EncoderADC.h" @@ -275,6 +276,11 @@ bool OpenLoopController::start(float freq_hz, float modulation_index) { return false; } + if (activeModulator() == &shepwmModulator()) { + Telemetry::printf("[OL] ERROR: SHEPWM is running; stop it first (shestop)"); + return false; + } + if (m_running || m_starting) { stop(); } @@ -368,6 +374,15 @@ void OpenLoopController::stepStartup(uint32_t now_ms) { } void OpenLoopController::stop() { + /* If the pattern modulator owns the slot (handoff mode), release it + * first so the outputs actually go quiet and the next start is clean. */ + if (Inverter::shepwmIsRunning()) { + Inverter::shepwmModulator().exit(); + if (Inverter::activeModulator() == &Inverter::shepwmModulator()) { + Inverter::setActiveModulator(nullptr); + } + } + /* Immediate coast: turn off the PWM outputs and assert the gate-driver * reset line so all six IGBTs stop switching right away. */ PWM_StopSPWM(); diff --git a/Images/Gen6FW/Src/Inverter/Drivers/CAN/CanBus.cpp b/Images/Gen6FW/Src/Inverter/Drivers/CAN/CanBus.cpp index 385bb152..4bc04211 100644 --- a/Images/Gen6FW/Src/Inverter/Drivers/CAN/CanBus.cpp +++ b/Images/Gen6FW/Src/Inverter/Drivers/CAN/CanBus.cpp @@ -1,5 +1,6 @@ #include "Inverter/Drivers/CAN/CanBus.h" +#include "Inverter/Drivers/CAN/FdcanFault.h" #include "Inverter/Drivers/Storage/RteParamStore.h" #include "Inverter/Telemetry.h" @@ -140,6 +141,10 @@ bool CanBus::init() { } } + /* Route error-status notifications (bus-off, error-passive, log overflow) + * to the FaultManager — only for buses that survived init. */ + (void)fdcanFaultInit(m_enabled[0], m_enabled[1]); + /* RX FIFO0 + error interrupts share IT0 on each peripheral. */ HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, 6, 0); HAL_NVIC_SetPriority(FDCAN2_IT0_IRQn, 6, 0); diff --git a/Images/Gen6FW/Src/Inverter/Drivers/CAN/FdcanFault.cpp b/Images/Gen6FW/Src/Inverter/Drivers/CAN/FdcanFault.cpp index 8d3d7b37..162d5d04 100644 --- a/Images/Gen6FW/Src/Inverter/Drivers/CAN/FdcanFault.cpp +++ b/Images/Gen6FW/Src/Inverter/Drivers/CAN/FdcanFault.cpp @@ -5,16 +5,19 @@ namespace Inverter { -bool fdcanFaultInit() { +bool fdcanFaultInit(bool enable_bus_a, bool enable_bus_b) { constexpr uint32_t kNotify = FDCAN_IT_BUS_OFF | FDCAN_IT_ERROR_PASSIVE | FDCAN_IT_ERROR_LOGGING_OVERFLOW; /* Both buses: error interrupts ride the IT0 line, whose NVIC is owned - * by the CanBus driver. */ - if (HAL_FDCAN_ActivateNotification(&hfdcan1, kNotify, 0) != HAL_OK) { + * by the CanBus driver. Only enabled buses are armed: a disabled bus + * must never raise a CAN fault. */ + if (enable_bus_a && + HAL_FDCAN_ActivateNotification(&hfdcan1, kNotify, 0) != HAL_OK) { return false; } - if (HAL_FDCAN_ActivateNotification(&hfdcan2, kNotify, 0) != HAL_OK) { + if (enable_bus_b && + HAL_FDCAN_ActivateNotification(&hfdcan2, kNotify, 0) != HAL_OK) { return false; } return true; diff --git a/Images/Gen6FW/Src/Inverter/Drivers/PWM/ModulationSwitch.cpp b/Images/Gen6FW/Src/Inverter/Drivers/PWM/ModulationSwitch.cpp new file mode 100644 index 00000000..95677586 --- /dev/null +++ b/Images/Gen6FW/Src/Inverter/Drivers/PWM/ModulationSwitch.cpp @@ -0,0 +1,170 @@ +#include "Inverter/Drivers/PWM/ModulationSwitch.h" + +#include "Inverter/Calibration/MotorCalibration.h" +#include "Inverter/Control/ControlSupervisor.h" +#include "Inverter/Control/FocControlManager.h" +#include "Inverter/Control/MotorParameters.h" +#include "Inverter/Control/OpenLoopController.h" +#include "Inverter/Drivers/PWM/Modulator.h" +#include "Inverter/Drivers/PWM/pwm.h" +#include "Inverter/Drivers/Sensors/EncoderADC.h" +#include "Inverter/Telemetry.h" + +namespace Inverter { +namespace { + +/* Where the pattern mode was entered from: decides what modulationToRamp() + * resumes. */ +bool s_patternFromFoc = false; +bool s_patternFromGraph = false; + +} // namespace + +ModulationMode modulationMode() { + return (activeModulator() == &shepwmModulator()) + ? ModulationMode::Pattern + : ModulationMode::Ramp; +} + +bool modulationToPattern(uint32_t pulses_per_quarter, float duty) { + if (shepwmIsRunning()) { + return true; /* already there */ + } + + if (focControlManager().isRunning()) { + /* Legacy FOC -> pattern: capture the applied voltage-vector angle and + * electrical frequency, suspend the current loop (outputs stay + * live), and enter the pattern phase-locked to that angle. */ + const float angle = focControlManager().electricalVoltageAngleRad(); + float fe = focControlManager().electricalSpeedRadPerSec() / 6.283185307f; + if (fe < 0.0f) fe = -fe; + if (fe < 0.1f) { + return false; + } + + shepwmSetPulsePattern(fe, pulses_per_quarter, duty); + focControlManager().suspendForHandoff(); + if (!shepwmModulator().enter(angle, duty)) { + (void)focControlManager().restartLastSetpoints(); + return false; + } + setActiveModulator(&shepwmModulator()); + s_patternFromFoc = true; + + Telemetry::printf("[MOD] FOC -> pattern: fe=%.2f Hz pulses/qtr=%lu duty=%.3f", + static_cast(fe), + static_cast(pulses_per_quarter), + static_cast(duty)); + return true; + } + + if (ControlSupervisor::instance().isRunning()) { + /* Graph FOC -> pattern: the generated control step dies with the + * TIM1 update ISR; outputs (and MOE) stay live. Angle from the + * encoder + calibration, led by a q-axis approximation. */ + const MotorCalibration& cal = MotorCalibration::instance(); + if (!cal.valid || cal.pole_count <= 0.0f) { + Telemetry::printf("[MOD] ERROR: motor calibration invalid; cannot phase-lock"); + return false; + } + const MotorParameters mp = buildMotorParametersFromCalibration(cal, 0.0f); + constexpr float kDegToRad = 3.14159265358979323846f / 180.0f; + const float rpm = encoderADC().rpmMech(); + float fe = rpm * mp.pole_pairs / 60.0f; + if (fe < 0.0f) fe = -fe; + if (fe < 0.1f) { + return false; + } + const float lead = (rpm >= 0.0f) ? 1.5707963268f : -1.5707963268f; + float angle = mp.encoder_offset_rad + + mp.encoder_sign * encoderADC().extrapolatedAngleDeg() * kDegToRad * + mp.pole_pairs / mp.encoder_cycles_per_rev + lead; + + shepwmSetPulsePattern(fe, pulses_per_quarter, duty); + PWM_DisableFocMode(); + PWM_StopUpdateInterrupt(); + if (!shepwmModulator().enter(angle, duty)) { + PWM_EnableFocMode(); + PWM_StartUpdateInterrupt(); + return false; + } + setActiveModulator(&shepwmModulator()); + s_patternFromGraph = true; + + Telemetry::printf("[MOD] graph-FOC -> pattern: fe=%.2f Hz pulses/qtr=%lu duty=%.3f", + static_cast(fe), + static_cast(pulses_per_quarter), + static_cast(duty)); + return true; + } + + if (!spwmIsRunning()) { + return false; + } + + const float angle = spwmAngleRad(); + const float fe = spwmFundamentalFreqHz(); + + shepwmSetPulsePattern(fe, pulses_per_quarter, duty); + PWM_StopSPWM(); + if (!shepwmModulator().enter(angle, duty)) { + /* Restore the ramp where it was. */ + PWM_StartSPWM(fe, spwmModulationIndex()); + spwmSetAngle(angle); + return false; + } + setActiveModulator(&shepwmModulator()); + s_patternFromFoc = false; + s_patternFromGraph = false; + + Telemetry::printf("[MOD] ramp -> pattern: fe=%.2f Hz pulses/qtr=%lu duty=%.3f", + static_cast(fe), + static_cast(pulses_per_quarter), + static_cast(duty)); + return true; +} + +bool modulationToRamp() { + if (!shepwmIsRunning()) { + return true; /* already there */ + } + + if (s_patternFromFoc) { + /* Pattern -> FOC: release the pins and restart the current loop + * with the last setpoints (flying restart into the spinning motor). */ + s_patternFromFoc = false; + shepwmModulator().exit(); + setActiveModulator(nullptr); + const bool ok = focControlManager().restartLastSetpoints(); + Telemetry::printf("[MOD] pattern -> FOC: %s", ok ? "restarted" : "RESTART FAILED"); + return ok; + } + + if (s_patternFromGraph) { + /* Pattern -> graph FOC: re-enable the update ISR; the generated + * domain resumes with its preserved state (ControlSupervisor never + * left Running). */ + s_patternFromGraph = false; + shepwmModulator().exit(); + setActiveModulator(nullptr); + PWM_EnableFocMode(); + PWM_StartUpdateInterrupt(); + Telemetry::printf("[MOD] pattern -> graph FOC: resumed"); + return true; + } + + const float angle = shepwmAngleRad(); + const float fe = shepwmFrequencyHz(); + const float mi = openLoopController().modulationIndex(); + + shepwmModulator().exit(); + setActiveModulator(nullptr); + PWM_StartSPWM(fe, mi); + spwmSetAngle(angle); + + Telemetry::printf("[MOD] pattern -> ramp: fe=%.2f Hz mi=%.3f", + static_cast(fe), static_cast(mi)); + return true; +} + +} // namespace Inverter diff --git a/Images/Gen6FW/Src/Inverter/Drivers/PWM/Modulator.cpp b/Images/Gen6FW/Src/Inverter/Drivers/PWM/Modulator.cpp new file mode 100644 index 00000000..e2066063 --- /dev/null +++ b/Images/Gen6FW/Src/Inverter/Drivers/PWM/Modulator.cpp @@ -0,0 +1,200 @@ +/** + * @file Modulator.cpp + * @brief Modulator slot registry + the async modulators (SVPWM, SPWM). + * + * The math below is the exact code moved out of pwm.cpp (step 1 of the + * multi-modulator plan: wrap existing modulation behind the Modulator + * interface with zero behavioral change). + */ + +#include "Inverter/Drivers/PWM/Modulator.h" +#include "Inverter/Drivers/PWM/pwm.h" + +#include + +namespace Inverter { +namespace { + +constexpr float kTwoPi = 6.283185307f; + +/* SVPWM linear over-modulation limit: 2/sqrt(3) */ +constexpr float kSvpwmMMax = 1.154700538f; + +float clampDuty(float d) { + if (d < 0.0f) return 0.0f; + if (d > 100.0f) return 100.0f; + return d; +} + +/** + * @brief SVPWM: stationary-frame voltage vector -> three-phase duties. + * + * Stateless aside from the computed duties. Externally clocked: the control + * loop (FOC hook, generated tim_isr domain) calls update() + commit() each + * control period. + */ +class SvpwmModulator final : public Modulator { +public: + const char* name() const override { return "svpwm"; } + bool runsInPwmIsr() const override { return false; } + + bool enter(float /*theta_e_rad*/, float /*modulation_index*/) override { return true; } + void exit() override {} + + void update(float valpha_v, float vbeta_v, float vdc_v) override { + if (vdc_v <= 1.0f) { + m_du = 50.0f; + m_dv = 50.0f; + m_dw = 50.0f; + return; + } + + /* Clamp the alpha/beta magnitude to the linear modulation limit before + * converting to three-phase voltages. The SVPWM linear limit is + * Vdc / sqrt(3). */ + const float sqrt3 = 1.7320508075688772f; + float valpha = valpha_v; + float vbeta = vbeta_v; + const float v_max_linear = (vdc_v / sqrt3) * 0.95f; + const float v_albe_sq = valpha * valpha + vbeta * vbeta; + if (v_albe_sq > v_max_linear * v_max_linear && v_albe_sq > 1e-12f) { + const float scale = v_max_linear / std::sqrt(v_albe_sq); + valpha *= scale; + vbeta *= scale; + } + + /* Inverse Clarke: alpha/beta -> A/B/C. */ + const float va = valpha; + const float vb = -0.5f * valpha + 0.5f * sqrt3 * vbeta; + const float vc = -0.5f * valpha - 0.5f * sqrt3 * vbeta; + + /* Min-max SVPWM zero-sequence injection. */ + const float v_max = (va > vb) ? ((va > vc) ? va : vc) : ((vb > vc) ? vb : vc); + const float v_min = (va < vb) ? ((va < vc) ? va : vc) : ((vb < vc) ? vb : vc); + const float vcom = 0.5f * (v_max + v_min); + + m_du = clampDuty(50.0f + 50.0f * (va - vcom) / vdc_v); + m_dv = clampDuty(50.0f + 50.0f * (vb - vcom) / vdc_v); + m_dw = clampDuty(50.0f + 50.0f * (vc - vcom) / vdc_v); + } + + void commit() override { + PWM_SetThreePhaseDuty(m_du, m_dv, m_dw); + } + +private: + float m_du = 50.0f; + float m_dv = 50.0f; + float m_dw = 50.0f; +}; + +/** + * @brief Open-loop SPWM ramp with min-max zero-sequence injection. + * + * Self-clocked: the TIM1 update ISR drives update() + commit() while this + * modulator owns the slot. Params arrive via spwmSetParams() (legacy + * PWM_SetSPWMParams flow); enter() restarts the ramp from angle zero. + */ +class SpwmModulator final : public Modulator { +public: + const char* name() const override { return "spwm"; } + bool runsInPwmIsr() const override { return true; } + + bool enter(float /*theta_e_rad*/, float /*modulation_index*/) override { + m_angle = 0.0f; + m_running = true; + return true; + } + + void exit() override { + m_running = false; + } + + void setParams(float fundamental_freq_hz, float modulation_index) { + if (fundamental_freq_hz < 0.0f) fundamental_freq_hz = 0.0f; + if (modulation_index < 0.0f) modulation_index = 0.0f; + if (modulation_index > kSvpwmMMax) modulation_index = kSvpwmMMax; + m_fundamental_freq_hz = fundamental_freq_hz; + m_modulation_index = modulation_index; + } + + void update(float /*valpha_v*/, float /*vbeta_v*/, float /*vdc_v*/) override { + float angle = m_angle; + const float m = m_modulation_index; + + /* Three-phase sinusoidal references, 120 deg apart. */ + const float u = m * sinf(angle); + const float v = m * sinf(angle - kTwoPi / 3.0f); + const float w = m * sinf(angle + kTwoPi / 3.0f); + + /* Min-max SVPWM zero-sequence injection to extend linear range to 2/sqrt(3). */ + const float v_max = (u > v) ? ((u > w) ? u : w) : ((v > w) ? v : w); + const float v_min = (u < v) ? ((u < w) ? u : w) : ((v < w) ? v : w); + const float v0 = -0.5f * (v_max + v_min); + + /* Convert to centered duty cycles [0, 100]. */ + m_du = clampDuty(50.0f + 50.0f * (u + v0)); + m_dv = clampDuty(50.0f + 50.0f * (v + v0)); + m_dw = clampDuty(50.0f + 50.0f * (w + v0)); + + /* Advance angle by one PWM period. */ + angle += kTwoPi * m_fundamental_freq_hz / PWM_GetFrequency(); + if (angle >= kTwoPi) { + angle -= kTwoPi; + ++m_elec_cycles; + } + m_angle = angle; + } + + void commit() override { + PWM_SetThreePhaseDuty(m_du, m_dv, m_dw); + } + + bool isRunning() const { return m_running; } + float angleRad() const { return m_running ? m_angle : 0.0f; } + void setAngle(float angle_rad) { + while (angle_rad < 0.0f) angle_rad += kTwoPi; + while (angle_rad >= kTwoPi) angle_rad -= kTwoPi; + m_angle = angle_rad; + } + uint32_t elecCycles() const { return m_elec_cycles; } + void resetElecCycles() { m_elec_cycles = 0; } + + float fundamentalFreqHz() const { return m_fundamental_freq_hz; } + float modulationIndex() const { return m_modulation_index; } + +private: + float m_angle = 0.0f; + float m_fundamental_freq_hz = 1.0f; + float m_modulation_index = 0.0f; + float m_du = 50.0f; + float m_dv = 50.0f; + float m_dw = 50.0f; + uint32_t m_elec_cycles = 0; + bool m_running = false; +}; + +SvpwmModulator s_svpwm; +SpwmModulator s_spwm; +Modulator* s_active = nullptr; + +} // namespace + +Modulator* activeModulator() { return s_active; } +void setActiveModulator(Modulator* m) { s_active = m; } + +Modulator& svpwmModulator() { return s_svpwm; } +Modulator& spwmModulator() { return s_spwm; } + +bool spwmIsRunning() { return s_spwm.isRunning(); } +float spwmAngleRad() { return s_spwm.angleRad(); } +uint32_t spwmElectricalCycles() { return s_spwm.elecCycles(); } +void spwmResetElectricalCycles() { s_spwm.resetElecCycles(); } +void spwmSetParams(float fundamental_freq_hz, float modulation_index) { + s_spwm.setParams(fundamental_freq_hz, modulation_index); +} +float spwmFundamentalFreqHz() { return s_spwm.fundamentalFreqHz(); } +float spwmModulationIndex() { return s_spwm.modulationIndex(); } +void spwmSetAngle(float angle_rad) { s_spwm.setAngle(angle_rad); } + +} // namespace Inverter diff --git a/Images/Gen6FW/Src/Inverter/Drivers/PWM/ShepwmModulator.cpp b/Images/Gen6FW/Src/Inverter/Drivers/PWM/ShepwmModulator.cpp new file mode 100644 index 00000000..e7214c36 --- /dev/null +++ b/Images/Gen6FW/Src/Inverter/Drivers/PWM/ShepwmModulator.cpp @@ -0,0 +1,408 @@ +/** + * @file ShepwmModulator.cpp + * @brief SHEPWM synchronous modulator (Option B: TIM1 forced-output modes). + * + * One TIM5 cycle = one electrical cycle (ARR = f_tim / fe). The switching + * pattern is a precomputed, ping-pong-buffered event list (timer count, + * phase, level) expanded from the offline-solved quarter-wave table family + * in SheTables.h (Tools/she_table_gen.py) with linear interpolation between + * MI grid points. A compare-chain ISR on TIM5 CH1 fires one edge ahead and + * writes TIM1 forced-output modes (OCxM) via the pwm driver; TIM1 always owns + * the gate pins, so dead time, the BKIN hardware trip and the ADC trigger + * cadence all stay in circuit. + * + * Glitch discipline: the slow context only ever writes the inactive buffer; + * the single pointer swap (+ARR) happens inside the wrap ISR. Worst-case + * failure is "new pattern applies next electrical cycle". + */ + +#include "Inverter/Drivers/PWM/Modulator.h" +#include "Inverter/Drivers/PWM/SheTables.h" +#include "Inverter/Drivers/PWM/pwm.h" + +#include "main.h" + +#include +#include + +namespace Inverter { +namespace { + +/* TIM5 is on APB1: 137.5 MHz timer clock (same as TIM2 in EncoderADC). */ +constexpr uint32_t kTim5ClockHz = 137500000UL; + +constexpr uint32_t kAnglesPerQuarter = shetab::kAnglesPerQuarter; + +/* Runtime N-pulse mode: per quarter, `npq` cells each hold one centered + * pulse (2 angles per cell). Capped so the ping-pong buffers stay small. */ +constexpr uint32_t kMaxPulsesPerQuarter = 128; +constexpr uint32_t kMaxAngles = 2U * kMaxPulsesPerQuarter; + +/* events/phase = 4*n_angles + 2 half-cycle boundary edges; 3 phases. */ +constexpr uint32_t kMaxEvents = 3U * (4U * kMaxAngles + 2U); + +/* Process events due within this many counts of "now" (29 ns): absorbs ISR + * latency and coincident edges from two phases. */ +constexpr uint32_t kDueGuardCounts = 4U; + +/* Electrical-angle offset between the FOC theta_e convention (phase-U + * fundamental peaks at theta_e = +90 deg) and the SHE table convention + * (phase-U leg switches at theta = 0). Verified on the scope during + * handoff bring-up (step 4). */ +constexpr float kSheAngleOffsetRad = 1.5707963268f; + +constexpr float kTwoPi = 6.283185307f; +constexpr float kPi = 3.1415926536f; + +struct SheEvent { + uint32_t cnt; /* TIM5 count of the edge within the electrical cycle */ + uint8_t phase; /* 0=U, 1=V, 2=W */ + uint8_t level; /* level AFTER the edge: 1 = high-side on, 0 = low-side on */ +}; + +struct ShePattern { + uint32_t arr = 0; + uint16_t count = 0; + SheEvent events[kMaxEvents]; +}; + +ShePattern s_patternBuf[2] __attribute__((section(".dma_buffers"))); + +/* Build-time angle scratch (slow context only; ISR never reads it). + * AXI SRAM for the same DTCMRAM-budget reason as the pattern buffers. */ +float s_angleScratch[kMaxAngles] __attribute__((section(".dma_buffers"))); + +class ShepwmModulator final : public Modulator { +public: + const char* name() const override { return "shepwm"; } + /* Self-timed on TIM5: neither the TIM1 ISR nor a control caller drives it. */ + bool runsInPwmIsr() const override { return false; } + void update(float, float, float) override {} + void commit() override {} + + /** + * @brief Build the event list for (fe_hz, mi) into the inactive buffer. + * + * Slow-context only. While running, the swap (and the new ARR) applies at + * the next electrical-cycle wrap; while stopped it just stages the buffer + * for enter(). + */ + void setPattern(float fe_hz, float mi) { + if (fe_hz < 0.1f) fe_hz = 0.1f; + m_fe_hz = fe_hz; + m_mi = mi; + m_npulses = 0; /* SHE table mode */ + stage(fe_hz, interpolateAngles(), kAnglesPerQuarter, 1U); + } + + /** + * @brief Runtime N-pulse mode: npq cells per quarter, each cell HIGH for + * the middle `duty` fraction (a centered low-notch of width 1-duty). + * + * No tables: angles are arithmetic. duty=1 -> square wave (six-step), + * lower duty -> lower fundamental. |fundamental| falls smoothly from + * 1.0 but crosses zero near duty~0.5 (bipolar-pattern physics: at 50% + * high time the cos-weighted mean cancels) — operate above ~0.7. + */ + void setPulsePattern(float fe_hz, uint32_t npq, float duty) { + if (fe_hz < 0.1f) fe_hz = 0.1f; + if (npq < 1U) npq = 1U; + if (npq > kMaxPulsesPerQuarter) npq = kMaxPulsesPerQuarter; + if (duty < 0.0f) duty = 0.0f; + if (duty > 1.0f) duty = 1.0f; + m_fe_hz = fe_hz; + m_duty = duty; + m_npulses = npq; + + /* Falling edge at (i + d/2)*cell, rising edge at (i + 1 - d/2)*cell; + * leg starts high at theta=0+ (SHE/notch convention). */ + const float cell = (kPi / 2.0f) / (float)npq; + for (uint32_t i = 0; i < npq; ++i) { + s_angleScratch[2U * i] = ((float)i + duty * 0.5f) * cell; + s_angleScratch[2U * i + 1U] = ((float)i + 1.0f - duty * 0.5f) * cell; + } + stage(fe_hz, s_angleScratch, 2U * npq, 1U); + } + + /** + * @brief Phase-locked takeover of the TIM1 outputs. + * + * theta_e_rad is the current electrical angle (FOC convention); the timer + * starts mid-cycle so the pattern continues seamlessly. Initial pin + * levels are level-matched to the pattern at the entry angle (no step). + */ + bool enter(float theta_e_rad, float /*modulation_index*/) override { + if (m_running) return true; + if (PWM_IsFocModeActive() || spwmIsRunning()) return false; + + /* Apply a staged pattern immediately (stopped: no wrap needed). */ + if (m_pending != kNoPending) { + m_active = m_pending; + m_pending = kNoPending; + } + ShePattern& p = s_patternBuf[m_active]; + if (p.count == 0) return false; + + /* SHE-frame angle: table theta=0 is the phase-U positive edge. */ + float theta = theta_e_rad - kSheAngleOffsetRad; + while (theta < 0.0f) theta += kTwoPi; + while (theta >= kTwoPi) theta -= kTwoPi; + const uint32_t start_cnt = + (uint32_t)((theta / kTwoPi) * (float)(p.arr + 1U)) % (p.arr + 1U); + + /* Level-match every phase to the pattern at the entry angle. */ + for (uint8_t ph = 0; ph < 3U; ++ph) { + PWM_ForcePhaseLevel(ph, levelOfPhaseAt(p, ph, start_cnt) != 0); + } + + /* Cursor: first event strictly after start_cnt. */ + m_cursor = 0; + while (m_cursor < p.count && p.events[m_cursor].cnt <= start_cnt) { + ++m_cursor; + } + + __HAL_RCC_TIM5_CLK_ENABLE(); + TIM5->CR1 = 0U; /* ARPE=0: ARR writes apply immediately */ + TIM5->PSC = 0U; + TIM5->ARR = p.arr; + TIM5->CNT = start_cnt; + TIM5->CCR1 = (m_cursor < p.count) ? p.events[m_cursor].cnt : p.events[0].cnt; + TIM5->SR = 0U; /* clear any stale flags */ + TIM5->DIER = TIM_DIER_CC1IE | TIM_DIER_UIE; + HAL_NVIC_SetPriority(TIM5_IRQn, 3, 0); + HAL_NVIC_EnableIRQ(TIM5_IRQn); + TIM5->CR1 = TIM_CR1_CEN; + + m_running = true; + return true; + } + + void exit() override { + if (!m_running) return; + TIM5->CR1 = 0U; + TIM5->DIER = 0U; + HAL_NVIC_DisableIRQ(TIM5_IRQn); + PWM_ReleaseForcedOutputs(); + m_running = false; + } + + bool isRunning() const { return m_running; } + float frequencyHz() const { return m_fe_hz; } + float modulationIndex() const { return m_mi; } + uint32_t wrapCount() const { return m_wrap_count; } + uint32_t edgeCount() const { return m_edge_count; } + /* N-pulse mode introspection: 0 = SHE table mode. */ + uint32_t pulseCount() const { return m_npulses; } + float duty() const { return m_duty; } + + /* Current pattern angle in the ramp/FOC frame (inverse of enter()'s + * offset): TIM5 position + the SHE-frame offset. 0 while stopped. */ + float angleRad() const { + if (!m_running) return 0.0f; + const ShePattern& p = s_patternBuf[m_active]; + float theta = ((float)TIM5->CNT / (float)(p.arr + 1U)) * kTwoPi + + kSheAngleOffsetRad; + while (theta >= kTwoPi) theta -= kTwoPi; + return theta; + } + + /* TIM5 update event = electrical-cycle wrap: the only safe mutation point. */ + void onWrap() { + ++m_wrap_count; + m_cursor = 0; + if (m_pending != kNoPending) { + m_active = m_pending; + m_pending = kNoPending; + TIM5->ARR = s_patternBuf[m_active].arr; /* ARPE=0: applies immediately */ + } + processDue(); + } + + /* TIM5 CH1 compare: apply due edges, arm the next one. */ + void onCompare() { + processDue(); + } + +private: + static constexpr uint8_t kNoPending = 0xFFU; + + /* Shared staging: build the event list into the inactive buffer and arm + * the wrap-synchronized swap. start_level is the leg level at theta=0+ + * (1 = SHE notch patterns, 0 = centered-pulse patterns). */ + void stage(float fe_hz, const float* angles, uint32_t n_angles, + uint8_t start_level) { + const uint8_t idx = (uint8_t)(m_active ^ 1U); + ShePattern& p = s_patternBuf[idx]; + p.arr = (uint32_t)lroundf((float)kTim5ClockHz / fe_hz) - 1U; + p.count = buildEvents(p.events, kMaxEvents, angles, n_angles, p.arr, + start_level); + m_pending = idx; /* staged; applied at next wrap or by enter() */ + } + + /* Interpolate the angle trajectory for m_mi between MI grid points. */ + const float* interpolateAngles() { + float x = (m_mi - shetab::kMiMin) / shetab::kMiStep; + if (x < 0.0f) x = 0.0f; + const float x_max = (float)(shetab::kMiCount - 1U); + if (x > x_max) x = x_max; + const uint32_t i = (uint32_t)x; + const uint32_t j = (i + 1U < shetab::kMiCount) ? i + 1U : i; + const float f = x - (float)i; + for (uint32_t k = 0; k < kAnglesPerQuarter; ++k) { + s_angleScratch[k] = shetab::kAngles[i][k] * (1.0f - f) + + shetab::kAngles[j][k] * f; + } + return s_angleScratch; + } + + /** + * @brief Expand quarter-wave angles into a merged 3-phase event list. + * + * Per phase (shift 0 / 120 / 240 deg), the leg waveform starts high at + * theta=0+, toggles at every angle-derived edge, and inverts at the + * half-cycle boundary: the boundary crossings at theta=0 and theta=pi are + * themselves switching instants (2 + 4N edges per phase per cycle), which + * is what gives the waveform its half-wave symmetry q(t+pi) = -q(t). + * Events carry the level after the edge. + */ + static uint16_t buildEvents(SheEvent* out, uint16_t max_events, + const float* angles, uint32_t n_angles, + uint32_t arr, uint8_t start_level) { + static const float kShift[3] = { + 0.0f, 2.0f * kPi / 3.0f, 4.0f * kPi / 3.0f + }; + const float scale = (float)(arr + 1U) / kTwoPi; + uint16_t n = 0; + + auto push = [&](float theta, uint8_t ph, uint8_t level) { + if (n >= max_events) return; + if (theta >= kTwoPi) theta -= kTwoPi; + uint32_t cnt = (uint32_t)lroundf(theta * scale); + if (cnt > arr) cnt = arr; + out[n].cnt = cnt; + out[n].phase = ph; + out[n].level = level; + ++n; + }; + + for (uint8_t ph = 0; ph < 3U; ++ph) { + /* Boundary edge at theta=0 (+shift): leg starts at start_level + * (high for SHE notches, low for centered pulses). */ + push(kShift[ph], ph, start_level); + uint8_t level = start_level; + for (uint32_t q = 0; q < 4U; ++q) { + if (q == 2U) { + /* Boundary edge at theta=pi (+shift): half-wave inversion. */ + level = (uint8_t)(start_level ^ 1U); + push(kShift[ph] + kPi, ph, level); + } + for (uint32_t i = 0; i < n_angles; ++i) { + const uint32_t idx = + (q == 0U || q == 2U) ? i : (n_angles - 1U - i); + float theta; + switch (q) { + case 0: theta = angles[idx]; break; + case 1: theta = kPi - angles[idx]; break; + case 2: theta = kPi + angles[idx]; break; + default: theta = kTwoPi - angles[idx]; break; + } + level ^= 1U; + push(theta + kShift[ph], ph, level); + } + } + } + std::sort(out, out + n, + [](const SheEvent& a, const SheEvent& b) { return a.cnt < b.cnt; }); + return n; + } + + /* Level of one phase at a given count: the level after its most recent + * edge; before its first edge it is the level left by its last edge of + * the previous cycle (the list wraps). */ + static uint8_t levelOfPhaseAt(const ShePattern& p, uint8_t phase, uint32_t cnt) { + uint8_t level = 0; + bool found = false; + for (uint16_t i = 0; i < p.count; ++i) { + if (p.events[i].phase != phase) continue; + if (p.events[i].cnt <= cnt) { + level = p.events[i].level; + found = true; + } + } + if (found) return level; + /* Wrap: take this phase's last edge of the cycle. */ + for (int32_t i = (int32_t)p.count - 1; i >= 0; --i) { + if (p.events[i].phase == phase) return p.events[i].level; + } + return 1; + } + + /* ISR core: apply every event due by now+guard, arm the next compare. */ + void processDue() { + const ShePattern& p = s_patternBuf[m_active]; + const uint32_t due = TIM5->CNT + kDueGuardCounts; + while (m_cursor < p.count && p.events[m_cursor].cnt <= due) { + const SheEvent& ev = p.events[m_cursor]; + PWM_ForcePhaseLevel(ev.phase, ev.level != 0U); + ++m_cursor; + ++m_edge_count; + } + if (m_cursor < p.count) { + TIM5->CCR1 = p.events[m_cursor].cnt; + } else { + /* All edges applied; next activity is the wrap handler. */ + TIM5->CCR1 = p.events[0].cnt; + } + } + + float m_fe_hz = 0.0f; + float m_mi = 0.0f; + float m_duty = 0.0f; + uint32_t m_npulses = 0; /* 0 = SHE table mode, >0 = N-pulse mode */ + volatile uint16_t m_cursor = 0; + volatile uint8_t m_active = 0; + volatile uint8_t m_pending = kNoPending; + bool m_running = false; + uint32_t m_wrap_count = 0; + uint32_t m_edge_count = 0; +}; + +ShepwmModulator s_shepwm; + +} // namespace + +Modulator& shepwmModulator() { return s_shepwm; } + +bool shepwmIsRunning() { return s_shepwm.isRunning(); } +void shepwmSetPattern(float fe_hz, float mi) { s_shepwm.setPattern(fe_hz, mi); } +float shepwmFrequencyHz() { return s_shepwm.frequencyHz(); } +float shepwmModulationIndex() { return s_shepwm.modulationIndex(); } +uint32_t shepwmWrapCount() { return s_shepwm.wrapCount(); } +uint32_t shepwmEdgeCount() { return s_shepwm.edgeCount(); } +void shepwmSetPulsePattern(float fe_hz, uint32_t npq, float duty) { + s_shepwm.setPulsePattern(fe_hz, npq, duty); +} +uint32_t shepwmPulseCount() { return s_shepwm.pulseCount(); } +float shepwmDuty() { return s_shepwm.duty(); } +float shepwmAngleRad() { return s_shepwm.angleRad(); } + +} // namespace Inverter + +/* TIME_DOMAIN: SHEPWM_EDGE_ISR + * Rate: 12N+6 events per electrical cycle (N=9: 114/cycle; ~11k/s at 100 Hz, + * ~34k/s at 300 Hz). Hard real-time edge placement; must preempt the + * control ISRs (priority 3, above ADC(4) and TIM1_UP(5)) to hold + * sub-microsecond edge jitter. The BKIN hardware trip is a register path, + * not an IRQ, and is unaffected. + * CODEGEN: Keep in the base image; this is a HAL primitive, not model content. + */ +extern "C" void TIM5_IRQHandler(void) { + const uint32_t sr = TIM5->SR; + TIM5->SR = ~(sr & (TIM_SR_UIF | TIM_SR_CC1IF)); /* rc_w0: clear served flags */ + if ((sr & TIM_SR_UIF) != 0U) { + Inverter::s_shepwm.onWrap(); + } + if ((sr & TIM_SR_CC1IF) != 0U) { + Inverter::s_shepwm.onCompare(); + } +} diff --git a/Images/Gen6FW/Src/Inverter/Drivers/PWM/pwm.cpp b/Images/Gen6FW/Src/Inverter/Drivers/PWM/pwm.cpp index f3d49126..de6ffc17 100644 --- a/Images/Gen6FW/Src/Inverter/Drivers/PWM/pwm.cpp +++ b/Images/Gen6FW/Src/Inverter/Drivers/PWM/pwm.cpp @@ -10,6 +10,7 @@ #include "pwm.h" #include "tim.h" #include "Inverter/AppState.h" +#include "Inverter/Drivers/PWM/Modulator.h" #include "Inverter/LoopStats.h" #include "mcp2221a_driver.h" #include @@ -42,15 +43,11 @@ static volatile float pwm_switching_freq_hz = (float)PWM_DEFAULT_SWITCHING_FREQ_ * (dual-update FOC). */ static volatile float pwm_update_freq_hz = (float)PWM_DEFAULT_SWITCHING_FREQ_HZ; -/* SPWM state, updated in the TIM1 update ISR. */ -static volatile float spwm_angle = 0.0f; -static volatile float spwm_fundamental_freq_hz = 1.0f; -static volatile float spwm_modulation_index = 0.0; -static volatile uint8_t spwm_running = 0; -static volatile uint32_t spwm_elec_cycles = 0; - -/* FOC mode flag. When set, the TIM1 update ISR does not run the SPWM ramp. */ -static volatile uint8_t foc_active = 0; +/* Closed-loop control hook. When set, the TIM1 update ISR runs this hook + * (the controller, which drives the SVPWM modulator via PWM_SetVoltageVector) + * instead of the active self-clocked modulator (SPWM). Registered by + * PWM_EnableFocMode(); nullptr in open-loop / idle. */ +static void (*pwm_control_hook)(void) = nullptr; /* CKD = DIV1 => t_DTS = 1 / TIM1_CLOCK_HZ. * Map the requested deadtime to the finest STM32 DTG encoding. */ @@ -109,10 +106,14 @@ void PWM_SetFrequency(uint32_t freq_hz) pwm_switching_freq_hz = (float)TIM1_CLOCK_HZ / (2.0f * (float)(arr + 1U) * (float)(psc + 1U)); - /* Default to one update event per switching period (RCR=1), matching the - * open-loop SPWM convention. FOC mode will override this when enabled. */ - TIM1->RCR = 1U; - pwm_update_freq_hz = pwm_switching_freq_hz; + /* One update event per switching period (RCR=1) in open-loop, dual-update + * (RCR=0) when a closed-loop control hook is registered. Previously this + * always forced RCR=1, silently halving the FOC rate on a live carrier + * change. */ + const bool closed_loop = (pwm_control_hook != nullptr); + TIM1->RCR = closed_loop ? 0U : 1U; + pwm_update_freq_hz = closed_loop ? 2.0f * pwm_switching_freq_hz + : pwm_switching_freq_hz; } void PWM_SetDeadTime(uint32_t deadtime_ns) @@ -176,44 +177,10 @@ void PWM_SetVoltageAngle(float angle_rad, float modulation_index) void PWM_SetVoltageVector(float valpha_v, float vbeta_v, float vdc_v) { - if (vdc_v <= 1.0f) { - PWM_SetThreePhaseDuty(50.0f, 50.0f, 50.0f); - return; - } - - /* Clamp the alpha/beta magnitude to the linear modulation limit before - * converting to three-phase voltages. The SVPWM linear limit is - * Vdc / sqrt(3). */ - const float sqrt3 = 1.7320508075688772f; - float valpha = valpha_v; - float vbeta = vbeta_v; - const float v_max_linear = (vdc_v / sqrt3) * 0.95f; - const float v_albe_sq = valpha * valpha + vbeta * vbeta; - if (v_albe_sq > v_max_linear * v_max_linear && v_albe_sq > 1e-12f) { - const float scale = v_max_linear / sqrtf(v_albe_sq); - valpha *= scale; - vbeta *= scale; - } - - /* Inverse Clarke: alpha/beta -> A/B/C. */ - float va = valpha; - float vb = -0.5f * valpha + 0.5f * sqrt3 * vbeta; - float vc = -0.5f * valpha - 0.5f * sqrt3 * vbeta; - - /* Min-max SVPWM zero-sequence injection. */ - float v_max = (va > vb) ? ((va > vc) ? va : vc) : ((vb > vc) ? vb : vc); - float v_min = (va < vb) ? ((va < vc) ? va : vc) : ((vb < vc) ? vb : vc); - float vcom = 0.5f * (v_max + v_min); - - float du = 50.0f + 50.0f * (va - vcom) / vdc_v; - float dv = 50.0f + 50.0f * (vb - vcom) / vdc_v; - float dw = 50.0f + 50.0f * (vc - vcom) / vdc_v; - - if (du < 0.0f) du = 0.0f; else if (du > 100.0f) du = 100.0f; - if (dv < 0.0f) dv = 0.0f; else if (dv > 100.0f) dv = 100.0f; - if (dw < 0.0f) dw = 0.0f; else if (dw > 100.0f) dw = 100.0f; - - PWM_SetThreePhaseDuty(du, dv, dw); + /* SVPWM modulator (externally clocked): compute duties, then write them. + * CCR preload makes the write land atomically at the next update event. */ + Inverter::svpwmModulator().update(valpha_v, vbeta_v, vdc_v); + Inverter::svpwmModulator().commit(); } /* TIME_DOMAIN: CLOSED_LOOP_MODULATION_START @@ -222,7 +189,9 @@ void PWM_SetVoltageVector(float valpha_v, float vbeta_v, float vdc_v) */ void PWM_EnableFocMode(void) { - foc_active = 1; + /* FOC drives the SVPWM modulator from the control hook. */ + pwm_control_hook = FocControlManager_OnPwmPeriod; + Inverter::setActiveModulator(&Inverter::svpwmModulator()); /* Dual-update FOC: run the control ISR at twice the PWM switching frequency * (both top and bottom of the center-aligned triangle). RCR=0 generates an * update event on every counter overflow/underflow. */ @@ -232,14 +201,17 @@ void PWM_EnableFocMode(void) void PWM_DisableFocMode(void) { - foc_active = 0; + pwm_control_hook = nullptr; + if (Inverter::activeModulator() == &Inverter::svpwmModulator()) { + Inverter::setActiveModulator(nullptr); + } TIM1->RCR = 1U; pwm_update_freq_hz = pwm_switching_freq_hz; } bool PWM_IsFocModeActive(void) { - return foc_active != 0; + return pwm_control_hook != nullptr; } void PWM_StartUpdateInterrupt(void) @@ -251,7 +223,7 @@ void PWM_StartUpdateInterrupt(void) void PWM_StopUpdateInterrupt(void) { - if (!spwm_running) { + if (!Inverter::spwmIsRunning()) { __HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE); HAL_NVIC_DisableIRQ(TIM1_UP_IRQn); } @@ -274,19 +246,15 @@ float PWM_GetUpdateFrequency(void) */ void PWM_StartSPWM(float fundamental_freq_hz, float modulation_index) { - if (fundamental_freq_hz < 0.0f) fundamental_freq_hz = 0.0f; - if (modulation_index < 0.0f) modulation_index = 0.0f; - if (modulation_index > SVPWM_M_MAX) modulation_index = SVPWM_M_MAX; - /* SPWM uses one update event per switching period (RCR=1) so the angle * advances at the switching frequency. */ TIM1->RCR = 1U; pwm_update_freq_hz = pwm_switching_freq_hz; - spwm_fundamental_freq_hz = fundamental_freq_hz; - spwm_modulation_index = modulation_index; - spwm_angle = 0.0f; - spwm_running = 1; + /* Hand the modulation slot to the self-clocked SPWM modulator. */ + Inverter::spwmSetParams(fundamental_freq_hz, modulation_index); + Inverter::spwmModulator().enter(0.0f, 0.0f); + Inverter::setActiveModulator(&Inverter::spwmModulator()); HAL_NVIC_SetPriority(TIM1_UP_IRQn, 5, 0); HAL_NVIC_EnableIRQ(TIM1_UP_IRQn); @@ -295,19 +263,17 @@ void PWM_StartSPWM(float fundamental_freq_hz, float modulation_index) void PWM_StopSPWM(void) { - spwm_running = 0; + Inverter::spwmModulator().exit(); + if (Inverter::activeModulator() == &Inverter::spwmModulator()) { + Inverter::setActiveModulator(nullptr); + } __HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE); HAL_NVIC_DisableIRQ(TIM1_UP_IRQn); } void PWM_SetSPWMParams(float fundamental_freq_hz, float modulation_index) { - if (fundamental_freq_hz < 0.0f) fundamental_freq_hz = 0.0f; - if (modulation_index < 0.0f) modulation_index = 0.0f; - if (modulation_index > SVPWM_M_MAX) modulation_index = SVPWM_M_MAX; - - spwm_fundamental_freq_hz = fundamental_freq_hz; - spwm_modulation_index = modulation_index; + Inverter::spwmSetParams(fundamental_freq_hz, modulation_index); } /* TIM1 update ISR callback. Runs at the PWM switching frequency. */ @@ -329,59 +295,58 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) * superseded once the graph contains nodes assigned to the tim_isr domain. */ // RTE_EMIT: tim_isr step - if (foc_active) { - FocControlManager_OnPwmPeriod(); + if (pwm_control_hook != nullptr) { + pwm_control_hook(); return; } - if (!spwm_running) return; + /* Drive the active self-clocked modulator (SPWM today). Externally + * clocked modulators (SVPWM) are driven by their control-loop caller; + * future synchronous modulators (SHEPWM) run on their own timebase. */ + Inverter::Modulator* mod = Inverter::activeModulator(); + if (mod == nullptr || !mod->runsInPwmIsr()) return; - float angle = spwm_angle; - float m = spwm_modulation_index; - - /* Three-phase sinusoidal references, 120° apart. */ - float u = m * sinf(angle); - float v = m * sinf(angle - TWO_PI / 3.0f); - float w = m * sinf(angle + TWO_PI / 3.0f); - - /* Min-max SVPWM zero-sequence injection to extend linear range to 2/sqrt(3). */ - float v_max = (u > v) ? ((u > w) ? u : w) : ((v > w) ? v : w); - float v_min = (u < v) ? ((u < w) ? u : w) : ((v < w) ? v : w); - float v0 = -0.5f * (v_max + v_min); - - /* Convert to centered duty cycles [0, 100]. */ - float du = 50.0f + 50.0f * (u + v0); - float dv = 50.0f + 50.0f * (v + v0); - float dw = 50.0f + 50.0f * (w + v0); - - if (du < 0.0f) du = 0.0f; else if (du > 100.0f) du = 100.0f; - if (dv < 0.0f) dv = 0.0f; else if (dv > 100.0f) dv = 100.0f; - if (dw < 0.0f) dw = 0.0f; else if (dw > 100.0f) dw = 100.0f; - - PWM_SetThreePhaseDuty(du, dv, dw); - - /* Advance angle by one PWM period. */ - angle += TWO_PI * spwm_fundamental_freq_hz / pwm_switching_freq_hz; - if (angle >= TWO_PI) { - angle -= TWO_PI; - ++spwm_elec_cycles; - } - spwm_angle = angle; + mod->update(0.0f, 0.0f, 0.0f); + mod->commit(); } uint32_t PWM_GetSPWMElectricalCycles(void) { - return spwm_elec_cycles; + return Inverter::spwmElectricalCycles(); } void PWM_ResetSPWMElectricalCycles(void) { - spwm_elec_cycles = 0; + Inverter::spwmResetElectricalCycles(); } float PWM_GetSPWMAngle(void) { - return spwm_running ? spwm_angle : 0.0f; + return Inverter::spwmAngleRad(); +} + +void PWM_ForcePhaseLevel(uint8_t phase, bool high) +{ + if (phase > 2) return; + const uint32_t mode = high ? TIM_OCMODE_ACTIVE : TIM_OCMODE_INACTIVE; + switch (phase) { + case 0: + MODIFY_REG(TIM1->CCMR1, TIM_CCMR1_OC1M, mode); + break; + case 1: + MODIFY_REG(TIM1->CCMR1, TIM_CCMR1_OC2M, mode << 8U); + break; + default: + MODIFY_REG(TIM1->CCMR2, TIM_CCMR2_OC3M, mode); + break; + } +} + +void PWM_ReleaseForcedOutputs(void) +{ + MODIFY_REG(TIM1->CCMR1, TIM_CCMR1_OC1M | TIM_CCMR1_OC2M, + TIM_OCMODE_PWM1 | (TIM_OCMODE_PWM1 << 8U)); + MODIFY_REG(TIM1->CCMR2, TIM_CCMR2_OC3M, TIM_OCMODE_PWM1); } void PWM_StartPhase(uint8_t phase) @@ -450,8 +415,8 @@ void PWM_PrintSPWMState(void) float dw = (arr == 0) ? 0.0f : (__HAL_TIM_GET_COMPARE(&htim1, pwm_phase_channels[2]) * 100.0f / (float)arr); MCP2221A_Printf("[SPWM] running=%u f=%.2f Hz m=%.3f | duties U=%.1f V=%.1f W=%.1f %%\r\n", - (unsigned)spwm_running, - (double)spwm_fundamental_freq_hz, - (double)spwm_modulation_index, + (unsigned)(Inverter::spwmIsRunning() ? 1U : 0U), + (double)Inverter::spwmFundamentalFreqHz(), + (double)Inverter::spwmModulationIndex(), (double)du, (double)dv, (double)dw); } diff --git a/Images/Gen6FW/Src/Inverter/Drivers/Sensors/EncoderADC.cpp b/Images/Gen6FW/Src/Inverter/Drivers/Sensors/EncoderADC.cpp index df7841e2..43c888e3 100644 --- a/Images/Gen6FW/Src/Inverter/Drivers/Sensors/EncoderADC.cpp +++ b/Images/Gen6FW/Src/Inverter/Drivers/Sensors/EncoderADC.cpp @@ -264,8 +264,8 @@ void EncoderADC::traceDump() { } void EncoderADC::onDmaComplete() { - /* Slim ISR: decode + publish only. No RPM math, no fault evaluation, - * no library calls — those live in diagnose() (main loop). This handler + /* Slim ISR: decode + publish + observer step. No fault evaluation, no + * library calls — those live in diagnose() (main loop). This handler * is ~5 us so its priority can sit above the control ISRs without * meaningfully delaying them. */ const uint16_t raw_sin = s_enc_dma_buffer[0]; @@ -275,6 +275,56 @@ void EncoderADC::onDmaComplete() { * fields atomically relative to the main-loop readers. */ const float angle = computeAngle(raw_sin, raw_cos); + /* Tracking observer (2nd-order PLL): theta/omega states corrected per + * sample by the wrapped angle error. Gains from OBS_BW_HZ / OBS_ZETA; + * dt from the DWT cycle counter so the observer stays correct when the + * encoder trigger follows the carrier (TIM1 TRGO2 during control). */ + { + constexpr float kPi = 3.14159265358979f; + constexpr float kDegToRad = kPi / 180.0f; + constexpr float kWn = 2.0f * kPi * OBS_BW_HZ; + constexpr float k1 = 2.0f * OBS_ZETA * kWn; + constexpr float k2 = kWn * kWn; + + const uint32_t now_cycles = DWT->CYCCNT; + const float dt = static_cast(now_cycles - m_obs_last_cycles) / + static_cast(SystemCoreClock); + m_obs_last_cycles = now_cycles; + + const float meas = angle * kDegToRad; + if (!m_rpm_init || dt <= 0.0f || dt > 2.0e-3f) { + /* First sample or a stalled stream: re-seed, never extrapolate + * a dead timer into a phantom speed. */ + m_obs_theta = meas; + m_obs_omega = 0.0f; + m_rpm_init = true; + } else { + float err = meas - m_obs_theta; + while (err > kPi) err -= 2.0f * kPi; + while (err < -kPi) err += 2.0f * kPi; + + /* Glitch rejection: a real rotor cannot move more than + * ~|omega|*dt between samples; an encoder/EMI outlier can be + * anywhere on the circle. Coast through implausible samples + * instead of letting one kick omega (and with it the FOC + * extrapolated angle). */ + const float err_lim = 4.0f * fabsf(m_obs_omega) * dt + 0.1f; + if (fabsf(err) > err_lim) { + ++m_obs_rejects; + m_obs_theta += m_obs_omega * dt; + } else { + /* Euler step of theta' = omega + k1*err, omega' = k2*err. + * (The k1 term MUST be scaled by dt: without it the loop + * overcorrects ~1/dt x and the speed estimate oscillates.) */ + m_obs_theta += (m_obs_omega + k1 * err) * dt; + m_obs_omega += k2 * err * dt; + } + while (m_obs_theta >= 2.0f * kPi) m_obs_theta -= 2.0f * kPi; + while (m_obs_theta < 0.0f) m_obs_theta += 2.0f * kPi; + } + m_rpm_ema = m_obs_omega * (60.0f / (2.0f * kPi)); + } + m_snapshot.angle = angle; m_snapshot.raw_sin = raw_sin; m_snapshot.raw_cos = raw_cos; @@ -370,42 +420,10 @@ void EncoderADC::onDmaError() { void EncoderADC::diagnose() { const uint32_t now_ms = HAL_GetTick(); - /* Mechanical speed, evaluated here (main loop) rather than in the DMA - * ISR: per-sample angle deltas at 10 kHz multiply angle noise by the - * full sample rate, which buries the estimate in EMI at high switching - * frequencies. Time-based window instead of sample-count window so the - * estimate is independent of the caller's cadence. */ - { - const float angle = m_snapshot.angle; - if (!m_rpm_init) { - m_rpm_init = true; - m_unwrapped_angle = angle; - m_window_ref_angle = angle; - m_rpm_prev_angle = angle; - m_rpm_filt_angle = angle; - m_rpm_ema = 0.0f; - m_rpm_window_ms = now_ms; - } else { - float delta = angle - m_rpm_prev_angle; - if (delta > 180.0f) delta -= 360.0f; - else if (delta < -180.0f) delta += 360.0f; - m_unwrapped_angle += delta; - m_rpm_prev_angle = angle; - - const uint32_t win_ms = now_ms - m_rpm_window_ms; - if (win_ms >= RPM_WINDOW_MS) { - const float win_deg = m_unwrapped_angle - m_window_ref_angle; - const float rpm_inst = (win_deg / 360.0f) * (60000.0f / static_cast(win_ms)); - m_rpm_ema += RPM_ALPHA * (rpm_inst - m_rpm_ema); - m_window_ref_angle = m_unwrapped_angle; - m_rpm_window_ms = now_ms; - } - } - } - - /* Signal-quality faults (main loop now): magnitude collapse and rail - * sticking, evaluated on the latest snapshot raws. Fault detection is - * slow by nature; per-call EMA replaces the per-sample one. */ + /* Speed estimation lives in the DMA ISR now (tracking observer); this + * main-loop pass only evaluates signal-quality faults, which are slow + * by nature: magnitude collapse and rail sticking, on the latest + * snapshot raws. */ const bool range_ok = (m_active_sin_max - m_active_sin_min > MIN_AMP_RANGE) && (m_active_cos_max - m_active_cos_min > MIN_AMP_RANGE); if (range_ok) { diff --git a/Images/Gen6FW/Src/Inverter/InverterMain.cpp b/Images/Gen6FW/Src/Inverter/InverterMain.cpp index 9cf97c4d..e6878e69 100644 --- a/Images/Gen6FW/Src/Inverter/InverterMain.cpp +++ b/Images/Gen6FW/Src/Inverter/InverterMain.cpp @@ -97,9 +97,6 @@ static void init() Telemetry::init(); Telemetry::set_period_us(10000); /* 100 Hz data frames */ - /* CAN error-status notifications (FDCAN2 is the active interface). */ - (void)Inverter::fdcanFaultInit(); - /* Supply rail monitoring (PVD/AVD/VOSRDY). */ (void)Inverter::supplyMonitorInit(); diff --git a/Images/Gen6FW/Src/Inverter/platform_api.cpp b/Images/Gen6FW/Src/Inverter/platform_api.cpp index 14261ded..c24011b5 100644 --- a/Images/Gen6FW/Src/Inverter/platform_api.cpp +++ b/Images/Gen6FW/Src/Inverter/platform_api.cpp @@ -1,6 +1,12 @@ #include "platform_api.h" #include "Inverter/Drivers/PWM/pwm.h" +#include "Inverter/Drivers/PWM/Modulator.h" +#include "Inverter/Drivers/PWM/ModulationSwitch.h" +#include "Inverter/Calibration/MotorCalibration.h" +#include "Inverter/Control/FocControlManager.h" +#include "Inverter/Control/OpenLoopController.h" +#include "Inverter/Drivers/Sensors/EncoderADC.h" #include "Inverter/Drivers/Sensors/ApplicationSensors.h" #include "Inverter/Drivers/Sensors/PhaseCurrentADC.h" #include "Inverter/Drivers/Sensors/EncoderADC.h" @@ -26,6 +32,70 @@ void platform_pwm_set_voltage_vector(float valpha, float vbeta, float vdc) { PWM_SetVoltageVector(valpha, vbeta, vdc); } +/* -------------------------------------------------------------------------- + * Modulation mode switching + * -------------------------------------------------------------------------- */ + +float platform_get_ol_freq_hz(void) { + return Inverter::openLoopController().frequencyHz(); +} + +float platform_get_elec_freq_hz(void) { + /* Controller-agnostic: encoder mechanical speed x pole pairs. Works for + * the graph FOC path, legacy FOC, open-loop — and in pattern mode it is + * what lets a supervisor SEE a stall (rotor stops, fe collapses) and + * hand back, instead of reading the pattern's own frozen frequency. */ + const Inverter::MotorCalibration& cal = Inverter::MotorCalibration::instance(); + if (cal.valid && cal.pole_count > 0.0f) { + const float fe = Inverter::encoderADC().rpmMech() * + (cal.pole_count * 0.5f) / 60.0f; + return fe < 0.0f ? -fe : fe; + } + if (Inverter::shepwmIsRunning()) { + return Inverter::shepwmFrequencyHz(); + } + if (Inverter::focControlManager().isRunning()) { + const float w = Inverter::focControlManager().electricalSpeedRadPerSec(); + return (w < 0.0f ? -w : w) / 6.283185307f; + } + return Inverter::openLoopController().frequencyHz(); +} + +uint8_t platform_modulation_mode(void) { + return static_cast(Inverter::modulationMode()); +} + +bool platform_modulation_to_pattern(uint32_t pulses_per_quarter, float duty) { + return Inverter::modulationToPattern(pulses_per_quarter, duty); +} + +bool platform_modulation_to_ramp(void) { + return Inverter::modulationToRamp(); +} + +void platform_pwm_set_carrier_hz(float freq_hz) { + if (freq_hz < 500.0f || freq_hz > 20000.0f) { + return; + } + const float cur = PWM_GetFrequency(); + if (freq_hz > cur - 5.0f && freq_hz < cur + 5.0f) { + return; /* deadband: ignore lerped churn */ + } + PWM_SetFrequency((uint32_t)(freq_hz + 0.5f)); +} + +/** + * @brief Live control-loop period [s] (1 / TIM1 update frequency). + * + * PI/integration nodes must use this rather than a baked Dt whenever the + * carrier can change at runtime (CarrierAuto), or integral gains drift + * with the carrier. + */ +float platform_get_control_dt(void) { + const float f = PWM_GetUpdateFrequency(); + return (f > 1.0f) ? 1.0f / f : 0.0f; +} + /* -------------------------------------------------------------------------- * Sensor inputs * -------------------------------------------------------------------------- */ diff --git a/Images/Gen6FW/docs/Multi_Modulator_SHEPWM_Plan.md b/Images/Gen6FW/docs/Multi_Modulator_SHEPWM_Plan.md new file mode 100644 index 00000000..d7597092 --- /dev/null +++ b/Images/Gen6FW/docs/Multi_Modulator_SHEPWM_Plan.md @@ -0,0 +1,284 @@ +# Plan: Scalable Multi-Modulator Motor Control Firmware (STM32H723ZG) + +**Status:** Investigated against the current Gen6FW base image + RTE toolchain (2026-08). +**Hardware verified:** STM32H723ZGTx (`.ioc`), DS13313 Rev 4 pin/AF tables. + +Each section ends with a **Feasibility** verdict grounded in the actual codebase. + +--- + +## 1. Architecture: Four Layers + +``` +Application — speed loop, state machine (app_loop domain, ~100 Hz–1 kHz) +Controller — FOC current loop (tim_isr domain, 10 kHz dual-update) +Modulator — swappable strategy: SVPWM / SPWM / SHEPWM / RCFM +PWM Driver — owns all TIM1 registers, shadow commits, dead time, trip +``` + +Core principle: the controller outputs `Vα, Vβ, θe, fe` and never knows which +modulator is active. Each modulator is a component with four entry points and +private per-instance state: + +```c +typedef struct mod mod_t; +typedef struct { + void (*update)(mod_t *m, float va, float vb, float theta_e, float fe); + void (*commit)(mod_t *m); /* write to TIM1 via pwm driver */ + bool (*enter) (mod_t *m, float theta_e, float mi); /* phase-locked take-over */ + void (*exit) (mod_t *m); +} mod_ops_t; +``` + +**Feasibility: HIGH — and half of it already exists.** + +- The PWM driver layer exists (`Src/Inverter/Drivers/PWM/pwm.cpp`): TIM1 + register ownership, dead time, break/MOE handling, `PWM_SetThreePhaseDuty`, + `PWM_SetVoltageVector` (SVPWM), open-loop SPWM ramp. +- What does *not* exist: the `mod_ops_t` dispatch layer and private instance + state. Today the "modulator" is an implicit `foc_active`/`spwm_running` + flag pair in `pwm.cpp`. +- **Constraint:** there are two consumers of the PWM driver — the legacy + `FocControlManager` (C++ path) and the generated `tim_isr` domain via + `platform_pwm_set()` (`Actuators.PwmOut` node) / `platform_pwm_set_voltage_vector()`. + The modulator interface must sit **below** these entry points, not replace + them, or the codegen contract (`platform_api.h`) breaks. + +## 2. Execution Context — mostly already at target + +| Plan assumption | Reality in Gen6FW | +|---|---| +| "Current: current sense on free-running timer" | **Wrong.** Already PWM-synchronous: TIM1 CH4 OC4REF → TRGO triggers the ADC1/ADC2 injected dual-simultaneous group at PWM bottom (`PhaseCurrentADC.cpp:118-144`). | +| "Target: ADC-complete ISR does sample → FOC → commit" | Partially present, deliberately split: ADC-complete ISR does sample + safety (`onInjectedConversionComplete`, `PhaseCurrentADC.cpp:326`); FOC runs in the TIM1 update ISR at 10 kHz dual-update (`FocControlManager::onPwmPeriod`, RCR=0). The codegen domains `adc_isr` and `tim_isr` already model this split. Unifying FOC into the ADC ISR is optional, not required. | +| "Target: angle extrapolation θe(t) = θs + ωe·Δt" | **Already done.** `EncoderADC::extrapolatedAngleDeg()` uses DWT-cycle timestamps + RPM EMA (`EncoderADC.cpp:222`), exposed to generated code via `platform_get_encoder_angle_latest()`. Only the legacy `FocControlManager` still reads the raw snapshot (`lastAngle()`) — minor cleanup, not new work. | + +**Feasibility: DONE / TRIVIAL.** Section 2 of the original plan is ~90% +already-implemented. Remaining: point the legacy FOC path at the extrapolated +angle and decide whether `commit()` runs from `tim_isr` or `adc_isr` (recommend +keeping the current split). + +## 3. Modulator Implementations + +**SVPWM / SPWM (async):** exist today in `pwm.cpp` and as graph nodes +(`Transforms.Svpwm` → `Actuators.PwmOut`). `update()` computes duties → +`commit()` writes CCR1..3; ARPE + OC preload are already ON +(`tim.c:51`, HAL PWM channel config), so the three writes land atomically at +the next update event. Wrapping them behind `mod_ops_t` is a pure refactor. + +**SHEPWM (sync):** own timebase on **TIM5** (32-bit), not the carrier timer: + +- `ARR = f_tim / fe` → one timer cycle = one electrical cycle. + TIM5 clock = 137.5 MHz (APB1, same as TIM2 per `EncoderADC.cpp:79,91`). + At 300 Hz: ARR ≈ 458,333 → **7.3 ns edge resolution**, ~8×10⁻⁴ ° angle + resolution — the plan's numbers hold. +- Angles stored per quarter cycle (quarter-wave symmetry); full cycle by + reflection. 51-pulse pattern → 51 angles + level list per MI value. +- `angle_to_cnt(α) = α/2π × ARR`. +- Edge fire options, in bring-up order: + 1. **Compare-chain ISR** (write next CCR one edge ahead). At 51 pulses, + 300 Hz: ~30k IRQ/s ≈ 1–2% CPU on a 550 MHz M7. **Start here.** + 2. **DMA circular buffer** — TIM5 CC-event-triggered DMA stream refilling + TIM5->CCRx from a pattern buffer. Zero CPU. Feasible with the free DMA + streams (only DMA2_Stream0 is in use for the encoder). + 3. ~~HRTIM~~ — **does not exist on STM32H723** (verified: no HRTIM anywhere + in DS13313). Note: `docs/MPC_Multi_Rate_Current_Sampling_Plan.md` also + mentions HRTIM as an option — that is an MCU change, not a redesign. +- TIM1 keeps running center-aligned the whole time: ADC trigger, encoder + TRGO2 sync, and FOC cadence never change. + +**RCFM (random carrier frequency):** nearly free once the interface exists — +vary TIM1 ARR/PSC per period under preload; commit at update event. + +**Timer budget (verified):** used = TIM1 (PWM + ADC/encoder triggers), TIM2 +(encoder 10 kHz idle trigger; idle during control), TIM3 (100 Hz slow-sensor +scan). **Free: TIM4, TIM5, TIM8 (advanced-control, 16-bit), TIM6/7, +TIM12–17, LPTIMs.** TIM5 is the clean SHE timebase; TIM8 is a spare +advanced timer if a future hardware rev routes pins to it. + +**Feasibility: HIGH for SVPWM/SPWM/RCFM; MEDIUM-HIGH for SHEPWM.** The one +real open problem is not timing but **modulation-index continuity**: SHE +angles are solved offline for a *fixed* MI, while FOC demands a continuous +`Vα/Vβ`. Discrete MI steps → torque disturbance. Options (choose during +design, see §9): (a) table family + interpolation, (b) hybrid — SHE edges +plus between-edge SVPWM trim, (c) restrict SHE to the voltage-saturated +high-speed region where the current loop is effectively amplitude-limited +anyway. This is the main control-design risk of the whole project. + +## 4. Glitch Safety + +**Software:** double-buffer SHE tables (ping-pong); slow task writes `build`, +edge ISR reads `active`; single pointer swap only at electrical-cycle wrap +(TIM5 update event). Never torn mid-cycle. + +**Hardware:** already mostly in place — ARPE + CCR preload ON, TIM1 RCR in +use, all mutations at cycle boundary. For SHE forced-mode writes (§6) OCxM +changes take effect immediately — which is exactly what edge placement wants, +but means the *ISR/DMA sequence itself* must be the only writer (guarded by +the pending-flag protocol, §5). + +Worst-case failure becomes: "new pattern applies next electrical cycle." + +**Feasibility: HIGH.** Matches existing driver invariants; the new part is +only the ping-pong table discipline. + +## 5. Supervisor (slow task) + +- Selection table: mode = f(fe, mi, quadrant) — data-driven, tunable. +- Hysteresis on every threshold (enter 40 Hz / exit 35 Hz) against chattering. +- Request → `pending` flag → fast context executes at the safe point + (electrical-cycle wrap for SHE; any update event for async mods). +- SHE entry phase-lock: `enter()` presets TIM5 CNT = `angle_to_cnt(θe)` using + the **extrapolated** encoder angle, aligns the table cursor, and level-matches + the initial pin state to avoid a voltage step. + +**Feasibility: HIGH.** +- `ControlSupervisor` exists but is a *lifecycle* manager (gate-driver + sequencing, PWM start/stop, fault response — `ControlSupervisor.h`) — the + modulation supervisor is a new, separate component that belongs in the + `app_loop` domain (soft real-time) with the safe-point executor in ISR. +- Tunable transition table: the FRAM KV store + `Values.Config` node + + `config set/get` shell are the existing mechanism — thresholds can be + runtime-tunable **today** without new tooling. +- Forced-angle diagnostic (`FocControlManager::setForcedAngleRate`) gives a + clean synthetic θe ramp — ideal for SHE bring-up before FOC is involved. + +## 6. Live Handoff at Speed — Option A is impossible on this hardware + +**Verified pin fact:** the six gate pins PE8–PE13 (TIM1 CH1N/1/2N/2/3N/3) have +**TIM1 as their only timer alternate function** (DS13313 Table 7; other AFs +are DFSDM/UART7/SPI4/SAI4/FMC/COMPx_OUT). TIM2/TIM5/TIM8 cannot reach them, +and the H723 has no HRTIM. **GPIO AF re-mux handoff (Option A) is physically +impossible on Gen6.** Drop it. + +**Option B — TIM1 always owns the pins (only and safest path):** + +- SHE timer (TIM5) never touches GPIO. Its compare events write TIM1's + output-compare **forced modes**: per edge, write CCMR1 (CH1+CH2 fields) and + CCMR2 (CH3 field) with `OCxM = FORCE_ACTIVE / FORCE_INACTIVE`. +- MOE stays set the entire time → the **BKIN hardware trip (gate-driver fault + on PE15) stays armed in every mode** — the original plan's worry about a + dead break input disappears. +- Dead time: the DTG stage acts on OCxREF transitions including forced-mode + changes, so complementary dead time stays in circuit. **Must be validated + on a scope during bring-up** (measure high/low-side overlap at forced + transitions; boot DT ≈ 705 ns, runtime default 1 µs). +- Edge jitter ≈ ISR latency (~0.5–1 µs with TIM5 CC IRQ at priority above + `tim_isr`/`adc_isr`, below the AWD/fault IRQs) ≈ 0.11° at 300 Hz — acceptable + for SHE. +- Exit path: restore `OCxM = PWM1` on all three channels; preloaded CCRs + resume normal SVPWM at the next update event — no glitch. +- Overcurrent stack is unchanged and mode-independent: gate-driver hardware + fault → BKIN (ns–µs); ADC1 analog watchdog on injected channels → fault; + software OC with consecutive-sample voting. + +**Feasibility: HIGH — simpler than the original plan**, because the +two-timer ownership dance and its safety caveats are gone. One timer owns the +pins, always; "handoff" is just who writes OCxM/CCR. + +## 7. Model-Based / Codegen Mapping + +| Architecture piece | Status in RTE today | +|---|---| +| Component blocks w/ per-instance data | **Exists** — class-based node templates (`class_header.h`, `class_definition.cpp`, `constructor.cpp`; see `Assets/NodeTemplates/README.md`, `CodeGenerator.cpp` constructorCode). | +| Async modulators as nodes | **Exists** — `Transforms.Svpwm` + `Actuators.PwmOut`; full FOC chain already runs as a graph (`Assets/Examples/foc_demo.json`: Clarke/Park/PI/InversePark/Svpwm in `tim_isr`, currents in `adc_isr`). | +| Runtime dispatch, one modulation slot | **Missing.** `Logic.Mux` is a *signal* mux — it cannot express component enter/exit (phase-lock, timer arming, safe-point swap). This is the biggest toolchain gap. | +| Supervisor chart | **Missing as a node type.** Expressible today with Var/Gate/Mux + config keys in `app_loop`, or hand-written in the base image. | +| Tunable calibration struct | **Exists** — FRAM KV + `Values.Config` + `config` shell. | +| Atomic regions | **Exists** — `platform_critical_enter/exit` in `platform_api.h`. | +| HAL primitives (forced-mode writes, TIM5 pattern engine, ping-pong tables) | **Missing** — new base-image driver + `platform_api` extensions. Per the plan's own boundary rule this is where the SHE machinery belongs. | +| "Hold-until-safe-point" rate transition | **Missing in codegen**; trivial in base image (pending flag + cycle-wrap check). | +| SIL/HIL harness | **Missing** — ngspice plant sim is roadmap, not built. On-target instruments exist and are permanent: spike recorder, `enc_trace`, `hz_*` rate telemetry, duty readback. | + +**Recommended boundary (unchanged from the plan, and confirmed correct by the +codebase):** the modulator *slot* and SHE engine are base-image HAL +primitives; the graph sees (a) the existing async-mod nodes (SVPWM etc.) and +(b) one new config/control surface for the supervisor (mode select as a +signal or config key, thresholds as KV). Do **not** try to express TIM5 +compare chains or OCxM forced writes in node code. + +**Feasibility: MEDIUM.** Base-image work is straightforward; making the slot +a first-class codegen concept (enter/update/commit/exit lifecycle, safe-point +semantics) is a genuine new toolchain feature and should be deferred until +the base-image implementation is proven on hardware. + +## 8. Bring-Up Order (revised) + +1. **Modulator interface refactor** — wrap existing SVPWM/SPWM behind + `mod_ops_t` in the base image, supervisor hard-wired to SVPWM. Zero + behavioral change; `platform_pwm_set*` signatures unchanged; validates the + architecture. Gate: existing graphs emit, build, and run identically. +2. **Legacy-path cleanup** — point `FocControlManager` at the extrapolated + encoder angle; retire the `foc_active`/`spwm_running` flag pair into the + modulator state. +3. **SHEPWM engine on TIM5, Option B** — compare-chain ISR writing TIM1 + forced modes; ping-pong tables; standalone test on the **forced-angle + ramp** at low bus voltage, current-limited supply, scope on phase voltage + and current. Validate dead time at forced transitions. +4. **Live handoff SVPWM ⇄ SHE** — phase-locked `enter()`/`exit()`, toggled + repeatedly at low voltage before raising bus; verify BKIN trip works + mid-SHE (inject gate-driver fault, confirm outputs idle). +5. **Supervisor + hysteresis + KV transition table** — tune on hardware + (dyno or bench motor); decide the MI-continuity strategy (§3) here. +6. **Optional:** DMA circular pattern feed; RCFM modulator. +7. **Codegen baking (last)** — once the base-image patterns are proven: + supervisor surface as nodes/config, document the modulation-slot contract + in `platform_api.h`, and only then consider a first-class + component-lifecycle feature in `InverterCodegen`. + +## 9. Open Questions to Resolve Before Starting Step 3 + +1. **MI continuity under SHE** — table family + interpolation vs hybrid trim + vs saturation-region-only (§3). This decides the table format and the + `enter()` contract. +2. **SHE operating region** — target fe range and pulse counts (the 35/40 Hz + thresholds in the original plan are placeholders; at 40–300 Hz a 51-pulse + pattern means 2–15 kHz effective switching — check device thermal limits, + current default is 2.5 kHz carrier / 5 kHz update). +3. **She angle source** — generated offline (Python script into a C table, or + FRAM KV blob)? Recommend offline solve → compile-time tables first, KV + upload later. +4. **Where `commit()` runs for SHE** — TIM5 CC ISR at what NVIC priority + relative to ADC (4) and TIM1_UP (5)? Must preempt the control ISRs to hold + edge jitter, must not preempt the AWD/fault path. +5. **Whether the legacy `FocControlManager` path stays** after graph-based + FOC reaches parity — affects how much of step 2 is worth doing. + +## 10. Hard Prerequisite for Closed-Loop FOC + SHEPWM: Current Oversampling / Ripple Compensation + +Bench testing showed that running FOC at low effective switching frequencies +(even with variable-carrier SVPWM, and certainly behind a fixed SHEPWM pattern) +produces unstable current feedback and can destroy hardware. The root cause is +not the modulator itself; it is the current measurement: + +- The existing base image samples phase currents once per PWM period at the + bottom (TIM1 CH4 TRGO → ADC1/ADC2 injected group). This is adequate at the + default 2.5 kHz carrier / 5 kHz update, but degrades as the carrier drops or + as the pattern becomes non-carrier-based. +- A single bottom sample does not represent the average phase current when + ripple is large, and the sampled value becomes a function of duty cycle and + electrical angle. +- Closing a current loop on that biased, ripple-dependent feedback leads to + instability and overcurrent trips / device failure. + +**Therefore, closed-loop FOC driving SHEPWM is gated on a separate foundation: +current oversampling and/or software ripple compensation.** Options: + +1. **Hardware or triggered multi-sample averaging** — configure the ADC to + capture several samples per PWM period (e.g., at multiple points in the + switching cycle) and average them in software. +2. **Software ripple compensation** — model the inductor current ripple from + the applied voltage vector, inductance, and back-EMF, then offset the + sampled value toward the average. +3. **Hybrid** — oversample to reduce noise, then apply a model-based correction + for the residual. + +Until this is validated against a high-bandwidth current reference across the +intended carrier/frequency envelope, **SHEPWM must remain a voltage-source +pattern only**: open-loop, handoff target, or supervisor-driven at speeds where +FOC is no longer required. Do not attempt to close FOC around SHEPWM without +first solving current measurement. + +This gates step 4 (live FOC ⇄ pattern handoff under load) and step 5 +(supervisor + closed-loop operation) for SHEPWM specifically. The modulator +refactor, SVPWM/SPWM work, and manual pattern shell commands can proceed +independently. diff --git a/Tools/she_table_gen.py b/Tools/she_table_gen.py new file mode 100644 index 00000000..9a80788e --- /dev/null +++ b/Tools/she_table_gen.py @@ -0,0 +1,252 @@ +#!/usr/bin/env python3 +"""Offline SHEPWM switching-angle solver for the Gen6 multi-modulator firmware. + +Solves the three-phase selective-harmonic-elimination equations for a grid of +modulation indices and emits a C++ table header consumed by ShepwmModulator. + +Waveform model (per phase leg, two-level q = +-1, quarter-wave symmetric): + q starts at +1 at theta=0+, toggles at each of the N angles in Q1, + Q2 mirrors Q1, second half-cycle is inverted. + Fourier (odd harmonics only; triplens cancel in 3-phase line voltages): + b_n / (4/pi) = 1 + 2 * sum_{i=1..N} (-1)^i * cos(n * alpha_i) + Equations per grid point (mi in per-unit of six-step fundamental, 1.0 = square): + fundamental: 1 + 2*sum((-1)^i cos(a_i)) = mi + elimination: 1 + 2*sum((-1)^i cos(n*a_i)) = 0 for the first N-1 + non-triplen odd harmonics (5, 7, 11, 13, ...) + +Solving: continuation along the MI grid (previous solution seeds the next), +with random restarts at the first grid point. Every solution is validated by +direct Fourier series evaluation before being accepted. + +Usage: + Tools/she_table_gen.py [--n 9] [--mi-min 0.50] [--mi-max 1.00] \ + [--mi-step 0.02] [--out Images/Gen6FW/Inc/Inverter/Drivers/PWM/SheTables.h] +""" + +import argparse +import sys + +import numpy as np +from scipy.optimize import least_squares + + +def harmonics_to_eliminate(count): + """First `count` odd, non-triplen harmonics starting at 5.""" + out = [] + n = 5 + while len(out) < count: + if n % 3 != 0: + out.append(n) + n += 2 + return out + + +def make_equations(mi, elim): + """Residual vector f(alpha) = 0 and its analytic Jacobian.""" + + def resid(alpha): + i = np.arange(1, len(alpha) + 1) + signs = (-1.0) ** i + r = [1.0 + 2.0 * np.sum(signs * np.cos(alpha)) - mi] + for n in elim: + r.append(1.0 + 2.0 * np.sum(signs * np.cos(n * alpha))) + return np.array(r) + + def jac(alpha): + i = np.arange(1, len(alpha) + 1) + signs = (-1.0) ** i + rows = [-2.0 * signs * np.sin(alpha)] + for n in elim: + rows.append(-2.0 * n * signs * np.sin(n * alpha)) + return np.array(rows) + + return resid, jac + + +def is_valid(alpha, tol_order=1e-9): + if np.any(alpha <= 1e-6) or np.any(alpha >= np.pi / 2 - 1e-6): + return False + return np.all(np.diff(alpha) > tol_order) + + +def solve_at(mi, elim, seed=None, rng=None, restarts=0): + """Solve SHE equations at one MI. Returns angles or None.""" + resid, jac = make_equations(mi, elim) + n_angles = len(elim) + 1 + + guesses = [] + if seed is not None: + guesses.append(np.array(seed, dtype=float)) + if rng is not None: + for _ in range(restarts): + g = np.sort(rng.uniform(0.05, np.pi / 2 - 0.05, size=n_angles)) + guesses.append(g) + + best = None + best_cost = np.inf + for g in guesses: + sol = least_squares(resid, g, jac=jac, bounds=(1e-4, np.pi / 2 - 1e-4), + xtol=1e-15, ftol=1e-15, gtol=1e-15, max_nfev=2000) + cost = np.max(np.abs(resid(sol.x))) + if cost < best_cost: + best_cost = cost + best = sol.x + if best_cost < 1e-9 and is_valid(best): + break + + if best is None or best_cost > 1e-7 or not is_valid(best): + return None + return best + + +def fourier_check(alpha, n_max=121): + """Direct Fourier magnitudes (per-unit of six-step) for odd harmonics.""" + i = np.arange(1, len(alpha) + 1) + signs = (-1.0) ** i + out = {} + for n in range(1, n_max, 2): + out[n] = abs(1.0 + 2.0 * np.sum(signs * np.cos(n * alpha))) + return out + + +def solve_branch(grid, elim, rng, seed_index, restarts=300): + """Solve across the grid by continuation from the best seed branch. + + Collects distinct solutions at grid[seed_index], continues each both + directions along the grid, and returns the branch covering the most grid + points as (table, covered_lo_idx, covered_hi_idx). + """ + seed_mi = grid[seed_index] + seeds = [] + for _ in range(restarts): + a = solve_at(seed_mi, elim, rng=rng, restarts=1) + if a is None: + continue + if all(np.max(np.abs(a - s)) > 1e-3 for s in seeds): + seeds.append(a) + print(f"seed mi={seed_mi:.2f}: {len(seeds)} distinct solution branches") + + best = None + for si, s in enumerate(seeds): + table = {seed_index: s} + a = s + hi = seed_index + for k in range(seed_index + 1, len(grid)): + nxt = solve_at(grid[k], elim, seed=a) + if nxt is None: + break + table[k] = nxt + a = nxt + hi = k + a = s + lo = seed_index + for k in range(seed_index - 1, -1, -1): + nxt = solve_at(grid[k], elim, seed=a) + if nxt is None: + break + table[k] = nxt + a = nxt + lo = k + print(f" branch {si}: covers mi={grid[lo]:.2f}..{grid[hi]:.2f} " + f"({hi - lo + 1} points)") + if best is None or (hi - lo) > (best[2] - best[1]): + best = (table, lo, hi) + + return best + + +def emit_header(path, n_angles, mi_min, mi_step, table): + rows = [] + for mi_idx, alpha in enumerate(table): + vals = ", ".join(f"{a:.10f}f" for a in alpha) + rows.append(f" {{ {vals} }}, // mi = {mi_min + mi_idx * mi_step:.2f}") + body = "\n".join(rows) + + content = f"""#pragma once + +/* Generated by Tools/she_table_gen.py -- do not edit. + * + * SHEPWM switching-angle family: {n_angles} angles per quarter cycle, + * MI grid {mi_min:.2f}..{mi_min + (len(table) - 1) * mi_step:.2f} step {mi_step:.2f} + * (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 + +namespace Inverter::shetab {{ + +inline constexpr uint32_t kAnglesPerQuarter = {n_angles}; +inline constexpr uint32_t kMiCount = {len(table)}; +inline constexpr float kMiMin = {mi_min:.6f}f; +inline constexpr float kMiStep = {mi_step:.6f}f; + +inline constexpr float kAngles[kMiCount][kAnglesPerQuarter] = {{ +{body} +}}; + +}} // namespace Inverter::shetab +""" + with open(path, "w") as f: + f.write(content) + + +def main(): + ap = argparse.ArgumentParser(description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("--n", type=int, default=9, + help="switching angles per quarter cycle (default 9; " + "eliminates harmonics 5..(first N-1 non-triplen odd))") + ap.add_argument("--mi-min", type=float, default=0.50) + ap.add_argument("--mi-max", type=float, default=0.90, + help="default 0.90: trajectory endpoint for N=9 (higher MI " + "needs lower-N 'gear shift' families)") + ap.add_argument("--mi-step", type=float, default=0.02) + ap.add_argument("--out", default="Images/Gen6FW/Inc/Inverter/Drivers/PWM/SheTables.h") + args = ap.parse_args() + + n_angles = args.n + elim = harmonics_to_eliminate(n_angles - 1) + grid = np.round(np.arange(args.mi_min, args.mi_max + 0.5 * args.mi_step, + args.mi_step), 6) + rng = np.random.default_rng(20260801) + + print(f"SHE solve: N={n_angles} angles/quarter, eliminating {elim}") + print(f"MI grid: {grid[0]:.2f}..{grid[-1]:.2f} step {args.mi_step:.2f} " + f"({len(grid)} points)") + + seed_index = int(np.argmin(np.abs(grid - 0.85))) + table_map, lo, hi = solve_branch(grid, elim, rng, seed_index) + + if lo > 0 or hi < len(grid) - 1: + print(f"FAIL: best branch covers mi={grid[lo]:.2f}..{grid[hi]:.2f}, " + f"not the full requested grid; adjust --mi-min/--mi-max.") + sys.exit(1) + + table = [table_map[k] for k in range(len(grid))] + + # Validate every grid point before emitting. + for k, alpha in enumerate(table): + spec = fourier_check(alpha) + worst_elim = max(spec[n] for n in elim) + uncontrolled = [n for n in sorted(spec) + if n not in elim and n % 3 != 0 and n > 1] + first_un = uncontrolled[0] if uncontrolled else 0 + print(f" mi={grid[k]:.2f} fund={spec[1]:.4f} " + f"worst_elim={worst_elim:.2e} " + f"first uncontrolled h{first_un}=" + f"{spec.get(first_un, 0) / max(first_un, 1):.3f} pu") + if abs(spec[1] - grid[k]) > 1e-6 or worst_elim > 1e-6: + print(f"FAIL: validation mismatch at mi={grid[k]:.2f}") + sys.exit(1) + + emit_header(args.out, n_angles, float(grid[0]), float(args.mi_step), table) + print(f"Wrote {args.out}: {len(table)} MI points x {n_angles} angles") + + +if __name__ == "__main__": + main()