_next_run(0),
_parent_x_offset(0),
_parent_y_offset(0),
-_parent("")
+_parent_z_offset(0),
+_parent(""),
+_break_count(0)
{
invisible = false;
setSMPath(scFileNode->getStringValue("submodel-path", ""));
setContactX1offset(scFileNode->getDoubleValue("contact-x1-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));
+ setZOffset(scFileNode->getDoubleValue("hitch-z-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.1));
setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.25));
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));
+ setInitialTunnel(scFileNode->getBoolValue("tunnel", false));
//we may need these later for towed vehicles
// setSubID(scFileNode->getIntValue("SubID", 0));
// setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
SGRawValuePointer<double>(&_x_offset));
props->tie("hitch/y-offset-ft",
SGRawValuePointer<double>(&_y_offset));
+ props->tie("hitch/z-offset-ft",
+ SGRawValuePointer<double>(&_z_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("hitch/parent-z-offset-ft",
+ SGRawValuePointer<double>(&_parent_z_offset));
props->tie("controls/constants/tow-angle/gain",
SGRawValuePointer<double>(&_tow_angle_gain));
props->tie("controls/constants/tow-angle/limit-deg",
FGAIShip::unbind();
props->untie("controls/constants/elevation-coeff");
+ props->untie("controls/constants/pitch-coeff");
props->untie("position/ht-AGL-ft");
- props->untie("controls/constants/pitch-coeff");
props->untie("hitch/rel-bearing-deg");
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/z-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("controls/constants/tow-angle/gain");
props->untie("controls/constants/tow-angle/limit-deg");
props->untie("controls/contact-x1-offset-ft");
void FGAIGroundVehicle::update(double dt) {
// SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name );
+ FGAIShip::update(dt);
RunGroundVehicle(dt);
-// FGAIShip::update(dt);
}
void FGAIGroundVehicle::setNoRoll(bool nr) {
_y_offset = y;
}
+void FGAIGroundVehicle::setZOffset(double z) {
+ _z_offset = z;
+}
+
void FGAIGroundVehicle::setPitchCoeff(double pc) {
_pitch_coeff = pc;
}
}
void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){
- //_tow_angle = ta * _tow_angle_gain;
- _tow_angle = pow(ta,2) * sign(ta);
- SG_CLAMP_RANGE(_tow_angle, -_tow_angle_limit, _tow_angle_limit);
+ ta *= _tow_angle_gain;
+ double factor = -0.0045 * speed + 1;
+ 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::getPitch() {
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;
+ double _z_offset_m = _parent_z_offset * SG_FEET_TO_METER;
SGVec3d front(-contact_offset_x1_m, 0, 0);
SGVec3d rear(-contact_offset_x2_m, 0, 0);
double elev_rear = 0;
double max_alt = 10000;
- if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 3000), elev_front,
- &_material, 0)){
- front_elev_m = elev_front;
+ if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 3000),
+ elev_front, &_material, 0)){
+ front_elev_m = elev_front + _z_offset_m;
} else
return false;
if (_new_waypoint){
//cout << "new waypoint, calculating pitch " << endl;
- curr_alt = curr->altitude * SG_METER_TO_FEET;
- prev_alt = prev->altitude * SG_METER_TO_FEET;
- d_alt = curr_alt - prev_alt;
-
+ curr_alt = curr->altitude;
+ prev_alt = prev->altitude;
+ 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),
SGGeod::fromDeg(curr->longitude, curr->latitude));
-
_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()),
SGGeod::fromDeg(curr->longitude, curr->latitude));
- //cout << "tunnel " << _tunnel
- // << " 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;
+ /*cout << "tunnel " << _tunnel
+ << " distance prev & curr " << 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;
* SG_FEET_TO_METER;
double hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft")
* SG_FEET_TO_METER;
-
+ double hitch_z_offset_m = _selected_ac->getDoubleValue("hitch/z-offset-ft")
+ * SG_FEET_TO_METER;
+
_selectedpos.setLatitudeDeg(lat);
_selectedpos.setLongitudeDeg(lon);
_selectedpos.setElevationFt(elevation);
_parent_x_offset = _selected_ac->getDoubleValue("hitch/x-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");
SGVec3d rear_hitch(-hitch_x_offset_m, hitch_y_offset_m, 0);
if (_dt_count < _next_run)
return;
- _next_run = 0.055 /*+ (0.015 * sg_random())*/;
+ _next_run = 0.05 /*+ (0.015 * sg_random())*/;
if (getPitch()){
setElevation(_elevation, _dt_count, _elevation_coeff);
if(_parent == ""){
AccelTo(prev->speed);
- FGAIShip::update(_dt_count);
_dt_count = 0;
return;
}
string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
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" ){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
<< " setting END: getting new waypoints ");
AdvanceFP();
setWPNames();
+ setTunnel(_initial_tunnel);
+ if(_restart) _missed_count = 200;
/*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
} else if (parent_waiting && !_waiting){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
}
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);
+ } else if (_range_ft > (_x_offset +_parent_x_offset)* 4
+ ){
+ SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name
+ << " rescue: reforming train " << _range_ft
+ );
setTowAngle(0, dt, 1);
- setSpeed(_parent_speed * 2);
+ setSpeed(_parent_speed + (10 * sign(_parent_speed)));
} else if (_parent_speed > 1){
} else
setSpeed(_parent_speed);
- FGAIShip::update(_dt_count);
+// FGAIShip::update(_dt_count);
_dt_count = 0;
}
_range_rate(0),
_roll_constant(0.001),
_hdg_constant(0.01),
-_roll_factor(-0.0083335)
+_roll_factor(-0.0083335),
+_restart(false)
{
invisible = false;
setRadius(scFileNode->getDoubleValue("turn-radius-ft", 2000));
std::string flightplan = scFileNode->getStringValue("flightplan");
setRepeat(scFileNode->getBoolValue("repeat", false));
+ setRestart(scFileNode->getBoolValue("restart", false));
setStartTime(scFileNode->getStringValue("time", ""));
setLeadAngleGain(scFileNode->getDoubleValue("lead-angle-gain", 1.5));
setLeadAngleLimit(scFileNode->getDoubleValue("lead-angle-limit-deg", 15));
SGRawValuePointer<double>(&_proportion));
props->tie("controls/fixed-turn-radius-ft",
SGRawValuePointer<double>(&_fixed_turn_radius));
+ props->tie("controls/restart",
+ SGRawValuePointer<bool>(&_restart));
}
void FGAIShip::unbind() {
props->untie("controls/constants/lead-angle/proportion");
props->untie("controls/fixed-turn-radius-ft");
props->untie("controls/constants/speed");
+ props->untie("controls/restart");
}
void FGAIShip::update(double dt) {
//we assume that at slow speed ships will manoeuvre using engines/bow thruster
- if (fabs(speed)<=5)
- _sp_turn_radius_ft = _fixed_turn_radius;
- else {
- // 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;
- else
- _sp_turn_radius_ft = turn_radius_ft;
+ if(type == "ship" || type == "carrier"){
+
+ if (fabs(speed)<=5)
+ _sp_turn_radius_ft = _fixed_turn_radius;
+ else {
+ // adjust turn radius for speed. The equation is very approximate.
+ // we need to allow for negative speeds
+ _sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + 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) {
// adjust turn radius for _rudder angle. The equation is even more approximate.
}
// set the _rudder limit by speed
- if (type == "ship"){
+ if (type == "ship" || type == "carrier"){
if (speed <= 40)
rudder_limit = (-0.825 * speed) + 35;
}
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;
SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0);
_hdg_lock = true;
_repeat = r;
}
+void FGAIShip::setRestart(bool r) {
+ _restart = r;
+}
+
void FGAIShip::setMissed(bool m) {
_missed = m;
props->setBoolValue("waypoint/missed", _missed);
_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() {
if (prev != 0)
if (_dt_count < _next_run && _start_sec < time_sec)
return;
- _next_run = 1.0 + (0.5 * sg_random());
+ _next_run = 0.05 + (0.025 * sg_random());
double until_time_sec = 0;
_missed = false;
if (_next_name == "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);
next = fp->getNextWaypoint();
}else if(_next_name == "END" || fp->getNextWaypoint() == 0) {
if (_repeat) {
- SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: "<< _name << "Flightplan restarting ");
+ SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: "<< _name << " Flightplan repeating ");
fp->restart();
prev = curr;
curr = fp->getCurrentWaypoint();
_missed_count = 0;
_lead_angle = 0;
AccelTo(prev->speed);
+ } else if (_restart){
+ SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " Flightplan restarting ");
+ _missed_count = 0;
+ initFlightPlan();
} else {
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " Flightplan dying ");
setDie(true);
bool init = false;
_start_sec = 0;
+ _tunnel = _initial_tunnel;
fp->restart();
fp->IncrementWaypoint(false);
_missed_count = 0;
_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)
init = true;
double brg = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
curr->latitude, curr->longitude);
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){
_lead_angle = atan2(xtrack_error_nm,(_wp_range * _proportion)) * SG_RADIANS_TO_DEGREES;
} else
_lead_angle = 0;
- _lead_angle *= _lead_angle_gain;
+ _lead_angle *= _lead_angle_gain * factor;
_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);
}