444 lines
12 KiB
Verilog
444 lines
12 KiB
Verilog
/*************** Precision **************
|
||
* The control loop is designed around these values, but generally
|
||
* does not hardcode them.
|
||
*
|
||
* Since α 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.
|
||
*
|
||
* Intermediate values are 48-bit fixed-point integers multiplied
|
||
* by the step size of the ADC. The first 18 bits are the whole
|
||
* number and sign bits. This means intermediate values correspond
|
||
* exactly to values as understood by the ADC, with extra precision.
|
||
*
|
||
* To get the normal fixed-point value of an intermediate value,
|
||
* multiply it by 78e-6. To convert a normal fixed-point integer
|
||
* to an intermediate value, multiply it by 1/78e-6. In both
|
||
* cases, the conversion constant is a normal fixed-point integer.
|
||
*
|
||
* For instance, to encode the value 78e-6 as an intermediate
|
||
* value, multiply it by 1/78e-6 to obtain 1. Thus the value
|
||
* should be stored as 1 (whole bit) followed by zeros (fractional
|
||
* bits).
|
||
*/
|
||
|
||
`include control_loop_cmds.vh
|
||
`define ERR_WID (ADC_WID + 1)
|
||
|
||
module control_loop
|
||
#(
|
||
parameter ADC_WID = 18,
|
||
/* Code assumes DAC_WID > ADC_WID. If/when this is not the
|
||
* case, truncation code must be changed.
|
||
*/
|
||
parameter DAC_WID = 24,
|
||
/* Analog Devices DACs have a register code in the upper 4 bits.
|
||
* The data follows it.
|
||
*/
|
||
parameter DAC_DATA_WID = 20,
|
||
parameter CONSTS_WID = 48, // larger than ADC_WID
|
||
parameter CONSTS_FRAC_WID = CONSTS_WID-15,
|
||
parameter DELAY_WID = 16,
|
||
/* [ERR_WID_SIZ-1:0] must be able to store
|
||
* ERR_WID (ADC_WID + 1).
|
||
*/
|
||
parameter ERR_WID_SIZ = 6,
|
||
parameter DATA_WID = CONSTS_WID,
|
||
parameter READ_DAC_DELAY = 5,
|
||
parameter CYCLE_COUNT_WID = 16
|
||
) (
|
||
input clk,
|
||
|
||
input signed [ADC_WID-1:0] measured_value,
|
||
output adc_conv,
|
||
output adc_arm,
|
||
input adc_finished,
|
||
|
||
output reg signed [DAC_WID-1:0] to_dac,
|
||
input signed [DAC_WID-1:0] from_dac,
|
||
output dac_ss,
|
||
output dac_arm,
|
||
input dac_finished,
|
||
|
||
/* Hacky ad-hoc read-write interface. */
|
||
input reg [CONTROL_LOOP_CMD_WIDTH-1:0] cmd,
|
||
input reg [DATA_WIDTH-1:0] word_in,
|
||
output reg [DATA_WIDTH-1:0] word_out,
|
||
input start_cmd,
|
||
output reg finish_cmd
|
||
);
|
||
|
||
/* The loop variables can be modified on the fly. Each
|
||
* modification takes effect on the next loop cycle.
|
||
* When a caller modifies a variable, the modified
|
||
* variable is saved in [name]_buffer and loaded at CYCLE_START.
|
||
*/
|
||
|
||
reg signed [ADC_WID-1:0] setpt = 0;
|
||
reg signed [ADC_WID-1:0] setpt_buffer = 0;
|
||
|
||
reg signed [CONSTS_WID-1:0] cl_alpha_reg = 0;
|
||
reg signed [CONSTS_WID-1:0] cl_alpha_reg_buffer = 0;
|
||
|
||
reg signed [CONSTS_WID-1:0] cl_p_reg = 0;
|
||
reg signed [CONSTS_WID-1:0] cl_p_reg_buffer = 0;
|
||
|
||
reg [DELAY_WID-1:0] dely = 0;
|
||
reg [DELAY_WID-1:0] dely_buffer = 0;
|
||
|
||
|
||
reg running = 0;
|
||
reg signed[DAC_DATA_WID-1:0] stored_dac_val = 0;
|
||
|
||
/* Registers for PI calculations */
|
||
reg signed [ERR_WID-1:0] err_prev = 0;
|
||
|
||
/****** State machine
|
||
* ┏━━━━━━━┓
|
||
* ┃ ↓
|
||
* ┗←━INITIATE_READ_FROM_DAC━━←━━━━┓
|
||
* ↓ ┃
|
||
* WAIT_FOR_DAC_READ ┃
|
||
* ↓ ┃
|
||
* WAIT_FOR_DAC_RESPONSE ┃ (on reset)
|
||
* ↓ (when value is read) ┃
|
||
* ┏━━CYCLE_START━━→━━━━━━━━━━━━━━━┛
|
||
* ↑ ↓ (wait time delay)
|
||
* ┃ WAIT_ON_ADC
|
||
* ┃ ↓
|
||
* ┃ WAIT_ON_MUL
|
||
* ┃ ↓
|
||
* ┃ WAIT_ON_DAC
|
||
* ┃ ↓
|
||
* ┗━━━━━━━┛
|
||
****** Outline
|
||
* There are two systems: the read-write interface and the loop.
|
||
* The read-write interface allows another module (i.e. the CPU)
|
||
* to access and change constants. When a constant is changed the
|
||
* loop must reset the values that are preserved between loops
|
||
* (previous adjustment and previous delay).
|
||
*
|
||
* When the loop starts it must find the current value from the
|
||
* DAC and write to it. The value from the DAC is then adjusted
|
||
* with the output of the control loop. Afterwards it does not
|
||
* need to query the DAC for the updated value since it was the one
|
||
* that updated the value in the first place.
|
||
*/
|
||
|
||
localparam CYCLE_START = 0;
|
||
localparam WAIT_ON_ADC = 1;
|
||
localparam WAIT_ON_MUL = 2;
|
||
localparam WAIT_ON_DAC = 3;
|
||
localparam INIT_READ_FROM_DAC = 4;
|
||
localparam WAIT_FOR_DAC_READ = 5;
|
||
localparam WAIT_FOR_DAC_RESPONSE = 6;
|
||
localparam STATESIZ = 3;
|
||
|
||
reg [STATESIZ-1:0] state = INIT_READ_FROM_DAC;
|
||
|
||
/**** Precision Propogation
|
||
*
|
||
* Measured value: ADC_WID.0
|
||
* Setpoint: ADC_WID.0
|
||
* - ----------------------------|
|
||
* e: ERR_WID.0
|
||
*
|
||
* α: CONSTS_WHOLE.CONSTS_FRAC | P: CONSTS_WHOLE.CONSTS_FRAC
|
||
* e: ERR_WID.0 | e_p: ERR_WID.0
|
||
* x ----------------------------| x-----------------------------
|
||
* αe: CONSTS_WHOLE+ERR_WID.CONSTS_FRAC - Pe_p: CONSTS_WHOLE+ERR_WID.CONSTS_FRAC
|
||
* + A_p: CONSTS_WHOLE+ERR_WID.CONSTS_FRAC
|
||
* + stored_dac_val << CONSTS_FRAC: DAC_DATA_WID.CONSTS_FRAC
|
||
* --------------------------------------------------------------
|
||
* A_p + αe - Pe_p + stored_dac_val: CONSTS_WHOLE+ERR_WID+1.CONSTS_FRAC
|
||
* --> discard fractional bits: CONSTS_WHOLE+ADC_WID+1.(DAC_DATA_WID - ADC_WID)
|
||
* --> Saturate-truncate: ADC_WID.(DAC_DATA_WID-ADC_WID)
|
||
* --> reinterpret and write into DAC: DAC_DATA_WID.0
|
||
*/
|
||
|
||
/**** Calculate Error ****/
|
||
wire [ERR_WID-1:0] err_cur = measured_value - setpoint;
|
||
|
||
/****** Multiplication *******
|
||
* Truncation of a fixed-point integer to a smaller buffer requires
|
||
* 1) truncating higher order bits
|
||
* 2) removing lower order bits
|
||
*
|
||
* The ADC number has no fractional digits, so the fixed point output
|
||
* is [CONSTS_WHOLE + ERR_WID].CONSTS_FRAC_WID
|
||
* with total width CONSTS_WID + ERR_WID
|
||
*
|
||
* Both multipliers are armed at the same time.
|
||
* Their output wires are ANDed together so the state machine
|
||
* progresses when both are finished.
|
||
*/
|
||
|
||
localparam MUL_WHOLE_WID = CONSTS_WHOLE + ERR_WID;
|
||
localparam MUL_FRAC_WID = CONSTS_FRAC;
|
||
localparam MUL_WID = MUL_WHOLE_WID + MUL_FRAC_WID;
|
||
|
||
reg arm_mul = 0;
|
||
|
||
wire alpha_err_fin;
|
||
wire signed [MUL_WID-1:0] alpha_err;
|
||
wire p_err_prev_fin;
|
||
wire signed [MUL_WID-1:0] p_err_prev;
|
||
|
||
wire mul_finished = alpha_err_fin & p_err_fin;
|
||
|
||
/* αe */
|
||
boothmul #(
|
||
.A1_LEN(CONSTS_WID),
|
||
.A2_LEN(ERR_WID),
|
||
.A2LEN_SIZ(ERR_WID_SIZ)
|
||
) boothmul_alpha_err_mul (
|
||
.clk(clk),
|
||
.arm(arm_mul),
|
||
.a1(cl_alpha_reg),
|
||
.a2(err),
|
||
.outn(alpha_err),
|
||
.fin(alpha_err_fin)
|
||
);
|
||
|
||
/* Pe_p */
|
||
boothmul #(
|
||
.A1_LEN(CONSTS_WID),
|
||
.A2_LEN(ERR_WID),
|
||
.A2LEN_SIZ(ERR_WID_SIZ)
|
||
) booth_mul_P_err_mul (
|
||
.clk(clk),
|
||
.arm(arm_mul),
|
||
.a1(cl_p_reg),
|
||
.a2(err_prev),
|
||
.outn(p_err_prev),
|
||
.fin(p_err_prev_fin)
|
||
);
|
||
|
||
/**** Subtraction after multiplication ****/
|
||
localparam SUB_WHOLE_WID = MUL_WHOLE_WID + 1;
|
||
localparam SUB_FRAC_WID = MUL_FRAC_WID;
|
||
localparam SUB_WID = SUB_WHOLE_WID + SUB_FRAC_WID;
|
||
|
||
reg signed [SUB_WID-1:0] adj_old = 0;
|
||
wire signed [SUB_WID-1:0] newadj = adj_old + alpha_err - p_err_prev
|
||
+ (stored_dac_val << CONSTS_FRAC);
|
||
|
||
/**** Discard fractional bits ****
|
||
* The truncation of the subtraction result first takes off the lower
|
||
* order bits:
|
||
* [ SUB_WHOLE_WID ].[ SUB_FRAC_WID ]
|
||
* [ SUB_WHOLE_WID ].[RTRUNC_FRAC_WID]############
|
||
* (SUB_FRAC_WID - RTRUNC_FRAC_WID)^
|
||
*/
|
||
|
||
localparam RTRUNC_FRAC_WID = DAC_DATA_WID - ADC_WID;
|
||
localparam RTRUNC_WHOLE_WID = SUB_WHOLE_WID;
|
||
localparam RTRUNC_WID = RTRUNC_WHOLE_WID + RTRUNC_FRAC_WID;
|
||
|
||
wire signed[RTRUNC_WID-1:0] rtrunc =
|
||
newadj[SUB_WID-1:SUB_FRAC_WID-RTRUNC_FRAC_WID];
|
||
|
||
/**** Truncate-Saturate ****
|
||
* Truncate the result into a value acceptable to the DAC.
|
||
* [ SUB_WHOLE_WID ].[RTRUNC_FRAC_WID]############
|
||
* [ADC_WID].[DAC_DATA_WID - ADC_WID]
|
||
* reinterpreted as
|
||
* [DAC_DATA_WID].0
|
||
*/
|
||
|
||
wire signed[DAC_DATA_WID-1:0] dac_adj_val;
|
||
|
||
intsat #(
|
||
.IN_LEN(RTRUNC_WID),
|
||
.LTRUNC(DAC_DATA_WID)
|
||
) sat_newadj_rtrunc (
|
||
.inp(rtrunc),
|
||
.outp(dac_adj_val)
|
||
);
|
||
|
||
reg [DELAY_WID-1:0] timer = 0;
|
||
|
||
reg [CYCLE_COUNT_WID-1:0] last_timer = 0;
|
||
reg [CYCLE_COUNT_WID-1:0] debug_timer = 0;
|
||
/**** Timing debug. ****/
|
||
always @ (posedge clk) begin
|
||
if (state == INIT_READ_FROM_DAC) begin
|
||
debug_timer <= 1;
|
||
last_timer <= debug_timer;
|
||
end else begin
|
||
debug_timer <= debug_timer + 1;
|
||
end
|
||
end
|
||
|
||
/**** Read-Write control interface. ****/
|
||
|
||
always @ (posedge clk) begin
|
||
if (start_cmd && !finish_cmd) begin
|
||
case (cmd)
|
||
CONTROL_LOOP_NOOP: CONTROL_LOOP_NOOP | CONTROL_LOOP_WRITE_BIT:
|
||
finish_cmd <= 1;
|
||
CONTROL_LOOP_STATUS: begin
|
||
word_out[DATA_WID-1:1] <= 0;
|
||
word_out[0] <= running;
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_STATUS | CONTROL_LOOP_WRITE_BIT:
|
||
running <= word_in[0];
|
||
finish_cmd <= 1;
|
||
CONTROL_LOOP_SETPT: begin
|
||
word_out[DATA_WID-1:ADC_WID] <= 0;
|
||
word_out[ADC_WID-1:0] <= setpt;
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_SETPT | CONTROL_LOOP_WRITE_BIT:
|
||
setpt_buffer <= word_in[ADC_WID-1:0];
|
||
finish_cmd <= 1;
|
||
CONTROL_LOOP_P: begin
|
||
word_out <= cl_p_reg;
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_P | CONTROL_LOOP_WRITE_BIT: begin
|
||
cl_p_reg_buffer <= word_in;
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_ALPHA: begin
|
||
word_out <= cl_alpha_reg;
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_ALPHA | CONTROL_LOOP_WRITE_BIT: begin
|
||
cl_alpha_reg_buffer <= word_in;
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_DELAY: begin
|
||
word_out[DATA_WID-1:DELAY_WID] <= 0;
|
||
word_out[DELAY_WID-1:0] <= dely;
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_DELAY | CONTROL_LOOP_WRITE_BIT: begin
|
||
dely_buffer <= word_in[DELAY_WID-1:0];
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_ERR: begin
|
||
word_out[DATA_WID-1:ERR_WID] <= 0;
|
||
word_out[ERR_WID-1:0] <= err_prev;
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_Z: begin
|
||
word_out[DATA_WID-1:DAC_DATA_WID] <= 0;
|
||
word_out[DAC_DATA_WID-1:0] <= stored_dac_val;
|
||
finish_cmd <= 1;
|
||
end
|
||
CONTROL_LOOP_CYCLES: begin
|
||
word_out[DATA_WID-1:CYCLE_COUNT_WID] <= 0;
|
||
word_out[CYCLE_COUNT_WID-1:0] <= last_timer;
|
||
finish_cmd <= 0;
|
||
end
|
||
end else if (!start_cmd) begin
|
||
finish_cmd <= 0;
|
||
end
|
||
end
|
||
|
||
/* This is not a race condition as long as two variables are
|
||
* not being assigned at the same time. Instead, the lower
|
||
* assign block will use the older values (i.e. the upper assign
|
||
* block only takes effect next clock cycle).
|
||
*/
|
||
|
||
always @ (posedge clk) begin
|
||
case (state)
|
||
INIT_READ_FROM_DAC: begin
|
||
if (running) begin
|
||
/* 1001[0....] is read from dac register */
|
||
to_dac <= b'1001 << DAC_DATA_WID;
|
||
dac_ss <= 1;
|
||
dac_arm <= 1;
|
||
state <= WAIT_FOR_DAC_READ;
|
||
end
|
||
end
|
||
WAIT_FOR_DAC_READ: begin
|
||
if (dac_finished) begin
|
||
state <= WAIT_FOR_DAC_RESPONSE;
|
||
dac_ss <= 0;
|
||
dac_arm <= 0;
|
||
timer <= 1;
|
||
end
|
||
end
|
||
WAIT_FOR_DAC_RESPONSE: begin
|
||
if (timer < READ_DAC_DELAY && timer != 0) begin
|
||
timer <= timer + 1;
|
||
end else if (timer == READ_DAC_DELAY) begin
|
||
dac_ss <= 1;
|
||
dac_arm <= 1;
|
||
to_dac <= 0;
|
||
timer <= 0;
|
||
end else if (dac_finished) begin
|
||
state <= CYCLE_START;
|
||
dac_ss <= 0;
|
||
dac_arm <= 0;
|
||
stored_dac_val <= from_dac;
|
||
end
|
||
end
|
||
CYCLE_START: begin
|
||
if (!running) begin
|
||
state <= INIT_READ_FROM_DAC;
|
||
end else if (timer < dely) begin
|
||
timer <= timer + 1;
|
||
end else begin
|
||
/* On change of constants, previous values are invalidated. */
|
||
if (setpt != setpt_buffer ||
|
||
cl_alpha_reg != cl_alpha_reg_buffer ||
|
||
cl_p_reg != cl_p_reg_buffer) begin
|
||
setpt <= setpt_buffer;
|
||
dely <= dely_buf;
|
||
cl_alpha_reg <= cl_alpha_reg_buffer;
|
||
cl_p_reg <= cl_p_reg_buffer;
|
||
adj_prev <= 0;
|
||
err_prev <= 0;
|
||
end
|
||
|
||
state <= WAIT_ON_ADC;
|
||
timer <= 0;
|
||
adc_arm <= 1;
|
||
adc_conv <= 1;
|
||
end
|
||
end
|
||
WAIT_ON_ADC: if (adc_finished) begin
|
||
adc_arm <= 0;
|
||
adc_conv <= 0;
|
||
arm_mul <= 1;
|
||
state <= WAIT_ON_MUL;
|
||
end
|
||
WAIT_ON_MUL: if (mul_finished) begin
|
||
arm_mul <= 0;
|
||
dac_arm <= 1;
|
||
dac_ss <= 1;
|
||
stored_dac_val <= dac_adj_val;
|
||
to_dac <= b'0001 << DAC_DATA_WID | dac_adj_val;
|
||
state <= WAIT_ON_DAC;
|
||
end
|
||
WAIT_ON_DAC: if (dac_finished) begin
|
||
state <= CYCLE_START;
|
||
dac_ss <= 0;
|
||
dac_arm <= 0;
|
||
|
||
err_prev <= err_cur;
|
||
adj_old <= newadj;
|
||
end
|
||
end
|
||
end
|
||
|
||
endmodule
|