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