From 88c42a9f4a0180022925e6cbed2a0cff92c0308b Mon Sep 17 00:00:00 2001
From: Peter McGoron <code@mcgoron.com>
Date: Sat, 12 Nov 2022 01:44:30 -0500
Subject: [PATCH] add printing of fixed point values in C++

---
 .../rtl/control_loop/calculate_dt_sim.cpp     |  8 +++
 firmware/rtl/control_loop/control_loop_math.v |  9 +---
 .../control_loop_math_implementation.cpp      | 53 +++++++++++++++++++
 .../control_loop_math_implementation.h        | 17 ++++++
 4 files changed, 80 insertions(+), 7 deletions(-)
 create mode 100644 firmware/rtl/control_loop/control_loop_math_implementation.cpp
 create mode 100644 firmware/rtl/control_loop/control_loop_math_implementation.h

diff --git a/firmware/rtl/control_loop/calculate_dt_sim.cpp b/firmware/rtl/control_loop/calculate_dt_sim.cpp
index 14acf9a..2021171 100644
--- a/firmware/rtl/control_loop/calculate_dt_sim.cpp
+++ b/firmware/rtl/control_loop/calculate_dt_sim.cpp
@@ -44,6 +44,14 @@ int main(int argc, char **argv) {
 			goto end;
 		}
 
+		struct fixed_point fxp = {
+			.val = real_dt,
+			.whole_len = 0,
+			.frac_len = 40
+		};
+
+		printf("%s\n", fxp_to_string(fxp).c_str());
+
 		run_clock();
 	}
 
diff --git a/firmware/rtl/control_loop/control_loop_math.v b/firmware/rtl/control_loop/control_loop_math.v
index dc6b14c..3f3ae00 100644
--- a/firmware/rtl/control_loop/control_loop_math.v
+++ b/firmware/rtl/control_loop/control_loop_math.v
@@ -28,11 +28,7 @@ module control_loop_math #(
 	parameter CONSTS_WHOLE = 8,
 	parameter CONSTS_FRAC = 40,
 `define CONSTS_WID (CONSTS_WHOLE + CONSTS_FRAC)
-	/* 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 = 14,
-	parameter [SEC_PER_CYCLE_WID-1:0] SEC_PER_CYCLE = 'b10101011110011,
+
 	parameter DAC_DATA_WID = 20,
 	parameter ADC_WID = 18,
 `define E_WID (ADC_WID + 1)
@@ -41,9 +37,8 @@ module control_loop_math #(
 	 * The OUT_FRAC value should usually but not always be the same as CONSTS_FRAC.
 	 */
 	parameter OUT_WHOLE = 20,
-	parameter OUT_FRAC = 40,
+	parameter OUT_FRAC = 40
 `define OUT_WID (OUT_WHOLE + OUT_FRAC)
-	parameter CYCLE_COUNT_WID = 18
 ) (
 	input clk,
 	input arm,
diff --git a/firmware/rtl/control_loop/control_loop_math_implementation.cpp b/firmware/rtl/control_loop/control_loop_math_implementation.cpp
new file mode 100644
index 0000000..089190c
--- /dev/null
+++ b/firmware/rtl/control_loop/control_loop_math_implementation.cpp
@@ -0,0 +1,53 @@
+#include "control_loop_math_implementation.h"
+
+#define BITMASK(n) (((V)1 << (n)) - 1)
+
+static V sat(V r, unsigned siz) {
+	if (r >= BITMASK(siz)) {
+		return BITMASK(siz);
+	} else if (r <= -BITMASK(siz)) {
+		V allzero = ~((V) 0);
+		// make (siz - 1) zero bits
+		return allzero & (allzero << (siz - 1));
+	} else {
+		return r; 
+	}
+}
+
+
+V calculate_dt(V cycles, unsigned siz) {
+	constexpr V sec_per_cycle = 0b10101011110011;
+
+	return sat(sec_per_cycle * cycles, siz);
+}
+
+static char d2c(int c) {
+	switch (c % 10) {
+	case 0: return '0';
+	case 1: return '1';
+	case 2: return '2';
+	case 3: return '3';
+	case 4: return '4';
+	case 5: return '5';
+	case 6: return '6';
+	case 7: return '7';
+	case 8: return '8';
+	case 9: return '9';
+	default: return '?';
+	}
+}
+
+std::string fxp_to_string(const struct fixed_point &fxp) {
+	std::string r = std::to_string((fxp.val >> fxp.frac_len) & BITMASK(fxp.whole_len));
+	V frac = fxp.val & BITMASK(fxp.frac_len);
+
+	r.push_back('.');
+
+	for (unsigned i = 0; i < fxp.frac_len; i++) {
+		frac *= 10;
+		r.push_back(d2c(frac >> fxp.frac_len));
+		frac &= BITMASK(fxp.frac_len);
+	}
+
+	return r;
+}
diff --git a/firmware/rtl/control_loop/control_loop_math_implementation.h b/firmware/rtl/control_loop/control_loop_math_implementation.h
new file mode 100644
index 0000000..158f4d8
--- /dev/null
+++ b/firmware/rtl/control_loop/control_loop_math_implementation.h
@@ -0,0 +1,17 @@
+#pragma once
+#include <cstdint>
+#include <string>
+#include <utility>
+#include <limits>
+
+using V = int64_t;
+constexpr V V_min = std::numeric_limits<V>::min();
+
+struct fixed_point {
+	V val;
+	unsigned whole_len;
+	unsigned frac_len;
+};
+
+V calculate_dt(V cycles, unsigned siz);
+std::string fxp_to_string(const struct fixed_point &fxp);