X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAICarrier.cxx;h=b6487519d026c05ea55bc026423f093d30ce9375;hb=b4fbde72b275dc75e8ecf7b44261aba642e5da0f;hp=9a15a840d7134e88f0271e85b9a6589e8224fa8e;hpb=1c3e2d4942fe74dac43f1f6af542f9de7d4825db;p=flightgear.git diff --git a/src/AIModel/AICarrier.cxx b/src/AIModel/AICarrier.cxx index 9a15a840d..b6487519d 100644 --- a/src/AIModel/AICarrier.cxx +++ b/src/AIModel/AICarrier.cxx @@ -15,885 +15,608 @@ // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software -// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. #ifdef HAVE_CONFIG_H # include #endif +#include #include #include -#include +#include +#include #include + #include #include
#include
#include "AICarrier.hxx" -#include "AIScenario.hxx" - -/** Value of earth radius (meters) */ -#define RADIUS_M SG_EQUATORIAL_RADIUS_M - - - -FGAICarrier::FGAICarrier(FGAIManager* mgr) : FGAIShip(mgr) { - _type_str = "carrier"; - _otype = otCarrier; - - +FGAICarrier::FGAICarrier() : FGAIShip(otCarrier) { } FGAICarrier::~FGAICarrier() { } +void FGAICarrier::readFromScenario(SGPropertyNode* scFileNode) { + if (!scFileNode) + return; + + FGAIShip::readFromScenario(scFileNode); + + setRadius(scFileNode->getDoubleValue("turn-radius-ft", 2000)); + setSign(scFileNode->getStringValue("pennant-number")); + setWind_from_east(scFileNode->getDoubleValue("wind_from_east", 0)); + setWind_from_north(scFileNode->getDoubleValue("wind_from_north", 0)); + setTACANChannelID(scFileNode->getStringValue("TACAN-channel-ID", "029Y")); + setMaxLat(scFileNode->getDoubleValue("max-lat", 0)); + setMinLat(scFileNode->getDoubleValue("min-lat", 0)); + setMaxLong(scFileNode->getDoubleValue("max-long", 0)); + setMinLong(scFileNode->getDoubleValue("min-long", 0)); + setMPControl(scFileNode->getBoolValue("mp-control", false)); + setAIControl(scFileNode->getBoolValue("ai-control", false)); + + SGPropertyNode* flols = scFileNode->getChild("flols-pos"); + if (flols) { + // Transform to the right coordinate frame, configuration is done in + // the usual x-back, y-right, z-up coordinates, computations + // in the simulation usual body x-forward, y-right, z-down coordinates + flols_off(0) = - flols->getDoubleValue("x-offset-m", 0); + flols_off(1) = flols->getDoubleValue("y-offset-m", 0); + flols_off(2) = - flols->getDoubleValue("z-offset-m", 0); + } else + flols_off = SGVec3d::zeros(); + + std::vector props = scFileNode->getChildren("parking-pos"); + std::vector::const_iterator it; + for (it = props.begin(); it != props.end(); ++it) { + string name = (*it)->getStringValue("name", "unnamed"); + // Transform to the right coordinate frame, configuration is done in + // the usual x-back, y-right, z-up coordinates, computations + // in the simulation usual body x-forward, y-right, z-down coordinates + double offset_x = -(*it)->getDoubleValue("x-offset-m", 0); + double offset_y = (*it)->getDoubleValue("y-offset-m", 0); + double offset_z = -(*it)->getDoubleValue("z-offset-m", 0); + double hd = (*it)->getDoubleValue("heading-offset-deg", 0); + ParkPosition pp(name, SGVec3d(offset_x, offset_y, offset_z), hd); + ppositions.push_back(pp); + } +} + void FGAICarrier::setWind_from_east(double fps) { - wind_from_east = fps; + wind_from_east = fps; } void FGAICarrier::setWind_from_north(double fps) { - wind_from_north = fps; + wind_from_north = fps; } void FGAICarrier::setMaxLat(double deg) { - max_lat = fabs(deg); + max_lat = fabs(deg); } void FGAICarrier::setMinLat(double deg) { - min_lat = fabs(deg); + min_lat = fabs(deg); } void FGAICarrier::setMaxLong(double deg) { - max_long = fabs(deg); + max_long = fabs(deg); } void FGAICarrier::setMinLong(double deg) { - min_long = fabs(deg); -} - -void FGAICarrier::setSolidObjects(const list& so) { - solid_objects = so; -} - -void FGAICarrier::setWireObjects(const list& wo) { - wire_objects = wo; -} - -void FGAICarrier::setCatapultObjects(const list& co) { - catapult_objects = co; -} - -void FGAICarrier::setParkingPositions(const list& p) { - ppositions = p; + min_long = fabs(deg); } void FGAICarrier::setSign(const string& s) { - sign = s; + sign = s; } void FGAICarrier::setTACANChannelID(const string& id) { - TACAN_channel_id = id; + TACAN_channel_id = id; } -void FGAICarrier::setFlolsOffset(const Point3D& off) { - flols_off = off; +void FGAICarrier::setMPControl(bool c) { + MPControl = c; } -void FGAICarrier::getVelocityWrtEarth(sgdVec3 v, sgdVec3 omega, sgdVec3 pivot) { - sgdCopyVec3(v, vel_wrt_earth ); - sgdCopyVec3(omega, rot_wrt_earth ); - sgdCopyVec3(pivot, rot_pivot_wrt_earth ); +void FGAICarrier::setAIControl(bool c) { + AIControl = c; } void FGAICarrier::update(double dt) { - - // For computation of rotation speeds we just use finite differences her. - // That is perfectly valid since this thing is not driven by accelerations - // but by just apply discrete changes at its velocity variables. - double old_hdg = hdg; - double old_roll = roll; - double old_pitch = pitch; - - // Update the velocity information stored in those nodes. - double v_north = 0.51444444*speed*cos(hdg * SGD_DEGREES_TO_RADIANS); - double v_east = 0.51444444*speed*sin(hdg * SGD_DEGREES_TO_RADIANS); - - double sin_lat = sin(pos.lat() * SGD_DEGREES_TO_RADIANS); - double cos_lat = cos(pos.lat() * SGD_DEGREES_TO_RADIANS); - double sin_lon = sin(pos.lon() * SGD_DEGREES_TO_RADIANS); - double cos_lon = cos(pos.lon() * SGD_DEGREES_TO_RADIANS); - double sin_roll = sin(roll * SGD_DEGREES_TO_RADIANS); - double cos_roll = cos(roll * SGD_DEGREES_TO_RADIANS); - double sin_pitch = sin(pitch * SGD_DEGREES_TO_RADIANS); - double cos_pitch = cos(pitch * SGD_DEGREES_TO_RADIANS); - double sin_hdg = sin(hdg * SGD_DEGREES_TO_RADIANS); - double cos_hdg = cos(hdg * SGD_DEGREES_TO_RADIANS); - - // Transform this back the the horizontal local frame. - sgdMat3 trans; - - // set up the transform matrix - trans[0][0] = cos_pitch*cos_hdg; - trans[0][1] = sin_roll*sin_pitch*cos_hdg - cos_roll*sin_hdg; - trans[0][2] = cos_roll*sin_pitch*cos_hdg + sin_roll*sin_hdg; - - trans[1][0] = cos_pitch*sin_hdg; - trans[1][1] = sin_roll*sin_pitch*sin_hdg + cos_roll*cos_hdg; - trans[1][2] = cos_roll*sin_pitch*sin_hdg - sin_roll*cos_hdg; - - trans[2][0] = -sin_pitch; - trans[2][1] = sin_roll*cos_pitch; - trans[2][2] = cos_roll*cos_pitch; - - sgdSetVec3( vel_wrt_earth, - - cos_lon*sin_lat*v_north - sin_lon*v_east, - - sin_lon*sin_lat*v_north + cos_lon*v_east, - cos_lat*v_north ); - sgGeodToCart(pos.lat() * SGD_DEGREES_TO_RADIANS, - pos.lon() * SGD_DEGREES_TO_RADIANS, - pos.elev(), rot_pivot_wrt_earth); - - // Now update the position and heading. This will compute new hdg and - // roll values required for the rotation speed computation. - FGAIShip::update(dt); - - - //automatic turn into wind with a target wind of 25 kts otd - if(turn_to_launch_hdg){ - TurnToLaunch(); - } else if(OutsideBox() || returning) {// check that the carrier is inside the operating box - ReturnToBox(); - } else { //if(!returning - TurnToBase(); - } //end if - - // Only change these values if we are able to compute them safely - if (dt < DBL_MIN) - sgdSetVec3( rot_wrt_earth, 0.0, 0.0, 0.0); - else { - // Compute the change of the euler angles. - double hdg_dot = SGD_DEGREES_TO_RADIANS * (hdg-old_hdg)/dt; - // Allways assume that the movement was done by the shorter way. - if (hdg_dot < - SGD_DEGREES_TO_RADIANS * 180) - hdg_dot += SGD_DEGREES_TO_RADIANS * 360; - if (hdg_dot > SGD_DEGREES_TO_RADIANS * 180) - hdg_dot -= SGD_DEGREES_TO_RADIANS * 360; - double pitch_dot = SGD_DEGREES_TO_RADIANS * (pitch-old_pitch)/dt; - // Allways assume that the movement was done by the shorter way. - if (pitch_dot < - SGD_DEGREES_TO_RADIANS * 180) - pitch_dot += SGD_DEGREES_TO_RADIANS * 360; - if (pitch_dot > SGD_DEGREES_TO_RADIANS * 180) - pitch_dot -= SGD_DEGREES_TO_RADIANS * 360; - double roll_dot = SGD_DEGREES_TO_RADIANS * (roll-old_roll)/dt; - // Allways assume that the movement was done by the shorter way. - if (roll_dot < - SGD_DEGREES_TO_RADIANS * 180) - roll_dot += SGD_DEGREES_TO_RADIANS * 360; - if (roll_dot > SGD_DEGREES_TO_RADIANS * 180) - roll_dot -= SGD_DEGREES_TO_RADIANS * 360; - /*cout << "euler derivatives = " - << roll_dot << " " << pitch_dot << " " << hdg_dot << endl;*/ - - // Now Compute the rotation vector in the carriers coordinate frame - // originating from the euler angle changes. - sgdVec3 body; - body[0] = roll_dot - hdg_dot*sin_pitch; - body[1] = pitch_dot*cos_roll + hdg_dot*sin_roll*cos_pitch; - body[2] = -pitch_dot*sin_roll + hdg_dot*cos_roll*cos_pitch; - - // Transform that back to the horizontal local frame. - sgdVec3 hl; - hl[0] = body[0]*trans[0][0] + body[1]*trans[0][1] + body[2]*trans[0][2]; - hl[1] = body[0]*trans[1][0] + body[1]*trans[1][1] + body[2]*trans[1][2]; - hl[2] = body[0]*trans[2][0] + body[1]*trans[2][1] + body[2]*trans[2][2]; - - // Now we need to project out rotation components ending in speeds in y - // direction in the hoirizontal local frame. - hl[1] = 0; - - // Transform that to the earth centered frame. - sgdSetVec3(rot_wrt_earth, - - cos_lon*sin_lat*hl[0] - sin_lon*hl[1] - cos_lat*cos_lon*hl[2], - - sin_lon*sin_lat*hl[0] + cos_lon*hl[1] - cos_lat*sin_lon*hl[2], - cos_lat*hl[0] - sin_lat*hl[2]); - } - - UpdateWind(dt); - UpdateTACAN(dt); - UpdateFlols(trans); + // Now update the position and heading. This will compute new hdg and + // roll values required for the rotation speed computation. + FGAIShip::update(dt); + + //automatic turn into wind with a target wind of 25 kts otd + //SG_LOG(SG_GENERAL, SG_ALERT, "AICarrier: MPControl " << MPControl << " AIControl " << AIControl); + if (!MPControl && AIControl){ + + if(turn_to_launch_hdg){ + TurnToLaunch(); + } else if(turn_to_recovery_hdg ){ + TurnToRecover(); + } else if(OutsideBox() || returning ) {// check that the carrier is inside + ReturnToBox(); // the operating box, + } else { + TurnToBase(); + } + + } else { + FGAIShip::TurnTo(tgt_heading); + FGAIShip::AccelTo(tgt_speed); + } + + UpdateWind(dt); + UpdateElevator(dt, transition_time); + UpdateJBD(dt, jbd_transition_time); + + // Transform that one to the horizontal local coordinate system. + SGQuatd ec2hl = SGQuatd::fromLonLat(pos); + // The orientation of the carrier wrt the horizontal local frame + SGQuatd hl2body = SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll); + // and postrotate the orientation of the AIModel wrt the horizontal + // local frame + SGQuatd ec2body = ec2hl*hl2body; + // The cartesian position of the carrier in the wgs84 world + SGVec3d cartPos = SGVec3d::fromGeod(pos); + + // The position of the eyepoint - at least near that ... + SGVec3d eyePos(globals->get_current_view()->get_view_pos()); + // Add the position offset of the AIModel to gain the earth + // centered position + SGVec3d eyeWrtCarrier = eyePos - cartPos; + // rotate the eyepoint wrt carrier vector into the carriers frame + eyeWrtCarrier = ec2body.transform(eyeWrtCarrier); + // the eyepoints vector wrt the flols position + SGVec3d eyeWrtFlols = eyeWrtCarrier - flols_off; + + // the distance from the eyepoint to the flols + dist = norm(eyeWrtFlols); + + // now the angle, positive angles are upwards + if (fabs(dist) < SGLimits::min()) { + angle = 0; + } else { + double sAngle = -eyeWrtFlols(2)/dist; + sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle)); + angle = SGMiscd::rad2deg(asin(sAngle)); + } + + // set the value of source + if ( angle <= 4.35 && angle > 4.01 ) + source = 1; + else if ( angle <= 4.01 && angle > 3.670 ) + source = 2; + else if ( angle <= 3.670 && angle > 3.330 ) + source = 3; + else if ( angle <= 3.330 && angle > 2.990 ) + source = 4; + else if ( angle <= 2.990 && angle > 2.650 ) + source = 5; + else if ( angle <= 2.650 ) + source = 6; + else + source = 0; } //end update -bool FGAICarrier::init() { - if (!FGAIShip::init()) - return false; - - // process the 3d model here - // mark some objects solid, mark the wires ... - - // The model should be used for altitude computations. - // To avoid that every detail in a carrier 3D model will end into - // the aircraft local cache, only set the HOT traversal bit on - // selected objects. - ssgEntity *sel = aip.getSceneGraph(); - // Clear the HOT traversal flag - mark_nohot(sel); - // Selectively set that flag again for wires/cats/solid objects. - // Attach a pointer to this carrier class to those objects. - mark_wires(sel, wire_objects); - mark_cat(sel, catapult_objects); - mark_solid(sel, solid_objects); - - _longitude_node = fgGetNode("/position/longitude-deg", true); - _latitude_node = fgGetNode("/position/latitude-deg", true); - _altitude_node = fgGetNode("/position/altitude-ft", true); - _dme_freq_node = fgGetNode("/instrumentation/dme/frequencies/selected-mhz", true); - _surface_wind_from_deg_node = - fgGetNode("/environment/config/boundary/entry[0]/wind-from-heading-deg", true); - _surface_wind_speed_node = - fgGetNode("/environment/config/boundary/entry[0]/wind-speed-kt", true); - - - turn_to_launch_hdg = false; - returning = false; - - initialpos = pos; - base_course = hdg; - base_speed = speed; - - return true; +bool FGAICarrier::init(bool search_in_AI_path) { + if (!FGAIShip::init(search_in_AI_path)) + return false; + + _longitude_node = fgGetNode("/position/longitude-deg", true); + _latitude_node = fgGetNode("/position/latitude-deg", true); + _altitude_node = fgGetNode("/position/altitude-ft", true); + + _launchbar_state_node = fgGetNode("/gear/launchbar/state", true); + + _surface_wind_from_deg_node = + fgGetNode("/environment/config/boundary/entry[0]/wind-from-heading-deg", true); + _surface_wind_speed_node = + fgGetNode("/environment/config/boundary/entry[0]/wind-speed-kt", true); + + + turn_to_launch_hdg = false; + turn_to_recovery_hdg = false; + turn_to_base_course = true; + returning = false; + in_to_wind = false; + + mOpBoxPos = pos; + base_course = hdg; + base_speed = speed; + + pos_norm = raw_pos_norm = 0; + elevators = false; + transition_time = 150; + time_constant = 0.005; + jbd_pos_norm = raw_jbd_pos_norm = 0; + jbd = false ; + jbd_transition_time = 3; + jbd_time_constant = 0.1; + return true; } void FGAICarrier::bind() { - FGAIShip::bind(); + FGAIShip::bind(); - props->untie("velocities/true-airspeed-kt"); - - props->tie("controls/flols/source-lights", + props->untie("velocities/true-airspeed-kt"); + + props->tie("controls/flols/source-lights", SGRawValuePointer(&source)); - props->tie("controls/flols/distance-m", + props->tie("controls/flols/distance-m", SGRawValuePointer(&dist)); - props->tie("controls/flols/angle-degs", + props->tie("controls/flols/angle-degs", SGRawValuePointer(&angle)); - props->tie("controls/turn-to-launch-hdg", + props->tie("controls/turn-to-launch-hdg", SGRawValuePointer(&turn_to_launch_hdg)); - props->tie("controls/in-to-wind", + props->tie("controls/in-to-wind", SGRawValuePointer(&turn_to_launch_hdg)); - props->tie("controls/base-course-deg", + props->tie("controls/base-course-deg", SGRawValuePointer(&base_course)); - props->tie("controls/base-speed-kts", + props->tie("controls/base-speed-kts", SGRawValuePointer(&base_speed)); - props->tie("controls/start-pos-lat-deg", - SGRawValuePointer(&initialpos[1])); - props->tie("controls/start-pos-long-deg", - SGRawValuePointer(&initialpos[0])); - props->tie("velocities/speed-kts", - SGRawValuePointer(&speed)); - props->tie("environment/surface-wind-speed-true-kts", + props->tie("controls/start-pos-lat-deg", + SGRawValueMethods(pos, &SGGeod::getLatitudeDeg)); + props->tie("controls/start-pos-long-deg", + SGRawValueMethods(pos, &SGGeod::getLongitudeDeg)); + props->tie("controls/mp-control", + SGRawValuePointer(&MPControl)); + props->tie("controls/ai-control", + SGRawValuePointer(&AIControl)); + props->tie("environment/surface-wind-speed-true-kts", SGRawValuePointer(&wind_speed_kts)); - props->tie("environment/surface-wind-from-true-degs", + props->tie("environment/surface-wind-from-true-degs", SGRawValuePointer(&wind_from_deg)); - props->tie("environment/rel-wind-from-degs", + props->tie("environment/rel-wind-from-degs", SGRawValuePointer(&rel_wind_from_deg)); - props->tie("environment/rel-wind-from-carrier-hdg-degs", + props->tie("environment/rel-wind-from-carrier-hdg-degs", SGRawValuePointer(&rel_wind)); - props->tie("environment/rel-wind-speed-kts", + props->tie("environment/rel-wind-speed-kts", SGRawValuePointer(&rel_wind_speed_kts)); - props->tie("controls/flols/wave-off-lights", - SGRawValuePointer(&wave_off_lights)); - - props->setBoolValue("controls/flols/cut-lights", false); - props->setBoolValue("controls/flols/wave-off-lights", false); - props->setBoolValue("controls/flols/cond-datum-lights", true); - props->setBoolValue("controls/crew", false); - - props->setStringValue("navaids/tacan/channel-ID", TACAN_channel_id.c_str()); - props->setStringValue("sign", sign.c_str()); + props->tie("environment/in-to-wind", + SGRawValuePointer(&in_to_wind)); + //props->tie("controls/flols/wave-off-lights", + // SGRawValuePointer(&wave_off_lights)); + props->tie("controls/elevators", + SGRawValuePointer(&elevators)); + props->tie("surface-positions/elevators-pos-norm", + SGRawValuePointer(&pos_norm)); + props->tie("controls/constants/elevators/trans-time-s", + SGRawValuePointer(&transition_time)); + props->tie("controls/constants/elevators/time-constant", + SGRawValuePointer(&time_constant)); + props->tie("controls/jbd", + SGRawValuePointer(&jbd)); + props->tie("surface-positions/jbd-pos-norm", + SGRawValuePointer(&jbd_pos_norm)); + props->tie("controls/constants/jbd/trans-time-s", + SGRawValuePointer(&jbd_transition_time)); + props->tie("controls/constants/jbd/time-constant", + SGRawValuePointer(&jbd_time_constant)); + props->tie("controls/turn-to-recovery-hdg", + SGRawValuePointer(&turn_to_recovery_hdg)); + props->tie("controls/turn-to-base-course", + SGRawValuePointer(&turn_to_base_course)); + + + props->setBoolValue("controls/flols/cut-lights", false); + props->setBoolValue("controls/flols/wave-off-lights", false); + props->setBoolValue("controls/flols/cond-datum-lights", true); + props->setBoolValue("controls/crew", false); + props->setStringValue("navaids/tacan/channel-ID", TACAN_channel_id.c_str()); + props->setStringValue("sign", sign.c_str()); + props->setBoolValue("controls/lighting/deck-lights", false); + props->setDoubleValue("controls/lighting/flood-lights-red-norm", 0); } + void FGAICarrier::unbind() { FGAIShip::unbind(); - + props->untie("velocities/true-airspeed-kt"); - props->untie("controls/flols/source-lights"); props->untie("controls/flols/distance-m"); props->untie("controls/flols/angle-degs"); props->untie("controls/turn-to-launch-hdg"); - props->untie("velocities/speed-kts"); props->untie("environment/wind-speed-true-kts"); props->untie("environment/wind-from-true-degs"); props->untie("environment/rel-wind-from-degs"); props->untie("environment/rel-wind-speed-kts"); - props->untie("controls/flols/wave-off-lights"); - + props->untie("environment/in-to-wind"); + //props->untie("controls/flols/wave-off-lights"); + props->untie("controls/elevators"); + props->untie("surface-positions/elevators-pos-norm"); + props->untie("controls/constants/elevators/trans-time-secs"); + props->untie("controls/constants/elevators/time-constant"); + props->untie("controls/jbd"); + props->untie("surface-positions/jbd/pos-norm"); + props->untie("controls/constants/jbd/trans-time-s"); + props->untie("controls/jbd-time-constant"); + props->untie("controls/mp-control"); + props->untie("controls/ai-control"); + props->untie("controls/turn-to-recovery-hdg"); + props->untie("controls/turn-to-base-course"); } -bool FGAICarrier::getParkPosition(const string& id, Point3D& geodPos, - double& hdng, sgdVec3 uvw) -{ - - // FIXME: does not yet cover rotation speeds. - list::iterator it = ppositions.begin(); - while (it != ppositions.end()) { - // Take either the specified one or the first one ... - if ((*it).name == id || id.empty()) { - ParkPosition ppos = *it; - geodPos = getGeocPosAt(ppos.offset); - hdng = hdg + ppos.heading_deg; - double shdng = sin(ppos.heading_deg * SGD_DEGREES_TO_RADIANS); - double chdng = cos(ppos.heading_deg * SGD_DEGREES_TO_RADIANS); - double speed_fps = speed*1.6878099; - sgdSetVec3(uvw, chdng*speed_fps, shdng*speed_fps, 0); - return true; - } - ++it; - } - - return false; -} - -void FGAICarrier::mark_nohot(ssgEntity* e) { - if (e->isAKindOf(ssgTypeBranch())) { - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - mark_nohot(kid); - - br->clrTraversalMaskBits(SSGTRAV_HOT); - - } else if (e->isAKindOf(ssgTypeLeaf())) { - e->clrTraversalMaskBits(SSGTRAV_HOT); - - } -} - -bool FGAICarrier::mark_wires(ssgEntity* e, const list& wire_objects, bool mark) { - bool found = false; - if (e->isAKindOf(ssgTypeBranch())) { - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - - list::const_iterator it; - for (it = wire_objects.begin(); it != wire_objects.end(); ++it) - mark = mark || (e->getName() && (*it) == e->getName()); - - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - found = mark_wires(kid, wire_objects, mark) || found; - - if (found) - br->setTraversalMaskBits(SSGTRAV_HOT); - - } else if (e->isAKindOf(ssgTypeLeaf())) { - list::const_iterator it; - for (it = wire_objects.begin(); it != wire_objects.end(); ++it) { - if (mark || (e->getName() && (*it) == e->getName())) { - e->setTraversalMaskBits(SSGTRAV_HOT); - ssgBase* ud = e->getUserData(); - if (ud) { - FGAICarrierHardware* ch = dynamic_cast(ud); - if (ch) { - SG_LOG(SG_GENERAL, SG_WARN, - "AICarrier: Carrier hardware gets marked twice!\n" - " You have propably a whole branch marked as" - " a wire which also includes other carrier hardware." - ); - } else { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found user data attached to a leaf node which " - "should be marked as a wire!\n ****Skipping!****"); - } - } else { - e->setUserData( FGAICarrierHardware::newWire( this ) ); - ssgLeaf *l = (ssgLeaf*)e; - if ( l->getNumLines() != 1 ) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found wires not modelled with exactly one line!"); - } - found = true; - } - } - } - } - return found; -} +bool FGAICarrier::getParkPosition(const string& id, SGGeod& geodPos, + double& hdng, SGVec3d& uvw) +{ -bool FGAICarrier::mark_solid(ssgEntity* e, const list& solid_objects, bool mark) { - bool found = false; - if (e->isAKindOf(ssgTypeBranch())) { - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - - list::const_iterator it; - for (it = solid_objects.begin(); it != solid_objects.end(); ++it) - mark = mark || (e->getName() && (*it) == e->getName()); - - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - found = mark_solid(kid, solid_objects, mark) || found; - - if (found) - br->setTraversalMaskBits(SSGTRAV_HOT); - - } else if (e->isAKindOf(ssgTypeLeaf())) { - list::const_iterator it; - for (it = solid_objects.begin(); it != solid_objects.end(); ++it) { - if (mark || (e->getName() && (*it) == e->getName())) { - e->setTraversalMaskBits(SSGTRAV_HOT); - ssgBase* ud = e->getUserData(); - if (ud) { - FGAICarrierHardware* ch = dynamic_cast(ud); - if (ch) { - SG_LOG(SG_GENERAL, SG_WARN, - "AICarrier: Carrier hardware gets marked twice!\n" - " You have propably a whole branch marked solid" - " which also includes other carrier hardware." - ); - } else { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found user data attached to a leaf node which " - "should be marked solid!\n ****Skipping!****"); - } - } else { - e->setUserData( FGAICarrierHardware::newSolid( this ) ); - found = true; + // FIXME: does not yet cover rotation speeds. + list::iterator it = ppositions.begin(); + while (it != ppositions.end()) { + // Take either the specified one or the first one ... + if ((*it).name == id || id.empty()) { + ParkPosition ppos = *it; + SGVec3d cartPos = getCartPosAt(ppos.offset); + geodPos = SGGeod::fromCart(cartPos); + hdng = hdg + ppos.heading_deg; + double shdng = sin(ppos.heading_deg * SGD_DEGREES_TO_RADIANS); + double chdng = cos(ppos.heading_deg * SGD_DEGREES_TO_RADIANS); + double speed_fps = speed*1.6878099; + uvw = SGVec3d(chdng*speed_fps, shdng*speed_fps, 0); + return true; } - } + ++it; } - } - return found; -} -bool FGAICarrier::mark_cat(ssgEntity* e, const list& cat_objects, bool mark) { - bool found = false; - if (e->isAKindOf(ssgTypeBranch())) { - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - - list::const_iterator it; - for (it = cat_objects.begin(); it != cat_objects.end(); ++it) - mark = mark || (e->getName() && (*it) == e->getName()); - - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - found = mark_cat(kid, cat_objects, mark) || found; - - if (found) - br->setTraversalMaskBits(SSGTRAV_HOT); - - } else if (e->isAKindOf(ssgTypeLeaf())) { - list::const_iterator it; - for (it = cat_objects.begin(); it != cat_objects.end(); ++it) { - if (mark || (e->getName() && (*it) == e->getName())) { - e->setTraversalMaskBits(SSGTRAV_HOT); - ssgBase* ud = e->getUserData(); - if (ud) { - FGAICarrierHardware* ch = dynamic_cast(ud); - if (ch) { - SG_LOG(SG_GENERAL, SG_WARN, - "AICarrier: Carrier hardware gets marked twice!\n" - "You have probably a whole branch marked as" - "a catapult which also includes other carrier hardware." - ); - } else { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found user data attached to a leaf node which " - "should be marked as a catapult!\n ****Skipping!****"); - } - } else { - e->setUserData( FGAICarrierHardware::newCatapult( this ) ); - ssgLeaf *l = (ssgLeaf*)e; - if ( l->getNumLines() != 1 ) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found a cat not modelled with exactly " - "one line!"); - } else { - // Now some special code to make sure the cat points in the right - // direction. The 0 index must be the backward end, the 1 index - // the forward end. - // Forward is positive x-direction in our 3D model, also the model - // as such is flattened when it is loaded, so we do not need to - // care for transforms ... - short v[2]; - l->getLine(0, v, v+1 ); - sgVec3 ends[2]; - for (int k=0; k<2; ++k) - sgCopyVec3( ends[k], l->getVertex( v[k] ) ); - - // When the 1 end is behind the 0 end, swap the coordinates. - if (ends[0][0] < ends[1][0]) { - sgCopyVec3( l->getVertex( v[0] ), ends[1] ); - sgCopyVec3( l->getVertex( v[1] ), ends[0] ); - } - - found = true; - } - } - } - } - } - return found; + return false; } -void FGAICarrier::UpdateFlols(sgdMat3 trans) { - - float in[3]; - float out[3]; - - double flolsXYZ[3], eyeXYZ[3]; - double lat, lon, alt; - Point3D eyepos; - Point3D flolspos; - -/* cout << "x_offset " << flols_x_offset - << " y_offset " << flols_y_offset - << " z_offset " << flols_z_offset << endl; - - cout << "roll " << roll - << " heading " << hdg - << " pitch " << pitch << endl; - - cout << "carrier lon " << pos[0] - << " lat " << pos[1] - << " alt " << pos[2] << endl;*/ - -// set the Flols intitial position to the carrier position - - flolspos = pos; - -/* cout << "flols lon " << flolspos[0] - << " lat " << flolspos[1] - << " alt " << flolspos[2] << endl;*/ - -// set the offsets in metres - -/* cout << "flols_x_offset " << flols_x_offset << endl - << "flols_y_offset " << flols_y_offset << endl - << "flols_z_offset " << flols_z_offset << endl;*/ - - in[0] = flols_off.x(); - in[1] = flols_off.y(); - in[2] = flols_off.z(); - -// multiply the input and transform matrices - - out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2]; - out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2]; - out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2]; - -// convert meters to ft to degrees of latitude - out[0] = (out[0] * 3.28083989501) /(366468.96 - 3717.12 * cos(flolspos[0] * SG_DEGREES_TO_RADIANS)); - -// convert meters to ft to degrees of longitude - out[1] = (out[1] * 3.28083989501)/(365228.16 * cos(flolspos[1] * SG_DEGREES_TO_RADIANS)); - -//print out the result -/* cout << "lat adjust deg" << out[0] - << " lon adjust deg " << out[1] - << " alt adjust m " << out[2] << endl;*/ - -// adjust Flols position - flolspos[0] += out[0]; - flolspos[1] += out[1]; - flolspos[2] += out[2]; - -// convert flols position to cartesian co-ordinates - - sgGeodToCart(flolspos[1] * SG_DEGREES_TO_RADIANS, - flolspos[0] * SG_DEGREES_TO_RADIANS, - flolspos[2] , flolsXYZ ); - - -/* cout << "flols X " << flolsXYZ[0] - << " Y " << flolsXYZ[1] - << " Z " << flolsXYZ[2] << endl; - -// check the conversion - - sgCartToGeod(flolsXYZ, &lat, &lon, &alt); - - cout << "flols check lon " << lon - << " lat " << lat - << " alt " << alt << endl; */ - -//get the current position of the pilot's eyepoint (cartesian cordinates) - - sgdCopyVec3( eyeXYZ, globals->get_current_view()->get_absolute_view_pos() ); - - /* cout << "Eye_X " << eyeXYZ[0] - << " Eye_Y " << eyeXYZ[1] - << " Eye_Z " << eyeXYZ[2] << endl; */ - - sgCartToGeod(eyeXYZ, &lat, &lon, &alt); - - eyepos[0] = lon * SG_RADIANS_TO_DEGREES; - eyepos[1] = lat * SG_RADIANS_TO_DEGREES; - eyepos[2] = alt; - -/* cout << "eye lon " << eyepos[0] - << " eye lat " << eyepos[1] - << " eye alt " << eyepos[2] << endl; */ - -//calculate the ditance from eye to flols - - dist = sgdDistanceVec3( flolsXYZ, eyeXYZ ); - -//apply an index error - - dist -= 100; - - //cout << "distance " << dist << endl; - - if ( dist < 5000 ) { - // calculate height above FLOLS - double y = eyepos[2] - flolspos[2]; - - // calculate the angle from the flols to eye - // above the horizontal - // double angle; - - if ( dist != 0 ) { - angle = asin( y / dist ); - } else { - angle = 0.0; - } - - angle *= SG_RADIANS_TO_DEGREES; - - - // cout << " height " << y << " angle " << angle ; - -// set the value of source - - if ( angle <= 4.35 && angle > 4.01 ) - { source = 1; } - else if ( angle <= 4.01 && angle > 3.670 ) - { source = 2; } - else if ( angle <= 3.670 && angle > 3.330 ) - { source = 3; } - else if ( angle <= 3.330 && angle > 2.990 ) - { source = 4; } - else if ( angle <= 2.990 && angle > 2.650 ) - { source = 5; } - else if ( angle <= 2.650 ) - { source = 6; } - else - { source = 0; } - -// cout << " source " << source << endl; - - } -} // end updateflols - // find relative wind - - - - void FGAICarrier::UpdateWind( double dt) { - double recip; - - //calculate the reciprocal hdg - - if (hdg >= 180){ - recip = hdg - 180; - } - else{ - recip = hdg + 180; - } - - //cout <<" heading: " << hdg << "recip: " << recip << endl; - //get the surface wind speed and direction wind_from_deg = _surface_wind_from_deg_node->getDoubleValue(); wind_speed_kts = _surface_wind_speed_node->getDoubleValue(); - - //calculate the surface wind speed north and east in kts + + //calculate the surface wind speed north and east in kts double wind_speed_from_north_kts = cos( wind_from_deg / SGD_RADIANS_TO_DEGREES )* wind_speed_kts ; double wind_speed_from_east_kts = sin( wind_from_deg / SGD_RADIANS_TO_DEGREES )* wind_speed_kts ; - - //calculate the carrier speed north and east in kts + + //calculate the carrier speed north and east in kts double speed_north_kts = cos( hdg / SGD_RADIANS_TO_DEGREES )* speed ; double speed_east_kts = sin( hdg / SGD_RADIANS_TO_DEGREES )* speed ; - + //calculate the relative wind speed north and east in kts double rel_wind_speed_from_east_kts = wind_speed_from_east_kts + speed_east_kts; double rel_wind_speed_from_north_kts = wind_speed_from_north_kts + speed_north_kts; - - //combine relative speeds north and east to get relative windspeed in kts - rel_wind_speed_kts = sqrt((rel_wind_speed_from_east_kts * rel_wind_speed_from_east_kts) + + //combine relative speeds north and east to get relative windspeed in kts + rel_wind_speed_kts = sqrt((rel_wind_speed_from_east_kts * rel_wind_speed_from_east_kts) + (rel_wind_speed_from_north_kts * rel_wind_speed_from_north_kts)); - + //calculate the relative wind direction - rel_wind_from_deg = atan(rel_wind_speed_from_east_kts/rel_wind_speed_from_north_kts) + rel_wind_from_deg = atan2(rel_wind_speed_from_east_kts, rel_wind_speed_from_north_kts) * SG_RADIANS_TO_DEGREES; - - // rationalise the output - if (rel_wind_speed_from_north_kts <= 0){ - rel_wind_from_deg = 180 + rel_wind_from_deg; - } - else{ - if(rel_wind_speed_from_east_kts <= 0){ - rel_wind_from_deg = 360 + rel_wind_from_deg; - } - } - + //calculate rel wind - rel_wind = rel_wind_from_deg - hdg ; - if (rel_wind > 180) rel_wind -= 360; - + rel_wind = rel_wind_from_deg - hdg; + SG_NORMALIZE_RANGE(rel_wind, -180.0, 180.0); + + //set in to wind property + InToWind(); + //switch the wave-off lights - if (InToWind()){ - wave_off_lights = false; - }else{ - wave_off_lights = true; - } - + //if (InToWind()) + // wave_off_lights = false; + //else + // wave_off_lights = true; + // cout << "rel wind: " << rel_wind << endl; }// end update wind + void FGAICarrier::TurnToLaunch(){ - - //calculate tgt speed - double tgt_speed = 25 - wind_speed_kts; - if (tgt_speed < 10) tgt_speed = 10; - - //turn the carrier - FGAIShip::TurnTo(wind_from_deg); - FGAIShip::AccelTo(tgt_speed); - - - -} // end turn to launch - + + // calculate tgt heading + if (wind_speed_kts < 3){ + tgt_heading = base_course; + } else { + tgt_heading = wind_from_deg; + } + + //calculate tgt speed + double tgt_speed = 25 - wind_speed_kts; + if (tgt_speed < 10) + tgt_speed = 10; + + //turn the carrier + FGAIShip::TurnTo(tgt_heading); + FGAIShip::AccelTo(tgt_speed); + +} + +void FGAICarrier::TurnToRecover(){ + + //these are the rules for adjusting heading to provide a relative wind + //down the angled flightdeck + + if (wind_speed_kts < 3){ + tgt_heading = base_course + 60; + } else if (rel_wind < -9 && rel_wind >= -180){ + tgt_heading = wind_from_deg; + } else if (rel_wind > -7 && rel_wind < 45){ + tgt_heading = wind_from_deg + 60; + } else if (rel_wind >=45 && rel_wind < 180){ + tgt_heading = wind_from_deg + 45; + } else + tgt_heading = hdg; + + SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0); + + //calculate tgt speed + double tgt_speed = 26 - wind_speed_kts; + if (tgt_speed < 10) + tgt_speed = 10; + + //turn the carrier + FGAIShip::TurnTo(tgt_heading); + FGAIShip::AccelTo(tgt_speed); + +} void FGAICarrier::TurnToBase(){ - + //turn the carrier - FGAIShip::TurnTo(base_course); - FGAIShip::AccelTo(base_speed); - -} // end turn to base + FGAIShip::TurnTo(base_course); + FGAIShip::AccelTo(base_speed); + +} + void FGAICarrier::ReturnToBox(){ - double course, distance; - - //get the carrier position - carrierpos = pos; - - //cout << "lat: " << carrierpos[1] << " lon: " << carrierpos[0] << endl; - + double course, distance, az2; + //calculate the bearing and range of the initial position from the carrier - geo_inverse_wgs_84(carrierpos[2], - carrierpos[1], - carrierpos[0], - initialpos[1], - initialpos[0], - &course, &az2, &distance); - + geo_inverse_wgs_84(pos, mOpBoxPos, &course, &az2, &distance); + distance *= SG_METER_TO_NM; //cout << "return course: " << course << " distance: " << distance << endl; //turn the carrier - FGAIShip::TurnTo(course); - FGAIShip::AccelTo(base_speed); - if (distance >= 1 ){ - returning = true; - }else{ - returning = false; - } - -} // end turn to base - - -void FGAICarrier::UpdateTACAN(double dt){ //update the TACAN - - //cout << "TACAN: " << TACAN_channel_id << endl; - - double max_range_nm = 100; //nm - - double dme_freq = _dme_freq_node->getDoubleValue(); - - //cout << "dme_freq: " << dme_freq << endl; - - if (TACAN_channel_id == "017X"){ - - //get the aircraft position - double longitude_deg = _longitude_node->getDoubleValue(); - double latitude_deg = _latitude_node->getDoubleValue(); - double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER; - - //get the carrier position - carrierpos = pos; - - //cout << "lat: " << carrierpos[1] << " lon: " << carrierpos[0] << endl; - - //calculate the bearing and range of the carrier from the aircraft - geo_inverse_wgs_84(altitude_m, - latitude_deg, - longitude_deg, - carrierpos[1], - carrierpos[0], - &bearing, &az2, &range); - - range *= SG_METER_TO_NM; - - - - double aircraft_horizon_nm = Horizon(altitude_m) * SG_METER_TO_NM; - double carrier_horizon_nm = Horizon(50) * SG_METER_TO_NM; - double horizon_nm = aircraft_horizon_nm + carrier_horizon_nm; - - if (range > horizon_nm || range > max_range_nm) { - range = 0; - bearing = 0 ; - } - /*cout << "bearing: " << bearing << " range: " << range << " altitude: " << altitude_m - << " horizon: " << horizon_nm << endl; */ - } else { - range = 0; - bearing = 0 ; - } // end if - -}// end update TACAN - -bool FGAICarrier::OutsideBox(){ //returns true if the carrier is outside operating box + FGAIShip::TurnTo(course); + FGAIShip::AccelTo(base_speed); + + if (distance >= 1) + returning = true; + else + returning = false; + +} // end turn to base + + +bool FGAICarrier::OutsideBox() { //returns true if the carrier is outside operating box if ( max_lat == 0 && min_lat == 0 && max_long == 0 && min_long == 0) { - SG_LOG(SG_GENERAL, SG_INFO,"AICarrier: No Operating Box defined" ); - return false; - } - - if (initialpos[1] >= 0){//northern hemisphere - if (pos[1] >= initialpos[1] + max_lat) {return true;} - else if (pos[1] <= initialpos[1] - min_lat) {return true;} - }else{ //southern hemisphere - if (pos[1] <= initialpos[1] - max_lat) {return true;} - else if (pos[1] >= initialpos[1] + min_lat) {return true;} + SG_LOG(SG_GENERAL, SG_DEBUG, "AICarrier: No Operating Box defined" ); + return false; + } + + if (mOpBoxPos.getLatitudeDeg() >= 0) { //northern hemisphere + if (pos.getLatitudeDeg() >= mOpBoxPos.getLatitudeDeg() + max_lat) + return true; + + if (pos.getLatitudeDeg() <= mOpBoxPos.getLatitudeDeg() - min_lat) + return true; + + } else { //southern hemisphere + if (pos.getLatitudeDeg() <= mOpBoxPos.getLatitudeDeg() - max_lat) + return true; + + if (pos.getLatitudeDeg() >= mOpBoxPos.getLatitudeDeg() + min_lat) + return true; } - - if (initialpos[0] >=0) {//eastern hemisphere - if (pos[0] >= initialpos[0] + max_long) {return true;} - else if (pos[0] <= initialpos[0] - min_long) {return true;} - }else{ //western hemisphere - if (pos[0] <= initialpos[0] - max_long) {return true;} - else if (pos[0] >= initialpos[0] + min_long) {return true;} + + if (mOpBoxPos.getLongitudeDeg() >=0) { //eastern hemisphere + if (pos.getLongitudeDeg() >= mOpBoxPos.getLongitudeDeg() + max_long) + return true; + + if (pos.getLongitudeDeg() <= mOpBoxPos.getLongitudeDeg() - min_long) + return true; + + } else { //western hemisphere + if (pos.getLongitudeDeg() <= mOpBoxPos.getLongitudeDeg() - max_long) + return true; + + if (pos.getLongitudeDeg() >= mOpBoxPos.getLongitudeDeg() + min_long) + return true; } - - SG_LOG(SG_GENERAL, SG_INFO,"AICarrier: Inside Operating Box" ); - - return false; + + SG_LOG(SG_GENERAL, SG_DEBUG, "AICarrier: Inside Operating Box" ); + return false; } // end OutsideBox -// return the distance to the horizon, given the altitude and the radius of the earth -float FGAICarrier::Horizon(float h) { return RADIUS_M * acos(RADIUS_M / (RADIUS_M + h)); } - -bool FGAICarrier::InToWind(){ - - // test - if ( fabs(rel_wind) < 5 ) return true; + +bool FGAICarrier::InToWind() { + in_to_wind = false; + + if ( fabs(rel_wind) < 10 ){ + in_to_wind = true; + return true; + } return false; - -} //end InToWind -int FGAICarrierHardware::unique_id = 1; +} + + +void FGAICarrier::UpdateElevator(double dt, double transition_time) { + + double step = 0; + + if ((elevators && pos_norm >= 1 ) || (!elevators && pos_norm <= 0 )) + return; + + // move the elevators + if ( elevators ) { + step = dt/transition_time; + if ( step > 1 ) + step = 1; + } else { + step = -dt/transition_time; + if ( step < -1 ) + step = -1; + } + // assume a linear relationship + raw_pos_norm += step; + + //low pass filter + pos_norm = (raw_pos_norm * time_constant) + (pos_norm * (1 - time_constant)); + + //sanitise the output + if (raw_pos_norm >= 1) { + raw_pos_norm = 1; + } else if (raw_pos_norm <= 0) { + raw_pos_norm = 0; + } + return; + +} // end UpdateElevator + +void FGAICarrier::UpdateJBD(double dt, double jbd_transition_time) { + + string launchbar_state = _launchbar_state_node->getStringValue(); + double step = 0; + + if (launchbar_state == "Engaged"){ + jbd = true; + } else { + jbd = false; + } + + if (( jbd && jbd_pos_norm >= 1 ) || ( !jbd && jbd_pos_norm <= 0 )){ + return; + } + + // move the jbds + if ( jbd ) { + step = dt/jbd_transition_time; + if ( step > 1 ) + step = 1; + } else { + step = -dt/jbd_transition_time; + if ( step < -1 ) + step = -1; + } + + // assume a linear relationship + raw_jbd_pos_norm += step; + + //low pass filter + jbd_pos_norm = (raw_jbd_pos_norm * jbd_time_constant) + (jbd_pos_norm * (1 - jbd_time_constant)); + + //sanitise the output + if (jbd_pos_norm >= 1) { + jbd_pos_norm = 1; + } else if (jbd_pos_norm <= 0) { + jbd_pos_norm = 0; + } + + return; + +} // end UpdateJBD