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