diff --git a/firmware/rtl/control_loop/control_loop.v b/firmware/rtl/control_loop/control_loop.v index 8df2d0f..8e5a957 100644 --- a/firmware/rtl/control_loop/control_loop.v +++ b/firmware/rtl/control_loop/control_loop.v @@ -1,40 +1,3 @@ -/*************** 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) @@ -58,7 +21,7 @@ module control_loop parameter ERR_WID_SIZ = 6, parameter DATA_WID = CONSTS_WID, parameter READ_DAC_DELAY = 5, - parameter CYCLE_COUNT_WID = 16 + parameter CYCLE_COUNT_WID = 18 ) ( input clk, @@ -99,7 +62,6 @@ 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; @@ -154,7 +116,13 @@ reg [STATESIZ-1:0] state = INIT_READ_FROM_DAC; * Measured value: ADC_WID.0 * Setpoint: ADC_WID.0 * - ----------------------------| - * e: ERR_WID.0 + * e_unsc: ERR_WID.0 + * x 78e-6: 0.CONSTS_FRAC + * - ----------------------------| + * e_uncropped: ERR_WID.CONSTS_FRAC + * ------------------------------------- + * e: CONSTS_WID.CONSTS_FRAC + * * * α: CONSTS_WHOLE.CONSTS_FRAC | P: CONSTS_WHOLE.CONSTS_FRAC * e: ERR_WID.0 | e_p: ERR_WID.0 @@ -170,7 +138,34 @@ reg [STATESIZ-1:0] state = INIT_READ_FROM_DAC; */ /**** Calculate Error ****/ -wire [ERR_WID-1:0] err_cur = measured_value - setpoint; +wire [ERR_WID-1:0] e_unsc = measured_value - setpoint; + +/**** convert error value to real value ****/ + +wire [ERR_WID+CONSTS_FRAC-1:0] e_uncrop; +reg mul_scale_err_fin = 0; + +boothmul #( + .A1_LEN(ERR_WID), + .A2_LEN(CONSTS_FRAC) +) mul_scale_err ( + .clk(clk), + .arm(arm_err_scale), + .a1(e_unsc), + .a2(adc_int_to_real), + .outn(e_uncrop), + .fin(mul_scale_err_fin) +); + +wire [CONSTS_WID-1:0] e; + +intsat #( + .A1_LEN(ERR_WID + CONSTS_FRAC), + .A2_LEN(CONSTS_WID) +) mul_scale_err_crop ( + .inp(e_uncrop), + .outp(e) +); /****** Multiplication ******* * Truncation of a fixed-point integer to a smaller buffer requires diff --git a/firmware/rtl/control_loop/control_loop_math.v b/firmware/rtl/control_loop/control_loop_math.v index 9bb1e5e..dc6b14c 100644 --- a/firmware/rtl/control_loop/control_loop_math.v +++ b/firmware/rtl/control_loop/control_loop_math.v @@ -1,3 +1,4 @@ +`undefineall /*************** Precision ************** * The control loop is designed around these values, but generally * does not hardcode them. @@ -23,102 +24,48 @@ * This is 127 to -128, with a resolution of 9.095e-13. */ -/* If this design needs to be faster, you can: - 1) Pipeline the design - 2) Use DSPs - - With symbiflow + yosys there is no way to explicitly instantiate - a DSP40 module. YOSYS may infer it but that might be unreliable. - */ - module control_loop_math #( parameter CONSTS_WHOLE = 8, parameter CONSTS_FRAC = 40, `define CONSTS_WID (CONSTS_WHOLE + CONSTS_FRAC) - /* This number is the conversion from ADC voltage units to - * a fixed-point number. - * A micro-optimization could roll the ADC reading and the multiplier - * together. - * The LSB of this number is 2**(-CONSTS_FRAC). - */ - parameter INT_TO_REAL_WID = 27, - parameter [INT_TO_REAL_WID-1:0] INT_TO_REAL = 'b101000111001001111101110010, /* This number is 1/(clock cycle). The number is interpreted so the least significant bit coincides with the LSB of a constant. */ parameter SEC_PER_CYCLE_WID = 14, parameter [SEC_PER_CYCLE_WID-1:0] SEC_PER_CYCLE = 'b10101011110011, - parameter DELAY_WID = 16, parameter DAC_DATA_WID = 20, parameter ADC_WID = 18, +`define E_WID (ADC_WID + 1) + /* How large the intermediate value should be. This should hold the ADC + * value /as an integer/ along with the P, I values. + * The OUT_FRAC value should usually but not always be the same as CONSTS_FRAC. + */ + parameter OUT_WHOLE = 20, + parameter OUT_FRAC = 40, +`define OUT_WID (OUT_WHOLE + OUT_FRAC) parameter CYCLE_COUNT_WID = 18 ) ( input clk, input arm, - output finished, + output reg finished, input [ADC_WID-1:0] setpt, input [ADC_WID-1:0] measured, input [`CONSTS_WID-1:0] cl_P, input [`CONSTS_WID-1:0] cl_I, - input [`CONSTS_WID-1:0] e_prev, input [CYCLE_COUNT_WID-1:0] cycles, - input [DELAY_WID-1:0] dely, + input [`ERR_WID-1:0] e_prev, + input [`OUT_WID-1:0] adjval_prev, - output reg [`CONSTS_WID-1:0] e_cur, - output reg [DAC_DATA_WID-1:0] adjval + output reg [`ERR_WID-1:0] e_cur, + output [`OUT_WID-1:0] adj_val ); -/**** Stage 1: Convert error to real value, calculate Δt = cycles/100MHz - * - * e_unscaled: ERR_WID.0 - * x INT_TO_REAL: 0.INT_TO_REAL_WID - *- ----------------------------- - * e_scaled_unsat: ERR_WID + INT_TO_REAL_WID - */ +/* Calculate current error */ +assign e_cur = setpt - measured; -`define ERR_WID (ADC_WID + 1) -wire [`ERR_WID-1:0] e_unscaled = setpt - measured; - -reg arm_stage_1 = 0; -wire mul_scale_err_fin; - -`define E_UNTRUNC_WID (`ERR_WID + INT_TO_REAL_WID) -wire [`E_UNTRUNC_WID-1:0] e_scaled_unsat; -boothmul #( - .A1_LEN(INT_TO_REAL_WID), - .A2_LEN(`ERR_WID) -) mul_scale_err ( - .clk(clk), - .arm(arm_stage_1), - .a1(INT_TO_REAL), - .a2(e_unscaled), - .outn(e_scaled_unsat), - .fin(mul_scale_err_fin) -); - -`define E_WID (`E_UNTRUNC_WID > `CONSTS_WID ? `CONSTS_WID : `E_UNTRUNC_WID) -wire [`E_WID-1:0] e; - -`define E_FRAC (`E_WID < `CONSTS_FRAC ? `E_WID : `E_WID - `CONSTS_FRAC) -`define E_WHOLE (`E_WID - `E_FRAC) - -/* Don't bother keeping numbers larger than the constant width - * since the value will always fit in it. */ - -generate if (`E_UNTRUNC_WID > `CONSTS_WID) begin - intsat #( - .IN_LEN(`E_UNTRUNC_WID), - .LTRUNC(`E_UNTRUNC_WID - `CONSTS_WHOLE) - ) sat_mul_scale_err ( - .inp(e_scaled_unsat), - .outp(e) - ); -end else begin - assign e = e_scaled_unsat; -end endgenerate - -/* cycles: CYCLE_COUNT_WID.0 +/**** Stage 1: calculate Δt = cycles/100MHz + * cycles: CYCLE_COUNT_WID.0 * SEC_PER_CYCLE: 0....SEC_PER_CYCLE_WID * -x-------------------------------- * dt_unsat: CYCLE_COUNT_WID + SEC_PER_CYCLE_WID @@ -126,9 +73,12 @@ end endgenerate * Optimization note: the total width can be capped to below 1. */ +reg arm_stage_1 = 0; + `define DT_UNSAT_WID (CYCLE_COUNT_WID + SEC_PER_CYCLE_WID) wire [`DT_UNSAT_WID-1:0] dt_unsat; wire mul_dt_fin; + boothmul #( .A1_LEN(CYCLE_COUNT_WID), .A2_LEN(SEC_PER_CYCLE_WID) @@ -145,7 +95,7 @@ boothmul #( wire [`DT_WID-1:0] dt; `define DT_WHOLE (`DT_WID < `CONSTS_FRAC ? 0 : `CONSTS_FRAC - `DT_WID) -`define DT_FRAC(`DT_WID - `DT_WHOLE) +`define DT_FRAC (`DT_WID - `DT_WHOLE) generate if (`DT_UNSAT_WID > `CONSTS_WID) begin intsat #( @@ -176,11 +126,12 @@ reg arm_stage2 = 0; wire [`CONSTS_WID-1:0] idt; mul_const #( -/* TODO: does this autoinfer CONSTS_WID? */ .CONSTS_WHOLE(CONSTS_WHOLE), .CONSTS_FRAC(CONSTS_FRAC), .IN_WHOLE(`DT_WHOLE), - .IN_FRAC(`DT_FRAC) + .IN_FRAC(`DT_FRAC), + .OUT_WHOLE(CONSTS_WHOLE), + .OUT_FRAC(CONSTS_FRAC) ) mul_const_idt ( .clk(clk), .inp(dt), @@ -190,7 +141,7 @@ mul_const #( .finished(stage2_finished) ); -wire [`CONSTS_WID:0] pidt_untrunc = cl_P + idt; +reg [`CONSTS_WID:0] pidt_untrunc; /* Assuming that the constraints on cl_P, I, and dt hold */ wire [`CONSTS_WID-1:0] pidt = pidt_untrunc[`CONSTS_WID-1:0]; @@ -201,36 +152,51 @@ reg arm_stage3 = 0; wire epidt_finished; wire pe_finished; -wire [`CONSTS_WID-1:0] epidt; +wire [`OUT_WID-1:0] epidt; mul_const #( .CONSTS_WHOLE(`CONSTS_WHOLE), .CONSTS_FRAC(`CONSTS_FRAC), - .IN_WHOLE(`E_WHOLE), - .IN_FRAC(`E_FRAC) + .IN_WHOLE(`E_WID), + .IN_FRAC(0), + .OUT_WHOLE(OUT_WHOLE), + .OUT_FRAC(OUT_FRAC) ) mul_const_epidt ( .clk(clk), - .inp(e), + .inp(e_cur), .const_in(idt), .arm(arm_stage3), .outp(epidt), .finished(epidt_finished) ); -wire [`CONSTS_WID-1:0] pe; +wire [`OUT_WID-1:0] pe; mul_const #( .CONSTS_WHOLE(`CONSTS_WHOLE), .CONSTS_FRAC(`CONSTS_FRAC), - .IN_WHOLE(`E_WHOLE), - .IN_FRAC(`E_FRAC) + .IN_WHOLE(`ERR_WID), + .IN_FRAC(0), + .OUT_WHOLE(OUT_WHOLE), + .OUT_FRAC(OUT_FRAC) ) mul_const_pe ( .clk(clk), - .inp(e), + .inp(e_prev), .const_in(idt), .arm(arm_stage3), .outp(pe), .finished(epidt_finished) ); +reg [`OUT_WID+1:0] adj_val_utrunc; +/* = prev_adj + epidt - pe; */ + +intsat #( + .IN_LEN(`OUT_WID + 2), + .LTRUNC(2) +) adj_val_sat ( + .inp(adj_val_utrunc), + .outp(adj_val) +); + /******* State machine ********/ localparam WAIT_ON_ARM = 0; localparam WAIT_ON_STAGE_1 = 1; @@ -258,6 +224,7 @@ always @ (posedge clk) begin end WAIT_ON_STAGE_2: begin if (stage2_finished) begin + pidt_untrunc <= cl_P + idt; arm_stage_2 <= 0; arm_stage_3 <= 1; state <= WAIT_ON_STAGE_3; @@ -265,6 +232,7 @@ always @ (posedge clk) begin end WAIT_ON_STAGE_3: begin if (epidt_finished && pe_finished) begin + adj_val_utrunc <= prev_adj + epidt - pe; arm_stage3 <= 0; finished <= 1; state <= WAIT_ON_DISARM; diff --git a/firmware/rtl/control_loop/control_loop_sim.cpp b/firmware/rtl/control_loop/control_loop_sim.cpp index 7aa5efb..f629ca4 100644 --- a/firmware/rtl/control_loop/control_loop_sim.cpp +++ b/firmware/rtl/control_loop/control_loop_sim.cpp @@ -157,17 +157,23 @@ static int64_t I_const, dt_const, P_const, setpt; static unsigned long seed, ; static void usage(char *argv0, int code) { - std::cout << argv0 << " -I I -t dt -d delay -s seed -S setpt -P p [+verilator...]" << std::endl; + std::cout << argv0 << " -d deviation -m mean -I I -t dt -d delay -s seed -S setpt -P p [+verilator...]" << std::endl; exit(code); } static void parse_args(int argc, char *argv[]) { - const char *optstring = "I:t:s:P:h"; + const char *optstring = "I:t:s:P:d:m:h"; int opt; Verilated::commandArgs(argc, argv); while ((opt = getopt(argc, argv, optstring)) != -1) { switch (opt) { + case 'm': + noise_mean = strtod(optstring, NULL); + break; + case 'd': + dev_mean = strtod(optstring, NULL); + break; case 'I': I_const = signed_to_fxp(optarg); break; @@ -203,9 +209,4 @@ int main(int argc, char **argv) { mod = new Vtop; mod->clk = 0; - mod->arm = 1; - mod->setpt = setpt; - mod->alpha = I_const * dt_const + P_const; - mod->cl_p = P_const; - mod->dy = 5; } diff --git a/firmware/rtl/control_loop/control_loop_sim_top.v b/firmware/rtl/control_loop/control_loop_sim_top.v index 6b68a4c..9382853 100644 --- a/firmware/rtl/control_loop/control_loop_sim_top.v +++ b/firmware/rtl/control_loop/control_loop_sim_top.v @@ -1,27 +1,70 @@ +`include control_loop_cmds.vh + module top #( parameter ADC_WID = 18, - parameter DAC_WID = 24, + parameter ADC_WID_LEN = 5, parameter ADC_POLARITY = 1, parameter ADC_PHASE = 0, + parameter ADC_CYCLE_HALF_WAIT = 5, + parameter ADC_TIMER_LEN = 3, + + parameter DAC_POLARITY = 0, + parameter DAC_PHASE = 1, parameter DAC_DATA_WID = 20, + parameter DAC_WID = 24, + parameter DAC_WID_LEN = 5, + parameter DAC_CYCLE_HALF_WAIT = 10, + parameter DAC_TIMER_LEN = 4, + parameter CONSTS_WID = 48, parameter DELAY_WID = 16 )( input clk, - input arm, input signed [ADC_WID-1:0] measured_data, - output signed [DAC_WID-1:0] output_data, - input signed [ADC_WID-1:0] setpt, - input signed [CONSTS_WID-1:0] alpha, - input signed [CONSTS_WID-1:0] cl_p, - input [DELAY_WID-1:0] dy, + input [DAC_DATA_WID-1:0] dac_in, + output [DAC_DATA_WID-1:0] dac_out, + output dac_input_ready, - output signed [ADC_WID:0] err, - output signed [CONSTS_WID-1:0] adj + input [CONSTS_WID-1:0] word_into_loop, + output [CONSTS_WID-1:0] word_outof_loop, + input start_cmd, + output finish_cmd, + input [CONTROL_LOOP_CMD_WIDTH-1:0] cmd ); +wire dac_miso; +wire dac_mosi; +wire dac_sck; +wire ss_L; + +spi_slave #( + .WID(DAC_WID), + .WID_LEN(DAC_WID_LEN), + .POLARITY(DAC_POLARITY), + .PHASE(DAC_PHASE) +) dac_slave ( + .clk(clk), + .sck(dac_sck), + .mosi(dac_mosi), + .miso(dac_miso), +); + +spi_master #( + .WID(DAC_WID), + .WID_LEN(DAC_WID_LEN), + .POLARITY(DAC_POLARITY), + .PHASE(DAC_PHASE), + .CYCLE_HALF_WAIT(DAC_CYCLE_HALF_WAIT), + .TIMER_LEN(DAC_TIMER_LEN) +) dac_master ( + .clk(clk), + .from_slave(dac_set_data), + .miso(dac_miso), + .to_slave( + .mosi(dac_mosi), + wire adc_sck; wire adc_ss; wire adc_miso; @@ -80,23 +123,7 @@ control_loop #( .DAC_PHASE(DAC_PHASE) ) cloop ( .clk(clk), - .arm(arm), - .adc_sck(adc_sck), - .adc_in(adc_miso), - .adc_conv(adc_ss), - - .dac_sck(dac_sck), - .dac_ss(dac_ss), - .dac_out(dac_mosi), - - .setpt_in(setpt), - .cl_alpha_in(alpha), - .cl_p_in(cl), - .delay_in(dy), - - .err(err_cur), - .adj(adj) ); endmodule diff --git a/firmware/rtl/control_loop/intro.md b/firmware/rtl/control_loop/intro.md index c1e9bc5..66cb009 100644 --- a/firmware/rtl/control_loop/intro.md +++ b/firmware/rtl/control_loop/intro.md @@ -62,3 +62,12 @@ same for fixed point binary and regular decimals. Addition: W1.F1 + W2.F2 = [max(W1,W2)+1].[max(F1,F2)] Multiplication: W1.F1W2.F2 = [W1+W2].[F1+F2] + + +When multiplying two fixed point integers, where the decimal points +do not correspond to the same points, then: + +* the output has the same number of bits as a normal addition/multiplication +* for multiplication, the LSB is interpreted as position `m+n`, where + `m` is the interpretation of the LSB of the first integer and `n` as + the LSB of the second. diff --git a/firmware/rtl/control_loop/mul_const.v b/firmware/rtl/control_loop/mul_const.v index cad1c89..cefb0af 100644 --- a/firmware/rtl/control_loop/mul_const.v +++ b/firmware/rtl/control_loop/mul_const.v @@ -1,26 +1,29 @@ module mul_const #( parameter CONSTS_WHOLE = 8, parameter CONSTS_FRAC = 40, - parameter CONSTS_WID = CONSTS_WHOLE + CONSTS_FRAC, +`define CONSTS_WID (CONSTS_WHOLE + CONSTS_FRAC) parameter IN_WHOLE = CONSTS_WHOLE, parameter IN_FRAC = CONSTS_FRAC, - parameter IN_WID = IN_WHOLE + IN_FRAC +`define IN_WID (IN_WHOLE + IN_FRAC) + parameter OUT_WHOLE = 20, + parameter OUT_FRAC = 40 +`define OUT_WID (OUT_WHOLE + OUT_FRAC) ) ( input clk, - input signed [IN_WID-1:0] inp, - input signed [CONSTS_WID-1:0] const_in, + input signed [`IN_WID-1:0] inp, + input signed [`CONSTS_WID-1:0] const_in, input arm, - output signed [CONSTS_WID-1:0] outp, + output signed [`OUT_WID-1:0] outp, output finished ); -localparam UNSAT_WID = CONSTS_WID + IN_WID; -wire signed [UNSAT_WID-1:0] unsat; +`define UNSAT_WID (`CONSTS_WID + `IN_WID) +wire signed [`UNSAT_WID-1:0] unsat; boothmul #( - .A1_LEN(CONSTS_WID), - .A2_LEN(IN_WID) + .A1_LEN(`CONSTS_WID), + .A2_LEN(`IN_WID) ) mul ( .clk(clk), .arm(arm), @@ -30,20 +33,26 @@ boothmul #( .fin(finished) ); -localparam RIGHTTRUNC_WID = UNSAT_WID - IN_FRAC; -wire signed [RIGHTTRUNC_WID-1:0] rtrunc = - unsat[UNSAT_WID-1:IN_FRAC]; +`define RIGHTTRUNC_WID (CONSTS_WHOLE + IN_WHOLE + OUT_FRAC) +`define UNSAT_FRAC (CONSTS_FRAC + IN_FRAC) +wire signed [`RIGHTTRUNC_WID-1:0] rtrunc = + unsat[`UNSAT_WID-1:(`UNSAT_FRAC - OUT_FRAC)]; -generate if (IN_WHOLE > 0) begin +generate if (OUT_WHOLE < CONSTS_WHOLE + IN_WHOLE) begin intsat #( - .IN_LEN(RIGHTTRUNC_WID), - .LTRUNC(IN_WHOLE) + .IN_LEN(`RIGHTTRUNC_WID), + .LTRUNC(CONSTS_WHOLE + IN_WHOLE - OUT_WHOLE) ) sat ( .inp(rtrunc), .outp(outp) ); -end else begin +end else if (OUT_WHOLE == CONSTS_WHOLE + IN_WHOLE) begin assign outp = rtrunc; +end else begin + assign outp[`RIGHTTRUNC_WID-1:0] = rtrunc; + assign outp[`OUT_WID-1:`RIGHTTRUNC_WID] = { + (`OUT_WID-`RIGHTTRUNC_WID){rtrunc[`RIGHTTRUNC_WID-1]} + }; end endgenerate `ifdef VERILATOR diff --git a/firmware/rtl/control_loop/mul_const_sim.cpp b/firmware/rtl/control_loop/mul_const_sim.cpp index f450e20..3661b62 100644 --- a/firmware/rtl/control_loop/mul_const_sim.cpp +++ b/firmware/rtl/control_loop/mul_const_sim.cpp @@ -29,12 +29,15 @@ static void init(int argc, char **argv) { static void satmul(int64_t const_in, int64_t inp) { int64_t r = const_in * inp; - if (r >= BITMASK(48)) + if (r >= BITMASK(48)) { return BITMASK(48); - else if (r <= -BITMASK(48)) - return -BITMASK(48); - else + } else if (r <= -BITMASK(48)) { + V allzero = ~((V) 0); + // make (siz - 1) zero bits + return allzero & (allzero << (siz - 1)); + } else { return r; + } } #define RUNS 10000 diff --git a/firmware/rtl/sign_extend.v b/firmware/rtl/sign_extend.v index 02965b7..ec0b8fd 100644 --- a/firmware/rtl/sign_extend.v +++ b/firmware/rtl/sign_extend.v @@ -2,8 +2,8 @@ module sign_extend #( parameter WID1 = 18, parameter WID2 = 24 ) ( - input signed [WID1-1:0] b1, - output signed [WID2-1:0] b2 + input [WID1-1:0] b1, + output [WID2-1:0] b2 ); assign b2[WID1-1:0] = b1;