From 5ff6b279b0056e8a1693b7518ccd562ed5547126 Mon Sep 17 00:00:00 2001
From: Peter McGoron <code@mcgoron.com>
Date: Mon, 21 Nov 2022 22:24:37 -0500
Subject: [PATCH] reverify math

---
 firmware/rtl/control_loop/control_loop_math.v | 27 +++++++++++++++----
 .../control_loop/control_loop_math_sim.cpp    |  2 ++
 .../rtl/control_loop/control_loop_sim.cpp     |  4 +--
 3 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/firmware/rtl/control_loop/control_loop_math.v b/firmware/rtl/control_loop/control_loop_math.v
index bb74c16..c3e12f0 100644
--- a/firmware/rtl/control_loop/control_loop_math.v
+++ b/firmware/rtl/control_loop/control_loop_math.v
@@ -129,6 +129,19 @@ intsat #(
 	.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.
  ************************/
@@ -150,7 +163,8 @@ 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 = 10;
+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;
@@ -251,12 +265,15 @@ always @ (posedge clk) begin
 	end
 	CALCULATE_A_PART_2: begin
 		add_sat <= tmpstore;
-		state <= CALCULATE_NEW_DAC_VALUE;
+		state <= CALCULATE_NEW_DAC_VALUE_PART_1;
 	end
-	CALCULATE_NEW_DAC_VALUE: begin
-		add_sat_dac <= saturated_add[CONSTS_FRAC+DAC_WID-1:CONSTS_FRAC]
-		             + stored_dac_val;
+	CALCULATE_NEW_DAC_VALUE_PART_1: begin
+		adj_sat <= saturated_add[`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
diff --git a/firmware/rtl/control_loop/control_loop_math_sim.cpp b/firmware/rtl/control_loop/control_loop_math_sim.cpp
index 0ba64ab..3141843 100644
--- a/firmware/rtl/control_loop/control_loop_math_sim.cpp
+++ b/firmware/rtl/control_loop/control_loop_math_sim.cpp
@@ -83,6 +83,7 @@ int main(int argc, char **argv) {
 	printf("running\n");
 	for (int i = 0; i < 200; i++) {
 		mod->measured = func.val(setting);
+		mod->stored_dac_val = setting;
 
 		calculate();
 		mod->e_prev = mod->e_cur;
@@ -100,6 +101,7 @@ int main(int argc, char **argv) {
 		printf("#%d: setting: %ld, measured: %ld, setpt: %ld, adj: %ld\n", i, setting, mod->measured, mod->setpt, adj);
 
 		setting += adj;
+		printf("new_dac_val %ld %ld\n", setting, mod->new_dac_val);
 	}
 
 	mod->final();
diff --git a/firmware/rtl/control_loop/control_loop_sim.cpp b/firmware/rtl/control_loop/control_loop_sim.cpp
index 6c6ac37..cdc4d53 100644
--- a/firmware/rtl/control_loop/control_loop_sim.cpp
+++ b/firmware/rtl/control_loop/control_loop_sim.cpp
@@ -59,7 +59,7 @@ int main(int argc, char **argv) {
 	set_value(10000, CONTROL_LOOP_SETPT);
 	set_value(1, CONTROL_LOOP_STATUS);
 
-	for (int tick = 0; tick < 10000; tick++) {
+	for (int tick = 0; tick < 100000; tick++) {
 		std::cout << tick << std::endl;
 		run_clock();
 		if (mod->request && !mod->fulfilled) {
@@ -69,7 +69,7 @@ int main(int argc, char **argv) {
 			mod->fulfilled = 0;
 		}
 
-		if (tick == 5000) {
+		if (tick == 50000) {
 			mod->cmd = CONTROL_LOOP_WRITE_BIT | CONTROL_LOOP_P;
 			mod->word_into_loop = 0b010111000010100011110101110000101000111;
 			mod->start_cmd = 1;