void WindAnalyser::_calcWind(NMEA_INFO *nmeaInfo,
DERIVED_INFO *derivedInfo) {
int i;
double av=0;
if (!numwindsamples) return;
// reject if average time step greater than 2.0 seconds
if ((windsamples[numwindsamples-1].t - windsamples[0].t)/
(numwindsamples-1)>2.0) {
return;
}
// find average
for (i=0; i<numwindsamples; i++) {
av += windsamples[i].mag;
}
av/= numwindsamples;
// find zero time for times above average
double rthisp;
int j;
int ithis = 0;
double rthismax = 0;
double rthismin = 0;
int jmax= -1;
int jmin= -1;
int idiff;
for (j=0; j<numwindsamples; j++) {
rthisp= 0;
for (i=0; i<numwindsamples; i++) {
if (i== j) continue;
ithis = (i+j)%numwindsamples;
idiff = i;
if (idiff>numwindsamples/2) {
idiff = numwindsamples-idiff;
}
rthisp += (windsamples[ithis].mag)*idiff;
}
if ((rthisp<rthismax)||(jmax==-1)) {
rthismax = rthisp;
jmax = j;
}
if ((rthisp>rthismin)||(jmin==-1)) {
rthismin = rthisp;
jmin = j;
}
}
if(jmin<0 || jmax<0) {
return;
}
// jmax is the point where most wind samples are below
// jmin is the point where most wind samples are above
maxVector = windsamples[jmax].v;
minVector = windsamples[jmin].v;
// attempt to fit cycloid
double phase;
double mag = 0.5*(windsamples[jmax].mag - windsamples[jmin].mag);
double wx, wy;
double cmag;
double rthis=0;
for (i=0; i<numwindsamples; i++) {
phase = ((i+jmax)%numwindsamples)*3.141592*2.0/numwindsamples;
wx = cos(phase)*av+mag;
wy = sin(phase)*av;
cmag = sqrt(wx*wx+wy*wy)-windsamples[i].mag;
rthis += cmag*cmag;
}
rthis/= numwindsamples;
LKASSERT(rthis>=0);
if (rthis<0) return; // UNMANAGED
rthis = sqrt(rthis);
int quality;
if (mag>1) {
quality = 5- iround(rthis/mag*3);
} else {
quality = 5- iround(rthis);
}
if (circleCount<3) quality--;
if (circleCount<2) quality--;
if (circleCount<1) return;
quality= min(quality,5); //5 is maximum quality, make sure we honour that.
Vector a;
#if BUGSTOP
LKASSERT(windsamples[jmax].mag!=0);
#endif
if (windsamples[jmax].mag==0) return;
a.x = -mag*maxVector.x/windsamples[jmax].mag;
a.y = -mag*maxVector.y/windsamples[jmax].mag;
if (quality<1) {
return; //measurment quality too low
}
if (a.x*a.x+a.y*a.y<30*30) {
// limit to reasonable values (60 knots), reject otherwise
slot_newEstimate(nmeaInfo, derivedInfo, a, quality);
}
}
void WindAnalyser::_calcWind(NMEA_INFO *nmeaInfo,
DERIVED_INFO *derivedInfo) {
if (windsamples.empty()) return;
const double circle_time = (windsamples.back().t - windsamples.front().t);
// reject if circle time greater than 50 second
if(circle_time > 50.0) {
return;
}
const int numwindsamples = windsamples.size();
// reject if average time step greater than 2.0 seconds
if ( (circle_time / (numwindsamples-1)) > 2.0) {
return;
}
// find average magnitude
double av=0;
for (int i=0; i<numwindsamples; i++) {
av += windsamples[i].mag;
}
av/= numwindsamples;
// find zero time for times above average
double rthismax = 0;
double rthismin = 0;
int jmax= -1;
int jmin= -1;
for (int j=0; j<numwindsamples; j++) {
double rthisp= 0;
for (int i=0; i<numwindsamples; i++) {
if (i== j) {
continue;
}
const int ithis = (i+j)%numwindsamples;
int idiff = i;
if (idiff>numwindsamples/2) {
idiff = numwindsamples-idiff;
}
rthisp += (windsamples[ithis].mag)*idiff;
}
if ((rthisp<rthismax)||(jmax==-1)) {
rthismax = rthisp;
jmax = j;
}
if ((rthisp>rthismin)||(jmin==-1)) {
rthismin = rthisp;
jmin = j;
}
}
if(jmin<0 || jmax<0) {
return;
}
// jmax is the point where most wind samples are below
// jmin is the point where most wind samples are above
maxVector = windsamples[jmax].v;
minVector = windsamples[jmin].v;
// attempt to fit cycloid
const double mag = 0.5*(windsamples[jmax].mag - windsamples[jmin].mag);
double rthis=0;
for (int i=0; i<numwindsamples; i++) {
const double phase = ((i+jmax)%numwindsamples)*3.141592*2.0/numwindsamples;
const double wx = cos(phase)*av+mag;
const double wy = sin(phase)*av;
const double cmag = sqrt(wx*wx+wy*wy)-windsamples[i].mag;
rthis += cmag*cmag;
}
rthis /= numwindsamples;
BUGSTOP_LKASSERT(rthis>=0);
if (rthis<0) {
return; // UNMANAGED
}
rthis = sqrt(rthis);
int quality;
if (mag>1) {
quality = 5- iround(rthis/mag*3);
} else {
quality = 5- iround(rthis);
}
if (circleCount<3) quality--;
if (circleCount<2) quality--;
if (circleCount<1) return;
if (quality<1) {
return; //measurement quality too low
}
quality= min(quality,5); //5 is maximum quality, make sure we honor that.
BUGSTOP_LKASSERT(windsamples[jmax].mag!=0);
if (windsamples[jmax].mag==0) {
return;
}
const Vector a = {
-mag*maxVector.x/windsamples[jmax].mag,
-mag*maxVector.y/windsamples[jmax].mag
};
if (a.x*a.x+a.y*a.y<30*30) {
// limit to reasonable values (60 knots), reject otherwise
slot_newEstimate(nmeaInfo, derivedInfo, a, quality);
}
}
