Lab_NHMFL
/
upsilon
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

more work on control_loop 

* Make SPI masters internal to control loop module
* Rename commands to use I isntead of alpha
* add ADC value -> DAC value conversion to control loop math
This commit is contained in:
Peter McGoron 2022-11-18 19:11:56 -05:00
parent 3a23ac6e92
commit 0c10dc921c
4 changed files with 135 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
firmware/rtl/control_loop/control_loop.v
View File

@ -4,15 +4,17 @@
module control_loop module control_loop
#( #(
parameter ADC_WID = 18, parameter ADC_WID = 18,
/* Code assumes DAC_WID > ADC_WID. If/when this is not the parameter ADC_WID_SIZ = 5,
* case, truncation code must be changed. parameter ADC_CYCLE_HALF_WAIT = 1,
parameter ADC_CYCLE_HALF_WAIT_SIZ = 1,
parameter ADC_POLARITY = 1,
parameter ADC_PHASE = 0,
/* The ADC takes maximum 527 ns to capture a value.
* The clock ticks at 10 ns. Change for different clocks!
*/ */
parameter DAC_WID = 24, parameter ADC_CONV_WAIT = 53,
/* Analog Devices DACs have a register code in the upper 4 bits. parameter ADC_CONV_WAIT_SIZ = 6,
* The data follows it. There may be some padding, but the length
* of a message is always 24 bits.
*/
parameter DAC_DATA_WID = 20,
parameter CONSTS_WHOLE = 21, parameter CONSTS_WHOLE = 21,
parameter CONSTS_FRAC = 43, parameter CONSTS_FRAC = 43,
`define CONSTS_WID (CONSTS_WHOLE + CONSTS_FRAC) `define CONSTS_WID (CONSTS_WHOLE + CONSTS_FRAC)
@ -24,20 +26,31 @@ module control_loop
`define DATA_WID `CONSTS_WID `define DATA_WID `CONSTS_WID
`define E_WID (ADC_WID + 1) `define E_WID (ADC_WID + 1)
parameter READ_DAC_DELAY = 5, parameter READ_DAC_DELAY = 5,
parameter CYCLE_COUNT_WID = 18 parameter CYCLE_COUNT_WID = 18,
parameter DAC_WID = 24,
/* Analog Devices DACs have a register code in the upper 4 bits.
* The data follows it. There may be some padding, but the length
* of a message is always 24 bits.
*/
parameter DAC_WID_SIZ = 5,
parameter DAC_DATA_WID = 20,
parameter DAC_POLARITY = 0,
parameter DAC_PHASE = 1,
parameter DAC_CYCLE_HALF_WAIT = 10,
parameter DAC_CYCLE_HALF_WAIT_SIZ = 4,
parameter DAC_SS_WAIT = 2,
parameter DAC_SS_WAIT_SIZ = 3
) ( ) (
input clk, input clk,
input signed [ADC_WID-1:0] measured_value, output dac_mosi,
output adc_conv, input dac_miso,
output adc_arm, output dac_ss_L,
input adc_finished, output dac_sck,
output reg signed [DAC_WID-1:0] to_dac, input adc_miso,
input signed [DAC_WID-1:0] from_dac, output adc_conv,
output dac_ss, output adc_sck,
output dac_arm,
input dac_finished,
/* Hacky ad-hoc read-write interface. */ /* Hacky ad-hoc read-write interface. */
input reg [CONTROL_LOOP_CMD_WIDTH-1:0] cmd, input reg [CONTROL_LOOP_CMD_WIDTH-1:0] cmd,
@ -47,24 +60,85 @@ module control_loop
output reg finish_cmd output reg finish_cmd
); );
/* The loop variables can be modified on the fly. Each /************ ADC and DAC modules ***************/
* modification takes effect on the next loop cycle.
* When a caller modifies a variable, the modified reg dac_arm;
* variable is saved in [name]_buffer and loaded at CYCLE_START. reg dac_finished;
reg [DAC_WID-1:0] to_dac;
wire [DAC_WID-1:0] from_dac;
spi_master_ss #(
.WID(DAC_WID),
.WID_LEN(DAC_WID_SIZ),
.CYCLE_HALF_WAIT(DAC_CYCLE_HALF_WAIT),
.TIMER_LEN(DAC_CYCLE_HALF_WAIT_SIZ),
.POLARITY(DAC_POLARITY),
.PHASE(DAC_PHASE),
.SS_WAIT(DAC_SS_WAIT),
.SS_WAIT_TIMER_LEN(DAC_SS_WAIT_SIZ)
) dac_master (
.clk(clk),
.arm(dac_arm),
.mosi(dac_mosi),
.miso(dac_miso),
.sck_wire(dac_sck),
.ss_L(dac_ss_L),
.finished(dac_finished),
.arm(dac_arm),
.from_slave(from_dac),
.to_slave(to_dac)
);
reg adc_arm;
reg adc_finished;
wire [ADC_WID-1:0] measured_value;
localparam [3-1:0] DAC_REGISTER = 3b'001;
spi_master_ss_no_write #(
.WID(ADC_WID),
.WID_LEN(ADC_WID_SIZ),
.CYCLE_HALF_WAIT(ADC_CYCLE_HALF_WAIT),
.TIMER_LEN(ADC_CYCLE_HALF_WAIT_SIZ),
.POLARITY(ADC_POLARITY),
.PHASE(ADC_PHASE),
.SS_WAIT(ADC_CONV_WAIT),
.SS_WAIT_TIMER_LEN(ADC_CONV_WAIT_SIZ)
) adc_master (
.clk(clk),
.arm(adc_arm),
.from_slave(measured_value),
.miso(adc_miso),
.sck_wire(adc_sck),
.ss_L(!ss_conv),
.finished(adc_finished)
);
/***************** PI Parameters *****************
* Parameters can be adjusted on the fly by the user. The modifications
* cannot happen during a calculation, but calculations occur in a matter
* of milliseconds. Instead, modifications are checked and applied at the
* start of each iteration (CYCLE_START). Before this, the new values
* have to be buffered.
*/ */
/* Setpoint: what should the ADC read */
reg signed [ADC_WID-1:0] setpt = 0; reg signed [ADC_WID-1:0] setpt = 0;
reg signed [ADC_WID-1:0] setpt_buffer = 0; reg signed [ADC_WID-1:0] setpt_buffer = 0;
/* Integral parameter */
reg signed [`CONSTS_WID-1:0] cl_I_reg = 0; reg signed [`CONSTS_WID-1:0] cl_I_reg = 0;
reg signed [`CONSTS_WID-1:0] cl_I_reg_buffer = 0; reg signed [`CONSTS_WID-1:0] cl_I_reg_buffer = 0;
/* Proportional parameter */
reg signed [`CONSTS_WID-1:0] cl_p_reg = 0; reg signed [`CONSTS_WID-1:0] cl_p_reg = 0;
reg signed [`CONSTS_WID-1:0] cl_p_reg_buffer = 0; reg signed [`CONSTS_WID-1:0] cl_p_reg_buffer = 0;
/* Delay parameter (to make the loop run slower) */
reg [DELAY_WID-1:0] dely = 0; reg [DELAY_WID-1:0] dely = 0;
reg [DELAY_WID-1:0] dely_buffer = 0; reg [DELAY_WID-1:0] dely_buffer = 0;
/************ Loop Control and Internal Parameters *************/
reg running = 0; reg running = 0;
reg signed [DAC_DATA_WID-1:0] stored_dac_val = 0; reg signed [DAC_DATA_WID-1:0] stored_dac_val = 0;
@ -72,10 +146,9 @@ reg [CYCLE_COUNT_WID-1:0] last_timer = 0;
reg [CYCLE_COUNT_WID-1:0] debug_timer = 0; reg [CYCLE_COUNT_WID-1:0] debug_timer = 0;
reg [`CONSTS_WID-1:0] adjval_prev = 0; reg [`CONSTS_WID-1:0] adjval_prev = 0;
/* Misc. registers for PI calculations */
reg signed [`E_WID-1:0] err_prev = 0; reg signed [`E_WID-1:0] err_prev = 0;
reg signed [`E_WID-1:0] e_cur = 0; wire signed [`E_WID-1:0] e_cur = 0;
reg signed [`CONSTS_WID-1:0] adj_val = 0; wire signed [`CONSTS_WID-1:0] adj_val = 0;
reg arm_math = 0; reg arm_math = 0;
reg math_finished = 0; reg math_finished = 0;
@ -159,6 +232,7 @@ end
* the main loop is clearing the dirty bit. * the main loop is clearing the dirty bit.
*/ */
wire write_control = state == CYCLE_START;
reg dirty_bit = 0; reg dirty_bit = 0;
always @ (posedge clk) begin always @ (posedge clk) begin
@ -199,13 +273,13 @@ always @ (posedge clk) begin
dirty_bit <= 1; dirty_bit <= 1;
end end
end end
CONTROL_LOOP_ALPHA: begin CONTROL_LOOP_I: begin
word_out <= cl_alpha_reg; word_out <= cl_I_reg;
finish_cmd <= 1; finish_cmd <= 1;
end end
CONTROL_LOOP_ALPHA | CONTROL_LOOP_WRITE_BIT: begin CONTROL_LOOP_I | CONTROL_LOOP_WRITE_BIT: begin
if (write_control) begin if (write_control) begin
cl_alpha_reg_buffer <= word_in; cl_I_reg_buffer <= word_in;
finish_cmd <= 1; finish_cmd <= 1;
dirty_bit <= 1; dirty_bit <= 1;
end end
@ -246,9 +320,7 @@ always @ (posedge clk) begin
case (state) case (state)
INIT_READ_FROM_DAC: begin INIT_READ_FROM_DAC: begin
if (running) begin if (running) begin
/* 1001[0....] is read from dac register */ to_dac <= {1, DAC_REGISTER, 20b'0};
to_dac <= b'1001 << DAC_DATA_WID;
dac_ss <= 1;
dac_arm <= 1; dac_arm <= 1;
state <= WAIT_FOR_DAC_READ; state <= WAIT_FOR_DAC_READ;
end end
@ -256,7 +328,6 @@ always @ (posedge clk) begin
WAIT_FOR_DAC_READ: begin WAIT_FOR_DAC_READ: begin
if (dac_finished) begin if (dac_finished) begin
state <= WAIT_FOR_DAC_RESPONSE; state <= WAIT_FOR_DAC_RESPONSE;
dac_ss <= 0;
dac_arm <= 0; dac_arm <= 0;
timer <= 1; timer <= 1;
end end
@ -265,9 +336,8 @@ always @ (posedge clk) begin
if (timer < READ_DAC_DELAY && timer != 0) begin if (timer < READ_DAC_DELAY && timer != 0) begin
timer <= timer + 1; timer <= timer + 1;
end else if (timer == READ_DAC_DELAY) begin end else if (timer == READ_DAC_DELAY) begin
dac_ss <= 1;
dac_arm <= 1; dac_arm <= 1;
to_dac <= 0; to_dac <= 24b'0;
timer <= 0; timer <= 0;
end else if (dac_finished) begin end else if (dac_finished) begin
state <= CYCLE_START; state <= CYCLE_START;
@ -297,21 +367,18 @@ always @ (posedge clk) begin
state <= WAIT_ON_ADC; state <= WAIT_ON_ADC;
timer <= 0; timer <= 0;
adc_arm <= 1; adc_arm <= 1;
adc_conv <= 1;
end end
end end
WAIT_ON_ADC: if (adc_finished) begin WAIT_ON_ADC: if (adc_finished) begin
adc_arm <= 0; adc_arm <= 0;
adc_conv <= 0;
arm_math <= 1; arm_math <= 1;
state <= WAIT_ON_MATH; state <= WAIT_ON_MATH;
end end
WAIT_ON_MATH: if (math_finished) begin WAIT_ON_MATH: if (math_finished) begin
arm_math <= 0; arm_math <= 0;
dac_arm <= 1; dac_arm <= 1;
dac_ss <= 1; stored_dac_val <= (stored_dac_val + adj_val[`CONSTS_WID-1:CONSTS_FRAC]);
stored_dac_val <= (stored_dac_val + dac_adj_val); to_dac <= {0, DAC_REGISTER, (dac_adj_val + adj_val[`CONSTS_WID-1:CONSTS_FRAC]);
to_dac <= b'0001 << DAC_DATA_WID | (dac_adj_val + stored_dac_val);
state <= WAIT_ON_DAC; state <= WAIT_ON_DAC;
end end
WAIT_ON_DAC: if (dac_finished) begin WAIT_ON_DAC: if (dac_finished) begin

2
firmware/rtl/control_loop/control_loop_cmds.vh
View File

@ -2,7 +2,7 @@
`define CONTROL_LOOP_STATUS 1 `define CONTROL_LOOP_STATUS 1
`define CONTROL_LOOP_SETPT 2 `define CONTROL_LOOP_SETPT 2
`define CONTROL_LOOP_P 3 `define CONTROL_LOOP_P 3
`define CONTROL_LOOP_ALPHA 4 `define CONTROL_LOOP_I 4
`define CONTROL_LOOP_ERR 5 `define CONTROL_LOOP_ERR 5
`define CONTROL_LOOP_Z 6 `define CONTROL_LOOP_Z 6
`define CONTROL_LOOP_CYCLES 7 `define CONTROL_LOOP_CYCLES 7

35
firmware/rtl/control_loop/control_loop_math.v
View File

@ -30,8 +30,13 @@ module control_loop_math #(
parameter ADC_WID = 18, parameter ADC_WID = 18,
parameter [`CONSTS_WID-1:0] SEC_PER_CYCLE = 'b10101011110011000, parameter [`CONSTS_WID-1:0] SEC_PER_CYCLE = 'b10101011110011000,
parameter CYCLE_COUNT_WID = 18 /* The conversion between the ADC bit (20/2**18) and DAC bit (20.48/2**20)
`define E_WID (ADC_WID + 1) * is 0.256.
*/
parameter [`CONSTS_WID-1:0] ADC_TO_DAC = 'b0100000110001001001101110100101111000110101,
parameter CYCLE_COUNT_WID = 18,
parameter DAC_WID = 20
`define E_WID (DAC_WID + 1)
) ( ) (
input clk, input clk,
input arm, input arm,
@ -123,13 +128,14 @@ intsat #(
); );
localparam WAIT_ON_ARM = 0; localparam WAIT_ON_ARM = 0;
localparam CALCULATE_DAC_E = 7;
localparam WAIT_ON_CALCULATE_DT = 1; localparam WAIT_ON_CALCULATE_DT = 1;
localparam CALCULATE_IDT = 2; localparam CALCULATE_IDT = 2;
localparam CALCULATE_EPIDT = 3; localparam CALCULATE_EPIDT = 3;
localparam CALCULATE_EP = 4; localparam CALCULATE_EP = 4;
localparam CALCULATE_A_PART_1 = 5; localparam CALCULATE_A_PART_1 = 5;
localparam CALCULATE_A_PART_2 = 6; localparam CALCULATE_A_PART_2 = 6;
localparam WAIT_ON_DISARM = 7; localparam WAIT_ON_DISARM = 8;
reg [4:0] state = WAIT_ON_ARM; reg [4:0] state = WAIT_ON_ARM;
reg signed [`CONSTS_WID+1-1:0] tmpstore = 0; reg signed [`CONSTS_WID+1-1:0] tmpstore = 0;
@ -139,18 +145,29 @@ always @ (posedge clk) begin
case (state) case (state)
WAIT_ON_ARM: WAIT_ON_ARM:
if (arm) begin if (arm) begin
e_cur <= setpt - measured; a1 <= setpt - measured;
a2 <= ADC_TO_DAC;
mul_arm <= 1;
state <= CALCULATE_DAC_E;
end else begin
finished <= 0;
end
CALCULATE_DAC_E:
if (mul_finished) begin
/* Discard other bits. This works without saturation because
* CONSTS_WHOLE = E_WID. */
e_cur <= mul_out[`CONSTS_WHOLE-1:CONSTS_FRAC];
a1 <= SEC_PER_CYCLE; a1 <= SEC_PER_CYCLE;
/* No sign extension, cycles is positive */ /* No sign extension, cycles is positive */
a2 <= {{(CONSTS_WHOLE - CYCLE_COUNT_WID){1'b0}}, cycles, {(CONSTS_FRAC){1'b0}}}; a2 <= {{(CONSTS_WHOLE - CYCLE_COUNT_WID){1'b0}}, cycles, {(CONSTS_FRAC){1'b0}}};
mul_arm <= 1; mul_arm <= 0;
state <= WAIT_ON_CALCULATE_DT; state <= WAIT_ON_CALCULATE_DT;
end else begin
finished <= 0;
end end
WAIT_ON_CALCULATE_DT: WAIT_ON_CALCULATE_DT:
if (mul_fin) begin if (!mul_arm) begin
mul_arm <= 1;
end else if (mul_fin) begin
mul_arm <= 0; mul_arm <= 0;
`ifdef DEBUG_CONTROL_LOOP_MATH `ifdef DEBUG_CONTROL_LOOP_MATH
@ -160,7 +177,7 @@ always @ (posedge clk) begin
a1 <= mul_out; /* a1 = Δt */ a1 <= mul_out; /* a1 = Δt */
a2 <= cl_I; a2 <= cl_I;
state <= CALCULATE_IDT; state <= CALCULATE_IDT;
end end
CALCULATE_IDT: CALCULATE_IDT:
if (!mul_arm) begin if (!mul_arm) begin
mul_arm <= 1; mul_arm <= 1;

12
firmware/rtl/control_loop/control_loop_sim_top.v
View File

@ -39,18 +39,6 @@ wire dac_mosi;
wire dac_sck; wire dac_sck;
wire ss_L; 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 #( spi_master #(
.WID(DAC_WID), .WID(DAC_WID),
.WID_LEN(DAC_WID_LEN), .WID_LEN(DAC_WID_LEN),