From 15f35e0bb2d0012f48c9c00762077e874c87bd51 Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <clyne@bitgloo.com>
Date: Sat, 1 Feb 2025 09:39:46 -0500
Subject: [PATCH] go fixed point

---
 Core/Src/main.c | 53 ++++++++++++++++++++++++-------------------------
 1 file changed, 26 insertions(+), 27 deletions(-)

diff --git a/Core/Src/main.c b/Core/Src/main.c
index 7c41e3f..45d16a6 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -62,8 +62,8 @@ static uint8_t I2S_Receive_Buffer[SAMPLE_COUNT * 2 * sizeof(sample_t)];
 float ln10;
 float MIC_REF_AMPL;
 
-static float DB_Sum_Squares = 0.f;
-static unsigned DB_Count = 0;
+static int64_t DB_Sum_Squares = 0.f;
+static int DB_Count = 0;
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -128,7 +128,6 @@ int main(void)
   MX_SPI1_Init();
   MX_USART2_UART_Init();
   /* USER CODE BEGIN 2 */
-  __enable_irq();
   HAL_SPI_TransmitReceive_DMA_Mixed(&hspi1,
       I2S_Frame_Buffer,
       I2S_Receive_Buffer,
@@ -138,9 +137,10 @@ int main(void)
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
+  HAL_PWR_EnableSleepOnExit();
+  __enable_irq();
   while (1)
   {
-    HAL_GPIO_WritePin(IDLE_GPIO_Port, IDLE_Pin, GPIO_PIN_SET);
     __WFI();
     /* USER CODE END WHILE */
 
@@ -441,46 +441,43 @@ error :
   return errorcode;
 }
 
-static inline float process(float in_div4)
+#define BITO (10)
+
+static inline void process(int64_t in_div4)
 {
-    static float z[4] = {0.f, 0.f, 0.f, 0.f};
+    static int64_t z[4] = {0, 0, 0, 0};
 
-    float out1 = qfp_fadd(in_div4, z[0]);
-    z[0] = qfp_fadd(qfp_fmul(out1, 1.062f), z[1]);
-    z[1] = qfp_fsub(qfp_fmul(out1, -0.14f), in_div4);
+    in_div4 <<= BITO;
+    int64_t out1 = (in_div4 + z[0]);
+    z[0] = ((out1 * 0x43f /*1.062f*/) >> BITO) + z[1];
+    z[1] = ((out1 * -0x8f /*-0.14f*/) >> BITO) - in_div4;
 
-    float out2 = qfp_fadd(out1, z[2]);
+    int64_t out2 = (out1 + z[2]);
     z[2] = out1;
 
-    float out3 = qfp_fadd(out2, z[3]);
-    z[3] = qfp_fsub(qfp_fmul(out3, 0.985f), out2);
+    int64_t out3 = (out2 + z[3]);
+    z[3] = ((out3 * 0x3f1 /*0.985f*/) >> BITO) - out2;
 
-    return out3;
+    DB_Sum_Squares += (out3 * out3) >> BITO;
 }
 
 static void processSampleBlock(sample_t *sample)
 {
   HAL_GPIO_WritePin(IDLE_GPIO_Port, IDLE_Pin, GPIO_PIN_RESET);
 
-  // SAMPLE_COUNT is number of samples in this block
-  // Divide by 2 for left channel only
-  // Divide by 8 for stride to reduce compute time
-#define STRIDE (2 * 8)
+#define STRIDE 2
 
-  for (int i = 0; i < SAMPLE_COUNT; i += STRIDE) {
+  for (int i = 0; i < SAMPLE_COUNT; i += 2 * STRIDE) {
     // 18-bit sample comes in as big-endian with right padding.
-    // 1. Convert to little-endian:
+    // Use REVSH to extract 18-bit reading divided by four for process().
     int samp;
-    asm("rev %0, %1" : "=r" (samp) : "r" (sample[i]));
-    // 2. Arithmetic shift right to remove padding, +2 to make "in_div4"
-    float f = process(qfp_int2float(samp >> (32 - MIC_BITS + 2)));
-
-    DB_Sum_Squares = qfp_fadd(qfp_fmul(f, f), DB_Sum_Squares);
+    asm("revsh %0, %1" : "=l" (samp) : "l" (sample[i]));
+    process(samp);
   }
-  DB_Count += SAMPLE_COUNT / STRIDE;
+  DB_Count += SAMPLE_COUNT / (2 * STRIDE);
 
-  if (DB_Count >= SAMPLES_PER_REPORT / STRIDE * 2) {
-    float rms = qfp_fsqrt(qfp_fdiv(DB_Sum_Squares, qfp_uint2float(DB_Count)));
+  if (DB_Count >= SAMPLES_PER_REPORT / STRIDE) {
+    float rms = qfp_fsqrt(qfp_int2float((DB_Sum_Squares >> BITO) / DB_Count));
     float db = qfp_fadd(qfp_fmul(qfp_flog10(qfp_fdiv(rms, MIC_REF_AMPL)), 20.f),
         MIC_OFFSET_DB + MIC_REF_DB);
     DB_Sum_Squares = 0.f;
@@ -488,6 +485,8 @@ static void processSampleBlock(sample_t *sample)
 
     printf("%d dB\r\n", qfp_float2int(db));
   }
+
+  HAL_GPIO_WritePin(IDLE_GPIO_Port, IDLE_Pin, GPIO_PIN_SET);
 }
 
 void SPI_DMATransmitReceiveCplt(DMA_HandleTypeDef *hdma)