go fixed point

1 files 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)