FGAIManager *aimgr;
string airport;
-
- double courseToUser;
- double distanceToDest;
+
double speed;
time_t
userLatitude,
userLongitude;
+ SGVec3d newPos(0, 0, 0);
+
+
if (fgGetBool("/sim/traffic-manager/enabled") == false)
return true;
//cerr << "About to find a flight " << endl;
if (flights.empty()) {
//execute this loop at least once.
- SG_LOG(SG_GENERAL, SG_INFO, "Scheduling for : " << modelPath << " " << registration << " " << homePort);
+ SG_LOG(SG_GENERAL, SG_BULK, "Scheduling for : " << modelPath << " " << registration << " " << homePort);
FGScheduledFlight *flight = 0;
do {
flight = findAvailableFlight(currentDestination, flightIdentifier);
depT = depT.substr(0,24);
arrT = arrT.substr(0,24);
- SG_LOG(SG_GENERAL, SG_INFO, " " << flight->getCallSign() << ":"
+ SG_LOG(SG_GENERAL, SG_BULK, " " << flight->getCallSign() << ":"
<< " " << flight->getDepartureAirport()->getId() << ":"
<< " " << depT << ":"
<< " \"" << flight->getArrivalAirport()->getId() << "\"" << ":"
flights.push_back(flight);
}
} while ((currentDestination != homePort) && (flight != 0));
- SG_LOG(SG_GENERAL, SG_INFO, cerr << " Done " << endl);
+ SG_LOG(SG_GENERAL, SG_BULK, cerr << " Done " << endl);
}
//cerr << " Done " << endl;
// No flights available for this aircraft
if (!(dep && arr))
return false;
- SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(dep->getLongitude(),
- dep->getLatitude(), 1));
- SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arr->getLongitude(),
- arr->getLatitude(), 1));
- SGVec3d _cross = cross(b, a);
+ if (dep != arr) {
+ SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(dep->getLongitude(),
+ dep->getLatitude(), 1));
+ SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arr->getLongitude(),
+ arr->getLatitude(), 1));
+ SGVec3d _cross = cross(b, a);
- angle = sgACos(dot(a, b));
+ angle = sgACos(dot(a, b));
- // Okay, at this point we have the angle between departure and
- // arrival airport, in degrees. From here we can interpolate the
- // position of the aircraft by calculating the ratio between
- // total time enroute and elapsed time enroute.
+ // Okay, at this point we have the angle between departure and
+ // arrival airport, in degrees. From here we can interpolate the
+ // position of the aircraft by calculating the ratio between
+ // total time enroute and elapsed time enroute.
- totalTimeEnroute = (*i)->getArrivalTime() - (*i)->getDepartureTime();
- if (now > (*i)->getDepartureTime())
- {
- //err << "Lat = " << lat << ", lon = " << lon << endl;
- //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
- elapsedTimeEnroute = now - (*i)->getDepartureTime();
- remainingTimeEnroute = (*i)->getArrivalTime() - now;
- SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is in progress.");
- }
- else
- {
- lat = dep->getLatitude();
- lon = dep->getLongitude();
- elapsedTimeEnroute = 0;
- remainingTimeEnroute = totalTimeEnroute;
- SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is pending.");
- }
-
- angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
-
-
- //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
- // << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
- sgdMat4 matrix;
- sgdMakeRotMat4(matrix, angle, _cross.sg());
- SGVec3d newPos(0, 0, 0);
- for(int j = 0; j < 3; j++)
- {
- for (int k = 0; k<3; k++)
- {
- newPos[j] += matrix[j][k]*a[k];
- }
- }
-
- if (now > (*i)->getDepartureTime())
- {
- SGGeoc geoc = SGGeoc::fromCart(newPos);
- lat = geoc.getLatitudeDeg();
- lon = geoc.getLongitudeDeg();
- }
- else
- {
- lat = dep->getLatitude();
- lon = dep->getLongitude();
- }
-
-
- SGWayPoint current (lon,
- lat,
- (*i)->getCruiseAlt(),
- SGWayPoint::SPHERICAL);
- SGWayPoint user ( userLongitude,
- userLatitude,
- (*i)->getCruiseAlt(),
- SGWayPoint::SPHERICAL);
- SGWayPoint dest ( arr->getLongitude(),
- arr->getLatitude(),
- (*i)->getCruiseAlt(),
- SGWayPoint::SPHERICAL);
- // We really only need distance to user
- // and course to destination
- user.CourseAndDistance(current, &courseToUser, &distanceToUser);
- dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
- speed = (distanceToDest*SG_METER_TO_NM) /
- ((double) remainingTimeEnroute/3600.0);
-
+ totalTimeEnroute = (*i)->getArrivalTime() - (*i)->getDepartureTime();
+ if (now > (*i)->getDepartureTime())
+ {
+ //err << "Lat = " << lat << ", lon = " << lon << endl;
+ //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
+ elapsedTimeEnroute = now - (*i)->getDepartureTime();
+ remainingTimeEnroute = (*i)->getArrivalTime() - now;
+ SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is in progress.");
+ }
+ else
+ {
+ lat = dep->getLatitude();
+ lon = dep->getLongitude();
+ elapsedTimeEnroute = 0;
+ remainingTimeEnroute = totalTimeEnroute;
+ SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is pending.");
+ }
+ angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
+ //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
+ // << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
+ sgdMat4 matrix;
+ sgdMakeRotMat4(matrix, angle, _cross.data());
+ for(int j = 0; j < 3; j++) {
+ for (int k = 0; k<3; k++) {
+ newPos[j] += matrix[j][k]*a[k];
+ }
+ }
+ }
+ SGGeod current;
+ if ((now > (*i)->getDepartureTime() && (dep != arr))) {
+ current = SGGeod::fromCart(newPos);
+ speed = SGGeodesy::distanceNm(current, arr->geod()) /
+ ((double) remainingTimeEnroute/3600.0);
+ } else {
+ current = dep->geod();
+ speed = 450;
+ }
+ SGGeod user = SGGeod::fromDegM(userLongitude, userLatitude, (*i)->getCruiseAlt());
+
+ distanceToUser = SGGeodesy::distanceNm(current, user);
// If distance between user and simulated aircaft is less
// then 500nm, create this flight. At jet speeds 500 nm is roughly
// to start a more detailed simulation of this aircraft.
SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic manager: " << registration << " is scheduled for a flight from "
<< dep->getId() << " to " << arr->getId() << ". Current distance to user: "
- << distanceToUser*SG_METER_TO_NM);
- if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDISTTOSTART)
+ << distanceToUser);
+ if (distanceToUser < TRAFFICTOAIDISTTOSTART)
{
string flightPlanName = dep->getId() + string("-") + arr->getId() +
string(".xml");
aircraft->setAltitude((*i)->getCruiseAlt()*100); // convert from FL to feet
aircraft->setSpeed(speed);
aircraft->setBank(0);
+
+ courseToDest = SGGeodesy::courseDeg(current, arr->geod());
aircraft->SetFlightPlan(new FGAIFlightPlan(aircraft, flightPlanName, courseToDest, deptime,
dep, arr,true, radius,
(*i)->getCruiseAlt()*100,
projects / flightgear.git / blobdiff