mirror of
https://github.com/thooge/esp32-nmea2000-obp60.git
synced 2025-12-15 23:13:07 +01:00
added calibration to buffer; separated buffer and wind code in opb60task; prepared simulation; getMin/Max fix for ringbuffer for invalid data; fix for chart center; cleanup code
This commit is contained in:
Ulrich Meine
@@ -1,45 +1,51 @@
|
||||
#include "OBPDataOperations.h"
|
||||
|
||||
void WindUtils::to2PI(double* a)
|
||||
double WindUtils::to2PI(double a)
|
||||
{
|
||||
while (*a < 0) {
|
||||
*a += 2 * M_PI;
|
||||
a = fmod(a, 2 * M_PI);
|
||||
if (a < 0.0) {
|
||||
a += 2 * M_PI;
|
||||
}
|
||||
*a = fmod(*a, 2 * M_PI);
|
||||
return a;
|
||||
}
|
||||
|
||||
void WindUtils::toPI(double* a)
|
||||
double WindUtils::toPI(double a)
|
||||
{
|
||||
*a += M_PI;
|
||||
to2PI(a);
|
||||
*a -= M_PI;
|
||||
a += M_PI;
|
||||
a = to2PI(a);
|
||||
a -= M_PI;
|
||||
|
||||
return a;
|
||||
}
|
||||
|
||||
void WindUtils::to360(double* a)
|
||||
double WindUtils::to360(double a)
|
||||
{
|
||||
while (*a < 0) {
|
||||
*a += 360;
|
||||
a = fmod(a, 360);
|
||||
if (a < 0.0) {
|
||||
a += 360;
|
||||
}
|
||||
*a = fmod(*a, 360);
|
||||
return a;
|
||||
}
|
||||
|
||||
void WindUtils::to180(double* a)
|
||||
double WindUtils::to180(double a)
|
||||
{
|
||||
*a += 180;
|
||||
to360(a);
|
||||
*a -= 180;
|
||||
a += 180;
|
||||
a = to360(a);
|
||||
a -= 180;
|
||||
|
||||
return a;
|
||||
}
|
||||
|
||||
void WindUtils::toCart(const double* phi, const double* r, double* x, double* y)
|
||||
{
|
||||
*x = *r * sin(radians(*phi));
|
||||
*y = *r * cos(radians(*phi));
|
||||
*x = *r * sin(*phi);
|
||||
*y = *r * cos(*phi);
|
||||
}
|
||||
|
||||
void WindUtils::toPol(const double* x, const double* y, double* phi, double* r)
|
||||
{
|
||||
*phi = 90 - degrees(atan2(*y, *x));
|
||||
to360(phi);
|
||||
*phi = (M_PI / 2) - atan2(*y, *x);
|
||||
*phi = to2PI(*phi);
|
||||
*r = sqrt(*x * *x + *y * *y);
|
||||
}
|
||||
|
||||
@@ -57,80 +63,96 @@ void WindUtils::addPolar(const double* phi1, const double* r1,
|
||||
|
||||
void WindUtils::calcTwdSA(const double* AWA, const double* AWS,
|
||||
const double* CTW, const double* STW, const double* HDT,
|
||||
double* TWD, double* TWS)
|
||||
double* TWD, double* TWS, double* TWA)
|
||||
{
|
||||
double AWD = *AWA + *HDT;
|
||||
double awd = *AWA + *HDT;
|
||||
awd = to2PI(awd);
|
||||
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);
|
||||
// Serial.println("\ncalcTwdSA: 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));
|
||||
// Normalize TWD and TWA to 0-360°
|
||||
*TWD = to2PI(*TWD);
|
||||
*TWA = toPI(*TWD - *HDT);
|
||||
// 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)
|
||||
const double* cogVal, const double* stwVal, const double* sogVal, const double* hdtVal,
|
||||
const double* hdmVal, const double* varVal, double* twdVal, double* twsVal, double* twaVal)
|
||||
{
|
||||
double hdt, ctw;
|
||||
double hdmVar = 3.0; // Magnetic declination, can be set from config if needed
|
||||
double twd, tws;
|
||||
double stw, hdt, ctw;
|
||||
double twd, tws, twa;
|
||||
static const double DBL_MIN = std::numeric_limits<double>::lowest();
|
||||
|
||||
if (*hdtVal == __DBL_MIN__) {
|
||||
if (*hdmVal != __DBL_MIN__) {
|
||||
hdt = *hdmVal + hdmVar; // Use corrected HDM if HDT is not available
|
||||
if (*hdtVal != DBL_MIN) {
|
||||
hdt = *hdtVal; // Use HDT if available
|
||||
} else {
|
||||
if (*hdmVal != DBL_MIN && *varVal != DBL_MIN) {
|
||||
hdt = *hdmVal + *varVal; // Use corrected HDM if HDT is not available
|
||||
hdt = to2PI(hdt);
|
||||
} else if (*cogVal != DBL_MIN) {
|
||||
hdt = *cogVal; // Use COG as fallback if HDT and HDM are 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__)) {
|
||||
if (*cogVal != DBL_MIN) {
|
||||
ctw = *cogVal; // Use COG as CTW if available
|
||||
// ctw = *cogVal + ((*cogVal - hdt) / 2); // Estimate CTW from COG
|
||||
} else {
|
||||
ctw = hdt; // 2nd approximation for CTW;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (*stwVal != DBL_MIN) {
|
||||
stw = *stwVal; // Use STW if available
|
||||
} else if (*sogVal != DBL_MIN) {
|
||||
stw = *sogVal;
|
||||
} else {
|
||||
// If STW and SOG are not available, we cannot calculate true wind
|
||||
return false;
|
||||
}
|
||||
|
||||
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);
|
||||
calcTwdSA(awaVal, awsVal, &ctw, stwVal, &hdt, &twd, &tws, &twa);
|
||||
*twdVal = twd;
|
||||
*twsVal = tws;
|
||||
*twaVal = twa;
|
||||
|
||||
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;
|
||||
void HstryBuf::fillWndBufSimData(tBoatHstryData& hstryBufs)
|
||||
// Fill most part of TWD and TWS history buffer with simulated data
|
||||
{
|
||||
double value = 20.0;
|
||||
int16_t value2 = 0;
|
||||
for (int i = 0; i < 900; i++) {
|
||||
value += random(-20, 20);
|
||||
value = WindUtils::to360(value);
|
||||
value2 = static_cast<int16_t>(value * DEG_TO_RAD * 1000);
|
||||
hstryBufs.twdHstry->add(value2);
|
||||
}
|
||||
|
||||
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}
|
||||
};
|
||||
/* double genTwdSimDat()
|
||||
{
|
||||
simTwd += random(-20, 20);
|
||||
if (simTwd < 0.0)
|
||||
simTwd += 360.0;
|
||||
if (simTwd >= 360.0)
|
||||
simTwd -= 360.0;
|
||||
|
||||
static struct PyModuleDef module = {
|
||||
PyModuleDef_HEAD_INIT,
|
||||
"truewind", // Module name
|
||||
NULL, // Optional docstring
|
||||
-1,
|
||||
methods
|
||||
};
|
||||
int16_t z = static_cast<int16_t>(DegToRad(simTwd) * 1000.0);
|
||||
pageData.boatHstry.twdHstry->add(z); // Fill the buffer with some test data
|
||||
|
||||
PyMODINIT_FUNC PyInit_truewind(void) {
|
||||
return PyModule_Create(&module);
|
||||
}*/
|
||||
simTws += random(-200, 150) / 10.0; // TWS value in knots
|
||||
simTws = constrain(simTws, 0.0f, 50.0f); // Ensure TWS is between 0 and 50 knots
|
||||
twsValue = simTws;
|
||||
}*/
|
||||
|
||||
Reference in New Issue
Block a user