_pitch(0),
_pitch_deg(0),
_speed_kt(0),
+_selected_ac(0),
_range_ft(0),
_relbrg (0),
_parent_speed(0),
+_dt_count(0),
+_next_run(0),
_parent_x_offset(0),
-_selected_ac(0)
+_parent_y_offset(0),
+_parent("")
+
{
invisible = false;
}
setNoRoll(scFileNode->getBoolValue("no-roll", true));
setName(scFileNode->getStringValue("name", "groundvehicle"));
setSMPath(scFileNode->getStringValue("submodel-path", ""));
- setContactX1offset(scFileNode->getDoubleValue("contact_x1_offset", 0.0));
- setContactX2offset(scFileNode->getDoubleValue("contact_x2_offset", 0.0));
+ setContactX1offset(scFileNode->getDoubleValue("contact-x1-offset", 0.0));
+ setContactX2offset(scFileNode->getDoubleValue("contact-x2-offset", 0.0));
setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 38.55));
+ setYOffset(scFileNode->getDoubleValue("hitch-y-offset", 0.0));
setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
- setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.5));
- setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.5));
+ setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.1));
+ setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.25));
setParentName(scFileNode->getStringValue("parent", ""));
setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 2.0));
setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0));
SGRawValuePointer<double>(&_range_ft));
props->tie("hitch/x-offset-ft",
SGRawValuePointer<double>(&_x_offset));
+ props->tie("hitch/y-offset-ft",
+ SGRawValuePointer<double>(&_y_offset));
props->tie("hitch/parent-x-offset-ft",
SGRawValuePointer<double>(&_parent_x_offset));
+ props->tie("hitch/parent-y-offset-ft",
+ SGRawValuePointer<double>(&_parent_y_offset));
props->tie("controls/constants/tow-angle/gain",
SGRawValuePointer<double>(&_tow_angle_gain));
props->tie("controls/constants/tow-angle/limit-deg",
SGRawValuePointer<double>(&_tow_angle_limit));
-
-
- //we may need these later for towed vehicles
-
- // (*this, &FGAIBallistic::getElevHitchToUser));
- //props->tie("position/x-offset",
- // SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset));
- //props->tie("position/y-offset",
- // SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getYOffset, &FGAIBase::setYoffset));
- //props->tie("position/z-offset",
- // SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getZOffset, &FGAIBase::setZoffset));
- //props->tie("position/tgt-x-offset",
- // SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtXOffset, &FGAIWingman::setTgtXOffset));
- //props->tie("position/tgt-y-offset",
- // SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtYOffset, &FGAIWingman::setTgtYOffset));
- //props->tie("position/tgt-z-offset",
- // SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtZOffset, &FGAIWingman::setTgtZOffset));
+ props->tie("controls/contact-x1-offset-ft",
+ SGRawValuePointer<double>(&_contact_x1_offset));
+ props->tie("controls/contact-x2-offset-ft",
+ SGRawValuePointer<double>(&_contact_x2_offset));
}
void FGAIGroundVehicle::unbind() {
props->untie("hitch/tow-angle-deg");
props->untie("hitch/range-ft");
props->untie("hitch/x-offset-ft");
+ props->untie("hitch/y-offset-ft");
props->untie("hitch/parent-x-offset-ft");
+ props->untie("hitch/parent-y-offset-ft");
props->untie("controls/constants/tow-angle/gain");
props->untie("controls/constants/tow-angle/limit-deg");
-
- //we may need these later for towed vehicles
- //props->untie("load/rel-brg-to-user-deg");
- //props->untie("load/elev-to-user-deg");
- //props->untie("velocities/vertical-speed-fps");
- //props->untie("position/x-offset");
- //props->untie("position/y-offset");
- //props->untie("position/z-offset");
- //props->untie("position/tgt-x-offset");
- //props->untie("position/tgt-y-offset");
- //props->untie("position/tgt-z-offset");
+ props->untie("controls/contact-x1-offset-ft");
+ props->untie("controls/contact-x2-offset-ft");
}
bool FGAIGroundVehicle::init(bool search_in_AI_path) {
return false;
invisible = false;
-
_limit = 200;
no_roll = true;
void FGAIGroundVehicle::update(double dt) {
// SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name );
- if (getPitch()){
- setElevation(_elevation, dt, _elevation_coeff);
- ClimbTo(_elevation_ft);
- setPitch(_pitch, dt, _pitch_coeff);
- PitchTo(_pitch_deg);
- }
-
- if(_parent !=""){
- setParent();
-
- string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
- bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
- _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
-
- if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){
- SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
- << " setting END: getting new waypoints ");
- AdvanceFP();
- setWPNames();
- /*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
- } else if (parent_waiting && !_waiting){
- SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
- << " setting WAIT/WAITUNTIL: getting new waypoints ");
- AdvanceFP();
- setWPNames();
- _waiting = true;
- } else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->name == "WAIT"){
- SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
- << " wait done: getting new waypoints ");
- _waiting = false;
- _wait_count = 0;
- fp->IncrementWaypoint(false);
- next = fp->getNextWaypoint();
-
- if (next->name == "WAITUNTIL" || next->name == "WAIT"
- || next->name == "END"){
- } else {
- prev = curr;
- fp->IncrementWaypoint(false);
- curr = fp->getCurrentWaypoint();
- next = fp->getNextWaypoint();
- }
-
- setWPNames();
- } else if (_range_ft > 1000){
-
- SG_LOG(SG_GENERAL, SG_INFO, "AIGroundVeh1cle: " << _name
- << " rescue: reforming train " << _range_ft << " " << _x_offset * 15);
-
- setTowAngle(0, dt, 1);
- setSpeed(_parent_speed * 2);
-
- } else if (_parent_speed > 1){
-
- setTowSpeed();
- setTowAngle(_relbrg, dt, 1);
-
- } else if (_parent_speed < -1){
-
- setTowSpeed();
-
- if (_relbrg < 0)
- setTowAngle(-(180 - (360 + _relbrg)), dt, 1);
- else
- setTowAngle(-(180 - _relbrg), dt, 1);
-
- } else
- setSpeed(_parent_speed);
- }
-
- FGAIShip::update(dt);
+ RunGroundVehicle(dt);
+// FGAIShip::update(dt);
}
void FGAIGroundVehicle::setNoRoll(bool nr) {
_x_offset = x;
}
+void FGAIGroundVehicle::setYOffset(double y) {
+ _y_offset = y;
+}
+
void FGAIGroundVehicle::setPitchCoeff(double pc) {
_pitch_coeff = pc;
}
}
void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){
- ta *= _tow_angle_gain;
- //_tow_angle = pow(ta,2) * sign(ta);
- //if (_tow_angle > _tow_angle_limit) _tow_angle = _tow_angle_limit;
- //if (_tow_angle < -_tow_angle_limit) _tow_angle = -_tow_angle_limit;
+ //_tow_angle = ta * _tow_angle_gain;
+ _tow_angle = pow(ta,2) * sign(ta);
SG_CLAMP_RANGE(_tow_angle, -_tow_angle_limit, _tow_angle_limit);
}
bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) {
- if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 10000),
- _elevation_m, &_material)){
- _ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
+ double height_m ;
+
+ if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 10000), height_m, &_material,0)){
+ _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;
if (_material) {
const vector<string>& names = _material->get_names();
_solid = _material->get_solid();
- _load_resistance = _material->get_load_resistance();
- _frictionFactor =_material->get_friction_factor();
- _elevation = _elevation_m * SG_METER_TO_FEET;
if (!names.empty())
props->setStringValue("material/name", names[0].c_str());
bool FGAIGroundVehicle::getPitch() {
- double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
- double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
- double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
+ if (!_tunnel){
+
+ double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
+ double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
+ double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
+
+ SGVec3d front(-contact_offset_x1_m, 0, 0);
+ SGVec3d rear(-contact_offset_x2_m, 0, 0);
+ SGVec3d Front = getCartPosAt(front);
+ SGVec3d Rear = getCartPosAt(rear);
- SGVec3d front(-contact_offset_x1_m, 0, 0);
- SGVec3d rear(-contact_offset_x2_m, 0, 0);
- SGVec3d Front = getCartPosAt(front);
- SGVec3d Rear = getCartPosAt(rear);
+ SGGeod geodFront = SGGeod::fromCart(Front);
+ SGGeod geodRear = SGGeod::fromCart(Rear);
- SGGeod geodFront = SGGeod::fromCart(Front);
- SGGeod geodRear = SGGeod::fromCart(Rear);
+ double front_elev_m = 0;
+ double rear_elev_m = 0;
+ double elev_front = 0;
+ double elev_rear = 0;
+ double max_alt = 10000;
- double front_elev_m = 0;
- double rear_elev_m = 0;
- double elev_front = 0;
- double elev_rear = 0;
+ if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 10000), elev_front,
+ &_material, 0)){
+ front_elev_m = elev_front;
+ } else
+ return false;
- if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 10000),
- elev_front, &_material)){
+ if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodRear, 10000),
+ elev_rear, &_material,0)){
+ rear_elev_m = elev_rear;
+ } else
+ return false;
- front_elev_m = elev_front;
+ if (vel >= 0){
+ double diff = front_elev_m - rear_elev_m;
+ _pitch = atan2 (diff,
+ fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
+ _elevation = (rear_elev_m + diff/2) * SG_METER_TO_FEET;
+ } else {
+ double diff = rear_elev_m - front_elev_m;
+ _pitch = atan2 (diff,
+ fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
+ _elevation = (front_elev_m + diff/2) * SG_METER_TO_FEET;
+ _pitch = -_pitch;
+ }
- //if (_material) {
- //
- //}
+ } else {
- } else
- return false;
+ if (prev->altitude == 0 || curr->altitude == 0) return false;
- if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodRear, 10000),
- elev_rear, &_material)){
- rear_elev_m = elev_rear;
- //if (_material) {
- // rear_elev_m = elev_rear;
- //}
+ static double distance;
+ static double d_alt;
+ static double curr_alt;
+ static double prev_alt;
- } else
- return false;
+ if (_new_waypoint){
+ curr_alt = curr->altitude * SG_METER_TO_FEET;
+ prev_alt = prev->altitude * SG_METER_TO_FEET;
+ d_alt = curr_alt - prev_alt;
+
+ distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->longitude, prev->latitude),
+ SGGeod::fromDeg(curr->longitude, curr->latitude));
+
+ _pitch = atan2(d_alt, distance * SG_METER_TO_FEET) * SG_RADIANS_TO_DEGREES;
+ }
+
+
+ double distance_to_go = SGGeodesy::distanceM(SGGeod::fromDeg(pos.getLongitudeDeg(), pos.getLatitudeDeg()),
+ SGGeod::fromDeg(curr->longitude, curr->latitude));
+
+ //cout << "distance curr & prev " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET
+ // << " distance to go " << distance_to_go * SG_METER_TO_FEET
+ // << " d_alt ft " << d_alt
+ // << endl;
+
+ if (distance_to_go > distance)
+ _elevation = prev_alt;
+ else
+ _elevation = curr_alt - (tan(_pitch * SG_DEGREES_TO_RADIANS) * distance_to_go * SG_METER_TO_FEET);
- if (vel >= 0){
- double diff = front_elev_m - rear_elev_m;
- _pitch = atan2 (diff,
- fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
- _elevation = (rear_elev_m + diff/2) * SG_METER_TO_FEET;
- } else {
- double diff = rear_elev_m - front_elev_m;
- _pitch = atan2 (diff,
- fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
- _elevation = (front_elev_m + diff/2) * SG_METER_TO_FEET;
- _pitch = -_pitch;
}
+ getGroundElev(pos);
+
return true;
}
model = _selected_ac;
} else {
model = ai->getChild(i);
+ string path = ai->getPath();
const string name = model->getStringValue("name");
if (!model->nChildren()){
double lat = _selected_ac->getDoubleValue("position/latitude-deg");
double lon = _selected_ac->getDoubleValue("position/longitude-deg");
double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
- double hitch_offset_m = _x_offset * SG_FEET_TO_METER;
+ double hitch_x_offset_m = _selected_ac->getDoubleValue("hitch/x-offset-ft")
+ * SG_FEET_TO_METER;
+ double hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft")
+ * SG_FEET_TO_METER;
+
_selectedpos.setLatitudeDeg(lat);
_selectedpos.setLongitudeDeg(lon);
_selectedpos.setElevationFt(elevation);
- SGVec3d rear_hitch(-hitch_offset_m, 0, 0);
+ _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
+ _parent_y_offset = _selected_ac->getDoubleValue("hitch/y-offset-ft");
+ _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
+
+ SGVec3d rear_hitch(-hitch_x_offset_m, hitch_y_offset_m, 0);
SGVec3d RearHitch = getCartHitchPosAt(rear_hitch);
SGGeod rearpos = SGGeod::fromCart(RearHitch);
// calculate the bearing and range of the second pos from the first
double az2, distance;
geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
- range = distance *= SG_METER_TO_NM;
+ range = distance * SG_METER_TO_NM;
}
double FGAIGroundVehicle::calcRelBearingDeg(double bearing, double heading)
{
double angle = bearing - heading;
-
SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
-
return angle;
}
void FGAIGroundVehicle::setTowSpeed(){
- _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
-
- // double diff = _range_ft - _parent_x_offset;
+ double diff = _range_ft - _x_offset;
double x = 0;
if (_range_ft > _x_offset * 3) x = 50;
if (_relbrg < -90 || _relbrg > 90){
setSpeed(_parent_speed - 5 - x);
//cout << _name << " case 1r _relbrg spd - 5 " << _relbrg << " " << diff << endl;
- }else if (_range_ft > _parent_x_offset + 0.25 && _relbrg >= -90 && _relbrg <= 90){
+ }else if (_range_ft > _x_offset + 0.25 && _relbrg >= -90 && _relbrg <= 90){
setSpeed(_parent_speed + 1 + x);
//cout << _name << " case 2r _relbrg spd + 1 " << _relbrg << " "
// << diff << " range " << _range_ft << endl;
- } else if (_range_ft < _parent_x_offset - 0.25 && _relbrg >= -90 && _relbrg <= 90){
+ } else if (_range_ft < _x_offset - 0.25 && _relbrg >= -90 && _relbrg <= 90){
setSpeed(_parent_speed - 1 - x);
//cout << _name << " case 3r _relbrg spd - 2 " << _relbrg << " "
// << diff << " " << _range_ft << endl;
}
}
+
+void FGAIGroundVehicle::RunGroundVehicle(double dt){
+
+ _dt_count += dt;
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Check execution time (currently once every 0.05 sec or 20 fps)
+ // Add a bit of randomization to prevent the execution of all flight plans
+ // in synchrony, which can add significant periodic framerate flutter.
+ ///////////////////////////////////////////////////////////////////////////
+
+ //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
+ if (_dt_count < _next_run)
+ return;
+
+ _next_run = 0.055 + (0.015 * sg_random());
+
+ if (getPitch()){
+ setElevation(_elevation, _dt_count, _elevation_coeff);
+ ClimbTo(_elevation_ft);
+ setPitch(_pitch, _dt_count, _pitch_coeff);
+ PitchTo(_pitch_deg);
+ }
+
+ if(_parent == ""){
+ AccelTo(prev->speed);
+ FGAIShip::update(_dt_count);
+ _dt_count = 0;
+ return;
+ }
+
+ setParent();
+
+ string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
+ bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
+
+ if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){
+ SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
+ << " setting END: getting new waypoints ");
+ AdvanceFP();
+ setWPNames();
+ /*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
+ } else if (parent_waiting && !_waiting){
+ SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
+ << " setting WAIT/WAITUNTIL: getting new waypoints ");
+ AdvanceFP();
+ setWPNames();
+ _waiting = true;
+ } else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->name == "WAIT"){
+ SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
+ << " wait done: getting new waypoints ");
+ _waiting = false;
+ _wait_count = 0;
+ fp->IncrementWaypoint(false);
+ next = fp->getNextWaypoint();
+
+ if (next->name == "WAITUNTIL" || next->name == "WAIT"
+ || next->name == "END"){
+ } else {
+ prev = curr;
+ fp->IncrementWaypoint(false);
+ curr = fp->getCurrentWaypoint();
+ next = fp->getNextWaypoint();
+ }
+
+ setWPNames();
+ } else if (_range_ft > _parent_x_offset * 4){
+ SG_LOG(SG_GENERAL, SG_INFO, "AIGroundVeh1cle: " << _name
+ << " rescue: reforming train " << _range_ft << " " << _x_offset * 15);
+
+ setTowAngle(0, dt, 1);
+ setSpeed(_parent_speed * 2);
+
+ } else if (_parent_speed > 1){
+
+ setTowSpeed();
+ setTowAngle(_relbrg, dt, 1);
+
+ } else if (_parent_speed < -1){
+
+ setTowSpeed();
+
+ if (_relbrg < 0)
+ setTowAngle(-(180 - (360 + _relbrg)), dt, 1);
+ else
+ setTowAngle(-(180 - _relbrg), dt, 1);
+
+ } else
+ setSpeed(_parent_speed);
+
+ FGAIShip::update(_dt_count);
+ _dt_count = 0;
+
+}
+
// end AIGroundvehicle
FGAIShip::FGAIShip(object_type ot) :
- FGAIBase(ot),
- _limit(40),
- _elevation_m(0),
- _elevation_ft(0),
- _tow_angle(0),
- _dt_count(0),
- _next_run(0),
- _lead_angle(0),
- _xtrack_error(0)
+FGAIBase(ot),
+_limit(40),
+_elevation_m(0),
+_elevation_ft(0),
+_tow_angle(0),
+_dt_count(0),
+_next_run(0),
+_lead_angle(0),
+_xtrack_error(0),
+_tunnel(false),
+_curr_alt(0),
+_prev_alt(0),
+_until_time(""),
+_fp_init(false),
+_missed(false),
+_waiting(false),
+_new_waypoint(true),
+_missed_count(0),
+_wait_count(0),
+_missed_time_sec(30),
+_day(86400),
+_wp_range(0),
+_old_range(0),
+_range_rate(0),
+_roll_constant(0.001),
+_hdg_constant(0.01),
+_roll_factor(-0.0083335)
{
+ invisible = false;
}
FGAIShip::~FGAIShip() {
setSpeedConstant(scFileNode->getDoubleValue("speed-constant", 0.5));
if (!flightplan.empty()) {
- SG_LOG(SG_GENERAL, SG_ALERT, "getting flightplan: " << _name );
+ SG_LOG(SG_GENERAL, SG_ALERT, "getting flightplan: " << _name );
FGAIFlightPlan* fp = new FGAIFlightPlan(flightplan);
setFlightPlan(fp);
curr = 0; // the one ahead
next = 0; // the next plus 1
- _until_time = "";
-
props->setStringValue("name", _name.c_str());
props->setStringValue("waypoint/name-prev", _prev_name.c_str());
props->setStringValue("waypoint/name-curr", _curr_name.c_str());
_rudder = 0.0;
no_roll = false;
- _roll_constant = 0.001;
- _hdg_constant = 0.01;
- _roll_factor = -0.0083335;
-
_rd_turn_radius_ft = _sp_turn_radius_ft = turn_radius_ft;
- _fp_init = false;
- _missed = false;
- _waiting = false;
- _new_waypoint = true;
-
- _missed_count = 0;
- _wait_count = 0;
- _missed_time_sec = 30;
-
- _day = 86400;
-
-
- _wp_range = _old_range = 0;
- _range_rate = 0;
-
if (fp)
_fp_init = initFlightPlan();
SGRawValuePointer<bool>(&_waiting));
props->tie("waypoint/lead-angle-deg",
SGRawValuePointer<double>(&_lead_angle));
+ props->tie("waypoint/tunnel",
+ SGRawValuePointer<bool>(&_tunnel));
+ props->tie("waypoint/alt-curr-m",
+ SGRawValuePointer<double>(&_curr_alt));
+ props->tie("waypoint/alt-prev-m",
+ SGRawValuePointer<double>(&_prev_alt));
props->tie("submodels/serviceable",
SGRawValuePointer<bool>(&_serviceable));
props->tie("controls/turn-radius-ft",
props->untie("waypoint/lead-angle-deg");
props->untie("waypoint/xtrack-error-ft");
props->untie("waypoint/waiting");
+ props->untie("waypoint/tunnel");
+ props->untie("waypoint/alt-curr-m");
+ props->untie("waypoint/alt-prev-m");
props->untie("submodels/serviceable");
props->untie("controls/turn-radius-ft");
props->untie("controls/turn-radius-corrected-ft");
// Only change these values if we are able to compute them safely
if (SGLimits<double>::min() < dt) {
// Now here is the finite difference ...
-
+
// Transform that one to the horizontal local coordinate system.
SGQuatd ec2hlNew = SGQuatd::fromLonLat(pos);
// compute the new orientation
dOr.getAngleAxis(dOrAngleAxis);
// divided by the time difference provides a rotation speed vector
dOrAngleAxis /= dt;
-
+
aip.setBodyAngularVelocity(dOrAngleAxis);
}
}
// adjust turn radius for speed. The equation is very approximate.
// we need to allow for negative speeds
if (type == "ship")
- _sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft;
+ _sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft;
else
_sp_turn_radius_ft = turn_radius_ft;
// set the _rudder limit by speed
if (type == "ship"){
- if (speed <= 40)
- rudder_limit = (-0.825 * speed) + 35;
- else
- rudder_limit = 2;
+ if (speed <= 40)
+ rudder_limit = (-0.825 * speed) + 35;
+ else
+ rudder_limit = 2;
} else
rudder_limit = 20;
setPrevName("");
if (curr != 0)
- setCurrName(curr->name);
+ setCurrName(curr->name);
else{
setCurrName("");
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: current wp name error" );
void FGAIShip::ProcessFlightPlan(double dt) {
double time_sec = getDaySeconds();
- double until_time_sec = 0;
- _missed = false;
_dt_count += dt;
///////////////////////////////////////////////////////////////////////////
_next_run = 1.0 + (0.5 * sg_random());
+ double until_time_sec = 0;
+ _missed = false;
+
// check to see if we've reached the point for our next turn
// if the range to the waypoint is less than the calculated turn
// radius we can start the turn to the next leg
if ((_wp_range < (sp_turn_radius_nm * 1.25)) || _missed || (_waiting && !_new_waypoint)) {
- if (_next_name == "END" || fp->getNextWaypoint() == 0) {
+ if (_next_name == "TUNNEL"){
+ _tunnel = !_tunnel;
+
+ //SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " TUNNEL ");
+
+ fp->IncrementWaypoint(false);
+ next = fp->getNextWaypoint();
+
+ if (next->name == "WAITUNTIL" || next->name == "WAIT"
+ || next->name == "END" || next->name == "TUNNEL")
+ return;
+
+ prev = curr;
+ fp->IncrementWaypoint(false);
+ curr = fp->getCurrentWaypoint();
+ next = fp->getNextWaypoint();
+
+ }else if(_next_name == "END" || fp->getNextWaypoint() == 0) {
if (_repeat) {
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: "<< _name << "Flightplan restarting ");
next = fp->getNextWaypoint();
if (next->name == "WAITUNTIL" || next->name == "WAIT"
- || next->name == "END")
+ || next->name == "END" || next->name == "TUNNEL")
return;
prev = curr;
_until_time = next->time;
setUntilTime(next->time);
if (until_time_sec > time_sec) {
- SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " "
+ SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " "
<< curr->name << " waiting until: "
<< _until_time << " " << until_time_sec << " now " << time_sec );
setSpeed(0);
_wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
_old_range = _wp_range;
setWPPos();
- AccelTo(prev->speed);
+ object_type type = getType();
+
+ if (type != 10)
+ AccelTo(prev->speed);
+
+ _curr_alt = curr->altitude;
+ _prev_alt = prev->altitude;
} else {
_new_waypoint = false;
else
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: Bearing or Range is not a finite number");
- _dt_count = 0;
+ _dt_count = 0;
} // end Processing FlightPlan
bool FGAIShip::initFlightPlan() {
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " initializing waypoints ");
bool init = false;
-
_start_sec = 0;
fp->restart();
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
- }
+ } // end while loop
if (!_start_time.empty()){
_start_sec = processTimeString(_start_time);
}
} else {
- setLatitude(prev->latitude);
- setLongitude(prev->longitude);
- setSpeed(prev->speed);
+ setLatitude(prev->latitude);
+ setLongitude(prev->longitude);
+ setSpeed(prev->speed);
}
setWPNames();
next = fp->getNextWaypoint();
if (next->name != "WAITUNTIL" && next->name != "WAIT"
- && next->name != "END") {
- prev = curr;
- fp->IncrementWaypoint(false);
- curr = fp->getCurrentWaypoint();
- next = fp->getNextWaypoint();
+ && next->name != "END") {
+ prev = curr;
+ fp->IncrementWaypoint(false);
+ curr = fp->getCurrentWaypoint();
+ next = fp->getNextWaypoint();
}
} else if (next->name == "WAITUNTIL") {
// so we will calculate the distance back up the track from the current waypoint
// then calculate the lat and lon.
- /*cout << "advancing flight plan done elapsed_sec: " << elapsed_sec
- << " " << day_sec << endl;*/
+ //cout << "advancing flight plan done elapsed_sec: " << elapsed_sec
+ // << " " << day_sec << endl;
double time_diff = elapsed_sec - day_sec;
double lat, lon, recip;
void FGAIShip::setWPPos() {
- if (curr->name == "END" || curr->name == "WAIT" || curr->name == "WAITUNTIL"){
- cout<< curr->name << endl;
- return;
+ if (curr->name == "END" || curr->name == "WAIT"
+ || curr->name == "WAITUNTIL" || curr->name == "TUNNEL"){
+ //cout << curr->name << " returning" << endl;
+ return;
}
double elevation_m = 0;
wppos.setLatitudeDeg(curr->latitude);
wppos.setLongitudeDeg(curr->longitude);
- wppos.setElevationFt(0);
+ wppos.setElevationM(0);
if (curr->on_ground){
wppos.setElevationM(elevation_m);
}
+ //cout << curr->name << " setting measured elev " << elevation_m << endl;
+
} else {
- wppos.setElevationFt(curr->altitude);
+ wppos.setElevationM(curr->altitude);
+ //cout << curr->name << " setting FP elev " << elevation_m << endl;
}
+ curr->altitude = wppos.getElevationM();
+
}
void FGAIShip::setXTrackError() {
curr->latitude, curr->longitude);
double xtrack_error_nm = sin((course - brg)* SG_DEGREES_TO_RADIANS) * _wp_range;
- //if (_wp_range > _sp_turn_radius_ft / (2 * 6076.1155)){
if (_wp_range > 0){
_lead_angle = atan2(xtrack_error_nm,(_wp_range * _proportion)) * SG_RADIANS_TO_DEGREES;
} else
_lead_angle *= _lead_angle_gain;
_xtrack_error = xtrack_error_nm * 6076.1155;
- if (_lead_angle<= -_lead_angle_limit)
- _lead_angle = -_lead_angle_limit;
- else if (_lead_angle >= _lead_angle_limit)
- _lead_angle = _lead_angle_limit;
+ SG_CLAMP_RANGE(_lead_angle, -_lead_angle_limit, _lead_angle_limit);
}
projects / flightgear.git / commitdiff