diff --git a/firmware/rtl/control_loop/control_loop_math.v b/firmware/rtl/control_loop/control_loop_math.v
index 45aaf4e..2eaee9b 100644
--- a/firmware/rtl/control_loop/control_loop_math.v
+++ b/firmware/rtl/control_loop/control_loop_math.v
@@ -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,15 +148,20 @@ 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_WID-1:CONSTS_FRAC + ADC_WID + 1] <=
-				{(`CONSTS_WID-(CONSTS_FRAC + ADC_WID + 1)){e_before_scale[ADC_WID+1-1]}};
-			a2 <= ADC_TO_DAC;
-			mul_arm <= 1;
-			state <= CALCULATE_DAC_E;
+			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)){a1[ADC_WID+1-1+CONSTS_FRAC]}};
+		a2 <= ADC_TO_DAC;
+		mul_arm <= 1;
+		state <= CALCULATE_DAC_E;
+	end
 	CALCULATE_DAC_E:
 		if (mul_fin) begin
 			/* Discard other bits. This works without saturation because
diff --git a/firmware/rtl/control_loop/control_loop_math_sim.cpp b/firmware/rtl/control_loop/control_loop_math_sim.cpp
index 559683f..0ba64ab 100644
--- a/firmware/rtl/control_loop/control_loop_math_sim.cpp
+++ b/firmware/rtl/control_loop/control_loop_math_sim.cpp
@@ -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) {