Пример #1
0
void AATDistance::ShiftTargetFromInFront(double longitude, double latitude,
                                         int taskwaypoint) {

  double course_bearing;

  // this point is in sector and is improved

  // JMW, now moves target to in line with previous target whenever
  // you are in AAT sector and improving on the target distance

  //JMWAAT  Task[taskwaypoint].AATTargetOffsetRadial = -1.0;

  if (Task[taskwaypoint].AATTargetLocked) {
    // have improved on the locked value, so unlock it in case user
    // wants to move it.
    Task[taskwaypoint].AATTargetOffsetRadius = -1.0;
    Task[taskwaypoint].AATTargetOffsetRadial = 0;
    Task[taskwaypoint].AATTargetLocked = false;
  }

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

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

  FindLatitudeLongitude(latitude, longitude,
                        course_bearing, AATCloseDistance(),
                        &Task[taskwaypoint].AATTargetLat,
                        &Task[taskwaypoint].AATTargetLon);
  // JMW, distance here was 100m, now changed to speed * 2

  UpdateTargetAltitude(Task[taskwaypoint]);

  TargetModified = true;
  CalculateAATIsoLines();
}
Пример #2
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();
}