[feat] add quadratic_fit module

lark1fq/main.c

@@ -10,6 +10,7 @@
 #include "cali.h"
 #include "debug.h"
 #include "cli.h"
+#include "calc.h"
 #include "stm32f4xx_rcc.h"
 
 uint32_t system_tick_cnt;
@@ -58,6 +59,7 @@ void system_init(void)
     device_init();
     ltc2640_init();
     ms5611_init();
+    calc_quadratic_fit_init(CALC_POINTS);
 }
 
 int main(void)

lark1fq/src/CMakeLists.txt

@@ -19,6 +19,7 @@ cali.c
 cali_flash.c
 debug.c
 cli.c
+calc.c
 )
 
 add_library(src STATIC ${FILELIST})

lark1fq/src/calc.c

@@ -0,0 +1,63 @@
+#include "calc.h"
+
+static NUM_TYPE quadratic_fit_sumx1_arr[CALC_POINTS] __attribute__((section(".bss_ccm")));
+static NUM_TYPE quadratic_fit_sumx2_arr[CALC_POINTS] __attribute__((section(".bss_ccm")));
+static NUM_TYPE quadratic_fit_sumx1 __attribute__((section(".bss_ccm")));
+static NUM_TYPE quadratic_fit_sumx2 __attribute__((section(".bss_ccm")));
+static NUM_TYPE quadratic_fit_sumx3 __attribute__((section(".bss_ccm")));
+static NUM_TYPE quadratic_fit_sumx4 __attribute__((section(".bss_ccm")));
+static NUM_TYPE quadratic_fit_d __attribute__((section(".bss_ccm")));
+static NUM_TYPE quadratic_fit_matrix[3][3] __attribute__((section(".bss_ccm")));
+
+void calc_quadratic_fit_init(int length)
+{
+    for (int i = 0; i < length; i++) {
+        quadratic_fit_sumx1_arr[i] = (NUM_TYPE)i;
+        quadratic_fit_sumx2_arr[i] = quadratic_fit_sumx1_arr[i] * quadratic_fit_sumx1_arr[i];
+    }
+    quadratic_fit_sumx1 = (NUM_TYPE)0;
+    quadratic_fit_sumx2 = (NUM_TYPE)0;
+    quadratic_fit_sumx3 = (NUM_TYPE)0;
+    quadratic_fit_sumx4 = (NUM_TYPE)0;
+    for (int i = 0; i < length; i++) {
+        quadratic_fit_sumx1 += quadratic_fit_sumx1_arr[i];
+        quadratic_fit_sumx2 += quadratic_fit_sumx2_arr[i];
+        quadratic_fit_sumx3 += quadratic_fit_sumx1_arr[i] * quadratic_fit_sumx2_arr[i];
+        quadratic_fit_sumx4 += quadratic_fit_sumx2_arr[i] * quadratic_fit_sumx2_arr[i];
+    }
+    quadratic_fit_d = \
+        quadratic_fit_sumx2 * quadratic_fit_sumx2 * quadratic_fit_sumx2 + \
+        quadratic_fit_sumx1 * quadratic_fit_sumx1 * quadratic_fit_sumx4 + \
+        (NUM_TYPE)length * quadratic_fit_sumx3 * quadratic_fit_sumx3 - \
+        (NUM_TYPE)length * quadratic_fit_sumx2 * quadratic_fit_sumx4 - \
+        (NUM_TYPE)2 * quadratic_fit_sumx1 * quadratic_fit_sumx2 * quadratic_fit_sumx3;
+    quadratic_fit_matrix[0][0] = quadratic_fit_sumx2 * quadratic_fit_sumx2 - quadratic_fit_sumx1 * quadratic_fit_sumx3;
+    quadratic_fit_matrix[0][1] = (NUM_TYPE)length * quadratic_fit_sumx3 - quadratic_fit_sumx1 * quadratic_fit_sumx2;
+    quadratic_fit_matrix[0][2] = quadratic_fit_sumx1 * quadratic_fit_sumx1 - (NUM_TYPE)length * quadratic_fit_sumx2;
+    quadratic_fit_matrix[1][0] = quadratic_fit_sumx1 * quadratic_fit_sumx4 - quadratic_fit_sumx2 * quadratic_fit_sumx3;
+    quadratic_fit_matrix[1][1] = quadratic_fit_sumx2 * quadratic_fit_sumx2 - (NUM_TYPE)length * quadratic_fit_sumx4;
+    quadratic_fit_matrix[1][2] = quadratic_fit_matrix[0][1];
+    quadratic_fit_matrix[2][0] = quadratic_fit_sumx3 * quadratic_fit_sumx3 - quadratic_fit_sumx2 * quadratic_fit_sumx4;
+    quadratic_fit_matrix[2][1] = quadratic_fit_matrix[1][0];
+    quadratic_fit_matrix[2][2] = quadratic_fit_matrix[0][0];
+}
+
+void calc_quadratic_fit(NUM_TYPE *buff, struct quadratic_coefficient_s *coeff, int length)
+{
+    NUM_TYPE sum_y1, sum_xy, sum_x2y;
+
+    if ((buff == (void *)0) || (coeff == (void *)0) || (length == 0)) {
+        return;
+    }
+    sum_y1 = (NUM_TYPE)0;
+    sum_xy = (NUM_TYPE)0;
+    sum_x2y = (NUM_TYPE)0;
+    for (int i = 0; i < length; i++) {
+        sum_y1 += buff[i];
+        sum_xy += quadratic_fit_sumx1_arr[i] * buff[i];
+        sum_x2y += quadratic_fit_sumx2_arr[i] * buff[i];
+    }
+   coeff->a = (sum_y1 * quadratic_fit_matrix[0][0] + sum_xy * quadratic_fit_matrix[0][1] + sum_x2y * quadratic_fit_matrix[0][2]) / quadratic_fit_d;
+   coeff->b = (sum_y1 * quadratic_fit_matrix[1][0] + sum_xy * quadratic_fit_matrix[1][1] + sum_x2y * quadratic_fit_matrix[1][2]) / quadratic_fit_d;
+   coeff->c = (sum_y1 * quadratic_fit_matrix[2][0] + sum_xy * quadratic_fit_matrix[2][1] + sum_x2y * quadratic_fit_matrix[2][2]) / quadratic_fit_d;
+}

lark1fq/src/calc.h

@@ -0,0 +1,25 @@
+#ifndef __CALC_H__
+#define __CALC_H__
+
+#define NUM_TYPE float
+#define CALC_POINTS (128)
+
+/* y = a * x^2 + b * x + c */
+struct quadratic_coefficient_s {
+    NUM_TYPE a;
+    NUM_TYPE b;
+    NUM_TYPE c;
+};
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+void calc_quadratic_fit_init(int length);
+void calc_quadratic_fit(NUM_TYPE *buff, struct quadratic_coefficient_s *coeff, int length);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CALC_H__ */