Added smoothing to PageCurrent; SET and DFT were changing rapidly

lib/obp60task/PageCurrent.cpp

@@ -88,6 +88,8 @@ private:
     String backlightMode;
     String leeKStd;
     double leeK;
+    movingAvgAngle<double> SetAvg {8}; // Store average of the last 8 values of SET angle values
+    movingAvg<double> DftAvg {8}; // Store average of the last 8 values of DFT speed value
 
     // Old values for hold function
     String sValueOld[NUM_VALS] = { "", "", "", "", "", "", "", "", "", "" };
@@ -319,6 +321,10 @@ public:
             leeK = (commonData->config->getString(commonData->config->leeK)).toDouble();
         }
         LOG_DEBUG(GwLog::DEBUG, "PageCurrent: leeKStd: %s, leeK: %.3f", leeKStd.c_str(), leeK);
+		
+		// Initialize average buffer for SET and DFT
+        SetAvg.begin();
+        DftAvg.begin();
     }
 
     virtual int handleKey(int key)
@@ -358,22 +364,23 @@ public:
             bValue[DFT]->getName().c_str(), bValue[DFT]->value, bValue[DFT]->valid, bValue[HDT]->getName().c_str(), bValue[HDT]->value, bValue[HDT]->valid,
             bValue[STW]->getName().c_str(), bValue[STW]->value, bValue[STW]->valid);
 
-        // Calculate current data
+        // Calculate current
         //***********************************************************
 
         if (!bValue[SET]->valid || !bValue[DFT]->valid) { // If SET or DRIFT not available, try to calculate them
-            if (bValue[SOG]->valid && bValue[COG]->valid && bValue[STW]->valid && bValue[AWA]->valid) { // calculate current only if all required values are available
+            if (bValue[SOG]->valid && bValue[COG]->valid && bValue[STW]->valid) {
+                // calculate current only if all required values are available; we accept missing AWA, because this is only required for leeway adjustment
 
                 if (!bValue[HDT]->valid) { // HDT not available
                     if (bValue[HDM]->valid) {
                         if (bValue[VAR]->valid) {
                             bValue[HDT]->value = bValue[HDM]->value + bValue[VAR]->value; // Use corrected HDM if HDT is not available
                             bValue[HDT]->value = WindUtils::to2PI(bValue[HDT]->value);
-                            bValue[HDT]->valid = true;
                         } else {
                             // if HDT cannot be fully substituted by HDM+VAR, continue with HDM only
                             bValue[HDT] = bValue[HDM];
                         }
+                        bValue[HDT]->valid = true;
                     }
                 }
 
@@ -386,7 +393,8 @@ public:
                 lay = calcLeeway(leeK, bValue[ROLL]->value, bValue[STW]->value);
 
                 if (bValue[HDT]->valid) { // That's either HDT or HDM
-                    current = calcSetAndDrift(lay, bValue[SOG]->value, bValue[COG]->value, bValue[STW]->value, bValue[HDT]->value, bValue[AWA]->value);
+                    double awaVal = (bValue[AWA]->valid ? bValue[AWA]->value : 0.0); // catch potentially missing AWA
+                    current = calcSetAndDrift(lay, bValue[SOG]->value, bValue[COG]->value, bValue[STW]->value, bValue[HDT]->value, awaVal);
                     bValue[SET]->value = current.set;
                     bValue[SET]->setFormat("formatCourse");
                     bValue[SET]->valid = true;
@@ -397,6 +405,12 @@ public:
             }
         }
 
+		// averaging current for smoother output
+        if (bValue[SET]->valid && bValue[DFT]->valid) { // Are SET and DRIFT now available?
+			bValue[SET]->value = SetAvg.reading(bValue[SET]->value);
+			bValue[DFT]->value = DftAvg.reading(bValue[DFT]->value);
+		}
+
         LOG_DEBUG(GwLog::DEBUG, "PageCurrent: leeKStd: %s, leeK: %.3f, lay: %.3f, roll: %.3f, stw: %.3f, awa: %.3f, hdt: %.3f, cog: %.3f, sog: %.3f, set: %.3f, dft: %.3f",
             leeKStd.c_str(), leeK, lay, bValue[ROLL]->value, bValue[STW]->value, bValue[AWA]->value, bValue[HDT]->value, bValue[COG]->value, bValue[SOG]->value,
             bValue[SET]->value, bValue[DFT]->value);