From 3f8d21e14658a514bad0624d0883b99a9a0e5a3b Mon Sep 17 00:00:00 2001
From: toni <toni@localhost.localdomain>
Date: Mon, 6 Nov 2017 18:04:55 +0100
Subject: [PATCH] fixed two bugs in ActivityButterPressure: 1) when barometer
 produces false measurements, we needed some kind of upper boundary 2) coding
 error static.. while initializing this object over multiple test iterations
 isnt the best idea

---
 grid/walk/v2/modules/WalkModuleHeadingVonMises.h |  1 +
 sensors/activity/ActivityButterPressure.h        | 12 ++++++++----
 sensors/activity/ActivityDetector.h              |  2 +-
 sensors/radio/WiFiProbabilityFree.h              |  2 +-
 4 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h
index 30be8e1..653f9e6 100644
--- a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h
+++ b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h
@@ -48,6 +48,7 @@ public:
         if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
 
         // adjust the state's heading using the control-data
+        //TODO: if motionaxis detects landscaping, do not do this!
         state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
 
         //set kappa of mises
diff --git a/sensors/activity/ActivityButterPressure.h b/sensors/activity/ActivityButterPressure.h
index 0dbc19d..d72d46b 100644
--- a/sensors/activity/ActivityButterPressure.h
+++ b/sensors/activity/ActivityButterPressure.h
@@ -27,6 +27,7 @@ public:
     private:
 
     std::vector<History> output;
+    std::vector<History> baroInterpHistory;
     Activity currentActivity;
     MovingAVG<float> mvAvg = MovingAVG<float>(20);
 
@@ -40,9 +41,12 @@ public:
     */
     const bool additionalLowpassFilter = false;
     const unsigned long diffSize = 20; //the number values used for finding the activity.
+    //TODO: some kind of algorithm to calculate the threshold
     const float threshold = 0.025f; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
+    const float upperBoundary = 0.1; // if this boundary is passed, we should have errors in barometer data
     Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.05f,2);
     Filter::ButterworthLP<float> butter2 =  Filter::ButterworthLP<float>(10,0.05f,2);
+    bool firstCall = false;
 
     FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
 
@@ -59,7 +63,6 @@ public:
     void add(const Timestamp& ts, const BarometerData& baro) {
 
         //init
-        static bool firstCall = false;
         if(!firstCall){
             butter.stepInitialization(baro.hPa);
             firstCall = true;
@@ -72,7 +75,9 @@ public:
 
         //interpolate & butter
         auto callback = [&] (const Timestamp ts, const float val) {
+            //interp
             float interpValue = val;
+            baroInterpHistory.push_back(History(ts, BarometerData(interpValue)));
 
             //butter
             float butterValue = butter.process(interpValue);
@@ -92,7 +97,6 @@ public:
                 for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){
 
                     float diffVal = output[i+1].data.hPa - output[i].data.hPa;
-
                     diff.push_back(diffVal);
                 }
 
@@ -111,10 +115,10 @@ public:
                     actValue = sum;
                 }
 
-                if(actValue > threshold){
+                if(actValue > threshold && actValue < upperBoundary){
                     currentActivity = Activity::WALKING_DOWN;
                 }
-                else if (actValue < -threshold){
+                else if (actValue < -threshold && actValue > -upperBoundary){
                     currentActivity = Activity::WALKING_UP;
                 }
                 else{
diff --git a/sensors/activity/ActivityDetector.h b/sensors/activity/ActivityDetector.h
index 7cf90dd..6c066de 100644
--- a/sensors/activity/ActivityDetector.h
+++ b/sensors/activity/ActivityDetector.h
@@ -140,7 +140,7 @@ private:
 			++xx;
 		#endif
 
-		if (std::abs(delta_hPa) < 0.042) {
+        if (std::abs(delta_hPa) < 0.042) {
 			current = Activity::WALKING;
 			return;
 		} else if (delta_hPa > 0) {
diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h
index c8f79e0..8f69abd 100644
--- a/sensors/radio/WiFiProbabilityFree.h
+++ b/sensors/radio/WiFiProbabilityFree.h
@@ -69,7 +69,7 @@ public:
 			Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
 			Assert::isTrue(age.ms() <= 60000, "found a 60 second old wifi measurement. maybe there is a coding error?");
 
-			Assert::isBetween(scanRSSI, -100.0f, -30.0f, "scan-rssi out of range");
+                        Assert::isBetween(scanRSSI, -100.0f, -20.0f, "scan-rssi out of range");
 			//Assert::isBetween(modelRSSI, -160.0f, -10.0f, "model-rssi out of range");
 
 			// sigma grows with measurement age