go fixed point

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