# include <config.h>
#endif
-#include <simgear/route/waypoint.hxx>
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
-#include <Main/viewer.hxx>
#include <Scenery/scenery.hxx>
#include <Scenery/tilemgr.hxx>
#include <Airports/dynamics.hxx>
FGAIBase::readFromScenario(scFileNode);
- setPerformance(scFileNode->getStringValue("class", "jet_transport"));
+ setPerformance("", scFileNode->getStringValue("class", "jet_transport"));
setFlightPlan(scFileNode->getStringValue("flightplan"),
scFileNode->getBoolValue("repeat", false));
setCallSign(scFileNode->getStringValue("callsign"));
void FGAIAircraft::bind() {
FGAIBase::bind();
- props->tie("controls/gear/gear-down",
- SGRawValueMethods<FGAIAircraft,bool>(*this,
- &FGAIAircraft::_getGearDown));
- props->tie("transponder-id",
- SGRawValueMethods<FGAIAircraft,const char*>(*this,
- &FGAIAircraft::_getTransponderCode));
+ tie("controls/gear/gear-down",
+ SGRawValueMethods<FGAIAircraft,bool>(*this,
+ &FGAIAircraft::_getGearDown));
+ tie("transponder-id",
+ SGRawValueMethods<FGAIAircraft,const char*>(*this,
+ &FGAIAircraft::_getTransponderCode));
}
-
-void FGAIAircraft::unbind() {
- FGAIBase::unbind();
-
- props->untie("controls/gear/gear-down");
- props->untie("transponder-id");
-}
-
-
void FGAIAircraft::update(double dt) {
FGAIBase::update(dt);
Run(dt);
Transform();
}
-void FGAIAircraft::setPerformance(const std::string& acclass) {
- static PerformanceDB perfdb; //TODO make it a global service
- setPerformance(perfdb.getDataFor(acclass));
- }
-
+void FGAIAircraft::setPerformance(const std::string& acType, const std::string& acclass)
+{
+ static PerformanceDB perfdb; //TODO make it a global service
+ _performance = perfdb.getDataFor(acType, acclass);
+}
+#if 0
void FGAIAircraft::setPerformance(PerformanceData *ps) {
_performance = ps;
}
-
+#endif
void FGAIAircraft::Run(double dt) {
FGAIAircraft::dt = dt;
void FGAIAircraft::checkVisibility()
{
double visibility_meters = fgGetDouble("/environment/visibility-m");
- FGViewer* vw = globals->get_current_view();
- invisible = (SGGeodesy::distanceM(vw->getPosition(), pos) > visibility_meters);
+ invisible = (SGGeodesy::distanceM(globals->get_view_position(), pos) > visibility_meters);
}
void FGAIAircraft::AccelTo(double speed) {
tgt_speed = speed;
- assertSpeed(speed);
+ //assertSpeed(speed);
if (!isStationary())
_needsGroundElevation = true;
}
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
+ if (!curr)
+ {
+ // Oops! FIXME
+ return;
+ }
+
if (! leadPointReached(curr)) {
controlHeading(curr);
controlSpeed(curr, next);
{
// err is negative when we passed too high
double vert_m = vert_ft * SG_FEET_TO_METER;
- double err_m = err * SG_FEET_TO_METER;
+ //double err_m = err * SG_FEET_TO_METER;
//double angle = atan2(vert_m, dist_m);
double speedMs = (speed * SG_NM_TO_METER) / 3600;
//double vs = cos(angle) * speedMs; // Now in m/s
<< "speedFraction << " << speedFraction << " "
<< "Currecnt speed << " << speed << " "
<< endl;
- raise(SIGSEGV);
+ //raise(SIGSEGV);
}
}
// Only do the proper hitlist stuff if we are within visible range of the viewer.
if (!invisible) {
- double visibility_meters = fgGetDouble("/environment/visibility-m");
- FGViewer* vw = globals->get_current_view();
-
- if (SGGeodesy::distanceM(vw->getPosition(), pos) > visibility_meters) {
+ double visibility_meters = fgGetDouble("/environment/visibility-m");
+ if (SGGeodesy::distanceM(globals->get_view_position(), pos) > visibility_meters) {
return;
}
setAltitude(prev->getAltitude());
if (prev->getSpeed() > 0.0)
- setHeading(fp->getBearing(prev->getLatitude(), prev->getLongitude(), curr));
+ setHeading(fp->getBearing(prev, curr));
else
- setHeading(fp->getBearing(curr->getLatitude(), curr->getLongitude(), prev));
+ setHeading(fp->getBearing(curr, prev));
// If next doesn't exist, as in incrementally created flightplans for
// AI/Trafficmanager created plans,
if (tgt_speed > -0.5) {
tgt_speed = -0.5;
}
- assertSpeed(tgt_speed);
+ //assertSpeed(tgt_speed);
if (fp->getPreviousWaypoint()->getSpeed() < tgt_speed) {
fp->getPreviousWaypoint()->setSpeed(tgt_speed);
}
// << lead_dist << " " << curr->name
// << " Ground target speed " << groundTargetSpeed << endl;
double bearing = 0;
- if (speed > 50) { // don't do bearing calculations for ground traffic
- bearing = getBearing(fp->getBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr));
+ // don't do bearing calculations for ground traffic
+ bearing = getBearing(fp->getBearing(pos, curr));
if (bearing < minBearing) {
minBearing = bearing;
if (minBearing < 10) {
} else {
speedFraction = 1.0;
}
- }
- }
+ }
if (trafficRef) {
//cerr << "Tracking callsign : \"" << fgGetString("/ai/track-callsign") << "\"" << endl;
-/* if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
- cerr << trafficRef->getCallSign() << " " << tgt_altitude_ft << " " << _getSpeed() << " "
- << _getAltitude() << " "<< _getLatitude() << " " << _getLongitude() << " " << dist_to_go << " " << lead_dist << " " << curr->name << " " << vs << " " << tgt_vs << " " << bearing << " " << minBearing << " " << speedFraction << endl;
- }*/
+ //if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
+ //cerr << trafficRef->getCallSign() << " " << tgt_altitude_ft << " " << _getSpeed() << " "
+ // << _getAltitude() << " "<< _getLatitude() << " " << _getLongitude() << " " << dist_to_go << " " << lead_dist << " " << curr->getName() << " " << vs << " " << //tgt_vs << " " << bearing << " " << minBearing << " " << speedFraction << endl;
+ //}
}
- if ((dist_to_go < lead_dist) || (bearing > (minBearing * 1.1))) {
+ if ((dist_to_go < lead_dist) ||
+ ((dist_to_go > prev_dist_to_go) && (bearing > (minBearing * 1.1))) ) {
minBearing = 360;
speedFraction = 1.0;
+ prev_dist_to_go = HUGE_VAL;
return true;
} else {
+ prev_dist_to_go = dist_to_go;
return false;
}
}
-bool FGAIAircraft::aiTrafficVisible() {
- SGGeod userPos(SGGeod::fromDeg(fgGetDouble("/position/longitude-deg"),
- fgGetDouble("/position/latitude-deg")));
-
- return (SGGeodesy::distanceNm(userPos, pos) <= TRAFFICTOAIDISTTODIE);
+bool FGAIAircraft::aiTrafficVisible()
+{
+ SGVec3d cartPos = SGVec3d::fromGeod(pos);
+ const double d2 = (TRAFFICTOAIDISTTODIE * SG_NM_TO_METER) *
+ (TRAFFICTOAIDISTTODIE * SG_NM_TO_METER);
+ return (distSqr(cartPos, globals->get_aircraft_position_cart()) < d2);
}
//cerr << trafficRef->getCallSign() << " has passed waypoint " << prev->name << " at speed " << speed << endl;
//cerr << "Passing waypoint : " << prev->getName() << endl;
if (prev->contains("PushBackPoint")) {
- dep->getDynamics()->releaseParking(fp->getGate());
+ // clearing the parking assignment will release the gate
+ fp->setGate(ParkingAssignment());
AccelTo(0.0);
//setTaxiClearanceRequest(true);
}
if (prev->contains(string("Accel"))) {
takeOffStatus = 3;
}
- /*if (prev->contains(string("final"))) {
-
- cerr << getCallSign() << " "
- << fp->getPreviousWaypoint()->getName()
- << ". Alt = " << altitude_ft
- << " vs " << vs
- << " horizontal speed " << speed
- << "Previous crossAT " << fp->getPreviousWaypoint()->getCrossat()
- << "Airport elevation" << getTrafficRef()->getArrivalAirport()->getElevation()
- << "Altitude difference " << (altitude_ft - fp->getPreviousWaypoint()->getCrossat()) << endl;
- }*/
+ //if (prev->contains(string("landing"))) {
+ // if (speed < _performance->vTaxi() * 2) {
+ // fp->shortenToFirst(2, "legend");
+ // }
+ //}
+ //if (prev->contains(string("final"))) {
+ //
+ // cerr << getCallSign() << " "
+ // << fp->getPreviousWaypoint()->getName()
+ // << ". Alt = " << altitude_ft
+ // << " vs " << vs
+ // << " horizontal speed " << speed
+ // << "Previous crossAT " << fp->getPreviousWaypoint()->getCrossat()
+ // << "Airport elevation" << getTrafficRef()->getArrivalAirport()->getElevation()
+ // << "Altitude difference " << (altitude_ft - fp->getPreviousWaypoint()->getCrossat()) << endl;
+ //q}
// This is the last taxi waypoint, and marks the the end of the flight plan
// so, the schedule should update and wait for the next departure time.
if (prev->contains("END")) {
* @param curr
*/
void FGAIAircraft::controlHeading(FGAIWaypoint* curr) {
- double calc_bearing = fp->getBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
+ double calc_bearing = fp->getBearing(pos, curr);
//cerr << "Bearing = " << calc_bearing << endl;
if (speed < 0) {
calc_bearing +=180;
- if (calc_bearing > 360)
- calc_bearing -= 360;
+ SG_NORMALIZE_RANGE(calc_bearing, 0.0, 360.0);
}
if (finite(calc_bearing)) {
cerr << "calc_bearing is not a finite number : "
<< "Speed " << speed
<< "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg()
- << "waypoint " << curr->getLatitude() << ", " << curr->getLongitude() << endl;
- cerr << "waypoint name " << curr->getName();
- exit(1); // FIXME
+ << ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() << endl;
+ cerr << "waypoint name: '" << curr->getName() << "'" << endl;
+ //exit(1); // FIXME
}
}
if (fabs(speed_diff) > 10) {
prevSpeed = speed;
- assertSpeed(speed);
+ //assertSpeed(speed);
if (next) {
fp->setLeadDistance(speed, tgt_heading, curr, next);
}
}
}
-void FGAIAircraft::updatePosition() {
- // convert speed to degrees per second
- double speed_north_deg_sec = cos( hdg * SGD_DEGREES_TO_RADIANS )
- * speed * 1.686 / ft_per_deg_lat;
- double speed_east_deg_sec = sin( hdg * SGD_DEGREES_TO_RADIANS )
- * speed * 1.686 / ft_per_deg_lon;
-
- // set new position
- pos.setLatitudeDeg( pos.getLatitudeDeg() + speed_north_deg_sec * dt);
- pos.setLongitudeDeg( pos.getLongitudeDeg() + speed_east_deg_sec * dt);
-}
-
-
void FGAIAircraft::updateHeading() {
// adjust heading based on current bank angle
if (roll == 0.0)
// If on ground, calculate heading change directly
if (onGround()) {
double headingDiff = fabs(hdg-tgt_heading);
- double bank_sense = 0.0;
+// double bank_sense = 0.0;
/*
double diff = fabs(hdg - tgt_heading);
if (diff > 180)
if (sum > 360.0)
sum -= 360.0;
if (fabs(sum - tgt_heading) > 0.0001) {
- bank_sense = -1.0;
+// bank_sense = -1.0;
} else {
- bank_sense = 1.0;
+// bank_sense = 1.0;
}
//if (trafficRef)
// cerr << trafficRef->getCallSign() << " Heading "
// << hdg << ". Target " << tgt_heading << ". Diff " << fabs(sum - tgt_heading) << ". Speed " << speed << endl;
//if (headingDiff > 60) {
groundTargetSpeed = tgt_speed; // * cos(headingDiff * SG_DEGREES_TO_RADIANS);
- assertSpeed(groundTargetSpeed);
+ //assertSpeed(groundTargetSpeed);
//groundTargetSpeed = tgt_speed - tgt_speed * (headingDiff/180);
//} else {
// groundTargetSpeed = tgt_speed;
//}
if (sign(groundTargetSpeed) != sign(tgt_speed))
groundTargetSpeed = 0.21 * sign(tgt_speed); // to prevent speed getting stuck in 'negative' mode
- assertSpeed(groundTargetSpeed);
+ //assertSpeed(groundTargetSpeed);
// Only update the target values when we're not moving because otherwise we might introduce an enormous target change rate while waiting a the gate, or holding.
if (speed != 0) {
if (headingDiff > 30.0) {
// << hdg << ". Target " << tgt_heading << ". Diff " << fabs(sum - tgt_heading) << ". Speed " << speed << "Heading change rate : " << headingChangeRate << " bacnk sence " << bank_sense << endl;
hdg += headingChangeRate * dt * sqrt(fabs(speed) / 15);
headingError = headingDiff;
+ if (fabs(headingError) < 1.0) {
+ hdg = tgt_heading;
+ }
} else {
if (fabs(speed) > 1.0) {
turn_radius_ft = 0.088362 * speed * speed
// find target vertical speed
if (use_perf_vs) {
if (altitude_ft < tgt_altitude_ft) {
- tgt_vs = tgt_altitude_ft - altitude_ft;
- if (tgt_vs > _performance->climbRate())
- tgt_vs = _performance->climbRate();
+ tgt_vs = std::min(tgt_altitude_ft - altitude_ft, _performance->climbRate());
} else {
- tgt_vs = tgt_altitude_ft - altitude_ft;
- if (tgt_vs < (-_performance->descentRate()))
- tgt_vs = -_performance->descentRate();
+ tgt_vs = std::max(tgt_altitude_ft - altitude_ft, -_performance->descentRate());
}
} else {
double vert_dist_ft = fp->getCurrentWaypoint()->getCrossat() - altitude_ft;
}
}
-string FGAIAircraft::atGate() {
- string tmp("");
- if (fp->getLeg() < 3) {
- if (trafficRef) {
- if (fp->getGate() > 0) {
- FGParking *park =
- trafficRef->getDepartureAirport()->getDynamics()->getParking(fp->getGate());
- tmp = park->getName();
- }
- }
+string FGAIAircraft::atGate()
+{
+ if ((fp->getLeg() < 3) && trafficRef) {
+ if (fp->getParkingGate()) {
+ return fp->getParkingGate()->ident();
+ }
}
- return tmp;
+
+ return string();
}
void FGAIAircraft::handleATCRequests() {
void FGAIAircraft::updateActualState() {
//update current state
//TODO have a single tgt_speed and check speed limit on ground on setting tgt_speed
- updatePosition();
+ double distance = speed * SG_KT_TO_MPS * dt;
+ pos = SGGeodesy::direct(pos, hdg, distance);
+
if (onGround())
- speed = _performance->actualSpeed(this, groundTargetSpeed, dt);
+ speed = _performance->actualSpeed(this, groundTargetSpeed, dt, holdPos);
else
- speed = _performance->actualSpeed(this, (tgt_speed *speedFraction), dt);
- assertSpeed(speed);
+ speed = _performance->actualSpeed(this, (tgt_speed *speedFraction), dt, false);
+ //assertSpeed(speed);
updateHeading();
roll = _performance->actualBankAngle(this, tgt_roll, dt);
projects / flightgear.git / blobdiff