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