double FindInsideAATSectorRange(double latitude, double longitude, int taskwaypoint, double course_bearing, double p_found) { double t_distance = FindInsideAATSectorDistance(latitude, longitude, taskwaypoint, course_bearing, p_found); return (p_found / max(1.0,t_distance))*2-1; }
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(); }