add calculate dt module with simulation

firmware/rtl/control_loop/Makefile

@@ -0,0 +1,32 @@
+# Makefile for tests and hardware verification.
+
+COMMON_CPP = control_loop_math_implementation.cpp
+
+COMMON= ${COMMON_CPP} control_loop_math_implementation.h
+
+obj_dir/Vmul_const.mk: mul_const_sim.cpp mul_const.v boothmul.v intsat.v
+	verilator --cc --exe -Wall --trace --trace-fst \
+		--top-module mul_const \
+		mul_const.v mul_const_sim.cpp
+
+obj_dir/Vmul_const: obj_dir/Vmul_const.mk
+	cd obj_dir && make -f Vmul_const.mk
+
+SEC_PER_CYCLE_WID=15
+CYCLE_COUNT_WID=18
+UNSAT_WID=(${SEC_PER_CYCLE_WID} + ${CYCLE_COUNT_WID})
+MAX_WID=48
+DT_WID=$(shell echo $$((${UNSAT_WID} > ${MAX_WID} ? ${MAX_WID} : ${UNSAT_WID})))
+
+obj_dir/Vcalculate_dt.mk: calculate_dt_sim.cpp calculate_dt.v ${COMMON}
+	verilator --cc --exe -Wall --trace --trace-fst \
+		--top-module calculate_dt \
+		-GSEC_PER_CYCLE_WID=${SEC_PER_CYCLE_WID} \
+		-GCYCLE_COUNT_WID=${CYCLE_COUNT_WID} \
+		-CFLAGS -DDT_WID=${DT_WID} \
+		calculate_dt.v calculate_dt_sim.cpp ${COMMON_CPP}
+obj_dir/Vcalculate_dt: obj_dir/Vcalculate_dt.mk
+	cd obj_dir && make -f Vcalculate_dt.mk
+
+test: obj_dir/Vcalculate_dt
+	obj_dir/Vcalculate_dt

firmware/rtl/control_loop/calculate_dt.v

@@ -0,0 +1,63 @@
+/* Calculate and truncate Δt = cycles/100MhZ.
+ * NOTE: boothmul is a SIGNED algorithm so both inputs are SIGNED.
+ * This means that SEC_PER_CYCLE must have a leading 0
+ * and that cycles must also have a leading zero.
+ */
+
+`undefineall
+module calculate_dt #(
+	/* 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 = 15,
+	parameter [SEC_PER_CYCLE_WID-1:0] SEC_PER_CYCLE = 'b010101011110011,
+	parameter CYCLE_COUNT_WID = 18,
+	parameter MAX_WID = 48
+) (
+	input clk,
+	input arm,
+	output finished,
+
+	input [CYCLE_COUNT_WID-1:0] cycles,
+
+/* Multiplication of Q18.0 and 14 lower siginifcant bits. */
+`define DT_WID_UNTRUNC (SEC_PER_CYCLE_WID + CYCLE_COUNT_WID)
+`define DT_WID (`DT_WID_UNTRUNC > MAX_WID ? MAX_WID : `DT_WID_UNTRUNC)
+
+	output [`DT_WID-1:0] dt
+);
+
+wire [`DT_WID_UNTRUNC-1:0] dt_untrunc;
+
+boothmul #(
+	.A1_LEN(CYCLE_COUNT_WID),
+	.A2_LEN(SEC_PER_CYCLE_WID)
+) mul (
+	.clk(clk),
+	.arm(arm),
+	.a1(cycles),
+	.a2(SEC_PER_CYCLE),
+	.outn(dt_untrunc),
+	.fin(finished)
+);
+
+generate if (`DT_WID_UNTRUNC > `DT_WID) begin
+	intsat #(
+		.IN_LEN(`DT_WID_UNTRUNC),
+		.LTRUNC(`DT_WID_UNTRUNC - `DT_WID)
+	) sat (
+		.inp(dt_untrunc),
+		.outp(dt)
+	);
+end else begin
+	assign dt = dt_untrunc;
+end endgenerate
+
+`ifdef VERILATOR
+initial begin
+	$dumpfile("calculate_dt.fst");
+	$dumpvars;
+end
+`endif
+
+endmodule

firmware/rtl/control_loop/calculate_dt_sim.cpp

@@ -0,0 +1,54 @@
+#include <cstdio>
+#include "control_loop_math_implementation.h"
+#include <verilated.h>
+#include "Vcalculate_dt.h"
+using ModType = Vcalculate_dt;
+
+uint32_t main_time = 0;
+double sc_time_stamp() {
+	return main_time;
+}
+
+ModType *mod;
+
+static void run_clock() {
+	for (int i = 0; i < 2; i++) {
+		mod->clk = !mod->clk;
+		mod->eval();
+		main_time++;
+	}
+}
+
+static void init(int argc, char **argv) {
+	Verilated::commandArgs(argc, argv);
+	Verilated::traceEverOn(true);
+	mod = new ModType;
+	mod->clk = 0;
+}
+
+int main(int argc, char **argv) {
+	int r = 0;
+
+	init(argc, argv);
+
+	for (V i = 1; i < ((1 << 17) - 1); i++) {
+		mod->cycles = i;
+		mod->arm = 1;
+		do { run_clock(); } while (!mod->finished);
+		mod->arm = 0;
+
+		V real_dt = calculate_dt(i, DT_WID);
+		if (mod->dt != real_dt) {
+			printf("(%lld) %lld != %lld\n", i, mod->dt, real_dt);
+			r = 1;
+			goto end;
+		}
+
+		run_clock();
+	}
+
+end:
+	mod->final();
+	delete mod;
+	return r;
+}