예제 #1
0
  bool CheckCondition(NMEA_INFO *Basic, DERIVED_INFO *Calculated) {

    wind_mag = Calculated->WindSpeed;
    wind_bearing = Calculated->WindBearing;

    if (!Calculated->Flying) {
      last_wind_mag = wind_mag;
      last_wind_bearing = wind_bearing;
      return false;
    }

    double mag_change = fabs(wind_mag - last_wind_mag);
    double dir_change = fabs(AngleLimit180(wind_bearing-last_wind_bearing));
    if (mag_change > 5/TOKNOTS) {
      return true;
    }
    if ((wind_mag>10/TOKNOTS) && (dir_change > 45)) {
      return true;
    }
    return false;
  };
예제 #2
0
파일: Turning.cpp 프로젝트: LK8000/LK8000
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);

}
예제 #3
0
void Heading(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
  double x0, y0, mag=0;
  static double LastTime = 0;
  static double lastHeading = 0;
  static double lastSpeed = 0;

  if (DoInit[MDI_HEADING]) {
	LastTime = 0;
	lastHeading = 0;
	DoInit[MDI_HEADING]=false;
  }

  if ((Basic->Speed>0)||(Calculated->WindSpeed>0)) {

    x0 = fastsine(Basic->TrackBearing)*Basic->Speed;
    y0 = fastcosine(Basic->TrackBearing)*Basic->Speed;
    x0 += fastsine(Calculated->WindBearing)*Calculated->WindSpeed;
    y0 += fastcosine(Calculated->WindBearing)*Calculated->WindSpeed;

    Calculated->Heading = AngleLimit360(atan2(x0,y0)*RAD_TO_DEG);

    if (!Calculated->Flying) {
      // don't take wind into account when on ground
      Calculated->Heading = Basic->TrackBearing;
    }

    // calculate turn rate in wind coordinates
    if(Basic->Time > LastTime) {
      double dT = Basic->Time - LastTime;

      LKASSERT(dT!=0);
      Calculated->TurnRateWind = AngleLimit180(Calculated->Heading
                                               - lastHeading)/dT;

      lastHeading = Calculated->Heading;
    }

    if (ISCAR) {
	// On ground, TAS is GS. Wind gradient irrilevant, normally.
	Calculated->TrueAirspeedEstimated = Basic->Speed;
	LKASSERT(AirDensityRatio(Calculated->NavAltitude)!=0);
	Calculated->IndicatedAirspeedEstimated = Basic->Speed/AirDensityRatio(Calculated->NavAltitude);
    } else {
	// calculate estimated true airspeed
	mag = isqrt4((unsigned long)(x0*x0*100+y0*y0*100))/10.0;
	Calculated->TrueAirspeedEstimated = mag;
	LKASSERT(AirDensityRatio(Calculated->NavAltitude)!=0);
	Calculated->IndicatedAirspeedEstimated = mag/AirDensityRatio(Calculated->NavAltitude);
    }
    // estimate bank angle (assuming balanced turn)
    double angle = atan(DEG_TO_RAD*Calculated->TurnRateWind*
			Calculated->TrueAirspeedEstimated/9.81);

    Calculated->BankAngle = RAD_TO_DEG*angle;

    if (ISCAR) {
	if(Basic->Time > LastTime) {
		Calculated->Gload = ((Basic->Speed - lastSpeed) / (Basic->Time-LastTime))/9.81;
		lastSpeed=Basic->Speed;
	} else {
		Calculated->Gload = 0;
	}
    } else {
	Calculated->Gload = 1.0/max(0.001,fabs(cos(angle)));
    }

    LastTime = Basic->Time;

    // estimate pitch angle (assuming balanced turn)
/*
    Calculated->PitchAngle = RAD_TO_DEG*
      atan2(Calculated->GPSVario-Calculated->Vario,
           Calculated->TrueAirspeedEstimated);
*/
	// should be used as here only when no real vario available
    Calculated->PitchAngle = RAD_TO_DEG*	
      atan2(Calculated->Vario,
           Calculated->TrueAirspeedEstimated);

    // update zigzag wind
    if (  ((AutoWindMode==D_AUTOWIND_ZIGZAG) || (AutoWindMode==D_AUTOWIND_BOTHCIRCZAG))
        && (!ReplayLogger::IsEnabled()) ) {
      double zz_wind_speed;
      double zz_wind_bearing;
      int quality=0;
      quality = WindKalmanUpdate(Basic, Calculated, &zz_wind_speed, &zz_wind_bearing);

      if (quality>0) {

        SetWindEstimate(zz_wind_speed, zz_wind_bearing);
	Calculated->WindSpeed   = zz_wind_speed;
	Calculated->WindBearing = zz_wind_bearing;

/* 100118 redundant!! removed. TOCHECK *
        Vector v_wind;
        v_wind.x = zz_wind_speed*cos(zz_wind_bearing*3.1415926/180.0);
        v_wind.y = zz_wind_speed*sin(zz_wind_bearing*3.1415926/180.0);
        LockFlightData();
        if (windanalyser) {
	  windanalyser->slot_newEstimate(Basic, Calculated, v_wind, quality);
        }
        UnlockFlightData();
*/
      }
    }
  // else basic speed is 0 and there is no wind.. 
  } else { 
    Calculated->Heading = Basic->TrackBearing;
    Calculated->TrueAirspeedEstimated = 0; // BUGFIX 100318
    Calculated->IndicatedAirspeedEstimated = 0; // BUGFIX 100318
  }

}
예제 #4
0
void AATDistance::ShiftTargetFromBehind(double longitude, double latitude,
                              int taskwaypoint) {

  // JMWAAT if being externally updated e.g. from task dialog, don't move it
  if (TargetDialogOpen) return;
  if (taskwaypoint==0) return;

  // best is decreasing or first entry in sector, so project
  // target in direction of improvement or first entry into sector

  double course_bearing;
  double course_bearing_orig;
  double d_total_orig;
  double d_total_this;

  d_total_this = DoubleLegDistance(taskwaypoint,
                                   longitude,
                                   latitude);

  d_total_orig = DoubleLegDistance(taskwaypoint,
                                   Task[taskwaypoint].AATTargetLon,
                                   Task[taskwaypoint].AATTargetLat);

  if (d_total_this>d_total_orig-2.0*AATCloseDistance()) {
    // this is better than the previous best! (or very close)
    ShiftTargetFromInFront(longitude, latitude, taskwaypoint);
    return;
  }

  // JMWAAT if locked, don't move it
  if (Task[taskwaypoint].AATTargetLocked) {
    // 20080615 JMW don't do this; locked stays locked
    // Task[taskwaypoint].AATTargetLocked = false; // JMWAAT JB
    return;
  }

  /*
  // check to see if deviation is big enough to adjust target along track
  DistanceBearing(Task[taskwaypoint-1].AATTargetLat,
                  Task[taskwaypoint-1].AATTargetLon,
                  latitude,
                  longitude,
                  NULL, &course_bearing);

  DistanceBearing(Task[taskwaypoint-1].AATTargetLat,
                  Task[taskwaypoint-1].AATTargetLon,
                  Task[taskwaypoint].AATTargetLat,
                  Task[taskwaypoint].AATTargetLon,
                  NULL, &course_bearing_orig);

  if (fabs(AngleLimit180(course_bearing-course_bearing_orig))<5.0) {
    // don't update it if course deviation is less than 5 degrees,
    // otherwise we end up wasting a lot of CPU in recalculating, and also
    // the target ends up drifting.
    return;
  }

  course_bearing = AngleLimit360(course_bearing+
                                 Task[taskwaypoint].AATTargetOffsetRadial);
  //JMWAAT  Task[taskwaypoint].AATTargetOffsetRadial = course_bearing;
  */

  DistanceBearing(Task[taskwaypoint-1].AATTargetLat,
                  Task[taskwaypoint-1].AATTargetLon,
                  latitude,
                  longitude,
                  NULL, &course_bearing);
  course_bearing = AngleLimit360(course_bearing+
                                 Task[taskwaypoint].AATTargetOffsetRadial);

  DistanceBearing(latitude,
                  longitude,
                  Task[taskwaypoint].AATTargetLat,
                  Task[taskwaypoint].AATTargetLon,
                  NULL, &course_bearing_orig);

  if (fabs(AngleLimit180(course_bearing-course_bearing_orig))<5.0) {
    // don't update it if course deviation is less than 5 degrees,
    // otherwise we end up wasting a lot of CPU in recalculating, and also
    // the target ends up drifting.
    return;
  }

  double max_distance =
    FindInsideAATSectorDistance(latitude,
                                longitude,
                                taskwaypoint,
                                course_bearing,
                                0);

  // total distance of legs from previous through this to next target
  double delta = max_distance/2;

  // move target in line with previous target along track
  // at an offset to improve on max distance

  double t_distance_lower = 0;
  double t_distance = delta*2;

  int steps = 0;

  do {
    // find target position along projected line but
    // make sure it is in sector, and set at a distance
    // to preserve total task distance
    // we are aiming to make d_total_this = d_total_orig

    double t_lat, t_lon;
    FindLatitudeLongitude(latitude, longitude,
                          course_bearing, t_distance,
                          &t_lat,
                          &t_lon);

    if (InAATTurnSector(t_lon, t_lat, taskwaypoint, 0)) {
      d_total_this = DoubleLegDistance(taskwaypoint,
                                       t_lon,
                                       t_lat);
      if (d_total_orig - d_total_this>0.0) {
        t_distance_lower = t_distance;
        // ok, can go further
        t_distance += delta;
      } else {
        t_distance -= delta;
      }
    } else {
      t_distance -= delta;
    }
    delta /= 2.0;
  } while ((delta>5.0) && (steps++<20));

  // now scan to edge of sector to find approximate range %
  if (t_distance_lower>5.0) {
    FindLatitudeLongitude(latitude, longitude,
                          course_bearing, t_distance_lower,
                          &Task[taskwaypoint].AATTargetLat,
                          &Task[taskwaypoint].AATTargetLon);

    UpdateTargetAltitude(Task[taskwaypoint]);

    Task[taskwaypoint].AATTargetOffsetRadius =
      FindInsideAATSectorRange(latitude,
                               longitude,
                               taskwaypoint, course_bearing,
                               t_distance_lower);
    TargetModified = true;
    CalculateAATIsoLines();

  }

  //  if ((!t_in_sector) && (d_diff_total>1.0)) {
    // JMW TODO enhancement: this is too short now so need to lengthen the
    // next waypoint if possible
    // (re discussion with paul mander)
  //  }
}
void CalculateAATTaskSectors()
{
  int i;
  int awp = ActiveTaskPoint;

  if(AATEnabled == FALSE || DoOptimizeRoute())
    return;

  double latitude = GPS_INFO.Latitude;
  double longitude = GPS_INFO.Longitude;
  double altitude = GPS_INFO.Altitude;

  LockTaskData();

  Task[0].AATTargetOffsetRadius = 0.0;
  Task[0].AATTargetOffsetRadial = 0.0;
  if (Task[0].Index>=0) {
    Task[0].AATTargetLat = WayPointList[Task[0].Index].Latitude;
    Task[0].AATTargetLon = WayPointList[Task[0].Index].Longitude;
  }

  for(i=1;i<MAXTASKPOINTS;i++) {
    if(ValidTaskPoint(i)) {
      if (!ValidTaskPoint(i+1)) {
        // This must be the final waypoint, so it's not an AAT OZ
        Task[i].AATTargetLat = WayPointList[Task[i].Index].Latitude;
        Task[i].AATTargetLon = WayPointList[Task[i].Index].Longitude;
        continue;
      }

      if(Task[i].AATType == SECTOR) {
        FindLatitudeLongitude (WayPointList[Task[i].Index].Latitude,
                                 WayPointList[Task[i].Index].Longitude, 
                               Task[i].AATStartRadial , 
                               Task[i].AATSectorRadius ,
                               &Task[i].AATStartLat,
                               &Task[i].AATStartLon);
        
        FindLatitudeLongitude (WayPointList[Task[i].Index].Latitude,
                               WayPointList[Task[i].Index].Longitude,
                               Task[i].AATFinishRadial , 
                               Task[i].AATSectorRadius,
                               &Task[i].AATFinishLat,
                               &Task[i].AATFinishLon);
      }

      // JMWAAT: if locked, don't move it
      if (i<awp) {
        // only update targets for current/later waypoints 
        continue;
      }

      Task[i].AATTargetOffsetRadius = 
        min(1.0, max(Task[i].AATTargetOffsetRadius,-1.0));

      Task[i].AATTargetOffsetRadial = 
        min(90.0, max(-90.0, Task[i].AATTargetOffsetRadial));

      double targetbearing;
      double targetrange;
      
      targetbearing = AngleLimit360(Task[i].Bisector+Task[i].AATTargetOffsetRadial);
      
      if(Task[i].AATType == SECTOR) {

        //AATStartRadial
        //AATFinishRadial

        targetrange = ((Task[i].AATTargetOffsetRadius+1.0)/2.0);

        double aatbisector = HalfAngle(Task[i].AATStartRadial, 
                                       Task[i].AATFinishRadial);

        if (fabs(AngleLimit180(aatbisector-targetbearing))>90) {
          // bisector is going away from sector 
          targetbearing = Reciprocal(targetbearing);
          targetrange = 1.0-targetrange;
        }
        if (!AngleInRange(Task[i].AATStartRadial,
                          Task[i].AATFinishRadial,
                          targetbearing,true)) {

          // Bisector is not within AAT sector, so
          // choose the closest radial as the target line

          if (fabs(AngleLimit180(Task[i].AATStartRadial-targetbearing))
              <fabs(AngleLimit180(Task[i].AATFinishRadial-targetbearing))) {
            targetbearing = Task[i].AATStartRadial;
          } else {
            targetbearing = Task[i].AATFinishRadial;
          }
        }

        targetrange*= Task[i].AATSectorRadius;

      } else {
        targetrange = Task[i].AATTargetOffsetRadius
          *Task[i].AATCircleRadius;
      }
      
      // TODO accuracy: if i=awp and in sector, range parameter needs to
      // go from current aircraft position to projection of target
      // out to the edge of the sector
      
      if (InAATTurnSector(longitude, latitude, i, altitude) && (awp==i) &&
          !Task[i].AATTargetLocked) {

        // special case, currently in AAT sector/cylinder
        
        double dist;
        double qdist;
        double bearing;
        
        // find bearing from last target through current aircraft position with offset
        DistanceBearing(Task[i-1].AATTargetLat,
                        Task[i-1].AATTargetLon,
                        latitude,
                        longitude,
                        &qdist, &bearing);

        bearing = AngleLimit360(bearing+Task[i].AATTargetOffsetRadial);

        dist = ((Task[i].AATTargetOffsetRadius+1)/2.0)*
          FindInsideAATSectorDistance(latitude, longitude, i, bearing);
        
        // if (dist+qdist>aatdistance.LegDistanceAchieved(awp)) {
        // JMW: don't prevent target from being closer to the aircraft
        // than the best achieved, so can properly plan arrival time        

        FindLatitudeLongitude (latitude,
                               longitude, 
                               bearing, 
                               dist,
                               &Task[i].AATTargetLat,
                               &Task[i].AATTargetLon);

        UpdateTargetAltitude(Task[i]);

        TargetModified = true;

        // }
        
      } else {
        
        FindLatitudeLongitude (WayPointList[Task[i].Index].Latitude,
                               WayPointList[Task[i].Index].Longitude, 
                               targetbearing, 
                               targetrange,
                               &Task[i].AATTargetLat,
                               &Task[i].AATTargetLon);
        
        UpdateTargetAltitude(Task[i]);
        TargetModified = true;
        
      }
    }
  }

  CalculateAATIsoLines();
  if (!TargetDialogOpen) {
    TargetModified = false;
    // allow target dialog to detect externally changed targets
  }

  UnlockTaskData();
}
예제 #6
0
bool InFinishSector(NMEA_INFO *Basic, DERIVED_INFO *Calculated, const int i)
{
  static int LastInSector = FALSE;
  double AircraftBearing;
  double FirstPointDistance;
  bool retval = false;

  if (WayPointList.empty()) return FALSE;

  if (!ValidFinish(Basic, Calculated)) return FALSE;

  // Finish invalid
  if (!ValidTaskPoint(i)) return FALSE;

  LockTaskData();

  // distance from aircraft to start point
  DistanceBearing(Basic->Latitude,
                  Basic->Longitude,
                  WayPointList[Task[i].Index].Latitude, 
                  WayPointList[Task[i].Index].Longitude,
                  &FirstPointDistance,
                  &AircraftBearing);
  bool inrange = false;
  inrange = (FirstPointDistance<FinishRadius);
  if (!inrange) {
    LastInSector = false;
  }

  if(!FinishLine) // Start Circle
    {
      retval = inrange;
      goto OnExit;
    }
        
  // Finish line
  AircraftBearing = AngleLimit180(AircraftBearing - Task[i].InBound);

  // JMW bugfix, was Bisector, which is invalid

  bool approaching;
  if(FinishLine==1) { // Finish line 
    approaching = ((AircraftBearing >= -90) && (AircraftBearing <= 90));
  } else {
    // FAI 90 degree
    approaching = !((AircraftBearing >= 135) || (AircraftBearing <= -135));
  }

  if (inrange) {

    if (LastInSector) {
      // previously approaching the finish line
      if (!approaching) {
        // now moving away from finish line
        LastInSector = false;
        retval = TRUE;
        goto OnExit;
      }
    } else {
      if (approaching) {
        // now approaching the finish line
        LastInSector = true;
      }
    }
    
  } else {
    LastInSector = false;
  }
 OnExit:
  UnlockTaskData();
  return retval;
}