From 5541461587fdafd3188ecd91597629afac792756 Mon Sep 17 00:00:00 2001
From: zhji <jzhgonha@163.com>
Date: Sat, 28 Dec 2024 16:15:29 +0800
Subject: [PATCH] [feat] add concentration neg drift compensation

---
 lark1fq/src/concentration.c | 45 ++++++++++++++++++++++++++++++++++++-
 lark1fq/src/concentration.h |  1 +
 2 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/lark1fq/src/concentration.c b/lark1fq/src/concentration.c
index d415b0e..f9f27c5 100755
--- a/lark1fq/src/concentration.c
+++ b/lark1fq/src/concentration.c
@@ -38,6 +38,49 @@ void concentration_init(void)
     }
 }
 
+int32_t concentration_neg_drift_compensation(uint8_t ch, int32_t reading)
+{
+    static uint8_t drift_update_enable[4] = {1, 1, 1, 1};
+    static uint8_t neg_reading_num[4] = {0, 0, 0, 0};
+    static int32_t neg_reading_sum[4] = {0, 0, 0, 0};
+    static int32_t zero_neg_drift[4] = {0, 0, 0, 0};
+    static int32_t reading_fixed[4] = {0, 0, 0, 0};
+    static uint8_t neg_fixed_num[4] = {0, 0, 0, 0};
+
+    reading_fixed[ch] = reading;
+    
+    if(drift_update_enable[ch]) {
+        if (reading < 0) {
+            neg_reading_sum[ch] += reading;
+            neg_reading_num[ch] ++;
+            reading_fixed[ch] = 0;
+            if(neg_reading_num[ch] >= 10) {
+                zero_neg_drift[ch] = neg_reading_sum[ch] / neg_reading_num[ch];
+                drift_update_enable[ch] = 0;
+                neg_reading_num[ch] = 0;
+                neg_reading_sum[ch] = 0;
+            }
+        } else {
+            neg_reading_num[ch] = 0;
+            neg_reading_sum[ch] = 0;
+        }
+    }
+    if (zero_neg_drift[ch]) {
+        reading_fixed[ch] = reading - zero_neg_drift[ch];
+        if (reading_fixed[ch] < 0) {
+            neg_fixed_num[ch]++;
+            if (neg_fixed_num[ch] >= 10) {
+                drift_update_enable[ch] = 1;
+                neg_fixed_num[ch] = 0;
+            }
+        } else {
+            neg_fixed_num[ch] = 0;
+        }
+    }
+
+    return ((reading_fixed[ch] > 0) ? reading_fixed[ch] : 0);
+}
+
 int32_t concentration_calc(uint8_t ch)
 {
     struct data_lookup_table_s *p; /* just for reduce text char in code */
@@ -105,7 +148,7 @@ int32_t concentration_calc(uint8_t ch)
     k = (p->concentration[idx_c] - p->concentration[idx_c - 1]) / (p->calc_b[idx_c] - p->calc_b[idx_c - 1]);
     temp = k * (cnt - p->calc_b[idx_c - 1]) + p->concentration[idx_c - 1];
 
-    return (int32_t)temp;
+    return concentration_neg_drift_compensation(ch, (int32_t)temp);
 }
 
 void concentration_loop(void)
diff --git a/lark1fq/src/concentration.h b/lark1fq/src/concentration.h
index 876868c..c3c945a 100755
--- a/lark1fq/src/concentration.h
+++ b/lark1fq/src/concentration.h
@@ -25,6 +25,7 @@ extern struct data_lookup_table_s data_lookup_table[4];
 
 void concentration_lookup_table_copy(uint8_t ch);
 void concentration_init(void);
+int32_t concentration_neg_drift_compensation(uint8_t ch, int32_t reading);
 int32_t concentration_calc(uint8_t ch);
 void concentration_loop(void);