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

correct simulation of control loop

This commit is contained in:
Peter McGoron 2022-11-19 12:55:55 -05:00
parent a0450fb0ff
commit 0114c449c3
2 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2,7 +2,7 @@
* The control loop is designed around these values, but generally
* does not hardcode them.
*
* Since α and P are precalculated outside of the loop, their
* 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.
*
@ -72,7 +72,7 @@ reg signed [`CONSTS_WID-1:0] a2;
/* verilator lint_off UNUSED */
wire signed [`CONSTS_WID+`CONSTS_WID-1:0] out_untrunc;
wire mul_fin;
reg mul_arm;
reg mul_arm = 0;
boothmul #(
.A1_LEN(`CONSTS_WID),
@ -128,6 +128,7 @@ intsat #(
);
localparam WAIT_ON_ARM = 0;
localparam CALCULATE_ERR = 9;
localparam CALCULATE_DAC_E = 7;
localparam WAIT_ON_CALCULATE_DT = 1;
localparam CALCULATE_IDT = 2;
@ -140,7 +141,6 @@ localparam WAIT_ON_DISARM = 8;
reg [4:0] state = WAIT_ON_ARM;
reg signed [`CONSTS_WID+1-1:0] tmpstore = 0;
wire signed [`CONSTS_WID-1:0] tmpstore_view = tmpstore[`CONSTS_WID-1:0];
wire [ADC_WID+1-1:0] e_before_scale = setpt - measured;
always @ (posedge clk) begin
@ -148,14 +148,19 @@ always @ (posedge clk) begin
WAIT_ON_ARM:
if (arm) begin
a1[CONSTS_FRAC-1:0] <= 0;
a1[CONSTS_FRAC+ADC_WID + 1-1:CONSTS_FRAC] <= e_before_scale;
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[`CONSTS_WID-1:CONSTS_FRAC + ADC_WID + 1] <=
{(`CONSTS_WID-(CONSTS_FRAC + ADC_WID + 1)){e_before_scale[ADC_WID+1-1]}};
{(`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 else begin
finished <= 0;
end
CALCULATE_DAC_E:
if (mul_fin) begin

6
firmware/rtl/control_loop/control_loop_math_sim.cpp
View File

@ -1,3 +1,4 @@
/* TODO: add ADC_TO_DAC multiplication and verify */
#include <cstdio>
#include <cstdint>
#include "control_loop_math_implementation.h"
@ -30,12 +31,13 @@ static void init(int argc, char **argv) {
using V = int64_t;
constexpr V per100 = 0b010101011110011000;
constexpr V adc_to_dac = 0b0100000110001001001101110100101111000110101;
static void calculate() {
/* Multiplication adds an extra CONSTS_FRAC bits to the end,
* truncate them. */
V err_cur = (V)mod->setpt - (V)mod->measured;
V err_cur = mulsat((V)mod->setpt - (V)mod->measured, adc_to_dac, 64, CONSTS_FRAC);
V dt = mulsat(per100, (V)mod->cycles << CONSTS_FRAC, 64, CONSTS_FRAC);
V idt = mulsat(dt, mod->cl_I, 64, CONSTS_FRAC);
V epidt = mulsat(err_cur << CONSTS_FRAC, mod->cl_P + idt, 64, CONSTS_FRAC);
@ -52,7 +54,6 @@ static void calculate() {
run_clock();
run_clock();
#if 0
/* Stupid bug: verilator does not sign-extend signed ports */
printf("err_cur %ld %ld\n", err_cur, sign_extend(mod->e_cur, E_WID));
@ -61,7 +62,6 @@ static void calculate() {
printf("epidt %ld %ld\n", epidt, mod->epidt_reg);
printf("ep %ld %ld\n", ep, mod->ep_reg);
printf("adj %ld %ld\n", new_adjval, mod->adj_val);
#endif
}
int main(int argc, char **argv) {