1
0
Fork 0

Add support for high speed trains. Change update rate to main frame rate, and move framerate-heavy functions to a 20Hz update cycle. Jitter is much reduced, at a small cost to framerate

Signed-off-by: v meazza <vivian.meazza@lineone.net>

Conflicts:

	src/AIModel/AIGroundVehicle.cxx
This commit is contained in:
Tim Moore 2009-11-01 00:15:14 +01:00
parent d974954b5a
commit d096455b7f
4 changed files with 121 additions and 63 deletions

View file

@ -45,7 +45,9 @@ _dt_count(0),
_next_run(0), _next_run(0),
_parent_x_offset(0), _parent_x_offset(0),
_parent_y_offset(0), _parent_y_offset(0),
_parent("") _parent_z_offset(0),
_parent(""),
_break_count(0)
{ {
invisible = false; invisible = false;
@ -64,16 +66,18 @@ void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) {
setSMPath(scFileNode->getStringValue("submodel-path", "")); setSMPath(scFileNode->getStringValue("submodel-path", ""));
setContactX1offset(scFileNode->getDoubleValue("contact-x1-offset", 0.0)); setContactX1offset(scFileNode->getDoubleValue("contact-x1-offset", 0.0));
setContactX2offset(scFileNode->getDoubleValue("contact-x2-offset", 0.0)); setContactX2offset(scFileNode->getDoubleValue("contact-x2-offset", 0.0));
setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 38.55)); setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 35.0));
setYOffset(scFileNode->getDoubleValue("hitch-y-offset", 0.0)); setYOffset(scFileNode->getDoubleValue("hitch-y-offset", 0.0));
setZOffset(scFileNode->getDoubleValue("hitch-z-offset", 0.0));
setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0)); setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0)); setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0)); setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.1)); setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.1));
setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.25)); setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.25));
setParentName(scFileNode->getStringValue("parent", "")); setParentName(scFileNode->getStringValue("parent", ""));
setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 2.0)); setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 1.0));
setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0)); setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0));
setInitialTunnel(scFileNode->getBoolValue("tunnel", false));
//we may need these later for towed vehicles //we may need these later for towed vehicles
// setSubID(scFileNode->getIntValue("SubID", 0)); // setSubID(scFileNode->getIntValue("SubID", 0));
// setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0)); // setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
@ -99,10 +103,14 @@ void FGAIGroundVehicle::bind() {
SGRawValuePointer<double>(&_x_offset)); SGRawValuePointer<double>(&_x_offset));
props->tie("hitch/y-offset-ft", props->tie("hitch/y-offset-ft",
SGRawValuePointer<double>(&_y_offset)); SGRawValuePointer<double>(&_y_offset));
props->tie("hitch/z-offset-ft",
SGRawValuePointer<double>(&_z_offset));
props->tie("hitch/parent-x-offset-ft", props->tie("hitch/parent-x-offset-ft",
SGRawValuePointer<double>(&_parent_x_offset)); SGRawValuePointer<double>(&_parent_x_offset));
props->tie("hitch/parent-y-offset-ft", props->tie("hitch/parent-y-offset-ft",
SGRawValuePointer<double>(&_parent_y_offset)); SGRawValuePointer<double>(&_parent_y_offset));
props->tie("hitch/parent-z-offset-ft",
SGRawValuePointer<double>(&_parent_z_offset));
props->tie("controls/constants/tow-angle/gain", props->tie("controls/constants/tow-angle/gain",
SGRawValuePointer<double>(&_tow_angle_gain)); SGRawValuePointer<double>(&_tow_angle_gain));
props->tie("controls/constants/tow-angle/limit-deg", props->tie("controls/constants/tow-angle/limit-deg",
@ -117,15 +125,17 @@ void FGAIGroundVehicle::unbind() {
FGAIShip::unbind(); FGAIShip::unbind();
props->untie("controls/constants/elevation-coeff"); props->untie("controls/constants/elevation-coeff");
props->untie("controls/constants/pitch-coeff");
props->untie("position/ht-AGL-ft"); props->untie("position/ht-AGL-ft");
props->untie("controls/constants/pitch-coeff");
props->untie("hitch/rel-bearing-deg"); props->untie("hitch/rel-bearing-deg");
props->untie("hitch/tow-angle-deg"); props->untie("hitch/tow-angle-deg");
props->untie("hitch/range-ft"); props->untie("hitch/range-ft");
props->untie("hitch/x-offset-ft"); props->untie("hitch/x-offset-ft");
props->untie("hitch/y-offset-ft"); props->untie("hitch/y-offset-ft");
props->untie("hitch/z-offset-ft");
props->untie("hitch/parent-x-offset-ft"); props->untie("hitch/parent-x-offset-ft");
props->untie("hitch/parent-y-offset-ft"); props->untie("hitch/parent-y-offset-ft");
props->untie("hitch/parent-y-offset-ft");
props->untie("controls/constants/tow-angle/gain"); props->untie("controls/constants/tow-angle/gain");
props->untie("controls/constants/tow-angle/limit-deg"); props->untie("controls/constants/tow-angle/limit-deg");
props->untie("controls/contact-x1-offset-ft"); props->untie("controls/contact-x1-offset-ft");
@ -145,9 +155,9 @@ bool FGAIGroundVehicle::init(bool search_in_AI_path) {
void FGAIGroundVehicle::update(double dt) { void FGAIGroundVehicle::update(double dt) {
// SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name ); // SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name );
FGAIShip::update(dt);
RunGroundVehicle(dt); RunGroundVehicle(dt);
// FGAIShip::update(dt);
} }
void FGAIGroundVehicle::setNoRoll(bool nr) { void FGAIGroundVehicle::setNoRoll(bool nr) {
@ -170,6 +180,10 @@ void FGAIGroundVehicle::setYOffset(double y) {
_y_offset = y; _y_offset = y;
} }
void FGAIGroundVehicle::setZOffset(double z) {
_z_offset = z;
}
void FGAIGroundVehicle::setPitchCoeff(double pc) { void FGAIGroundVehicle::setPitchCoeff(double pc) {
_pitch_coeff = pc; _pitch_coeff = pc;
} }
@ -201,9 +215,12 @@ void FGAIGroundVehicle::setParentName(const string& p) {
} }
void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){ void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){
//_tow_angle = ta * _tow_angle_gain; ta *= _tow_angle_gain;
_tow_angle = pow(ta,2) * sign(ta); double factor = -0.0045 * speed + 1;
SG_CLAMP_RANGE(_tow_angle, -_tow_angle_limit, _tow_angle_limit); double limit = _tow_angle_limit * factor;
// cout << "speed "<< speed << " _factor " << _factor<<" " <<_tow_angle_limit<< endl;
_tow_angle = pow(ta,2) * sign(ta) * factor;
SG_CLAMP_RANGE(_tow_angle, -limit, limit);
} }
bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) { bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) {
@ -242,10 +259,10 @@ bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) {
bool FGAIGroundVehicle::getPitch() { bool FGAIGroundVehicle::getPitch() {
if (!_tunnel){ if (!_tunnel){
double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0); double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER; double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER; double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
double _z_offset_m = _parent_z_offset * SG_FEET_TO_METER;
SGVec3d front(-contact_offset_x1_m, 0, 0); SGVec3d front(-contact_offset_x1_m, 0, 0);
SGVec3d rear(-contact_offset_x2_m, 0, 0); SGVec3d rear(-contact_offset_x2_m, 0, 0);
@ -261,9 +278,9 @@ bool FGAIGroundVehicle::getPitch() {
double elev_rear = 0; double elev_rear = 0;
double max_alt = 10000; double max_alt = 10000;
if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 3000), elev_front, if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 3000),
&_material, 0)){ elev_front, &_material, 0)){
front_elev_m = elev_front; front_elev_m = elev_front + _z_offset_m;
} else } else
return false; return false;
@ -297,26 +314,26 @@ bool FGAIGroundVehicle::getPitch() {
if (_new_waypoint){ if (_new_waypoint){
//cout << "new waypoint, calculating pitch " << endl; //cout << "new waypoint, calculating pitch " << endl;
curr_alt = curr->altitude * SG_METER_TO_FEET; curr_alt = curr->altitude;
prev_alt = prev->altitude * SG_METER_TO_FEET; prev_alt = prev->altitude;
d_alt = curr_alt - prev_alt; cout << "prev_alt" <<prev_alt << endl;
d_alt = (curr_alt - prev_alt) * SG_METER_TO_FEET;
//_elevation = prev->altitude;
distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->longitude, prev->latitude), distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->longitude, prev->latitude),
SGGeod::fromDeg(curr->longitude, curr->latitude)); SGGeod::fromDeg(curr->longitude, curr->latitude));
_pitch = atan2(d_alt, distance * SG_METER_TO_FEET) * SG_RADIANS_TO_DEGREES; _pitch = atan2(d_alt, distance * SG_METER_TO_FEET) * SG_RADIANS_TO_DEGREES;
// cout << "new waypoint, calculating pitch " << _pitch << endl; //cout << "new waypoint, calculating pitch " << _pitch <<
// " " << _pitch_offset << " " << _elevation <<endl;
} }
double distance_to_go = SGGeodesy::distanceM(SGGeod::fromDeg(pos.getLongitudeDeg(), pos.getLatitudeDeg()), double distance_to_go = SGGeodesy::distanceM(SGGeod::fromDeg(pos.getLongitudeDeg(), pos.getLatitudeDeg()),
SGGeod::fromDeg(curr->longitude, curr->latitude)); SGGeod::fromDeg(curr->longitude, curr->latitude));
//cout << "tunnel " << _tunnel /*cout << "tunnel " << _tunnel
// << " distance curr & prev " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET << " distance prev & curr " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET
// << " distance to go " << distance_to_go * SG_METER_TO_FEET << " distance to go " << distance_to_go * SG_METER_TO_FEET
// << " d_alt ft " << d_alt << " d_alt ft " << d_alt
// << endl; << endl;*/
if (distance_to_go > distance) if (distance_to_go > distance)
_elevation = prev_alt; _elevation = prev_alt;
@ -367,13 +384,17 @@ void FGAIGroundVehicle::setParent() {
* SG_FEET_TO_METER; * SG_FEET_TO_METER;
double hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft") double hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft")
* SG_FEET_TO_METER; * SG_FEET_TO_METER;
double hitch_z_offset_m = _selected_ac->getDoubleValue("hitch/z-offset-ft")
* SG_FEET_TO_METER;
_selectedpos.setLatitudeDeg(lat); _selectedpos.setLatitudeDeg(lat);
_selectedpos.setLongitudeDeg(lon); _selectedpos.setLongitudeDeg(lon);
_selectedpos.setElevationFt(elevation); _selectedpos.setElevationFt(elevation);
_parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft"); _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
_parent_y_offset = _selected_ac->getDoubleValue("hitch/y-offset-ft"); _parent_y_offset = _selected_ac->getDoubleValue("hitch/y-offset-ft");
_parent_z_offset = _selected_ac->getDoubleValue("hitch/z-offset-ft");
_parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt"); _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
SGVec3d rear_hitch(-hitch_x_offset_m, hitch_y_offset_m, 0); SGVec3d rear_hitch(-hitch_x_offset_m, hitch_y_offset_m, 0);
@ -534,7 +555,7 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){
if (_dt_count < _next_run) if (_dt_count < _next_run)
return; return;
_next_run = 0.055 /*+ (0.015 * sg_random())*/; _next_run = 0.05 /*+ (0.015 * sg_random())*/;
if (getPitch()){ if (getPitch()){
setElevation(_elevation, _dt_count, _elevation_coeff); setElevation(_elevation, _dt_count, _elevation_coeff);
@ -545,7 +566,6 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){
if(_parent == ""){ if(_parent == ""){
AccelTo(prev->speed); AccelTo(prev->speed);
FGAIShip::update(_dt_count);
_dt_count = 0; _dt_count = 0;
return; return;
} }
@ -554,12 +574,15 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){
string parent_next_name = _selected_ac->getStringValue("waypoint/name-next"); string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting"); bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
bool parent_restart = _selected_ac->getBoolValue("controls/restart");
if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){ if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
<< " setting END: getting new waypoints "); << " setting END: getting new waypoints ");
AdvanceFP(); AdvanceFP();
setWPNames(); setWPNames();
setTunnel(_initial_tunnel);
if(_restart) _missed_count = 200;
/*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/ /*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
} else if (parent_waiting && !_waiting){ } else if (parent_waiting && !_waiting){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
@ -585,12 +608,14 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){
} }
setWPNames(); setWPNames();
} else if (_range_ft > _parent_x_offset * 4){ } else if (_range_ft > (_x_offset +_parent_x_offset)* 4
SG_LOG(SG_GENERAL, SG_INFO, "AIGroundVeh1cle: " << _name ){
<< " rescue: reforming train " << _range_ft << " " << _x_offset * 15); SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name
<< " rescue: reforming train " << _range_ft
);
setTowAngle(0, dt, 1); setTowAngle(0, dt, 1);
setSpeed(_parent_speed * 2); setSpeed(_parent_speed + (10 * sign(_parent_speed)));
} else if (_parent_speed > 1){ } else if (_parent_speed > 1){
@ -609,7 +634,7 @@ void FGAIGroundVehicle::RunGroundVehicle(double dt){
} else } else
setSpeed(_parent_speed); setSpeed(_parent_speed);
FGAIShip::update(_dt_count); // FGAIShip::update(_dt_count);
_dt_count = 0; _dt_count = 0;
} }

View file

@ -53,6 +53,7 @@ private:
void setContactX2offset(double x2); void setContactX2offset(double x2);
void setXOffset(double x); void setXOffset(double x);
void setYOffset(double y); void setYOffset(double y);
void setZOffset(double z);
void setPitchCoeff(double pc); void setPitchCoeff(double pc);
void setElevCoeff(double ec); void setElevCoeff(double ec);
@ -91,8 +92,8 @@ private:
double _x_offset, _y_offset; double _x_offset, _y_offset;
double _range_ft; double _range_ft;
double _relbrg; double _relbrg;
double _parent_speed, _parent_x_offset, _parent_y_offset; double _parent_speed, _parent_x_offset, _parent_y_offset, _parent_z_offset;
double _dt_count, _next_run; double _dt_count, _next_run, _break_count;
const SGMaterial* _material; const SGMaterial* _material;
const SGPropertyNode *_selected_ac; const SGPropertyNode *_selected_ac;

View file

@ -68,7 +68,8 @@ _old_range(0),
_range_rate(0), _range_rate(0),
_roll_constant(0.001), _roll_constant(0.001),
_hdg_constant(0.01), _hdg_constant(0.01),
_roll_factor(-0.0083335) _roll_factor(-0.0083335),
_restart(false)
{ {
invisible = false; invisible = false;
@ -89,6 +90,7 @@ void FGAIShip::readFromScenario(SGPropertyNode* scFileNode) {
setRadius(scFileNode->getDoubleValue("turn-radius-ft", 2000)); setRadius(scFileNode->getDoubleValue("turn-radius-ft", 2000));
std::string flightplan = scFileNode->getStringValue("flightplan"); std::string flightplan = scFileNode->getStringValue("flightplan");
setRepeat(scFileNode->getBoolValue("repeat", false)); setRepeat(scFileNode->getBoolValue("repeat", false));
setRestart(scFileNode->getBoolValue("restart", false));
setStartTime(scFileNode->getStringValue("time", "")); setStartTime(scFileNode->getStringValue("time", ""));
setLeadAngleGain(scFileNode->getDoubleValue("lead-angle-gain", 1.5)); setLeadAngleGain(scFileNode->getDoubleValue("lead-angle-gain", 1.5));
setLeadAngleLimit(scFileNode->getDoubleValue("lead-angle-limit-deg", 15)); setLeadAngleLimit(scFileNode->getDoubleValue("lead-angle-limit-deg", 15));
@ -196,6 +198,8 @@ void FGAIShip::bind() {
SGRawValuePointer<double>(&_proportion)); SGRawValuePointer<double>(&_proportion));
props->tie("controls/fixed-turn-radius-ft", props->tie("controls/fixed-turn-radius-ft",
SGRawValuePointer<double>(&_fixed_turn_radius)); SGRawValuePointer<double>(&_fixed_turn_radius));
props->tie("controls/restart",
SGRawValuePointer<bool>(&_restart));
} }
void FGAIShip::unbind() { void FGAIShip::unbind() {
@ -231,6 +235,7 @@ void FGAIShip::unbind() {
props->untie("controls/constants/lead-angle/proportion"); props->untie("controls/constants/lead-angle/proportion");
props->untie("controls/fixed-turn-radius-ft"); props->untie("controls/fixed-turn-radius-ft");
props->untie("controls/constants/speed"); props->untie("controls/constants/speed");
props->untie("controls/restart");
} }
void FGAIShip::update(double dt) { void FGAIShip::update(double dt) {
@ -332,17 +337,26 @@ void FGAIShip::Run(double dt) {
//we assume that at slow speed ships will manoeuvre using engines/bow thruster //we assume that at slow speed ships will manoeuvre using engines/bow thruster
if (fabs(speed)<=5) if(type == "ship" || type == "carrier"){
_sp_turn_radius_ft = _fixed_turn_radius;
else { if (fabs(speed)<=5)
// adjust turn radius for speed. The equation is very approximate. _sp_turn_radius_ft = _fixed_turn_radius;
// we need to allow for negative speeds else {
if (type == "ship") // adjust turn radius for speed. The equation is very approximate.
_sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft; // we need to allow for negative speeds
else _sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft;
_sp_turn_radius_ft = turn_radius_ft; }
} else {
if (fabs(speed) <= 40)
_sp_turn_radius_ft = _fixed_turn_radius;
else {
// adjust turn radius for speed.
_sp_turn_radius_ft = turn_radius_ft;
}
}
}
if (_rudder <= -0.25 || _rudder >= 0.25) { if (_rudder <= -0.25 || _rudder >= 0.25) {
// adjust turn radius for _rudder angle. The equation is even more approximate. // adjust turn radius for _rudder angle. The equation is even more approximate.
@ -404,7 +418,7 @@ void FGAIShip::Run(double dt) {
} }
// set the _rudder limit by speed // set the _rudder limit by speed
if (type == "ship"){ if (type == "ship" || type == "carrier"){
if (speed <= 40) if (speed <= 40)
rudder_limit = (-0.825 * speed) + 35; rudder_limit = (-0.825 * speed) + 35;
@ -456,13 +470,6 @@ void FGAIShip::ClimbTo(double altitude) {
} }
void FGAIShip::TurnTo(double heading) { void FGAIShip::TurnTo(double heading) {
//double relbrg_corr = _relbrg;
//if ( relbrg_corr > 5)
// relbrg_corr = 5;
//else if( relbrg_corr < -5)
// relbrg_corr = -5;
tgt_heading = heading - _lead_angle + _tow_angle; tgt_heading = heading - _lead_angle + _tow_angle;
SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0); SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0);
_hdg_lock = true; _hdg_lock = true;
@ -511,6 +518,10 @@ void FGAIShip::setRepeat(bool r) {
_repeat = r; _repeat = r;
} }
void FGAIShip::setRestart(bool r) {
_restart = r;
}
void FGAIShip::setMissed(bool m) { void FGAIShip::setMissed(bool m) {
_missed = m; _missed = m;
props->setBoolValue("waypoint/missed", _missed); props->setBoolValue("waypoint/missed", _missed);
@ -548,6 +559,15 @@ void FGAIShip::setFixedTurnRadius(double ftr) {
_fixed_turn_radius = ftr; _fixed_turn_radius = ftr;
} }
void FGAIShip::setInitialTunnel(bool t) {
_initial_tunnel = t;
setTunnel(_initial_tunnel);
}
void FGAIShip::setTunnel(bool t) {
_tunnel = t;
}
void FGAIShip::setWPNames() { void FGAIShip::setWPNames() {
if (prev != 0) if (prev != 0)
@ -614,7 +634,7 @@ void FGAIShip::ProcessFlightPlan(double dt) {
if (_dt_count < _next_run && _start_sec < time_sec) if (_dt_count < _next_run && _start_sec < time_sec)
return; return;
_next_run = 1.0 + (0.5 * sg_random()); _next_run = 0.05 + (0.025 * sg_random());
double until_time_sec = 0; double until_time_sec = 0;
_missed = false; _missed = false;
@ -657,7 +677,7 @@ void FGAIShip::ProcessFlightPlan(double dt) {
if (_next_name == "TUNNEL"){ if (_next_name == "TUNNEL"){
_tunnel = !_tunnel; _tunnel = !_tunnel;
//SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " TUNNEL "); SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " " << sp_turn_radius_nm );
fp->IncrementWaypoint(false); fp->IncrementWaypoint(false);
next = fp->getNextWaypoint(); next = fp->getNextWaypoint();
@ -674,7 +694,7 @@ void FGAIShip::ProcessFlightPlan(double dt) {
}else if(_next_name == "END" || fp->getNextWaypoint() == 0) { }else if(_next_name == "END" || fp->getNextWaypoint() == 0) {
if (_repeat) { if (_repeat) {
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: "<< _name << "Flightplan restarting "); SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: "<< _name << " Flightplan repeating ");
fp->restart(); fp->restart();
prev = curr; prev = curr;
curr = fp->getCurrentWaypoint(); curr = fp->getCurrentWaypoint();
@ -687,6 +707,10 @@ void FGAIShip::ProcessFlightPlan(double dt) {
_missed_count = 0; _missed_count = 0;
_lead_angle = 0; _lead_angle = 0;
AccelTo(prev->speed); AccelTo(prev->speed);
} else if (_restart){
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " Flightplan restarting ");
_missed_count = 0;
initFlightPlan();
} else { } else {
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " Flightplan dying "); SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " Flightplan dying ");
setDie(true); setDie(true);
@ -802,6 +826,7 @@ bool FGAIShip::initFlightPlan() {
bool init = false; bool init = false;
_start_sec = 0; _start_sec = 0;
_tunnel = _initial_tunnel;
fp->restart(); fp->restart();
fp->IncrementWaypoint(false); fp->IncrementWaypoint(false);
@ -859,7 +884,7 @@ bool FGAIShip::initFlightPlan() {
_missed_count = 0; _missed_count = 0;
_new_waypoint = true; _new_waypoint = true;
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " done initialising waypoints "); SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " done initialising waypoints " << _tunnel);
if (prev) if (prev)
init = true; init = true;
@ -1068,15 +1093,17 @@ void FGAIShip::setXTrackError() {
double brg = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(), double brg = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
curr->latitude, curr->longitude); curr->latitude, curr->longitude);
double xtrack_error_nm = sin((course - brg)* SG_DEGREES_TO_RADIANS) * _wp_range; double xtrack_error_nm = sin((course - brg)* SG_DEGREES_TO_RADIANS) * _wp_range;
double factor = -0.0045 * speed + 1;
double limit = _lead_angle_limit * factor;
if (_wp_range > 0){ if (_wp_range > 0){
_lead_angle = atan2(xtrack_error_nm,(_wp_range * _proportion)) * SG_RADIANS_TO_DEGREES; _lead_angle = atan2(xtrack_error_nm,(_wp_range * _proportion)) * SG_RADIANS_TO_DEGREES;
} else } else
_lead_angle = 0; _lead_angle = 0;
_lead_angle *= _lead_angle_gain; _lead_angle *= _lead_angle_gain * factor;
_xtrack_error = xtrack_error_nm * 6076.1155; _xtrack_error = xtrack_error_nm * 6076.1155;
SG_CLAMP_RANGE(_lead_angle, -_lead_angle_limit, _lead_angle_limit); SG_CLAMP_RANGE(_lead_angle, -limit, limit);
} }

View file

@ -61,6 +61,9 @@ public:
void setRudderConstant(double rc); void setRudderConstant(double rc);
void setSpeedConstant(double sc); void setSpeedConstant(double sc);
void setFixedTurnRadius(double ft); void setFixedTurnRadius(double ft);
void setTunnel(bool t);
void setInitialTunnel(bool t);
void setWPNames(); void setWPNames();
void setWPPos(); void setWPPos();
double sign(double x); double sign(double x);
@ -69,13 +72,13 @@ public:
bool _serviceable; bool _serviceable;
bool _waiting; bool _waiting;
bool _new_waypoint; bool _new_waypoint;
bool _tunnel; bool _tunnel, _initial_tunnel;
bool _restart;
virtual const char* getTypeString(void) const { return "ship"; } virtual const char* getTypeString(void) const { return "ship"; }
double _rudder_constant, _speed_constant, _hdg_constant, _limit ; double _rudder_constant, _speed_constant, _hdg_constant, _limit ;
double _elevation_m, _elevation_ft; double _elevation_m, _elevation_ft;
double _missed_range, _tow_angle, _wait_count, _wp_range; double _missed_range, _tow_angle, _wait_count, _missed_count,_wp_range;
FGAIFlightPlan::waypoint* prev; // the one behind you FGAIFlightPlan::waypoint* prev; // the one behind you
FGAIFlightPlan::waypoint* curr; // the one ahead FGAIFlightPlan::waypoint* curr; // the one ahead
@ -92,6 +95,7 @@ private:
virtual void reinit() { init(); } virtual void reinit() { init(); }
void setRepeat(bool r); void setRepeat(bool r);
void setRestart(bool r);
void setMissed(bool m); void setMissed(bool m);
void setServiceable(bool s); void setServiceable(bool s);
@ -119,7 +123,7 @@ private:
double _roll_constant, _roll_factor; double _roll_constant, _roll_factor;
double _sp_turn_radius_ft, _rd_turn_radius_ft, _fixed_turn_radius; double _sp_turn_radius_ft, _rd_turn_radius_ft, _fixed_turn_radius;
double _old_range, _range_rate; double _old_range, _range_rate;
double _dt_count, _missed_count; double _dt_count;
double _next_run; double _next_run;
double _missed_time_sec; double _missed_time_sec;
double _start_sec; double _start_sec;
@ -137,6 +141,7 @@ private:
bool _repeat; bool _repeat;
bool _fp_init; bool _fp_init;
bool _missed; bool _missed;
}; };