void DoNearestAlternate(NMEA_INFO *Basic, DERIVED_INFO *Calculated, int AltWaypoint) { #if BUGSTOP LKASSERT(ValidWayPoint(AltWaypoint)); #endif if (!ValidWayPoint(AltWaypoint)) return; double *altwp_gr = &WayPointCalc[AltWaypoint].GR; double *altwp_arrival = &WayPointCalc[AltWaypoint].AltArriv[AltArrivMode]; *altwp_gr = CalculateGlideRatio( WayPointCalc[AltWaypoint].Distance, Calculated->NavAltitude - WayPointList[AltWaypoint].Altitude - GetSafetyAltitude(AltWaypoint)); *altwp_arrival = CalculateWaypointArrivalAltitude(Basic, Calculated, AltWaypoint); WayPointCalc[AltWaypoint].VGR = GetVisualGlideRatio(*altwp_arrival, *altwp_gr); }
/* * Used by Alternates and BestAlternate * Colors VGR are used by DrawNearest &c. */ void DoAlternates(NMEA_INFO *Basic, DERIVED_INFO *Calculated, int AltWaypoint) { CScopeLock(LockTaskData, UnlockTaskData); #ifdef GTL2 // If flying an AAT and working on the RESWP_OPTIMIZED waypoint, then use // this "optimized" waypoint to store data for the AAT virtual waypoint. if ((AltWaypoint == RESWP_OPTIMIZED) && (!ISPARAGLIDER || (AATEnabled && !DoOptimizeRoute()))) { WayPointList[RESWP_OPTIMIZED].Latitude = Task[ActiveWayPoint].AATTargetLat; WayPointList[RESWP_OPTIMIZED].Longitude = Task[ActiveWayPoint].AATTargetLon; WayPointList[RESWP_OPTIMIZED].Altitude = WayPointList[Task[ActiveWayPoint].Index].Altitude; WaypointAltitudeFromTerrain(&WayPointList[RESWP_OPTIMIZED]); _stprintf(WayPointList[RESWP_OPTIMIZED].Name, _T("!%s"),WayPointList[Task[ActiveWayPoint].Index].Name); } #endif // handle virtual wps as alternates if (AltWaypoint<=RESWP_END) { if (!ValidResWayPoint(AltWaypoint)) return; } else { if (!ValidWayPoint(AltWaypoint)) return; } double *altwp_dist = &WayPointCalc[AltWaypoint].Distance; double *altwp_gr = &WayPointCalc[AltWaypoint].GR; double *altwp_arrival = &WayPointCalc[AltWaypoint].AltArriv[AltArrivMode]; DistanceBearing(WayPointList[AltWaypoint].Latitude, WayPointList[AltWaypoint].Longitude, Basic->Latitude, Basic->Longitude, altwp_dist, NULL); *altwp_gr = CalculateGlideRatio( *altwp_dist, Calculated->NavAltitude - WayPointList[AltWaypoint].Altitude - GetSafetyAltitude(AltWaypoint)); // We need to calculate arrival also for BestAlternate, since the last "reachable" could be // even 60 seconds old and things may have changed drastically *altwp_arrival = CalculateWaypointArrivalAltitude(Basic, Calculated, AltWaypoint); WayPointCalc[AltWaypoint].VGR = GetVisualGlideRatio(*altwp_arrival, *altwp_gr); }
void MapWindow::LKCalculateWaypointReachable(const bool forced) { #if USEONEHZLIMITER if (!forced) ONEHZLIMITER; #endif static short multicalc_slot=0; // -1 (which becomes immediately 0) will force full loading on startup, but this is not good // because currently we are not waiting for ProgramStarted=3 // and the first scan is made while still initializing other things short numslots=1; // TODO assign numslots with a function, based also on available CPU time if (NumberOfWayPoints>200) { numslots=NumberOfWayPoints/400; // keep numslots optimal if (numslots<MULTICALC_MINROBIN) numslots=MULTICALC_MINROBIN; // seconds for full scan, as this is executed at 1Hz if (numslots>MULTICALC_MAXROBIN) numslots=MULTICALC_MAXROBIN; // When waypointnumber has changed, we wont be using an exceeded multicalc_slot, which would crash the sw // In this case, we shall probably continue for the first round to calculate without going from the beginning // but this is not a problem, we are round-robin all the time here. if (++multicalc_slot>numslots) multicalc_slot=1; } else { multicalc_slot=0; // forcing full scan } unsigned int i; double waypointDistance, waypointBearing,altitudeRequired,altitudeDifference; // LandableReachable is used only by the thermal bar indicator in MapWindow2, after here // apparently, is used to tell you if you are below final glide but in range for a landable wp // Since nov 2011 we dont user LandableReachable in FinalGlide anymore. // However it is still to be understood what drawbacks we might have by changing calculations here. LandableReachable = false; if (!WayPointList) return; unsigned int scanstart; unsigned int scanend; #if DEBUGCW unsigned int numwpscanned=0; #endif LockTaskData(); if (multicalc_slot==0) { scanstart=0; // including this scanend=NumberOfWayPoints; // will be used -1, so up to this excluded value #if DEBUGCW StartupStore(_T("... wps=%d multicalc_slot=0 ignored numslot=%d, full scan %d < %d%s"),NumberOfWayPoints, numslots,scanstart,scanend,NEWLINE); #endif } else { scanstart=(NumberOfWayPoints/numslots)*(multicalc_slot-1); if (multicalc_slot==numslots) scanend=NumberOfWayPoints; else scanend=scanstart+(NumberOfWayPoints/numslots); #if DEBUGCW StartupStore(_T("... wps=%d multicalc_slot=%d of %d, scan %d < %d%s"),NumberOfWayPoints, multicalc_slot, numslots,scanstart,scanend,NEWLINE); #endif } int overtarg=GetOvertargetIndex(); if (overtarg<0) overtarg=999999; for(i=scanstart;i<scanend;i++) { // signed Overtgarget -1 becomes a very high number, casted unsigned if ( ( ((WayPointCalc[i].AltArriv[AltArrivMode] >=0)||(WayPointList[i].Visible)) && (WayPointCalc[i].IsLandable || (WayPointList[i].Style==STYLE_THERMAL))) || WaypointInTask(i) || (i==(unsigned int)overtarg) ) { DistanceBearing(DrawInfo.Latitude, DrawInfo.Longitude, WayPointList[i].Latitude, WayPointList[i].Longitude, &waypointDistance, &waypointBearing); WayPointCalc[i].Distance=waypointDistance; WayPointCalc[i].Bearing=waypointBearing; CalculateGlideRatio(waypointDistance, DerivedDrawInfo.NavAltitude - WayPointList[i].Altitude - GetSafetyAltitude(i)); altitudeRequired = GlidePolar::MacCreadyAltitude (GetMacCready(i,0), waypointDistance, waypointBearing, DerivedDrawInfo.WindSpeed, DerivedDrawInfo.WindBearing, 0,0,true,0) + WayPointList[i].Altitude + GetSafetyAltitude(i) - DerivedDrawInfo.EnergyHeight; WayPointCalc[i].AltReqd[AltArrivMode] = altitudeRequired; WayPointList[i].AltArivalAGL = DerivedDrawInfo.NavAltitude - altitudeRequired; if(WayPointList[i].AltArivalAGL >=0){ WayPointList[i].Reachable = TRUE; if (CheckLandableReachableTerrainNew(&DrawInfo, &DerivedDrawInfo, waypointDistance, waypointBearing)) { if ((signed)i!=TASKINDEX) { LandableReachable = true; } } else { WayPointList[i].Reachable = FALSE; } } else { WayPointList[i].Reachable = FALSE; } #if DEBUGCW numwpscanned++; #endif } // if landable or in task } // for all waypoints // This is wrong, because multicalc will not necessarily find the LandableReachable at each pass // As of nov 2011 it is better not to change it, and let further investigation after 3.0 if (!LandableReachable) // indentation wrong here for(i=scanstart;i<scanend;i++) { if(!WayPointList[i].Visible && WayPointList[i].FarVisible) { // visible but only at a distance (limit this to 100km radius) if( WayPointCalc[i].IsLandable ) { #if DEBUGCW numwpscanned++; #endif DistanceBearing(DrawInfo.Latitude, DrawInfo.Longitude, WayPointList[i].Latitude, WayPointList[i].Longitude, &waypointDistance, &waypointBearing); WayPointCalc[i].Distance=waypointDistance; // VENTA6 WayPointCalc[i].Bearing=waypointBearing; if (waypointDistance<100000.0) { altitudeRequired = GlidePolar::MacCreadyAltitude (GetMacCready(i,0), waypointDistance, waypointBearing, // 091221 DerivedDrawInfo.WindSpeed, DerivedDrawInfo.WindBearing, 0,0,true,0) + WayPointList[i].Altitude + GetSafetyAltitude(i); altitudeDifference = DerivedDrawInfo.NavAltitude + DerivedDrawInfo.EnergyHeight - altitudeRequired; WayPointList[i].AltArivalAGL = altitudeDifference; WayPointCalc[i].AltReqd[AltArrivMode] = altitudeRequired; if(altitudeDifference >=0){ WayPointList[i].Reachable = TRUE; if (CheckLandableReachableTerrainNew(&DrawInfo, &DerivedDrawInfo, waypointDistance, waypointBearing)) { LandableReachable = true; } else WayPointList[i].Reachable = FALSE; } else { WayPointList[i].Reachable = FALSE; } } else { WayPointList[i].Reachable = FALSE; } // <100000 } // landable wp } // visible or far visible } // for all waypoint UnlockTaskData(); #if DEBUGCW StartupStore(_T("...... processed wps: %d\n"),numwpscanned); #endif }
// This is also called by DoNearest and it is overwriting AltitudeRequired double CalculateWaypointArrivalAltitude(NMEA_INFO *Basic, DERIVED_INFO *Calculated, int i) { CScopeLock Lock(LockTaskData, UnlockTaskData); double altReqd; double wDistance, wBearing; double wStartDistance=0, wStartBearing=0; double safetyaltitudearrival; safetyaltitudearrival=GetSafetyAltitude(i); DistanceBearing(Basic->Latitude, Basic->Longitude, WayPointList[i].Latitude, WayPointList[i].Longitude, &wDistance, &wBearing); WayPointCalc[i].Distance = wDistance; WayPointCalc[i].Bearing = wBearing; if (ISCAR) { simpleETE(Basic,Calculated,i); return (Basic->Altitude-WayPointList[i].Altitude); } altReqd = GlidePolar::MacCreadyAltitude ( GetMacCready(i,GMC_DEFAULT), wDistance, wBearing, Calculated->WindSpeed, Calculated->WindBearing, 0, 0, true, &WayPointCalc[i].NextETE); // if gates are in use with a real task, and we are at start // then calculate ETE for reaching the cylinder. Also working when we are // in the wrong side of cylinder if (UseGates() && !DoOptimizeRoute()) { if (ActiveTaskPoint==0 && i==Task[0].Index ) { if (PGStartOut) { if (CorrectSide()) { // start out, from outside cylinder wStartDistance=wDistance-StartRadius; wStartBearing=wBearing; } else { // start out, but inside cylinder wStartDistance=StartRadius-wDistance; wStartBearing=wBearing+180; if (wStartBearing>360) wStartBearing-=360; } } else { if (CorrectSide()) { // start in, correct side is inside cylinder wStartDistance=StartRadius-wDistance; wStartBearing=wBearing+180; if (wStartBearing>360) wStartBearing-=360; } else { // start in, and we are still outside wStartDistance=wDistance-StartRadius; wStartBearing=wBearing; } } // we don't use GetMacCready(i,GMC_DEFAULT) GlidePolar::MacCreadyAltitude ( MACCREADY, wStartDistance, wStartBearing, Calculated->WindSpeed, Calculated->WindBearing, 0, 0, true, &WayPointCalc[i].NextETE); #ifdef DEBUGTGATES StartupStore(_T("wStartDistance=%f wStartBearing=%f\n"),wStartDistance,wStartBearing); #endif } } // we should build a function for this since it is used also in lkcalc WayPointCalc[i].AltReqd[AltArrivMode] = altReqd+safetyaltitudearrival+WayPointList[i].Altitude -Calculated->EnergyHeight; WayPointCalc[i].AltArriv[AltArrivMode] = Calculated->NavAltitude + Calculated->EnergyHeight - altReqd - WayPointList[i].Altitude - safetyaltitudearrival; /* WayPointCalc[i].AltArriv[ALTA_AVEFF] = Calculated->NavAltitude - (wDistance / GetCurrentEfficiency(Calculated, 0)) - WayPointList[i].Altitude -safetyaltitudearrival; WayPointCalc[i].AltReqd[ALTA_AVEFF] = Calculated->NavAltitude - WayPointCalc[i].AltArriv[ALTA_AVEFF]; WayPointCalc[i].NextETE=600.0; */ // for GA recalculate simple ETE if (ISGAAIRCRAFT) { simpleETE(Basic,Calculated,i); } return(WayPointCalc[i].AltArriv[AltArrivMode]); }