Diff: main.cpp

Revision:

3:08488e544b3e

Parent:

2:e7ce63ada1d2

Child:

4:fb20b0b3f57d

diff -r e7ce63ada1d2 -r 08488e544b3e main.cpp
--- a/main.cpp	Tue Jul 05 11:57:09 2016 +0000
+++ b/main.cpp	Tue Jul 05 12:39:11 2016 +0000
@@ -9,14 +9,25 @@
 FastAnalogIn Iget(PTE20);
 AnalogIn Vbat(PTB0);
 bool active = true;
-float k = 0.01, iget = Iget.read(), vbat = Vbat.read(), vbat_rest = Vbat.read();
+
+class Lag {
+    float k, v;
+public:
+    Lag(float seed, float factor = 0.005) { v = seed; k = factor; }
+    float operator()(float in) { return v = k * in + (1 - k) * v; }
+    operator float() { return v; }
+};
+inline float Iin(float x) { return x * 32.9697 + 1/60.0; }
+inline float Vin(float x) { return x * 34.08516 + 0.06567447; }
+
+Lag iget(Iin(Iget.read())), vbat(Vin(Vbat.read())), vbat_rest(Vin(Vbat.read()));
 void filter() {
     if (active) {
-        iget = k * Iget.read() * 32.9697 + 0.0001666 + (1 - k) * iget;
-        vbat = k * Vbat.read() * 36.7538 + (1 - k) * vbat;
+        iget(Iin(Iget.read()));
+        vbat(Vin(Vbat.read()));
     }
     if (Iset.read() == 0)
-        vbat_rest = k * Vbat.read() * 36.7538 + (1 - k) * vbat_rest;
+        vbat_rest(Vin(Vbat.read()));
 }
 void rest(const void *) {
     while (true) {
@@ -70,8 +81,8 @@
 
     while (true) {
         //pc.printf("Iset %f    Iget %6d %f    Vbat %6d %f    \r", Iset.read(), Iget.read_u16(), Iget.read(), Vbat.read_u16(), Vbat.read());
-        pc.printf("Iset %f      Iget %8d %f %f", Iset.read(), Iget.read_u16(), Iget.read() * 32.9697 + 0.0001666, iget);
-        pc.printf("      Vbat %8d %f %f %f\r", Vbat.read_u16(), Vbat.read() * 36.7538, vbat, vbat_rest);
+        pc.printf("Iset %f      Iget %8d %f %f", Iset.read(), Iget.read_u16(), Iin(Iget.read()), (float)iget);
+        pc.printf("      Vbat %8d %f %f %f\r", Vbat.read_u16(), Vin(Vbat.read()), (float)vbat, (float)vbat_rest);
         Thread::wait(500);
     }
 }