// Current Waypoint calculations for task (no safety?) called only once at beginning // of DoCalculations, using MACCREADY void AltitudeRequired(NMEA_INFO *Basic, DERIVED_INFO *Calculated, const double this_maccready) { // LockFlightData(); (void)Basic; LockTaskData(); if(ValidTaskPoint(ActiveWayPoint)) { int index; double wp_alt = FAIFinishHeight(Basic, Calculated, ActiveWayPoint); double height_above_wp = Calculated->NavAltitude + Calculated->EnergyHeight - wp_alt; Calculated->NextAltitudeRequired = GlidePolar::MacCreadyAltitude(this_maccready, Calculated->WaypointDistance, Calculated->WaypointBearing, Calculated->WindSpeed, Calculated->WindBearing, 0, 0, true, NULL, height_above_wp, CRUISE_EFFICIENCY ); if (this_maccready==0 ) Calculated->NextAltitudeRequired0=Calculated->NextAltitudeRequired; else Calculated->NextAltitudeRequired0 = GlidePolar::MacCreadyAltitude(0, Calculated->WaypointDistance, Calculated->WaypointBearing, Calculated->WindSpeed, Calculated->WindBearing, 0, 0, true, NULL, height_above_wp, CRUISE_EFFICIENCY ); Calculated->NextAltitudeRequired += wp_alt; Calculated->NextAltitudeRequired0 += wp_alt; // VENTA6 Calculated->NextAltitudeDifference = Calculated->NavAltitude + Calculated->EnergyHeight - Calculated->NextAltitudeRequired; Calculated->NextAltitudeDifference0 = Calculated->NavAltitude + Calculated->EnergyHeight - Calculated->NextAltitudeRequired0; // We set values only for current destination active waypoint. index=TASKINDEX; WayPointCalc[index].AltArriv[ALTA_MC]=1111.0; WayPointCalc[index].AltArriv[ALTA_SMC]=2222.0; // FIX 091012 WayPointCalc[index].AltArriv[ALTA_MC0]=3333.0; WayPointCalc[index].AltArriv[ALTA_AVEFF]=1234.0; } else { Calculated->NextAltitudeRequired = 0; Calculated->NextAltitudeDifference = 0; Calculated->NextAltitudeDifference0 = 0; // VENTA6 } UnlockTaskData(); // UnlockFlightData(); }
double SpeedHeight(NMEA_INFO *Basic, DERIVED_INFO *Calculated) { (void)Basic; if (Calculated->TaskDistanceToGo<=0) { return 0; } // Fraction of task distance covered double d_fraction = Calculated->TaskDistanceCovered/ (Calculated->TaskDistanceCovered+Calculated->TaskDistanceToGo); double dh_start = Calculated->TaskStartAltitude; double dh_finish = FAIFinishHeight(Basic, Calculated, -1); // Excess height return Calculated->NavAltitude - (dh_start*(1.0-d_fraction)+dh_finish*(d_fraction)); }
void TaskStatistics(NMEA_INFO *Basic, DERIVED_INFO *Calculated, const double this_maccready) { if (!ValidTaskPoint(ActiveWayPoint) || ((ActiveWayPoint>0) && !ValidTaskPoint(ActiveWayPoint-1))) { Calculated->LegSpeed = 0; Calculated->LegDistanceToGo = 0; Calculated->LegDistanceCovered = 0; Calculated->LegTimeToGo = 0; if (!AATEnabled) { Calculated->AATTimeToGo = 0; } // Calculated->TaskSpeed = 0; Calculated->TaskDistanceToGo = 0; Calculated->TaskDistanceCovered = 0; Calculated->TaskTimeToGo = 0; Calculated->LKTaskETE = 0; Calculated->TaskTimeToGoTurningNow = -1; Calculated->TaskAltitudeRequired = 0; Calculated->TaskAltitudeDifference = 0; Calculated->TaskAltitudeDifference0 = 0; Calculated->TaskAltitudeArrival = 0; Calculated->TerrainWarningLatitude = 0.0; Calculated->TerrainWarningLongitude = 0.0; Calculated->GRFinish = INVALID_GR; Calculated->FinalGlide = false; CheckGlideThroughTerrain(Basic, Calculated); // BUGFIX 091123 // no task selected, so work things out at current heading GlidePolar::MacCreadyAltitude(this_maccready, 100.0, Basic->TrackBearing, Calculated->WindSpeed, Calculated->WindBearing, &(Calculated->BestCruiseTrack), &(Calculated->VMacCready), (Calculated->FinalGlide==true), NULL, 1.0e6, CRUISE_EFFICIENCY); return; } // LockFlightData(); LockTaskData(); // Calculate Task Distances // First calculate distances for this waypoint double LegCovered, LegToGo=0, LegXTD=0, LegCurrentCourse; double LegDistance, LegBearing=0; bool calc_turning_now; double w1lat; double w1lon; double w0lat; double w0lon; if (AATEnabled && (ActiveWayPoint>0) && (ValidTaskPoint(ActiveWayPoint))) { w1lat = Task[ActiveWayPoint].AATTargetLat; w1lon = Task[ActiveWayPoint].AATTargetLon; } else { w1lat = WayPointList[TASKINDEX].Latitude; w1lon = WayPointList[TASKINDEX].Longitude; } DistanceBearing(Basic->Latitude, Basic->Longitude, w1lat, w1lon, &LegToGo, &LegBearing); if (AATEnabled && (ActiveWayPoint>0) && ValidTaskPoint(ActiveWayPoint+1) && Calculated->IsInSector && (this_maccready>0.1) ) { calc_turning_now = true; } else { calc_turning_now = false; } if (ActiveWayPoint<1) { LegCovered = 0; LegCurrentCourse=LegBearing; if (ValidTaskPoint(ActiveWayPoint+1)) { // BUGFIX 091221 LegToGo=0; } } else { if (AATEnabled) { LKASSERT((ActiveWayPoint-1)>=0); // TODO accuracy: Get best range point to here... w0lat = Task[ActiveWayPoint-1].AATTargetLat; w0lon = Task[ActiveWayPoint-1].AATTargetLon; } else { LKASSERT((ActiveWayPoint-1)>=0); LKASSERT(ValidTaskPoint(ActiveWayPoint-1)); w0lat = WayPointList[Task[ActiveWayPoint-1].Index].Latitude; w0lon = WayPointList[Task[ActiveWayPoint-1].Index].Longitude; } DistanceBearing(w1lat, w1lon, w0lat, w0lon, &LegDistance, NULL); LegCovered = ProjectedDistance(w0lon, w0lat, w1lon, w1lat, Basic->Longitude, Basic->Latitude, &LegXTD, &LegCurrentCourse); if ((StartLine==0) && (ActiveWayPoint==1)) { // Correct speed calculations for radius // JMW TODO accuracy: legcovered replace this with more accurate version // LegDistance -= StartRadius; LegCovered = max(0.0, LegCovered-StartRadius); } } Calculated->LegDistanceToGo = LegToGo; Calculated->LegDistanceCovered = LegCovered; Calculated->LegCrossTrackError = LegXTD; Calculated->LegActualTrueCourse = LegCurrentCourse; Calculated->TaskDistanceCovered = LegCovered; if (Basic->Time > Calculated->LegStartTime) { if (flightstats.LegStartTime[ActiveWayPoint]<0) { flightstats.LegStartTime[ActiveWayPoint] = Basic->Time; } Calculated->LegSpeed = Calculated->LegDistanceCovered / (Basic->Time - Calculated->LegStartTime); } // Now add distances for start to previous waypoint if (!AATEnabled) { for(int i=0;i< ActiveWayPoint-1; i++) { if (!ValidTaskPoint(i) || !ValidTaskPoint(i+1)) continue; w1lat = WayPointList[Task[i].Index].Latitude; w1lon = WayPointList[Task[i].Index].Longitude; w0lat = WayPointList[Task[i+1].Index].Latitude; w0lon = WayPointList[Task[i+1].Index].Longitude; DistanceBearing(w1lat, w1lon, w0lat, w0lon, &LegDistance, NULL); Calculated->TaskDistanceCovered += LegDistance; } } else if (ActiveWayPoint>0) { // JMW added correction for distance covered Calculated->TaskDistanceCovered = aatdistance.DistanceCovered(Basic->Longitude, Basic->Latitude, ActiveWayPoint); } CheckTransitionFinalGlide(Basic, Calculated); // accumulators double TaskAltitudeRequired = 0; double TaskAltitudeRequired0 = 0; Calculated->TaskDistanceToGo = 0; Calculated->TaskTimeToGo = 0; Calculated->LKTaskETE = 0; Calculated->TaskTimeToGoTurningNow = 0; Calculated->TaskAltitudeArrival = 0; double LegTime0; // Calculate Final Glide To Finish int FinalWayPoint = getFinalWaypoint(); double final_height = FAIFinishHeight(Basic, Calculated, -1); double total_energy_height = Calculated->NavAltitude + Calculated->EnergyHeight; double height_above_finish = total_energy_height - final_height; if (ISPARAGLIDER) { TaskAltitudeRequired = final_height; TaskAltitudeRequired0 = final_height; } // Now add it for remaining waypoints int task_index= FinalWayPoint; double StartBestCruiseTrack = -1; while ((task_index>ActiveWayPoint) && (ValidTaskPoint(task_index))) { double this_LegTimeToGo; bool this_is_final = (task_index==FinalWayPoint) || ForceFinalGlide; this_is_final = true; // JMW CHECK FGAMT if (AATEnabled) { w1lat = Task[task_index].AATTargetLat; w1lon = Task[task_index].AATTargetLon; w0lat = Task[task_index-1].AATTargetLat; w0lon = Task[task_index-1].AATTargetLon; } else { w1lat = WayPointList[Task[task_index].Index].Latitude; w1lon = WayPointList[Task[task_index].Index].Longitude; w0lat = WayPointList[Task[task_index-1].Index].Latitude; w0lon = WayPointList[Task[task_index-1].Index].Longitude; } double NextLegDistance, NextLegBearing; DistanceBearing(w0lat, w0lon, w1lat, w1lon, &NextLegDistance, &NextLegBearing); double LegAltitude = GlidePolar:: MacCreadyAltitude(this_maccready, NextLegDistance, NextLegBearing, Calculated->WindSpeed, Calculated->WindBearing, 0, 0, this_is_final, &this_LegTimeToGo, height_above_finish, CRUISE_EFFICIENCY); double LegAltitude0 = GlidePolar:: MacCreadyAltitude(0, NextLegDistance, NextLegBearing, Calculated->WindSpeed, Calculated->WindBearing, 0, 0, true, &LegTime0, 1.0e6, CRUISE_EFFICIENCY ); if (LegTime0>=0.9*ERROR_TIME) { // can't make it, so assume flying at current mc LegAltitude0 = LegAltitude; } TaskAltitudeRequired += LegAltitude; TaskAltitudeRequired0 += LegAltitude0; if(ISPARAGLIDER) { // if required altitude is less than previous turpoint altitude, // use previous turn point altitude double w0Alt = FAIFinishHeight(Basic, Calculated, task_index-1); if(TaskAltitudeRequired < w0Alt) { Calculated->TaskAltitudeArrival += w0Alt - TaskAltitudeRequired; TaskAltitudeRequired = w0Alt; } if(TaskAltitudeRequired0 < w0Alt) { TaskAltitudeRequired0 = w0Alt; } } Calculated->TaskDistanceToGo += NextLegDistance; Calculated->TaskTimeToGo += this_LegTimeToGo; if (task_index==1) { StartBestCruiseTrack = NextLegBearing; } if (calc_turning_now) { if (task_index == ActiveWayPoint+1) { double NextLegDistanceTurningNow, NextLegBearingTurningNow; double this_LegTimeToGo_turningnow=0; DistanceBearing(Basic->Latitude, Basic->Longitude, w1lat, w1lon, &NextLegDistanceTurningNow, &NextLegBearingTurningNow); GlidePolar:: MacCreadyAltitude(this_maccready, NextLegDistanceTurningNow, NextLegBearingTurningNow, Calculated->WindSpeed, Calculated->WindBearing, 0, 0, this_is_final, &this_LegTimeToGo_turningnow, height_above_finish, CRUISE_EFFICIENCY); Calculated->TaskTimeToGoTurningNow += this_LegTimeToGo_turningnow; } else { Calculated->TaskTimeToGoTurningNow += this_LegTimeToGo; } } height_above_finish-= LegAltitude; task_index--; } // current waypoint, do this last! if (AATEnabled && (ActiveWayPoint>0) && ValidTaskPoint(ActiveWayPoint+1) && Calculated->IsInSector) { if (Calculated->WaypointDistance<AATCloseDistance()*3.0) { LegBearing = AATCloseBearing(Basic, Calculated); } } #ifdef BCT_ALT_FIX // Don't calculate BCT yet. LegAltitude will be used to calculate // task altitude difference, which will then be used to calculate BCT. #endif double LegAltitude = GlidePolar::MacCreadyAltitude(this_maccready, LegToGo, LegBearing, Calculated->WindSpeed, Calculated->WindBearing, #ifdef BCT_ALT_FIX 0, #else &(Calculated->BestCruiseTrack), #endif &(Calculated->VMacCready), // (Calculated->FinalGlide==1), true, // JMW CHECK FGAMT &(Calculated->LegTimeToGo), height_above_finish, CRUISE_EFFICIENCY); double LegAltitude0 = GlidePolar::MacCreadyAltitude(0, LegToGo, LegBearing, Calculated->WindSpeed, Calculated->WindBearing, 0, 0, true, &LegTime0, 1.0e6, CRUISE_EFFICIENCY ); #ifndef BCT_ALT_FIX // fix problem of blue arrow wrong in task sector if (StartBestCruiseTrack>=0) // use it only if assigned, workaround if (Calculated->IsInSector && (ActiveWayPoint==0)) { // set best cruise track to first leg bearing when in start sector Calculated->BestCruiseTrack = StartBestCruiseTrack; } #endif // JMW TODO accuracy: Use safetymc where appropriate if (LegTime0>= 0.9*ERROR_TIME) { // can't make it, so assume flying at current mc LegAltitude0 = LegAltitude; } TaskAltitudeRequired += LegAltitude; TaskAltitudeRequired0 += LegAltitude0; Calculated->TaskDistanceToGo += LegToGo; Calculated->TaskTimeToGo += Calculated->LegTimeToGo; #ifndef BCT_ALT_FIX height_above_finish-= LegAltitude; #endif if (calc_turning_now) { Calculated->TaskTimeToGoTurningNow += Basic->Time-Calculated->TaskStartTime; } else { Calculated->TaskTimeToGoTurningNow = -1; } if (ISPARAGLIDER) { Calculated->TaskAltitudeRequired = TaskAltitudeRequired; } else { Calculated->TaskAltitudeRequired = TaskAltitudeRequired + final_height; TaskAltitudeRequired0 += final_height; } Calculated->TaskAltitudeDifference = total_energy_height - Calculated->TaskAltitudeRequired; Calculated->TaskAltitudeDifference0 = total_energy_height - TaskAltitudeRequired0; Calculated->NextAltitudeDifference0 = total_energy_height - Calculated->NextAltitudeRequired0; Calculated->TaskAltitudeArrival += Calculated->TaskAltitudeDifference; Calculated->GRFinish= CalculateGlideRatio(Calculated->TaskDistanceToGo, Calculated->NavAltitude - final_height); if (Calculated->TaskSpeedAchieved >0) Calculated->LKTaskETE = Calculated->TaskDistanceToGo/Calculated->TaskSpeedAchieved; else Calculated->LKTaskETE=0; #ifdef BCT_ALT_FIX // This MCA call's only purpose is to update BestCruiseTrack (BCT). // It must occur after TaskAltitudeDifference (TAD) is updated, // since BCT depends on TAD. GlidePolar::MacCreadyAltitude(this_maccready, LegToGo, LegBearing, Calculated->WindSpeed, Calculated->WindBearing, &(Calculated->BestCruiseTrack), 0, true, 0, height_above_finish, CRUISE_EFFICIENCY, Calculated->TaskAltitudeDifference); // fix problem of blue arrow wrong in task sector if (StartBestCruiseTrack>=0) // use it only if assigned, workaround if (Calculated->IsInSector && (ActiveWayPoint==0)) { // set best cruise track to first leg bearing when in start sector Calculated->BestCruiseTrack = StartBestCruiseTrack; } height_above_finish-= LegAltitude; #endif CheckGlideThroughTerrain(Basic, Calculated); CheckForceFinalGlide(Basic, Calculated); UnlockTaskData(); }
void DoAutoMacCready(NMEA_INFO *Basic, DERIVED_INFO *Calculated) { if (!Calculated->AutoMacCready) return; bool is_final_glide = false; bool is_conical_ess = false; double ConeSlope = 0.0; // LockFlightData(); LockTaskData(); double mc_new = MACCREADY; static bool first_mc = true; if (AutoMcMode == amcEquivalent) { if ((!Calculated->Circling) && (!Calculated->OnGround)) { if (Calculated->EqMc >= 0) { // MACCREADY = LowPassFilter(MACCREADY,Calculated->EqMc,0.8); CheckSetMACCREADY(Calculated->EqMc); } else { // -1.0 is used as an invalid flag. Normally flying at -1 MC means almost flying // at stall speed, which is pretty unusual. Maybe in wave conditions? if (Calculated->EqMc >-1) { CheckSetMACCREADY(Calculated->EqMc*-1); } } } UnlockTaskData(); return; } // otherwise, if AutoMc for finalglide or "both", return if no goto if (ValidTaskPoint(ActiveWayPoint)) { if (Calculated->FinalGlide && ActiveIsFinalWaypoint()) { is_final_glide = true; } else { first_mc = true; } if (DoOptimizeRoute() && Calculated->NextAltitude > 0.) { // Special case for Conical end of Speed section int Type = -1; GetTaskSectorParameter(ActiveWayPoint, &Type, NULL); ConeSlope = Task[ActiveWayPoint].PGConeSlope; if (Type == CONE && ConeSlope > 0.0) { is_final_glide = true; is_conical_ess = true; } } } double av_thermal = -1; if (flightstats.ThermalAverage.y_ave > 0) { if (Calculated->Circling && (Calculated->AverageThermal > 0)) { #if BUGSTOP LKASSERT((flightstats.ThermalAverage.sum_n + 1) != 0); #endif if (flightstats.ThermalAverage.sum_n == -1) { flightstats.ThermalAverage.sum_n = -0.99; } av_thermal = (flightstats.ThermalAverage.y_ave * flightstats.ThermalAverage.sum_n + Calculated->AverageThermal) / (flightstats.ThermalAverage.sum_n + 1); } else { av_thermal = flightstats.ThermalAverage.y_ave; } } else if (Calculated->Circling && (Calculated->AverageThermal > 0)) { // insufficient stats, so use this/last thermal's average av_thermal = Calculated->AverageThermal; } if (!ValidTaskPoint(ActiveWayPoint)) { if (av_thermal > 0) { mc_new = av_thermal; } else { mc_new = 0; } } else if (((AutoMcMode == amcFinalGlide) || (AutoMcMode == amcFinalAndClimb)) && is_final_glide) { if (Calculated->TaskAltitudeDifference0 > 0) { // only change if above final glide with zero Mc // otherwise when we are well below, it will wind Mc back to // zero #if BUGSTOP LKASSERT((Calculated->WaypointDistance + 1) != 0); #endif if (Calculated->WaypointDistance < 0) Calculated->WaypointDistance = 0; // temporary but ok double slope = (Calculated->NavAltitude + Calculated->EnergyHeight - FAIFinishHeight(Basic, Calculated, ActiveWayPoint)) / (Calculated->WaypointDistance + 1); double mc_pirker = PirkerAnalysis(Basic, Calculated, Calculated->WaypointBearing, slope); mc_pirker = max(0.0, mc_pirker); if (first_mc) { // don't allow Mc to wind down to zero when first achieving // final glide; but do allow it to wind down after that if (mc_pirker >= mc_new) { mc_new = mc_pirker; first_mc = false; } else if (AutoMcMode == amcFinalAndClimb) { // revert to averager based auto Mc if (av_thermal > 0) { mc_new = av_thermal; } } } else { mc_new = mc_pirker; } if (is_conical_ess) { const double VOpt = GlidePolar::FindSpeedForSlope(ConeSlope); const double eqMC = GlidePolar::EquMC(VOpt); if(mc_new > eqMC) { mc_new = eqMC; } } } else { // below final glide at zero Mc, never achieved final glide if (first_mc && (AutoMcMode == amcFinalAndClimb)) { // revert to averager based auto Mc if (av_thermal > 0) { mc_new = av_thermal; } } } } else if ((AutoMcMode == amcAverageClimb) || ((AutoMcMode == amcFinalAndClimb)&& !is_final_glide)) { if (av_thermal > 0) { mc_new = av_thermal; } } CheckSetMACCREADY(LowPassFilter(MACCREADY, mc_new, 0.6)); UnlockTaskData(); // UnlockFlightData(); }
bool TaskAltitudeRequired(NMEA_INFO *Basic, DERIVED_INFO *Calculated, double this_maccready, double *Vfinal, double *TotalTime, double *TotalDistance, int *ifinal) { int i; double w1lat; double w1lon; double w0lat; double w0lon; double LegTime, LegDistance, LegBearing, LegAltitude; bool retval = false; // Calculate altitude required from start of task bool isfinal=true; LegAltitude = 0; double TotalAltitude = 0; *TotalTime = 0; *TotalDistance = 0; *ifinal = 0; LockTaskData(); double heightFinal = FAIFinishHeight(Basic, Calculated, -1); double height_above_finish = FAIFinishHeight(Basic, Calculated, 0) - heightFinal; for(i=MAXTASKPOINTS-2;i>=0;i--) { if (!ValidTaskPoint(i) || !ValidTaskPoint(i+1)) continue; w1lat = WayPointList[Task[i].Index].Latitude; w1lon = WayPointList[Task[i].Index].Longitude; w0lat = WayPointList[Task[i+1].Index].Latitude; w0lon = WayPointList[Task[i+1].Index].Longitude; if (AATEnabled) { w1lat = Task[i].AATTargetLat; w1lon = Task[i].AATTargetLon; // also use optimized finish point for PG optimized task. if (!isfinal || DoOptimizeRoute()) { w0lat = Task[i+1].AATTargetLat; w0lon = Task[i+1].AATTargetLon; } } DistanceBearing(w1lat, w1lon, w0lat, w0lon, &LegDistance, &LegBearing); *TotalDistance += LegDistance; LegAltitude = GlidePolar::MacCreadyAltitude(this_maccready, LegDistance, LegBearing, Calculated->WindSpeed, Calculated->WindBearing, 0, 0, true, &LegTime, height_above_finish, CRUISE_EFFICIENCY ); // JMW CHECK FGAMT height_above_finish-= LegAltitude; TotalAltitude += LegAltitude; if( ISPARAGLIDER ) { // if required altitude is less than previous turpoint altitude, // use previous turn point altitude double w1Alt = FAIFinishHeight(Basic, Calculated, i); if( (TotalAltitude+heightFinal) < w1Alt ) { TotalAltitude = w1Alt; } } if (LegTime<0) { retval = false; goto OnExit; } else { *TotalTime += LegTime; } if (isfinal) { *ifinal = i+1; if (LegTime>0) { *Vfinal = LegDistance/LegTime; } } isfinal = false; } if (*ifinal==0) { retval = false; goto OnExit; } TotalAltitude += FAIFinishHeight(Basic, Calculated, -1); if (!ValidTaskPoint(*ifinal)) { Calculated->TaskAltitudeRequiredFromStart = TotalAltitude; retval = false; } else { Calculated->TaskAltitudeRequiredFromStart = TotalAltitude; retval = true; } OnExit: UnlockTaskData(); return retval; }
static void UpdateValuesRules(void) { WndProperty *wp; TCHAR Temp[80]; wp = (WndProperty*)wf->FindByName(TEXT("prpValidStart")); if (wp) { if (CALCULATED_INFO.ValidStart) { // LKTOKEN _@M677_ = "TRUE" wp->SetText(gettext(TEXT("_@M677_"))); } else { // LKTOKEN _@M278_ = "FALSE" wp->SetText(gettext(TEXT("_@M278_"))); } } wp = (WndProperty*)wf->FindByName(TEXT("prpValidFinish")); if (wp) { if (CALCULATED_INFO.ValidFinish) { // LKTOKEN _@M677_ = "TRUE" wp->SetText(gettext(TEXT("_@M677_"))); } else { // LKTOKEN _@M278_ = "FALSE" wp->SetText(gettext(TEXT("_@M278_"))); } } wp = (WndProperty*)wf->FindByName(TEXT("prpStartTime")); if (wp) { if (CALCULATED_INFO.TaskStartTime>0) { Units::TimeToText(Temp, (int)TimeLocal((int)(CALCULATED_INFO.TaskStartTime))); wp->SetText(Temp); } else { wp->SetText(TEXT("")); } } wp = (WndProperty*)wf->FindByName(TEXT("prpStartSpeed")); if (wp) { if (CALCULATED_INFO.TaskStartTime>0) { _stprintf(Temp, TEXT("%.0f %s"), TASKSPEEDMODIFY*CALCULATED_INFO.TaskStartSpeed, Units::GetTaskSpeedName()); wp->SetText(Temp); } else { wp->SetText(TEXT("")); } } // StartMaxHeight, StartMaxSpeed; // double start_h; LockTaskData(); wp = (WndProperty*)wf->FindByName(TEXT("prpStartPoint")); if (ValidTaskPoint(0)) { // start_h = WayPointList[Task[0].Index].Altitude; if (wp) { wp->SetText(WayPointList[Task[0].Index].Name); } } else { // start_h = 0; if (wp) { wp->SetText(TEXT("")); } } wp = (WndProperty*)wf->FindByName(TEXT("prpStartHeight")); if (wp) { if (CALCULATED_INFO.TaskStartTime>0) { _stprintf(Temp, TEXT("%.0f %s"), (CALCULATED_INFO.TaskStartAltitude)*ALTITUDEMODIFY, Units::GetAltitudeName()); wp->SetText(Temp); } else { wp->SetText(TEXT("")); } } wp = (WndProperty*)wf->FindByName(TEXT("prpFinishAlt")); if (wp) { double finish_min = FAIFinishHeight(&GPS_INFO, &CALCULATED_INFO, -1); _stprintf(Temp, TEXT("%.0f %s"), finish_min*ALTITUDEMODIFY, Units::GetAltitudeName()); wp->SetText(Temp); } UnlockTaskData(); }
void TaskSpeed(NMEA_INFO *Basic, DERIVED_INFO *Calculated, const double this_maccready) { int ifinal; static double LastTime = 0; static double LastTimeStats = 0; double TotalTime=0, TotalDistance=0, Vfinal=0; if (!ValidTaskPoint(ActiveWayPoint)) return; if (Calculated->ValidFinish) return; if (!Calculated->Flying) return; // in case we leave early due to error Calculated->TaskSpeedAchieved = 0; Calculated->TaskSpeed = 0; if (ActiveWayPoint<=0) { // no task speed before start Calculated->TaskSpeedInstantaneous = 0; return; } // LockFlightData(); LockTaskData(); if (TaskAltitudeRequired(Basic, Calculated, this_maccready, &Vfinal, &TotalTime, &TotalDistance, &ifinal)) { double t0 = TotalTime; // total time expected for task double t1 = Basic->Time-Calculated->TaskStartTime; // time elapsed since start double d0 = TotalDistance; // total task distance double d1 = Calculated->TaskDistanceCovered; // actual distance covered double dr = Calculated->TaskDistanceToGo; // distance remaining double hf = FAIFinishHeight(Basic, Calculated, -1); double h0 = Calculated->TaskAltitudeRequiredFromStart-hf; // total height required from start (takes safety arrival alt // and finish waypoint altitude into account) double h1 = max(0.0, Calculated->NavAltitude-hf); // height above target double dFinal; // final glide distance // equivalent speed double v2, v1; if ((t1<=0) || (d1<=0) || (d0<=0) || (t0<=0) || (h0<=0)) { // haven't started yet or not a real task Calculated->TaskSpeedInstantaneous = 0; //? Calculated->TaskSpeed = 0; goto OnExit; } // JB's task speed... double hx = max(0.0, SpeedHeight(Basic, Calculated)); double t1mod = t1-hx/MacCreadyOrAvClimbRate(Basic, Calculated, this_maccready); // only valid if flown for 5 minutes or more if (t1mod>300.0) { Calculated->TaskSpeedAchieved = d1/t1mod; } else { Calculated->TaskSpeedAchieved = d1/t1; } Calculated->TaskSpeed = Calculated->TaskSpeedAchieved; if (Vfinal<=0) { // can't reach target at current mc goto OnExit; } // distance that can be usefully final glided from here // (assumes average task glide angle of d0/h0) // JMW TODO accuracy: make this more accurate by working out final glide // through remaining turnpoints. This will more correctly account // for wind. #if BUGSTOP LKASSERT(h0!=0); #endif if (h0==0) h0=1; dFinal = min(dr, d0*min(1.0,max(0.0,h1/h0))); if (Calculated->ValidFinish) { dFinal = 0; } double dc = max(0.0, dr-dFinal); // amount of extra distance to travel in cruise/climb before final glide // actual task speed achieved so far v1 = d1/t1; #ifdef OLDTASKSPEED // time at end of final glide // equivalent time elapsed after final glide double t2 = t1+dFinal/Vfinal; // equivalent distance travelled after final glide // equivalent distance to end of final glide double d2 = d1+dFinal; // average speed to end of final glide from here v2 = d2/t2; Calculated->TaskSpeed = max(v1,v2); #else // average speed to end of final glide from here, weighted // according to how much extra time would be spent in cruise/climb // the closer dc (the difference between remaining distance and // final glidable distance) gets to zero, the closer v2 approaches // the average speed to end of final glide from here // in other words, the more we consider the final glide part to have // been earned. // this will be bogus at fast starts though... LKASSERT((t1+dc/v1+dFinal/Vfinal)!=0); LKASSERT((t1+dFinal/Vfinal)!=0); if (v1>0) { v2 = (d1+dc+dFinal)/(t1+dc/v1+dFinal/Vfinal); } else { v2 = (d1+dFinal)/(t1+dFinal/Vfinal); } Calculated->TaskSpeed = v2; #endif if(Basic->Time < LastTime) { LastTime = Basic->Time; } else if (Basic->Time-LastTime >=1.0) { double dt = Basic->Time-LastTime; LastTime = Basic->Time; // Calculate contribution to average task speed. // This is equal to the change in virtual distance // divided by the time step // This is a novel concept. // When climbing at the MC setting, this number should // be similar to the estimated task speed. // When climbing slowly or when flying off-course, // this number will drop. // In cruise at the optimum speed in zero lift, this // number will be similar to the estimated task speed. // A low pass filter is applied so it doesn't jump around // too much when circling. // If this number is higher than the overall task average speed, // it means that the task average speed is increasing. // When cruising in sink, this number will decrease. // When cruising in lift, this number will increase. // Therefore, it shows well whether at any time the glider // is wasting time. // VNT 090723 NOTICE: all of this is totally crazy. Did anyone ever cared to check // what happens with MC=0 ? Did anyone care to tell people how a simple "ETE" or TaskSpeed // has been complicated over any limit? // TODO: start back from scratch, not possible to trust any number here. static double dr_last = 0; double mc_safe = max(0.1,this_maccready); double Vstar = max(1.0,Calculated->VMacCready); #if BUGSTOP LKASSERT(dt!=0); #endif if (dt==0) dt=1; double vthis = (Calculated->LegDistanceCovered-dr_last)/dt; vthis /= AirDensityRatio(Calculated->NavAltitude); dr_last = Calculated->LegDistanceCovered; double ttg = max(1.0, Calculated->LegTimeToGo); // double Vav = d0/max(1.0,t0); double Vrem = Calculated->LegDistanceToGo/ttg; double Vref = // Vav; Vrem; double sr = -GlidePolar::SinkRate(Vstar); double height_diff = max(0.0, -Calculated->TaskAltitudeDifference); if (Calculated->timeCircling>30) { mc_safe = max(mc_safe, Calculated->TotalHeightClimb/Calculated->timeCircling); } // circling percentage during cruise/climb double rho_cruise = max(0.0,min(1.0,mc_safe/(sr+mc_safe))); double rho_climb = 1.0-rho_cruise; #if BUGSTOP LKASSERT(mc_safe!=0); #endif if (mc_safe==0) mc_safe=0.1; double time_climb = height_diff/mc_safe; // calculate amount of time in cruise/climb glide double rho_c = max(0.0, min(1.0, time_climb/ttg)); if (Calculated->FinalGlide) { if (rho_climb>0) { rho_c = max(0.0, min(1.0, rho_c/rho_climb)); } if (!Calculated->Circling) { if (Calculated->TaskAltitudeDifference>0) { rho_climb *= rho_c; rho_cruise *= rho_c; // Vref = Vrem; } } } LKASSERT(mc_safe!=0); double w_comp = min(10.0,max(-10.0,Calculated->Vario/mc_safe)); double vdiff = vthis/Vstar + w_comp*rho_cruise + rho_climb; if (vthis > SAFTEYSPEED*2) { vdiff = 1.0; // prevent funny numbers when starting mid-track } // Calculated->Experimental = vdiff*100.0; vdiff *= Vref; if (t1<5) { Calculated->TaskSpeedInstantaneous = vdiff; // initialise } else { static int lastActiveWayPoint = 0; static double tsi_av = 0; static int n_av = 0; if ((ActiveWayPoint==lastActiveWayPoint) && (Calculated->LegDistanceToGo>1000.0) && (Calculated->LegDistanceCovered>1000.0)) { Calculated->TaskSpeedInstantaneous = LowPassFilter(Calculated->TaskSpeedInstantaneous, vdiff, 0.1); // update stats if(Basic->Time < LastTimeStats) { LastTimeStats = Basic->Time; tsi_av = 0; n_av = 0; } else if (n_av>=60) { tsi_av/= n_av; flightstats.Task_Speed. least_squares_update( max(0.0, Basic->Time-Calculated->TaskStartTime)/3600.0, max(0.0, min(100.0,tsi_av))); LastTimeStats = Basic->Time; tsi_av = 0; n_av = 0; } tsi_av += Calculated->TaskSpeedInstantaneous; n_av ++; } else { Calculated->TaskSpeedInstantaneous = LowPassFilter(Calculated->TaskSpeedInstantaneous, vdiff, 0.5); // Calculated->TaskSpeedInstantaneous = vdiff; tsi_av = 0; n_av = 0; } lastActiveWayPoint = ActiveWayPoint; } } } OnExit: UnlockTaskData(); }
void DoAutoMacCready(NMEA_INFO *Basic, DERIVED_INFO *Calculated) { if (!Calculated->AutoMacCready) return; bool is_final_glide = false; // LockFlightData(); LockTaskData(); double mc_new = MACCREADY; static bool first_mc = true; if ( AutoMcMode==amcEquivalent ) { if ( (!Calculated->Circling) && (!Calculated->OnGround)) { if (Calculated->EqMc>=0) { // MACCREADY = LowPassFilter(MACCREADY,Calculated->EqMc,0.8); MACCREADY = Calculated->EqMc; } else { // -1.0 is used as an invalid flag. Normally flying at -1 MC means almost flying // at stall speed, which is pretty unusual. Maybe in wave conditions? if (Calculated->EqMc >-1) { MACCREADY=Calculated->EqMc*-1; } } } UnlockTaskData(); return; } // otherwise, if AutoMc for finalglide or "both", return if no goto if (!ValidTaskPoint(ActiveWayPoint)) { UnlockTaskData(); return; } if (Calculated->FinalGlide && ActiveIsFinalWaypoint()) { is_final_glide = true; } else { first_mc = true; } double av_thermal = -1; if (flightstats.ThermalAverage.y_ave>0) { if (Calculated->Circling && (Calculated->AverageThermal>0)) { LKASSERT((flightstats.ThermalAverage.sum_n+1)!=0); av_thermal = (flightstats.ThermalAverage.y_ave *flightstats.ThermalAverage.sum_n + Calculated->AverageThermal)/ (flightstats.ThermalAverage.sum_n+1); } else { av_thermal = flightstats.ThermalAverage.y_ave; } } else if (Calculated->Circling && (Calculated->AverageThermal>0)) { // insufficient stats, so use this/last thermal's average av_thermal = Calculated->AverageThermal; } if (!ValidTaskPoint(ActiveWayPoint)) { if (av_thermal>0) { mc_new = av_thermal; } } else if ( ((AutoMcMode==amcFinalGlide)||(AutoMcMode==amcFinalAndClimb)) && is_final_glide) { if (Calculated->TaskAltitudeDifference0>0) { // only change if above final glide with zero Mc // otherwise when we are well below, it will wind Mc back to // zero LKASSERT((Calculated->WaypointDistance+1)!=0);; double slope = (Calculated->NavAltitude + Calculated->EnergyHeight - FAIFinishHeight(Basic, Calculated, ActiveWayPoint))/ (Calculated->WaypointDistance+1); double mc_pirker = PirkerAnalysis(Basic, Calculated, Calculated->WaypointBearing, slope); mc_pirker = max(0.0, mc_pirker); if (first_mc) { // don't allow Mc to wind down to zero when first achieving // final glide; but do allow it to wind down after that if (mc_pirker >= mc_new) { mc_new = mc_pirker; first_mc = false; } else if (AutoMcMode==amcFinalAndClimb) { // revert to averager based auto Mc if (av_thermal>0) { mc_new = av_thermal; } } } else { mc_new = mc_pirker; } } else { // below final glide at zero Mc, never achieved final glide if (first_mc && (AutoMcMode==amcFinalAndClimb)) { // revert to averager based auto Mc if (av_thermal>0) { mc_new = av_thermal; } } } } else if ( (AutoMcMode==amcAverageClimb) || ((AutoMcMode==amcFinalAndClimb)&& !is_final_glide) ) { if (av_thermal>0) { mc_new = av_thermal; } } MACCREADY = LowPassFilter(MACCREADY,mc_new,0.6); UnlockTaskData(); // UnlockFlightData(); }