upsilon/firmware/rtl/control_loop/control_loop_math.v.m4

301 lines
7.3 KiB
Plaintext

m4_changequote(`⟨', `⟩')
m4_changecom(⟨/*⟩, ⟨*/⟩)
/*************** Precision **************
* The control loop is designed around these values, but generally
* does not hardcode them.
*
* Since I and P are precalculated outside of the loop, their
* conversion to numbers the loop understands is done outside of
* the loop and in the kernel.
*
* The 18-bit ADC is twos-compliment, -10.24V to 10.24V,
* with 78μV per increment.
* The 20-bit DAC is twos-compliment, -10V to 10V.
*
* The `P` constant has a minimum value of 1e-7 with a precision
* of 1e-9, and a maxmimum value of 1.
*
* The `I` constant has a minimum value of 1e-4 with a precision
* of 1e-6 and a maximum value of 100.
*
* Δt is cycles/100MHz. This makes Δt at least 10 ns, with a
* maximum of 1 ms.
*
* [1 : sign][20: whole][43: fractional]
*/
module control_loop_math #(
parameter CONSTS_WHOLE = 21,
parameter CONSTS_FRAC = 43,
m4_define(M4_CONSTS_WID, (CONSTS_WHOLE + CONSTS_FRAC))
parameter CONSTS_SIZ=7,
parameter ADC_WID = 18,
parameter [M4_CONSTS_WID-1:0] SEC_PER_CYCLE = 'b10101011110011000,
/* The conversion between the ADC bit (20/2**18) and DAC bit (20.48/2**20)
* is 0.256.
*/
parameter [M4_CONSTS_WID-1:0] ADC_TO_DAC = 64'b0100000110001001001101110100101111000110101,
parameter CYCLE_COUNT_WID = 18,
parameter DAC_WID = 20
m4_define(M4_E_WID, (DAC_WID + 1))
) (
input clk,
input arm,
output reg finished,
input signed [ADC_WID-1:0] setpt,
input signed [ADC_WID-1:0] measured,
input signed [M4_CONSTS_WID-1:0] cl_P,
input signed [M4_CONSTS_WID-1:0] cl_I,
input signed [CYCLE_COUNT_WID-1:0] cycles,
input signed [M4_E_WID-1:0] e_prev,
input signed [M4_CONSTS_WID-1:0] adjval_prev,
input signed [DAC_WID-1:0] stored_dac_val,
`ifdef DEBUG_CONTROL_LOOP_MATH
output reg [M4_CONSTS_WID-1:0] dt_reg,
output reg [M4_CONSTS_WID-1:0] idt_reg,
output reg [M4_CONSTS_WID-1:0] epidt_reg,
output reg [M4_CONSTS_WID-1:0] ep_reg,
`endif
output reg signed [M4_E_WID-1:0] e_cur,
output signed [DAC_WID-1:0] new_dac_val,
output signed [M4_CONSTS_WID-1:0] adj_val
);
/*******
* Multiplier segment.
* Multiplies two 64 bit numbers and right-saturate + truncates it
* to be a 64 bit output, according to fixed-point rules.
*/
reg signed [M4_CONSTS_WID-1:0] a1;
reg signed [M4_CONSTS_WID-1:0] a2;
/* verilator lint_off UNUSED */
wire signed [M4_CONSTS_WID+M4_CONSTS_WID-1:0] out_untrunc;
wire mul_fin;
reg mul_arm = 0;
boothmul #(
.A1_LEN(M4_CONSTS_WID),
.A2_LEN(M4_CONSTS_WID),
.A2LEN_SIZ(CONSTS_SIZ)
) multiplier (
.a1(a1),
.a2(a2),
.clk(clk),
.outn(out_untrunc),
.fin(mul_fin),
.arm(mul_arm)
);
/****************************
* QX.Y * QX.Y = Q(2X).(2Y)
* This right-truncation gets rid of the lowest Y bits.
* Q(2X).Y
*/
m4_define(M4_OUT_RTRUNC_WID, (M4_CONSTS_WID+M4_CONSTS_WID-CONSTS_FRAC))
wire signed [M4_OUT_RTRUNC_WID-1:0] out_rtrunc
= out_untrunc[M4_CONSTS_WID+M4_CONSTS_WID-1:CONSTS_FRAC];
wire signed [M4_CONSTS_WID-1:0] mul_out;
/***************************
* Saturate higher X bits away.
* Q(2X).Y -> QX.Y
*/
intsat #(
.IN_LEN(M4_OUT_RTRUNC_WID),
.LTRUNC(CONSTS_WHOLE)
) multiplier_saturate (
.inp(out_rtrunc),
.outp(mul_out)
);
/*************************
* Safely get rid of high bit in addition.
************************/
reg signed [M4_CONSTS_WID+1-1:0] add_sat;
wire signed [M4_CONSTS_WID-1:0] saturated_add;
intsat #(
.IN_LEN(M4_CONSTS_WID + 1),
.LTRUNC(1)
) addition_saturate (
.inp(add_sat),
.outp(saturated_add)
);
/************************
* Safely truncate down adjustment value.
***********************/
reg signed [CONSTS_WHOLE-1:0] adj_sat;
wire signed [DAC_WID-1:0] adj_final;
intsat #(
.IN_LEN(CONSTS_WHOLE),
.LTRUNC(CONSTS_WHOLE - DAC_WID)
) adj_saturate (
.inp(adj_sat),
.outp(adj_final)
);
/************************
* Safely calculate new DAC value.
************************/
reg signed [DAC_WID+1-1:0] add_sat_dac;
intsat #(
.IN_LEN(DAC_WID+1),
.LTRUNC(1)
) dac_saturate (
.inp(add_sat_dac),
.outp(new_dac_val)
);
localparam WAIT_ON_ARM = 0;
localparam CALCULATE_ERR = 9;
localparam CALCULATE_DAC_E = 7;
localparam WAIT_ON_CALCULATE_DT = 1;
localparam CALCULATE_IDT = 2;
localparam CALCULATE_EPIDT = 3;
localparam CALCULATE_EP = 4;
localparam CALCULATE_A_PART_1 = 5;
localparam CALCULATE_A_PART_2 = 6;
localparam CALCULATE_NEW_DAC_VALUE_PART_1 = 10;
localparam CALCULATE_NEW_DAC_VALUE_PART_2 = 11;
localparam WAIT_ON_DISARM = 8;
reg [4:0] state = WAIT_ON_ARM;
reg signed [M4_CONSTS_WID+1-1:0] tmpstore = 0;
wire signed [M4_CONSTS_WID-1:0] tmpstore_view = tmpstore[M4_CONSTS_WID-1:0];
always @ (posedge clk) begin
case (state)
WAIT_ON_ARM:
if (arm) begin
a1[CONSTS_FRAC-1:0] <= 0;
a1[CONSTS_FRAC+ADC_WID + 1-1:CONSTS_FRAC] <= setpt - measured;
state <= CALCULATE_ERR;
end else begin
finished <= 0;
mul_arm <= 0;
end
CALCULATE_ERR: begin
/* Sign-extend */
a1[M4_CONSTS_WID-1:CONSTS_FRAC + ADC_WID + 1] <=
{(M4_CONSTS_WID-(CONSTS_FRAC + ADC_WID + 1)){a1[ADC_WID+1-1+CONSTS_FRAC]}};
a2 <= ADC_TO_DAC;
mul_arm <= 1;
state <= CALCULATE_DAC_E;
end
CALCULATE_DAC_E:
if (mul_fin) begin
/* Discard other bits. This works without saturation because
* CONSTS_WHOLE = E_WID. */
e_cur <= mul_out[M4_CONSTS_WID-1:CONSTS_FRAC];
a1 <= SEC_PER_CYCLE;
/* No sign extension, cycles is positive */
a2 <= {{(CONSTS_WHOLE - CYCLE_COUNT_WID){1'b0}}, cycles, {(CONSTS_FRAC){1'b0}}};
mul_arm <= 0;
state <= WAIT_ON_CALCULATE_DT;
end
WAIT_ON_CALCULATE_DT:
if (!mul_arm) begin
mul_arm <= 1;
end else if (mul_fin) begin
mul_arm <= 0;
`ifdef DEBUG_CONTROL_LOOP_MATH
dt_reg <= mul_out;
`endif
a1 <= mul_out; /* a1 = Δt */
a2 <= cl_I;
state <= CALCULATE_IDT;
end
CALCULATE_IDT:
if (!mul_arm) begin
mul_arm <= 1;
end else if (mul_fin) begin
mul_arm <= 0;
add_sat <= (mul_out + cl_P);
`ifdef DEBUG_CONTROL_LOOP_MATH
idt_reg <= mul_out;
`endif
a2 <= {{(CONSTS_WHOLE-M4_E_WID){e_cur[M4_E_WID-1]}},e_cur, {(CONSTS_FRAC){1'b0}}};
state <= CALCULATE_EPIDT;
end
CALCULATE_EPIDT:
if (!mul_arm) begin
a1 <= saturated_add;
mul_arm <= 1;
end else if (mul_fin) begin
mul_arm <= 0;
tmpstore <= {mul_out[M4_CONSTS_WID-1],mul_out};
`ifdef DEBUG_CONTROL_LOOP_MATH
epidt_reg <= mul_out;
`endif
a1 <= cl_P;
a2 <= {{(CONSTS_WHOLE-M4_E_WID){e_prev[M4_E_WID-1]}},e_prev, {(CONSTS_FRAC){1'b0}}};
state <= CALCULATE_EP;
end
CALCULATE_EP:
if (!mul_arm) begin
mul_arm <= 1;
end else if (mul_fin) begin
`ifdef DEBUG_CONTROL_LOOP_MATH
ep_reg <= mul_out;
`endif
mul_arm <= 0;
add_sat <= (tmpstore_view - mul_out);
state <= CALCULATE_A_PART_1;
end
CALCULATE_A_PART_1: begin
tmpstore <= saturated_add + adjval_prev;
state <= CALCULATE_A_PART_2;
end
CALCULATE_A_PART_2: begin
add_sat <= tmpstore;
state <= CALCULATE_NEW_DAC_VALUE_PART_1;
end
CALCULATE_NEW_DAC_VALUE_PART_1: begin
adj_sat <= saturated_add[M4_CONSTS_WID-1:CONSTS_FRAC];
adj_val <= saturated_add;
state <= CALCULATE_NEW_DAC_VALUE_PART_2;
end
CALCULATE_NEW_DAC_VALUE_PART_2: begin
add_sat_dac <= adj_final + stored_dac_val;
state <= WAIT_ON_DISARM;
end
WAIT_ON_DISARM: begin
adj_val <= saturated_add;
if (!arm) begin
state <= WAIT_ON_ARM;
finished <= 0;
end else begin
finished <= 1;
end
end
endcase
end
`ifdef VERILATOR
initial begin
$dumpfile("control_loop_math.fst");
$dumpvars;
end
`endif
endmodule