void TestZigZag(double V_wind, double theta_wind) {
double t, V_tas, V_gps, theta_gps, theta_glider;
int i;
for (i=0; i<=NUM_SAMPLES; i++) {
t = i;
V_tas = 20.0;
theta_glider = sin(t*M_PI*2.0/NUM_SAMPLES)*30*DEGTORAD;
double V_gps_x = V_tas * sin(theta_glider) - V_wind*sin(theta_wind);
double V_gps_y = V_tas * cos(theta_glider) - V_wind*cos(theta_wind);
V_gps = sqrt(V_gps_x*V_gps_x+V_gps_y*V_gps_y);
theta_gps = atan2(V_gps_x,V_gps_y);
myzigzag.AddPoint(t, V_tas, V_gps, theta_gps);
}
// ok, ready to calculate
if (myzigzag.CheckSpread(t+1, 0.0)) {
// data is ok to make an estimate
double V_wind_estimate=1;
double theta_wind_estimate=0;
double percent_error;
percent_error = myzigzag.StartSearch(V_wind_estimate, theta_wind_estimate);
myzigzag.Estimate(&V_wind_estimate, &theta_wind_estimate, &percent_error);
DebugStore("%2.1f %2.1f %03.0f %03.0f %2.1f # test zigzag\n",
V_wind,
V_wind_estimate,
theta_wind/DEGTORAD,
theta_wind_estimate/DEGTORAD,
percent_error
);
}
}
void
WindStore::newWind(const NMEA_INFO *nmeaInfo, DERIVED_INFO *derivedInfo,
Vector &wind)
{
//
double mag = sqrt(wind.x*wind.x+wind.y*wind.y);
double bearing;
if (wind.y == 0 && wind.x == 0)
bearing = 0;
else
bearing = atan2(wind.y, wind.x)*RAD_TO_DEG;
if (mag<30) { // limit to reasonable values
derivedInfo->WindSpeed = mag;
if (bearing<0) {
bearing += 360;
}
derivedInfo->WindBearing = bearing;
} else {
// TODO code: give warning, wind estimate bogus or very strong!
}
#ifdef DEBUG_WIND
DebugStore("%f %f 0 # wind estimate\n",wind.x,wind.y);
#endif
}
static bool WindZigZagCheckAirData(NMEA_INFO* Basic, DERIVED_INFO* Calculated)
{
static double tLast = -1;
static double bearingLast=0;
bool airdata_invalid = false;
if (!Calculated->Flying) {
airdata_invalid = true;
} else if (fabs(Calculated->TurnRate)>20.0) {
airdata_invalid = true;
#ifdef DEBUG_ZIGZAG_A
DebugStore("zigzag airdata invalid - turn rate\n");
#endif
} else if (fabs(Basic->TrueAirspeed)<GlidePolar::Vminsink*0.8) {
airdata_invalid = true;
#ifdef DEBUG_ZIGZAG_A
DebugStore("zigzag airdata invalid - true airspeed\n");
#endif
} else if (fabs(Basic->Speed)<2.5) {
airdata_invalid = true;
#ifdef DEBUG_ZIGZAG_A
DebugStore("zigzag airdata invalid - ground speed\n");
#endif
} else if (Basic->AccelerationAvailable && (fabs(Basic->AccelZ-1.0)>0.3)) {
airdata_invalid = true;
#ifdef DEBUG_ZIGZAG_A
DebugStore("zigzag airdata invalid - acceleration\n");
#endif
}
if (airdata_invalid) {
tLast = Basic->Time; // blackout for SAMPLE_RATE seconds
return false;
}
if ((Basic->Time<tLast+SAMPLE_RATE) &&
(fabs(bearingLast-Basic->TrackBearing)<10.0)) {
return false;
} else {
tLast = Basic->Time;
bearingLast = Basic->TrackBearing;
}
return true;
}
bool CheckSpread(double time, double error) {
double spread = CheckValidity(time);
if (spread<0) {
#ifdef DEBUG_ZIGZAG_A
DebugStore("zigzag time invalid\n");
#endif
return false;
}
spread /= DEGTORAD;
double minspread;
minspread = 10.0+30.0/(1.0+error*error/30.0);
// nominal spread required is 40 degrees, smaller if large
// error is detected
if ((spread>360.0)||(spread< minspread)) {
// invalid if really circling or if not enough zig-zag
#ifdef DEBUG_ZIGZAG_A
DebugStore("zigzag spread invalid %03.1f\n", spread);
#endif
return false;
}
return true;
}
void CuSondeLevel::updateThermalIndex(unsigned short level,
bool newdata) {
double hlevel = level*CUSONDE_HEIGHTSTEP;
tempDry = DALR*(hlevel-CuSonde::hGround)+CuSonde::maxGroundTemperature;
// thermal index is difference in dry temp and environmental temp
thermalIndex = airTemp-tempDry;
#ifdef DEBUG_CUSONDE
if (newdata) {
DebugStore("%g %g %g %g # temp measurement \r\n",
hlevel, airTemp, dewpoint, thermalIndex);
}
#endif
}
/**
* Calculates the ThermalIndex for the given height level
*
* ThermalIndex is the difference in dry temp and environmental temp
* at the specified altitude.
* @param level level = altitude / CUSONDE_HEIGHTSTEP
* @param newdata Function logs data to debug file if true
*/
void CuSondeLevel::updateThermalIndex(unsigned short level,
bool newdata) {
double hlevel = level*CUSONDE_HEIGHTSTEP;
// Calculate the dry temperature at altitude = hlevel
tempDry = DALR*(hlevel-CuSonde::hGround)+CuSonde::maxGroundTemperature;
// Calculate ThermalIndex
thermalIndex = airTemp-tempDry;
#ifdef DEBUG_CUSONDE
if (newdata) {
DebugStore("%g %g %g %g # temp measurement \r\n",
hlevel, airTemp, dewpoint, thermalIndex);
}
#endif
}
void WindAnalyser::slot_newEstimate(NMEA_INFO *nmeaInfo,
DERIVED_INFO *derivedInfo,
Vector a, int quality)
{
#ifdef DEBUG_WIND
const char *type;
if (quality>=6) {
type = "external wind";
} else {
type = "wind circling";
}
DebugStore("%f %f %d # %s\n", a.x, a.y, quality, type);
#endif
windstore.slot_measurement(nmeaInfo, derivedInfo, a, quality);
}
void LastThermalStats(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
static int LastCircling = FALSE;
if((Calculated->Circling == FALSE) && (LastCircling == TRUE)
&& (Calculated->ClimbStartTime>=0))
{
double ThermalTime = Calculated->CruiseStartTime
- Calculated->ClimbStartTime;
if(ThermalTime >0)
{
double ThermalGain = Calculated->CruiseStartAlt + Calculated->EnergyHeight
- Calculated->ClimbStartAlt;
if (ThermalGain>0) {
if (ThermalTime>THERMAL_TIME_MIN) {
Calculated->LastThermalAverage = ThermalGain/ThermalTime;
Calculated->LastThermalGain = ThermalGain;
Calculated->LastThermalTime = ThermalTime;
flightstats.ThermalAverage.
least_squares_update(Calculated->LastThermalAverage);
#ifdef DEBUG_STATS
DebugStore("%f %f # thermal stats\n",
flightstats.ThermalAverage.m,
flightstats.ThermalAverage.b
);
#endif
if (EnableThermalLocator) {
ThermalSources(Basic, Calculated);
}
}
}
}
}
LastCircling = Calculated->Circling;
}
void CuSonde::findCloudBase(unsigned short level) {
if (cslevels[level+1].nmeasurements==0) return;
if (cslevels[level].nmeasurements==0) return;
double dti = (cslevels[level+1].tempDry-cslevels[level+1].dewpoint)
-(cslevels[level].tempDry-cslevels[level].dewpoint);
cslevels[level].cloudBase = -1;
if (fabs(dti)<1.0e-3) {
return;
}
// ti = dlevel * dti + ti0;
// (-3 - ti0)/dti = dlevel;
LKASSERT(dti!=0);
double dlevel = -(cslevels[level].tempDry-cslevels[level].dewpoint)/dti;
double dcloudbase = (level+dlevel)*CUSONDE_HEIGHTSTEP;
if (dlevel>0.0) {
if (dlevel>1.0) {
if ((level+2<CUSONDE_NUMLEVELS)
&& (cslevels[level+2].nmeasurements>0)) {
// estimated point should be in next level.
return;
}
}
cslevels[level].cloudBase = dcloudbase;
// set the overall cloudbase to this value
cloudBase = dcloudbase;
#ifdef DEBUG_CUSONDE
DebugStore("%g # cloud base \r\n", cloudBase);
#endif
}
}
void CuSonde::findThermalHeight(unsigned short level) {
if (cslevels[level+1].nmeasurements==0) return;
if (cslevels[level].nmeasurements==0) return;
double dti = cslevels[level+1].thermalIndex - cslevels[level].thermalIndex;
cslevels[level].thermalHeight = -1;
if (fabs(dti)<1.0e-3) {
return;
}
// ti = dlevel * dti + ti0;
// (-1.6 - ti0)/dti = dlevel;
LKASSERT(dti!=0);
double dlevel = (TITHRESHOLD-cslevels[level].thermalIndex)/dti;
double dthermalheight = (level+dlevel)*CUSONDE_HEIGHTSTEP;
if (dlevel>0.0) {
if (dlevel>1.0) {
if ((level+2<CUSONDE_NUMLEVELS)
&& (cslevels[level+2].nmeasurements>0)) {
// estimated point should be in next level.
return;
}
}
cslevels[level].thermalHeight = dthermalheight;
// set the overall thermal height to this value
thermalHeight = dthermalheight;
#ifdef DEBUG_CUSONDE
DebugStore("%g # thermal height \r\n", thermalHeight);
#endif
}
}
// This is called at the end of slow calculation AirspaceWarning function, after tables have been filled up
void AirspaceWarnListProcess(NMEA_INFO *Basic, DERIVED_INFO *Calculated){
if (!InitDone) return;
LockList();
__try{
for (List<AirspaceInfo_c>::Node* it = AirspaceWarnings.begin(); it;){
it->data.TimeOut--; // age the entry
if (it->data.TimeOut < 0){
it->data.Predicted = false;
it->data.Inside = false;
it->data.WarnLevel = 0;
int hDistance = 0;
int vDistance = 0;
int Bearing = 0;
AirspaceWarnListCalcDistance(Basic, Calculated,
it->data.IsCircle,
it->data.AirspaceIndex,
&hDistance, &Bearing, &vDistance);
it->data.hDistance = hDistance; // for all: update data
it->data.vDistance = vDistance;
it->data.Bearing = Bearing;
it->data.TimeOut = OUTSIDE_CHECK_INTERVAL; // retrigger checktimeout
if (calcWarnLevel(&it->data))
AirspaceWarnListDoNotify(asaWarnLevelIncreased, &it->data);
UpdateAirspaceAckBrush(&it->data, 0);
if (it->data.Acknowledge == 4){ // whole day achnowledged
if (it->data.SortKey > 25000) {
it->data.TimeOut = 60; // JMW hardwired why?
}
continue;
}
if ((hDistance > 2500) || (abs(vDistance) > 250)){
// far away remove from warning list
AirspaceInfo_c asi = it->data;
it = AirspaceWarnings.erase(it);
UpdateAirspaceAckBrush(&asi, -1);
AirspaceWarnListDoNotify(asaItemRemoved, &asi);
continue;
} else {
if ((hDistance > 500) || (abs(vDistance) > 100)){
// close clear inside ack and auto ACK til closer
if (it->data.Acknowledge > 2)
if (--(it->data.InsideAckTimeOut) < 0){ // timeout the
it->data.Acknowledge = 1;
}
} else { // very close, just update ack timer
it->data.InsideAckTimeOut = AcknowledgementTime / OUTSIDE_CHECK_INTERVAL;
// 20sec outside check interval prevent down ACK on circling
}
}
AirspaceWarnListDoNotify(asaItemChanged, &it->data);
}
// SortTheList();
/*
// just for debugging
TCHAR szMessageBuffer[1024];
if (it->data.IsCircle){
int i = it->data.AirspaceIndex;
wsprintf(szMessageBuffer, TEXT("%20s %d %d %5d %5d"), AirspaceCircle[i].Name, it->data.Inside, it->data.Predicted, (int)it->data.hDistance, (int)it->data.vDistance);
} else {
int i = it->data.AirspaceIndex;
wsprintf(szMessageBuffer, TEXT("%20s %d %d %5d %5d"), AirspaceArea[i].Name, it->data.Inside, it->data.Predicted, (int)it->data.hDistance, (int)it->data.vDistance);
}
devPutVoice(NULL, szMessageBuffer);
*/
it = it->next;
}
AirspaceWarnListSort();
#ifdef DEBUG_AIRSPACE
DebugStore("%d # airspace\n", AirspaceWarnGetItemCount());
#endif
AirspaceWarnListDoNotify(asaProcessEnd, NULL);
}__finally {
UnLockList();
}
}
//
// This is called FORCED when changing multimap
//
void SetTopologyBounds(const RECT& rcin, const ScreenProjection& _Proj, const bool force) {
static rectObj bounds_active;
static double range_active = 1.0;
bool unchanged=false;
static double oldmapscale=0;
static short runnext=0;
rectObj bounds_screen = MapWindow::CalculateScreenBounds(1.0, rcin, _Proj);
if (!force) {
if (oldmapscale!=MapWindow::zoom.Scale()) {
#if DEBUG_STB2
StartupStore(_T("... scale changed to %f\n"),MapWindow::zoom.Scale());
#endif
oldmapscale=MapWindow::zoom.Scale();
} else
unchanged=true;
}
bool recompute = false;
// only recalculate which shapes when bounds change significantly
// need to have some trigger for this..
// trigger if the border goes outside the stored area
if (!RectangleIsInside(bounds_active, bounds_screen)) {
#if DEBUG_STB
StartupStore(_T("(recompute for out of rectangle)\n"));
#endif
recompute = true;
}
// also trigger if the scale has changed heaps
double range_real = max((bounds_screen.maxx-bounds_screen.minx),
(bounds_screen.maxy-bounds_screen.miny));
double range = max(MINRANGE,range_real);
double scale = range/range_active;
if (max(scale, 1.0/scale)>2) { // tighter threshold
#if DEBUG_STB
StartupStore(_T("(recompute for OUT OF SCALE)\n"),scale);
#endif
recompute = true;
}
if (recompute || force || LKSW_ForceNearestTopologyCalculation) {
#if DEBUG_STB
StartupStore(_T("..... Run Recompute, Force=%d LKSW=%d unchanged=%d\n"),force,LKSW_ForceNearestTopologyCalculation,unchanged);
#endif
#if 0 // 121208
// make bounds bigger than screen
if (range_real<MINRANGE) {
scale = BORDERFACTOR*MINRANGE/range_real;
} else {
// 121208 this is creating problems after a low zoom
// scale = BORDERFACTOR;
}
#endif
bounds_active = MapWindow::CalculateScreenBounds(scale, rcin, _Proj);
range_active = max((bounds_active.maxx-bounds_active.minx),
(bounds_active.maxy-bounds_active.miny));
for (int z=0; z<MAXTOPOLOGY; z++) {
if (TopoStore[z]) {
TopoStore[z]->triggerUpdateCache=true;
}
}
#if USETOPOMARKS
if (topo_marks) {
topo_marks->triggerUpdateCache = true;
}
#endif
// now update visibility of objects in the map window
MapWindow::ScanVisibility(&bounds_active);
runnext=NUMRUNS;
unchanged=false;
} else {
if (unchanged) { // nothing has changed, can we skip?
#if DEBUG_STB2
StartupStore(_T("... Nothing has changed\n"));
#endif
if (runnext<=0) {
#if DEBUG_STB2
StartupStore(_T("..... no runnext, skip\n"));
#endif
return;
} else {
#if DEBUG_STB2
StartupStore(_T("..... ONE MORE run\n"));
#endif
runnext--;
}
} else
runnext=NUMRUNS;
}
#if DEBUG_STB
StartupStore(_T("------ Run full update --- \n"));
#endif
// check if things have come into or out of scale limit
for (int z=0; z<MAXTOPOLOGY; z++) {
if (TopoStore[z]) {
TopoStore[z]->TriggerIfScaleNowVisible();
}
}
// ok, now update the caches
#if USETOPOMARKS
if (topo_marks) {
topo_marks->updateCache(bounds_active);
}
#endif
// check if any needs to have cache updates because wasnt
// visible previously when bounds moved
bool sneaked = false;
bool rta;
// we will make sure we update at least one cache per call
// to make sure eventually everything gets refreshed
int total_shapes_visible = 0;
for (int z = 0; z < MAXTOPOLOGY; z++) {
if (TopoStore[z]) {
rta = MapWindow::RenderTimeAvailable() || force || !sneaked;
if (TopoStore[z]->triggerUpdateCache) {
sneaked = true;
}
TopoStore[z]->updateCache(bounds_active, !rta);
total_shapes_visible += TopoStore[z]->shapes_visible_count;
}
}
#ifdef DEBUG_GRAPHICS
DebugStore("%d # shapes\n", total_shapes_visible);
#endif
}
int WindZigZagUpdate(NMEA_INFO* Basic, DERIVED_INFO* Calculated,
double *zzwindspeed, double *zzwindbearing) {
static double tLastEstimate = -1;
if (!Basic->AirspeedAvailable) {
return 0;
}
#if (WINDOWSPC>0)
#ifdef DEBUG_ZIGZAG
TestZigZagLoop();
#endif
#endif
// TODO accuracy: correct TAS for vertical speed if dynamic pullup
if ((Basic->Time<= tLastEstimate)||(tLastEstimate==-1)) {
tLastEstimate = Basic->Time-UPDATE_RATE;
}
if (!WindZigZagCheckAirData(Basic, Calculated)) {
return 0;
}
// ok to add a point
myzigzag.AddPoint(Basic->Time, Basic->TrueAirspeed,
Basic->Speed, Basic->TrackBearing*DEGTORAD);
#ifdef DEBUG_ZIGZAG_A
DebugStore("%f %03.0f %03.0f %03.0f # zigpoint\n",
Basic->Time,
Basic->TrueAirspeed,
Basic->Speed,
Basic->TrackBearing);
#endif
// don't update wind from zigzag more often than
// every UPDATE_RATE seconds, so it is balanced with respect
// to circling
if (Basic->Time<tLastEstimate+UPDATE_RATE) {
return 0;
}
double V_wind_estimate = Calculated->WindSpeed;
double theta_wind_estimate = Calculated->WindBearing*DEGTORAD;
double percent_error =
myzigzag.StartSearch(V_wind_estimate, theta_wind_estimate);
// Check spread of zig-zag manoeuver
if (!myzigzag.CheckSpread(Basic->Time, percent_error)) {
return 0;
}
double v_error = percent_error*Basic->TrueAirspeed/100.0;
if (v_error<0.5) {
// don't refine search if error is small
#ifdef DEBUG_ZIGZAG
DebugStore("zigzag error small %02.0f %03.1f\n",
percent_error, v_error);
#endif
return 0;
}
if (myzigzag.Estimate(&V_wind_estimate,
&theta_wind_estimate,
&percent_error)) {
// ok, we have made an update
tLastEstimate = Basic->Time;
theta_wind_estimate /= DEGTORAD;
*zzwindspeed = V_wind_estimate;
*zzwindbearing = theta_wind_estimate;
// calculate error quality
int quality;
// double pes = v_error/(V_SCALE/NUM_V_POINTS);
//quality = iround(0.5+4.5/(1.0+percent_error*percent_error/30.0));
quality = max(1,5-iround(percent_error/2));
if (Calculated->Circling) {
quality = max(1,quality/2); // de-value updates in circling mode
}
#ifdef DEBUG_ZIGZAG
DebugStore("%f %3.1f %03.0f %3.1f %03.0f %f %d # zigzag\n",
Basic->Time,
V_wind_estimate,
theta_wind_estimate,
Calculated->WindSpeed,
Calculated->WindBearing,
percent_error,
quality);
#endif
return quality;
} else {
#ifdef DEBUG_ZIGZAG
DebugStore("zigzag estimate failed to improve\n");
#endif
}
return 0;
}