thooge
/
esp32-nmea2000-obp60
mirror of https://github.com/thooge/esp32-nmea2000-obp60.git
1
0
Fork 0
Code Releases Activity

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

This commit is contained in:
Ulrich Meine 2025-07-19 00:26:37 +02:00
parent bb99978177
commit c48c6a2e48
5 changed files with 287 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
lib/obp60task/OBPDataOperations.cpp Normal file
View File

@ -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);
}*/

48
lib/obp60task/OBPDataOperations.h Normal file
View File

@ -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);
} */

59
lib/obp60task/OBPSensorTask.cpp
View File

@ -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);

11
lib/obp60task/OBPSensorTask.h
View File

@ -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);

104
lib/obp60task/obp60task.cpp
View File

@ -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)