From c48c6a2e48c2af172946928e0a3fa96a57142fa7 Mon Sep 17 00:00:00 2001 From: Ulrich Meine Date: Sat, 19 Jul 2025 00:26:37 +0200 Subject: [PATCH] Move buffer handling to obp60task; reset OBPSensorTask; add true wind calculation --- lib/obp60task/OBPDataOperations.cpp | 136 ++++++++++++++++++++++++++++ lib/obp60task/OBPDataOperations.h | 48 ++++++++++ lib/obp60task/OBPSensorTask.cpp | 59 ------------ lib/obp60task/OBPSensorTask.h | 11 --- lib/obp60task/obp60task.cpp | 104 ++++++++++++++++++++- 5 files changed, 287 insertions(+), 71 deletions(-) create mode 100644 lib/obp60task/OBPDataOperations.cpp create mode 100644 lib/obp60task/OBPDataOperations.h diff --git a/lib/obp60task/OBPDataOperations.cpp b/lib/obp60task/OBPDataOperations.cpp new file mode 100644 index 0000000..daca553 --- /dev/null +++ b/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); +}*/ \ No newline at end of file diff --git a/lib/obp60task/OBPDataOperations.h b/lib/obp60task/OBPDataOperations.h new file mode 100644 index 0000000..2ea8fdc --- /dev/null +++ b/lib/obp60task/OBPDataOperations.h @@ -0,0 +1,48 @@ +#pragma once +// #include +#include "GwApi.h" +#include +#include + +// #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); +} */ \ No newline at end of file diff --git a/lib/obp60task/OBPSensorTask.cpp b/lib/obp60task/OBPSensorTask.cpp index 9a46788..311da1c 100644 --- a/lib/obp60task/OBPSensorTask.cpp +++ b/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 // 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 twdHstry(960); // Circular buffer to store wind direction values; store 960 TWD values for 16 minutes history - RingBuffer twsHstry(960); // Circular buffer to store wind speed values (TWS) - RingBuffer 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(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(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(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); diff --git a/lib/obp60task/OBPSensorTask.h b/lib/obp60task/OBPSensorTask.h index 60a94e0..7dd9f24 100644 --- a/lib/obp60task/OBPSensorTask.h +++ b/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& twdHstry, RingBuffer& twsHstry, RingBuffer& 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); diff --git a/lib/obp60task/obp60task.cpp b/lib/obp60task/obp60task.cpp index 8d0012e..2d4e72b 100644 --- a/lib/obp60task/obp60task.cpp +++ b/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 twdHstry(960); // Circular buffer to store wind direction values; store 960 TWD values for 16 minutes history + RingBuffer twsHstry(960); // Circular buffer to store wind speed values (TWS) + RingBuffer 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::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(twdVal)); + + if (twsBVal->valid) { + twsVal = static_cast(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)