void GlueMapWindow::UpdateDisplayMode() { /* not using MapWindowBlackboard here because these methods are called by the main thread */ enum DisplayMode new_mode = GetNewDisplayMode(CommonInterface::GetUIState(), CommonInterface::Calculated()); if (DisplayMode != new_mode && new_mode == DM_CIRCLING) offsetHistory.reset(); DisplayMode = new_mode; SwitchZoomClimb(); }
void GlueMapWindow::UpdateDisplayMode() { /* not using MapWindowBlackboard here because these methods are called by the main thread */ enum DisplayMode new_mode = CommonInterface::GetUIState().display_mode; const bool was_circling = last_display_mode == DisplayMode::CIRCLING; const bool is_circling = new_mode == DisplayMode::CIRCLING; if (!was_circling && is_circling) offset_history.Reset(); last_display_mode = new_mode; if (is_circling != was_circling) SwitchZoomClimb(); }
void GlueMapWindow::UpdateDisplayMode() { /* not using MapWindowBlackboard here because these methods are called by the main thread */ enum DisplayMode new_mode = GetNewDisplayMode(CommonInterface::GetUIState(), CommonInterface::Calculated()); bool was_circling = (display_mode == DM_CIRCLING); bool is_circling = (new_mode == DM_CIRCLING); if (!was_circling && is_circling) offset_history.Reset(); display_mode = new_mode; if (is_circling != was_circling) SwitchZoomClimb(); }
void Turning(NMEA_INFO *Basic, DERIVED_INFO *Calculated) { static double LastTrack = 0; static double StartTime = 0; static double StartLong = 0; static double StartLat = 0; static double StartAlt = 0; static double StartEnergyHeight = 0; static double LastTime = 0; static int MODE = CRUISE; static bool LEFT = FALSE; double Rate; static double LastRate=0; double dRate; double dT; if (!Calculated->Flying) { if (MODE!=CRUISE) { #if TESTBENCH StartupStore(_T(".... Not flying, still circling -> Cruise forced!\n")); #endif goto _forcereset; } return; } // Back in time in IGC replay mode? if(Basic->Time <= LastTime) { #if TESTBENCH StartupStore(_T("...... Turning check is back in time. Now=%f Last=%f: reset %s\n"),Basic->Time, LastTime,WhatTimeIsIt()); #endif _forcereset: LastTime = Basic->Time; // 101216 PV not sure of this.. LastTrack = 0; StartTime = 0; StartLong = 0; StartLat = 0; StartAlt = 0; StartEnergyHeight = 0; LastTime = 0; LEFT = FALSE; if (MODE!=CRUISE) { MODE = CRUISE; // Finally do the transition to cruise Calculated->Circling = false; SwitchZoomClimb(Basic, Calculated, false, LEFT); InputEvents::processGlideComputer(GCE_FLIGHTMODE_CRUISE); } return; } dT = Basic->Time - LastTime; LastTime = Basic->Time; #if BUGSTOP LKASSERT(dT!=0); #else if (dT==0) dT=1; #endif Rate = AngleLimit180(Basic->TrackBearing-LastTrack)/dT; #if DEBUGTURN StartupStore(_T("... Rate=%f in time=%f\n"),Rate,dT); #endif if (dT<2.0 && dT!=0) { // time step ok // calculate acceleration dRate = (Rate-LastRate)/dT; double dtlead=0.3; // integrate assuming constant acceleration, for one second Calculated->NextTrackBearing = Basic->TrackBearing + dtlead*(Rate+0.5*dtlead*dRate); // s = u.t+ 0.5*a*t*t Calculated->NextTrackBearing = AngleLimit360(Calculated->NextTrackBearing); } else { // time step too big, so just take it at last measurement Calculated->NextTrackBearing = Basic->TrackBearing; } Calculated->TurnRate = Rate; // JMW limit rate to 50 deg per second otherwise a big spike // will cause spurious lock on circling for a long time if (Rate>50) { Rate = 50; } if (Rate<-50) { Rate = -50; } // average rate, to detect essing static double rate_history[60]; double rate_ave=0; for (int i=59; i>0; i--) { rate_history[i] = rate_history[i-1]; rate_ave += rate_history[i]; } rate_history[0] = Rate; rate_ave /= 60; // THIS IS UNUSED in 4.0 Calculated->Essing = fabs(rate_ave)*100/MinTurnRate; if (MODE==CLIMB||MODE==WAITCRUISE) Rate = LowPassFilter(LastRate,Rate,0.9); else Rate = LowPassFilter(LastRate,Rate,0.3); LastRate = Rate; if(Rate <0) { if (LEFT) { // OK, already going left } else { LEFT = true; } Rate *= -1; } else { if (!LEFT) { // OK, already going right } else { LEFT = false; } } PercentCircling(Basic, Calculated, Rate); LastTrack = Basic->TrackBearing; bool forcecruise = false; bool forcecircling = false; #if 1 // UNUSED, EnableExternalTriggerCruise not configurable, set to false since ever if (EnableExternalTriggerCruise ) { if (ExternalTriggerCruise && ExternalTriggerCircling) { // this should never happen ExternalTriggerCircling = false; } forcecruise = ExternalTriggerCruise; forcecircling = ExternalTriggerCircling; } #endif switch(MODE) { case CRUISE: double cruise_turnthreshold; if (ISPARAGLIDER) cruise_turnthreshold=5; else cruise_turnthreshold=4; if((Rate >= cruise_turnthreshold)||(forcecircling)) { // This is initialising the potential thermalling start // We still dont know if we are really circling for thermal StartTime = Basic->Time; StartLong = Basic->Longitude; StartLat = Basic->Latitude; StartAlt = Calculated->NavAltitude; StartEnergyHeight = Calculated->EnergyHeight; #if DEBUGTURN StartupStore(_T("... CRUISE -> WAITCLIMB\n")); #endif MODE = WAITCLIMB; } if (forcecircling) { MODE = WAITCLIMB; } else { break; } case WAITCLIMB: if (forcecruise) { MODE = CRUISE; break; } double waitclimb_turnthreshold; double cruiseclimbswitch; if (ISPARAGLIDER) { waitclimb_turnthreshold=5; cruiseclimbswitch=15; // this should be finetuned for PGs } else { waitclimb_turnthreshold=4; cruiseclimbswitch=15; // this is ok for gliders } if((Rate >= waitclimb_turnthreshold)||(forcecircling)) { // WE CANNOT do this, because we also may need Circling mode to detect FF!! // if( (Calculated->FreeFlying && ((Basic->Time - StartTime) > cruiseclimbswitch))|| forcecircling) { if( (!ISCAR && !ISGAAIRCRAFT && ((Basic->Time - StartTime) > cruiseclimbswitch))|| forcecircling) { #ifdef TOW_CRUISE // If free flight (FF) hasn�t yet been detected, then we may // still be on tow. The following prevents climb mode from // engaging due to normal on-aerotow turns. if (!Calculated->FreeFlying && (fabs(Calculated->TurnRate) < 12)) break; #endif #if DEBUGTURN StartupStore(_T("... WAITCLIMB -> CLIMB\n")); #endif Calculated->Circling = true; // JMW Transition to climb MODE = CLIMB; // Probably a replay flight, with fast forward with no cruise init if (StartTime==0) { StartTime = Basic->Time; StartLong = Basic->Longitude; StartLat = Basic->Latitude; StartAlt = Calculated->NavAltitude; StartEnergyHeight = Calculated->EnergyHeight; } Calculated->ClimbStartLat = StartLat; Calculated->ClimbStartLong = StartLong; Calculated->ClimbStartAlt = StartAlt+StartEnergyHeight; Calculated->ClimbStartTime = StartTime; if (flightstats.Altitude_Ceiling.sum_n>0) { // only update base if have already climbed, otherwise // we will catch the takeoff height as the base. flightstats.Altitude_Base. least_squares_update(max(0.0, Calculated->ClimbStartTime - Calculated->TakeOffTime)/3600.0, StartAlt); } SwitchZoomClimb(Basic, Calculated, true, LEFT); InputEvents::processGlideComputer(GCE_FLIGHTMODE_CLIMB); } } else { // nope, not turning, so go back to cruise #if DEBUGTURN StartupStore(_T("... WAITCLIMB -> CRUISE\n")); #endif MODE = CRUISE; } break; case CLIMB: if ( (AutoWindMode == D_AUTOWIND_CIRCLING) || (AutoWindMode==D_AUTOWIND_BOTHCIRCZAG) ) { LockFlightData(); windanalyser->slot_newSample(Basic, Calculated); UnlockFlightData(); } double climb_turnthreshold; if (ISPARAGLIDER) climb_turnthreshold=10; else climb_turnthreshold=4; if((Rate < climb_turnthreshold)||(forcecruise)) { StartTime = Basic->Time; StartLong = Basic->Longitude; StartLat = Basic->Latitude; StartAlt = Calculated->NavAltitude; StartEnergyHeight = Calculated->EnergyHeight; // JMW Transition to cruise, due to not properly turning MODE = WAITCRUISE; #if DEBUGTURN StartupStore(_T("... CLIMB -> WAITCRUISE\n")); #endif } if (forcecruise) { MODE = WAITCRUISE; } else { break; } case WAITCRUISE: if (forcecircling) { MODE = CLIMB; break; } double waitcruise_turnthreshold; double climbcruiseswitch; if (ISPARAGLIDER) { waitcruise_turnthreshold=10; climbcruiseswitch=15; } else { waitcruise_turnthreshold=4; climbcruiseswitch=9; // ok for gliders } // // Exiting climb mode? // if((Rate < waitcruise_turnthreshold) || forcecruise) { if( ((Basic->Time - StartTime) > climbcruiseswitch) || forcecruise) { // We are no more in climb mode if (StartTime==0) { StartTime = Basic->Time; StartLong = Basic->Longitude; StartLat = Basic->Latitude; StartAlt = Calculated->NavAltitude; StartEnergyHeight = Calculated->EnergyHeight; } Calculated->CruiseStartLat = StartLat; Calculated->CruiseStartLong = StartLong; Calculated->CruiseStartAlt = StartAlt; Calculated->CruiseStartTime = StartTime; // Here we assign automatically this last thermal to the L> multitarget if (Calculated->ThermalGain >100) { // Force immediate calculation of average thermal, it would be made // during next cycle, but we need it here immediately AverageThermal(Basic,Calculated); if (EnableThermalLocator) { InsertThermalHistory(Calculated->ClimbStartTime, Calculated->ThermalEstimate_Latitude, Calculated->ThermalEstimate_Longitude, Calculated->ClimbStartAlt, Calculated->NavAltitude, Calculated->AverageThermal); } else { InsertThermalHistory(Calculated->ClimbStartTime, Calculated->ClimbStartLat, Calculated->ClimbStartLong, Calculated->ClimbStartAlt, Calculated->NavAltitude, Calculated->AverageThermal); } } InitLDRotary(&rotaryLD); InitWindRotary(&rotaryWind); flightstats.Altitude_Ceiling. least_squares_update(max(0.0, Calculated->CruiseStartTime - Calculated->TakeOffTime)/3600.0, Calculated->CruiseStartAlt); // Finally do the transition to cruise Calculated->Circling = false; MODE = CRUISE; #if DEBUGTURN StartupStore(_T("... WAITCRUISE -> CRUISE\n")); #endif SwitchZoomClimb(Basic, Calculated, false, LEFT); InputEvents::processGlideComputer(GCE_FLIGHTMODE_CRUISE); } // climbcruiseswitch time in range } else { // Rate>Minturnrate, back to climb, turning again #if DEBUGTURN StartupStore(_T("... WAITCRUISE -> CLIMB\n")); #endif MODE = CLIMB; } break; default: // error, go to cruise MODE = CRUISE; } // generate new wind vector if altitude changes or a new // estimate is available if (AutoWindMode>D_AUTOWIND_MANUAL && AutoWindMode <D_AUTOWIND_EXTERNAL) { LockFlightData(); windanalyser->slot_Altitude(Basic, Calculated); UnlockFlightData(); } if (EnableThermalLocator) { if (Calculated->Circling) { thermallocator.AddPoint(Basic->Time, Basic->Longitude, Basic->Latitude, Calculated->NettoVario); thermallocator.Update(Basic->Time, Basic->Longitude, Basic->Latitude, Calculated->WindSpeed, Calculated->WindBearing, Basic->TrackBearing, &Calculated->ThermalEstimate_Longitude, &Calculated->ThermalEstimate_Latitude, &Calculated->ThermalEstimate_W, &Calculated->ThermalEstimate_R); } else { Calculated->ThermalEstimate_W = 0; Calculated->ThermalEstimate_R = -1; thermallocator.Reset(); } } // update atmospheric model CuSonde::updateMeasurements(Basic, Calculated); }