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