Move buffer handling to obp60task; reset OBPSensorTask; add true wind calculation

lib/obp60task/OBPDataOperations.cpp

@@ -0,0 +1,136 @@
+#include "OBPDataOperations.h"
+
+void WindUtils::to2PI(double* a)
+{
+    while (*a < 0) {
+        *a += 2 * M_PI;
+    }
+    *a = fmod(*a, 2 * M_PI);
+}
+
+void WindUtils::toPI(double* a)
+{
+    *a += M_PI;
+    to2PI(a);
+    *a -= M_PI;
+}
+
+void WindUtils::to360(double* a)
+{
+    while (*a < 0) {
+        *a += 360;
+    }
+    *a = fmod(*a, 360);
+}
+
+void WindUtils::to180(double* a)
+{
+    *a += 180;
+    to360(a);
+    *a -= 180;
+}
+
+void WindUtils::toCart(const double* phi, const double* r, double* x, double* y)
+{
+    *x = *r * sin(radians(*phi));
+    *y = *r * cos(radians(*phi));
+}
+
+void WindUtils::toPol(const double* x, const double* y, double* phi, double* r)
+{
+    *phi = 90 - degrees(atan2(*y, *x));
+    to360(phi);
+    *r = sqrt(*x * *x + *y * *y);
+}
+
+void WindUtils::addPolar(const double* phi1, const double* r1,
+    const double* phi2, const double* r2,
+    double* phi, double* r)
+{
+    double x1, y1, x2, y2;
+    toCart(phi1, r1, &x1, &y1);
+    toCart(phi2, r2, &x2, &y2);
+    x1 += x2;
+    y1 += y2;
+    toPol(&x1, &y1, phi, r);
+}
+
+void WindUtils::calcTwdSA(const double* AWA, const double* AWS,
+    const double* CTW, const double* STW, const double* HDT,
+    double* TWD, double* TWS)
+{
+    double AWD = *AWA + *HDT;
+    double stw = -*STW;
+    Serial.println("calcTwdSA: AWA: " + String(*AWA) + ", AWS: " + String(*AWS) + ", CTW: " + String(*CTW) + ", STW: " + String(*STW) + ", HDT: " + String(*HDT));
+    addPolar(&AWD, AWS, CTW, &stw, TWD, TWS);
+
+    // Normalize TWD to 0-360°
+    while (*TWD < 0)
+        *TWD += 360;
+    while (*TWD >= 360)
+        *TWD -= 360;
+
+    Serial.println("calcTwdSA: TWD: " + String(*TWD) + ", TWS: " + String(*TWS));
+}
+
+bool WindUtils::calcTrueWind(const double* awaVal, const double* awsVal,
+    const double* cogVal, const double* stwVal, const double* hdtVal,
+    const double* hdmVal, double* twdVal, double* twsVal)
+{
+    double hdt, ctw;
+    double hdmVar = 3.0; // Magnetic declination, can be set from config if needed
+    double twd, tws;
+
+    if (*hdtVal == __DBL_MIN__) {
+        if (*hdmVal != __DBL_MIN__) {
+            hdt = *hdmVal + hdmVar; // Use corrected HDM if HDT is not available
+        } else {
+            return false; // Cannot calculate without valid HDT or HDM
+        }
+    }
+    ctw = *hdtVal + ((*cogVal - *hdtVal) / 2); // Estimate CTW from COG
+
+    if ((*awaVal == __DBL_MIN__) || (*awsVal == __DBL_MIN__) || (*cogVal == __DBL_MIN__) || (*stwVal == __DBL_MIN__)) {
+        // Cannot calculate true wind without valid AWA, AWS, COG, or STW
+        return false;
+    } else {
+        calcTwdSA(awaVal, awsVal, cogVal, stwVal, hdtVal, &twd, &tws);
+        *twdVal = twd;
+        *twsVal = tws;
+
+        return true;
+    }
+}
+
+/*
+// make function available in Python for testing
+static PyObject* true_wind(PyObject* self, PyObject* args) {
+    double AWA,AWS,CTW,STW,HDT,TWS,TWD;
+    if (!PyArg_ParseTuple(args, "ddddd", &AWA, &AWS, &CTW, &STW, &HDT)) {
+        return NULL;
+    }
+
+    calc_true_wind(&AWA, &AWS, &CTW, &STW, &HDT, &TWD, &TWS);
+
+    PyObject* twd = PyFloat_FromDouble(TWD);
+    PyObject* tws = PyFloat_FromDouble(TWS);
+    PyObject* tw = PyTuple_Pack(2,twd,tws);
+    return tw;
+}
+
+static PyMethodDef methods[] = {
+    {"true_wind", true_wind, METH_VARARGS, NULL},
+    {NULL, NULL, 0, NULL}
+};
+
+static struct PyModuleDef module = {
+    PyModuleDef_HEAD_INIT,
+    "truewind",    // Module name
+    NULL,          // Optional docstring
+    -1,
+    methods
+};
+
+PyMODINIT_FUNC PyInit_truewind(void) {
+    return PyModule_Create(&module);
+}*/

lib/obp60task/OBPDataOperations.h

@@ -0,0 +1,48 @@
+#pragma once
+// #include <Python.h>
+#include "GwApi.h"
+#include <Arduino.h>
+#include <math.h>
+
+// #define radians(a) (a*0.017453292519943295)
+// #define degrees(a) (a*57.29577951308232)
+
+class WindUtils {
+
+public:
+    static void to360(double* a);
+    static void to180(double* a);
+    static void to2PI(double* a);
+    static void toPI(double* a);
+    static void toCart(const double* phi, const double* r, double* x, double* y);
+    static void toPol(const double* x, const double* y, double* phi, double* r);
+    static void addPolar(const double* phi1, const double* r1,
+        const double* phi2, const double* r2,
+        double* phi, double* r);
+    static bool calcTrueWind(const double* awaVal, const double* awsVal,
+        const double* cogVal, const double* stwVal, const double* hdtVal,
+        const double* hdmVal, double* twdVal, double* twsVal);
+    static void calcTwdSA(const double* AWA, const double* AWS,
+        const double* CTW, const double* STW, const double* HDT,
+        double* TWD, double* TWS);
+};
+
+/*
+// make function available in Python for testing
+static PyObject* true_wind(PyObject* self, PyObject* args);
+static PyMethodDef methods[] = {
+    {"true_wind", true_wind, METH_VARARGS, NULL},
+    {NULL, NULL, 0, NULL}
+};
+
+static struct PyModuleDef module = {
+    PyModuleDef_HEAD_INIT,
+    "truewind",    // Module name
+    NULL,          // Optional docstring
+    -1,
+    methods
+};
+
+PyMODINIT_FUNC PyInit_truewind(void) {
+    return PyModule_Create(&module);
+} */

lib/obp60task/OBPSensorTask.cpp

@@ -17,7 +17,6 @@
 #include "ObpNmea0183.h"                // Check NMEA0183 sentence for uncorrect content
 #include "OBP60Extensions.h"            // Lib for hardware extensions
 #include "movingAvg.h"                  // Lib for moving average building
-#include "OBPRingBuffer.h"              // Lib with ring buffer for history storage of some boat data
 #include "time.h"                       // For getting NTP time
 #include <ESP32Time.h>                  // Internal ESP32 RTC clock
 
@@ -70,14 +69,6 @@ void sensorTask(void *param){
     batV.begin();
     batC.begin();
 
-    // Create ring buffers for history storage of some boat data
-    // later read additonal data types from config and specify buffers accordingly
-    RingBuffer<int16_t> twdHstry(960); // Circular buffer to store wind direction values; store 960 TWD values for 16 minutes history
-    RingBuffer<int16_t> twsHstry(960); // Circular buffer to store wind speed values (TWS)
-    RingBuffer<int16_t> dbtHstry(960); // Circular buffer to store water depth values (DBT)
-    // Link ring buffer pointers to shared memory for transfer to pages
-    shared->setHstryBuf(twdHstry, twsHstry, dbtHstry);
-
     // Start timer
     Timer1.start(); // Start Timer1 for blinking LED
 
@@ -356,24 +347,6 @@ void sensorTask(void *param){
         }
     }
 
-    // Boat data history buffer initialization
-    // later, read data types from config and specify hstryValList accordingly
-    GwApi::BoatValue *twdBVal=new GwApi::BoatValue(GwBoatData::_TWD);
-    GwApi::BoatValue *twsBVal=new GwApi::BoatValue(GwBoatData::_TWS);
-    GwApi::BoatValue *dbtBVal=new GwApi::BoatValue(GwBoatData::_DBT);
-    GwApi::BoatValue *hstryValList[]={twdBVal, twsBVal, dbtBVal}; // List of boat values for history storage
-    api->getBoatDataValues(3, hstryValList);
-    int hstryUpdFreq = 1000; // Update frequency for history buffers in ms
-    int hstryMinVal = 0; // Minimum value for these history buffers
-    int twdHstryMax = 6283; // Max value for wind direction (TWD) in rad (0...2*PI), shifted by 1000 for 3 decimals
-    int twsHstryMax = 1000; // Max value for wind speed (TWS) in m/s, shifted by 10 for 1 decimal
-    int dbtHstryMax = 3276; // Max value for depth in m (=327), shifted by 10 for 1 decimal
-    // Initialize history buffers with meta data
-    twdHstry.setMetaData(twdBVal->getName(), twdBVal->getFormat(), hstryUpdFreq, hstryMinVal, twdHstryMax);
-    twsHstry.setMetaData(twsBVal->getName(), twsBVal->getFormat(), hstryUpdFreq, hstryMinVal, twsHstryMax);
-    dbtHstry.setMetaData(dbtBVal->getName(), dbtBVal->getFormat(), hstryUpdFreq, hstryMinVal, dbtHstryMax);
-    bool simulation = api->getConfig()->getBool(api->getConfig()->useSimuData, false);
-
     int rotoffset = api->getConfig()->getConfigItem(api->getConfig()->rotOffset,true)->asInt();
 
     static long loopCounter = 0;    // Loop counter for 1Wire data transmission
@@ -387,7 +360,6 @@ void sensorTask(void *param){
     long starttime11 = millis();    // Copy GPS data to RTC all 5min
     long starttime12 = millis();    // Get RTC data all 500ms
     long starttime13 = millis();    // Get 1Wire sensor data all 2s
-    unsigned long starttime20 = millis(); // Get history TWD, TWS, DBT data each 1s
 
     tN2kMsg N2kMsg;
     shared->setSensorData(sensors); //set initially read values
@@ -804,37 +776,6 @@ void sensorTask(void *param){
             }
         }
 
-        // Read TWD, TWS, DBT data from boatData each 1000ms for history and windplot display
-        if((millis() > starttime20 + 1000) && !simulation){
-            int16_t val;
-            starttime20 = millis();
-            api->getBoatDataValues(3, hstryValList);
-            if (twdBVal->valid) {
-                val = static_cast<int16_t>(std::round(twdBVal->value * 1000)); // Shift value to store decimals in int16_t);
-                if (val < hstryMinVal || val > twdHstryMax) {
-                    val = INT16_MIN; // Add invalid value
-                }
-                twdHstry.add(val);
-            }
-            if (twsBVal->valid) {
-                val = static_cast<int16_t>(twsBVal->value * 10); // Shift value to store decimals in int16_t
-                if (val < hstryMinVal || val > twsHstryMax) {
-                    val = INT16_MIN; // Add invalid value
-                }
-                twsHstry.add(val);
-            }
-            if (dbtBVal->valid) {
-                val = static_cast<int16_t>(dbtBVal->value * 10); // Shift value to store decimals in int16_t
-                if (val < hstryMinVal || val > dbtHstryMax) {
-                    val = INT16_MIN; // Add invalid value
-                }
-                dbtHstry.add(val);
-            }
-
-            int counttime = millis() - starttime20;
-            api->getLogger()->logDebug(GwLog::ERROR,"SensorTask write time: %d", counttime);
-        }
-
         shared->setSensorData(sensors);
     }
     vTaskDelete(NULL);

lib/obp60task/OBPSensorTask.h

@@ -8,7 +8,6 @@ class SharedData{
     private:
         SemaphoreHandle_t locker;
         SensorData sensors;
-        tBoatHstryData boatHstry;
     public:
         GwApi *api=NULL;
         SharedData(GwApi *api){
@@ -23,16 +22,6 @@ class SharedData{
             GWSYNCHRONIZED(&locker);
             return sensors;
         }
-        void setHstryBuf(RingBuffer<int16_t>& twdHstry, RingBuffer<int16_t>& twsHstry, RingBuffer<int16_t>& dbtHstry) {
-            GWSYNCHRONIZED(&locker);
-            boatHstry={&twdHstry, &twsHstry, &dbtHstry};
-//            api->getLogger()->logDebug(GwLog::ERROR, "SharedData setHstryBuf - passed object ptrs: TWD: %p, TWS: %p, STW: %p", &twdHstry, &twsHstry, &dbtHstry);
-//            api->getLogger()->logDebug(GwLog::ERROR, "SharedData setHstryBuf: TWD: %p, TWS: %p, STW: %p", boatHstry.twdHstry, boatHstry.twsHstry, boatHstry.dbtHstry);
-        }
-        tBoatHstryData getHstryBuf() {
-            GWSYNCHRONIZED(&locker);
-            return boatHstry;
-        }
 };
 
 void createSensorTask(SharedData *shared);

lib/obp60task/obp60task.cpp

@@ -13,6 +13,8 @@
 #include "OBP60Extensions.h"            // Functions lib for extension board
 #include "OBP60Keypad.h"                // Functions for keypad
 #include "BoatDataCalibration.h"        // Functions lib for data instance calibration
+#include "OBPRingBuffer.h"              // Functions lib with ring buffer for history storage of some boat data
+#include "OBPDataOperations.h"          // Functions lib for data operations such as true wind calculation
 
 #ifdef BOARD_OBP40S3
 #include "driver/rtc_io.h"              // Needs for weakup from deep sleep
@@ -390,6 +392,11 @@ void OBP60Task(GwApi *api){
     commonData.logger=logger;
     commonData.config=config;
 
+    // Create ring buffers for history storage of some boat data
+    RingBuffer<int16_t> twdHstry(960); // Circular buffer to store wind direction values; store 960 TWD values for 16 minutes history
+    RingBuffer<int16_t> twsHstry(960); // Circular buffer to store wind speed values (TWS)
+    RingBuffer<int16_t> dbtHstry(960); // Circular buffer to store water depth values (DBT)
+
 #ifdef HARDWARE_V21
     // Keyboard coordinates for page footer
     initKeys(commonData);
@@ -525,6 +532,8 @@ void OBP60Task(GwApi *api){
             LOG_DEBUG(GwLog::DEBUG,"added fixed value %s to page %d",value->getName().c_str(),i);
             pages[i].parameters.values.push_back(value); 
        }
+        // Add boat history data to page parameters
+        pages[i].parameters.boatHstry = {&twdHstry, &twsHstry, &dbtHstry};
     }
     // add out of band system page (always available)
     Page *syspage = allPages.pages[0]->creator(commonData);
@@ -532,6 +541,33 @@ void OBP60Task(GwApi *api){
     // Read all calibration data settings from config
     calibrationData.readConfig(config, logger);
 
+    // List of boat values for history storage
+    GwApi::BoatValue *twdBVal=new GwApi::BoatValue(GwBoatData::_TWD);
+    GwApi::BoatValue *twsBVal=new GwApi::BoatValue(GwBoatData::_TWS);
+    GwApi::BoatValue *dbtBVal=new GwApi::BoatValue(GwBoatData::_DBT);
+    GwApi::BoatValue *hstryValList[]={twdBVal, twsBVal, dbtBVal};
+    api->getBoatDataValues(3, hstryValList);
+    // Boat data history buffer initialization
+    int hstryUpdFreq = 1000; // Update frequency for history buffers in ms
+    int hstryMinVal = 0; // Minimum value for these history buffers
+    int twdHstryMax = 6283; // Max value for wind direction (TWD) in rad (0...2*PI), shifted by 1000 for 3 decimals
+    int twsHstryMax = 1000; // Max value for wind speed (TWS) in m/s, shifted by 10 for 1 decimal
+    int dbtHstryMax = 3276; // Max value for depth in m (=327), shifted by 10 for 1 decimal
+    // Initialize history buffers with meta data
+    twdHstry.setMetaData(twdBVal->getName(), twdBVal->getFormat(), hstryUpdFreq, hstryMinVal, twdHstryMax);
+    twsHstry.setMetaData(twsBVal->getName(), twsBVal->getFormat(), hstryUpdFreq, hstryMinVal, twsHstryMax);
+    dbtHstry.setMetaData(dbtBVal->getName(), dbtBVal->getFormat(), hstryUpdFreq, hstryMinVal, dbtHstryMax);
+    bool simulation = api->getConfig()->getBool(api->getConfig()->useSimuData, false);
+
+    // List of boat values for true winds calculation
+    GwApi::BoatValue *awaBVal=new GwApi::BoatValue(GwBoatData::_AWA);
+    GwApi::BoatValue *awsBVal=new GwApi::BoatValue(GwBoatData::_AWS);
+    GwApi::BoatValue *cogBVal=new GwApi::BoatValue(GwBoatData::_COG);
+    GwApi::BoatValue *stwBVal=new GwApi::BoatValue(GwBoatData::_STW);
+    GwApi::BoatValue *hdtBVal=new GwApi::BoatValue(GwBoatData::_HDT);
+    GwApi::BoatValue *hdmBVal=new GwApi::BoatValue(GwBoatData::_HDM);
+    GwApi::BoatValue *WndCalcValList[]={awaBVal, awsBVal, cogBVal, stwBVal, hdtBVal, hdmBVal};
+
     // Display screenshot handler for HTTP request
     // http://192.168.15.1/api/user/OBP60Task/screenshot
     api->registerRequestHandler("screenshot", [api, &pageNumber, pages](AsyncWebServerRequest *request) {
@@ -587,6 +623,18 @@ void OBP60Task(GwApi *api){
     GwApi::BoatValue *lon = boatValues.findValueOrCreate("LON");        // Load GpsLongitude
     GwApi::BoatValue *hdop = boatValues.findValueOrCreate("HDOP");       // Load GpsHDOP
 
+/*    // Boat values for wind conversion for main loop; will be calculated in case values are not available by sensors
+    GwApi::BoatValue *twd = boatValues.findValueOrCreate("TWD");        // True Wind Direction
+    GwApi::BoatValue *tws = boatValues.findValueOrCreate("TWS");        // True Wind Speed
+    GwApi::BoatValue *twa = boatValues.findValueOrCreate("TWA");        // True Wind Angle
+    GwApi::BoatValue *awaBVal = boatValues.findValueOrCreate("AWA");        // Apparent Wind Angle
+    GwApi::BoatValue *awsBVal = boatValues.findValueOrCreate("AWS");        // Apparent Wind Speed
+    GwApi::BoatValue *cogBVal = boatValues.findValueOrCreate("COG");        // Course Over Ground
+    GwApi::BoatValue *stwBVal = boatValues.findValueOrCreate("STW");        // Speed Through Water
+    GwApi::BoatValue *hdtBVal = boatValues.findValueOrCreate("HDT");        // Heading True
+    GwApi::BoatValue *ctwBVal = boatValues.findValueOrCreate("CTW");        // Course Through Water
+    GwApi::BoatValue *hdmBVal = boatValues.findValueOrCreate("HDM");        // Heading Magnetic
+*/
     LOG_DEBUG(GwLog::LOG,"obp60task: start mainloop");
     
     commonData.time = boatValues.findValueOrCreate("GPST");      // Load GpsTime
@@ -600,6 +648,7 @@ void OBP60Task(GwApi *api){
     long starttime3 = millis();     // Display update all 1s
     long starttime4 = millis();     // Delayed display update after 4s when select a new page
     long starttime5 = millis();     // Calculate sunrise and sunset all 1s
+    unsigned long starttime10 = millis(); // Get history TWD, TWS, DBT data and calculate true winds each 1s
 
     pages[pageNumber].page->setupKeys(); // Initialize keys for first page
 
@@ -789,6 +838,60 @@ void OBP60Task(GwApi *api){
                 }
             }
             
+            // Read TWD, TWS, DBT data from boatData each 1000ms for history and windplot display
+            if((millis() > starttime10 + 1000) && !simulation) {
+                double twdVal, twsVal, dbtVal;
+                double awaVal, awsVal, cogVal, stwVal, hdtVal, hdmVal;
+                double DBL_MIN = std::numeric_limits<double>::lowest();
+
+                starttime10 = millis();
+                LOG_DEBUG(GwLog::DEBUG,"History buffer write cycle");
+                api->getBoatDataValues(3, hstryValList);
+
+                if (!twdBVal->valid || !twsBVal->valid) {
+                    api->getBoatDataValues(6, WndCalcValList); // Get all values for true wind calculation
+                    awaVal = awaBVal->valid ? awaBVal->value * RAD_TO_DEG : __DBL_MIN__;
+                    awsVal = awsBVal->valid ? awsBVal->value : __DBL_MIN__;
+                    cogVal = cogBVal->valid ? cogBVal->value  * RAD_TO_DEG : __DBL_MIN__;
+                    stwVal = stwBVal->valid ? stwBVal->value : __DBL_MIN__;
+                    hdtVal = hdtBVal->valid ? hdtBVal->value * RAD_TO_DEG : __DBL_MIN__;
+                    hdmVal = hdmBVal->valid ? hdmBVal->value * RAD_TO_DEG : __DBL_MIN__; 
+                    LOG_DEBUG(GwLog::ERROR,"obp60task - Read data: AWA: %f, AWS: %f, COG: %f, STW: %f, HDT: %f, TWD=%f, TWS=%f", awaBVal->value, awsBVal->value,
+                            cogBVal->value, stwBVal->value, hdtBVal->value, twdBVal->value, twsBVal->value);
+
+                    bool isCalculated = WindUtils::calcTrueWind(&awaVal, &awsVal, &cogVal, &stwVal, &cogVal, &hdmVal, &twdVal, &twsVal); // Calculate true wind if TWD not available
+//                    bool isCalculated = WindUtils::calcTrueWind(&awaVal, &awsVal, &cogVal, &stwVal, &hdtVal, &hdmVal, &twdVal, &twsVal); // Calculate true wind if TWD not available
+                    LOG_DEBUG(GwLog::ERROR,"obp60task - calc Wind: AWA: %f, AWS: %f, COG: %f, STW: %f, HDT: %f, TWD=%f, TWS=%f, converted? %d", awaBVal->value * RAD_TO_DEG, awsBVal->value * 3.6 / 1.852,
+                            cogBVal->value * RAD_TO_DEG, stwBVal->value * 3.6 / 1.852, hdtBVal->value * RAD_TO_DEG, twdVal, twsVal, isCalculated);
+                }
+                if (twdBVal->valid) {
+                    twdVal = std::round(twdBVal->value * 1000); // Shift value to store decimals in int16_t);
+                    if (twdVal < hstryMinVal || twdVal > twdHstryMax) {
+                        twdVal = INT16_MIN; // Add invalid value
+                    }
+                }
+                twdHstry.add(static_cast<int16_t>(twdVal));
+
+                if (twsBVal->valid) {
+                    twsVal = static_cast<int16_t>(twsBVal->value * 10); // Shift value to store decimals in int16_t
+                    if (twsVal < hstryMinVal || twsVal > twsHstryMax) {
+                        twsVal = INT16_MIN; // Add invalid value
+                    }
+                }
+                twsHstry.add(twsVal);
+
+                if (dbtBVal->valid) {
+                    dbtVal = dbtBVal->value * 10; // Shift value to store decimals in int16_t
+                    if (dbtVal < hstryMinVal || dbtVal > dbtHstryMax) {
+                        dbtVal = INT16_MIN; // Add invalid value
+                    }
+                    dbtHstry.add(dbtVal);
+                }
+
+                int counttime = millis() - starttime10;
+                LOG_DEBUG(GwLog::ERROR,"obp60task: History buffer write time: %d", counttime);
+            }
+
             // Refresh display data, default all 1s
             currentPage = pages[pageNumber].page;
             int pagetime = 1000;
@@ -839,7 +942,6 @@ void OBP60Task(GwApi *api){
                             currentPage->displayNew(pages[pageNumber].parameters);
                             lastPage=pageNumber;
                         }
-                        pages[pageNumber].parameters.boatHstry = shared->getHstryBuf(); // Add boat history data to page parameters
                         //call the page code
                         LOG_DEBUG(GwLog::DEBUG,"calling page %d",pageNumber);
                         // Show footer if enabled (together with header)