From e12afdc85abaa861a7f889cffb73ff69972329fc Mon Sep 17 00:00:00 2001
From: jzh <jzhgonha@163.com>
Date: Sun, 21 Jul 2024 16:11:38 +0800
Subject: [PATCH] [feat] add quadratic_fit module

---
 lark1fq/main.c             |  2 ++
 lark1fq/src/CMakeLists.txt |  1 +
 lark1fq/src/calc.c         | 63 ++++++++++++++++++++++++++++++++++++++
 lark1fq/src/calc.h         | 25 +++++++++++++++
 4 files changed, 91 insertions(+)
 create mode 100755 lark1fq/src/calc.c
 create mode 100755 lark1fq/src/calc.h

diff --git a/lark1fq/main.c b/lark1fq/main.c
index c7a4ef5..02245ad 100755
--- a/lark1fq/main.c
+++ b/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)
diff --git a/lark1fq/src/CMakeLists.txt b/lark1fq/src/CMakeLists.txt
index f231d3a..e8848d2 100755
--- a/lark1fq/src/CMakeLists.txt
+++ b/lark1fq/src/CMakeLists.txt
@@ -19,6 +19,7 @@ cali.c
 cali_flash.c
 debug.c
 cli.c
+calc.c
 )
 
 add_library(src STATIC ${FILELIST})
diff --git a/lark1fq/src/calc.c b/lark1fq/src/calc.c
new file mode 100755
index 0000000..d775d0d
--- /dev/null
+++ b/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;
+}
diff --git a/lark1fq/src/calc.h b/lark1fq/src/calc.h
new file mode 100755
index 0000000..56feb01
--- /dev/null
+++ b/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__ */