X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAICarrier.cxx;h=fba537a6764c2021c0b21002df5b985e14458b01;hb=afbc360eeded20d4447ab005ee0e234a43f068cd;hp=6c33639e30b92f3c650555d052660d78f43faaa1;hpb=127e84cee93d9029539575dbfedac853881e6503;p=flightgear.git diff --git a/src/AIModel/AICarrier.cxx b/src/AIModel/AICarrier.cxx index 6c33639e3..fba537a67 100644 --- a/src/AIModel/AICarrier.cxx +++ b/src/AIModel/AICarrier.cxx @@ -15,384 +15,610 @@ // // 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" - -FGAICarrier::FGAICarrier(FGAIManager* mgr) : FGAIShip(mgr) { +FGAICarrier::FGAICarrier() : FGAIShip(otCarrier) { } FGAICarrier::~FGAICarrier() { } -void FGAICarrier::setSolidObjects(const list& so) { - solid_objects = so; +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)); + setCallSign(scFileNode->getStringValue("callsign", "")); + + + 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::setWireObjects(const list& wo) { - wire_objects = wo; +void FGAICarrier::setWind_from_east(double fps) { + wind_from_east = fps; } -void FGAICarrier::setCatapultObjects(const list& co) { - catapult_objects = co; +void FGAICarrier::setWind_from_north(double fps) { + wind_from_north = fps; } -void FGAICarrier::getVelocityWrtEarth(sgVec3 v) { - sgCopyVec3(v, vel_wrt_earth ); +void FGAICarrier::setMaxLat(double deg) { + max_lat = fabs(deg); } -void FGAICarrier::update(double dt) { - UpdateFlols(dt); - FGAIShip::update(dt); - - // 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); - sgSetVec3( 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 ); +void FGAICarrier::setMinLat(double deg) { + min_lat = fabs(deg); +} + +void FGAICarrier::setMaxLong(double deg) { + max_long = fabs(deg); +} + +void FGAICarrier::setMinLong(double deg) { + min_long = fabs(deg); +} + +void FGAICarrier::setSign(const string& s) { + sign = s; +} + +void FGAICarrier::setTACANChannelID(const string& id) { + TACAN_channel_id = id; +} + +void FGAICarrier::setMPControl(bool c) { + MPControl = c; +} +void FGAICarrier::setAIControl(bool c) { + AIControl = c; } -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, false); - - return true; +void FGAICarrier::update(double dt) { + // 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(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() { - FGAIBase::bind(); + FGAIShip::bind(); - 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", - SGRawValuePointer(&dist)); - props->setBoolValue("controls/flols/cut-lights", false); - props->setBoolValue("controls/flols/wave-off-lights", false); - props->setBoolValue("controls/flols/cond-datum-lights", true); - } + props->tie("controls/flols/distance-m", + SGRawValuePointer(&dist)); + props->tie("controls/flols/angle-degs", + SGRawValuePointer(&angle)); + props->tie("controls/turn-to-launch-hdg", + SGRawValuePointer(&turn_to_launch_hdg)); + props->tie("controls/in-to-wind", + SGRawValuePointer(&turn_to_launch_hdg)); + props->tie("controls/base-course-deg", + SGRawValuePointer(&base_course)); + props->tie("controls/base-speed-kts", + SGRawValuePointer(&base_speed)); + 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", + SGRawValuePointer(&wind_from_deg)); + props->tie("environment/rel-wind-from-degs", + SGRawValuePointer(&rel_wind_from_deg)); + props->tie("environment/rel-wind-from-carrier-hdg-degs", + SGRawValuePointer(&rel_wind)); + props->tie("environment/rel-wind-speed-kts", + SGRawValuePointer(&rel_wind_speed_kts)); + 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() { - FGAIBase::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("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("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"); } - -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::getParkPosition(const string& id, SGGeod& geodPos, + double& hdng, SGVec3d& 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; + 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 false; } -bool FGAICarrier::mark_wires(ssgEntity* e, const list& wire_objects) { - bool found = false; - if (e->isAKindOf(ssgTypeBranch())) { - - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - found = mark_wires(kid, wire_objects) || 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 (e->getName() && (*it) == e->getName()) { - e->setTraversalMaskBits(SSGTRAV_HOT); - 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!"); - } +// find relative wind +void FGAICarrier::UpdateWind( double dt) { + + //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 + 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 + 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) + + (rel_wind_speed_from_north_kts * rel_wind_speed_from_north_kts)); + + //calculate the relative wind direction + rel_wind_from_deg = atan2(rel_wind_speed_from_east_kts, rel_wind_speed_from_north_kts) + * SG_RADIANS_TO_DEGREES; + + //calculate rel wind + 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; + + // cout << "rel wind: " << rel_wind << endl; + +}// end update wind - found = true; - } + +void FGAICarrier::TurnToLaunch(){ + + // calculate tgt heading + if (wind_speed_kts < 3){ + tgt_heading = base_course; + } else { + tgt_heading = wind_from_deg; } - } - return found; + + //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); + } -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); - e->setUserData( FGAICarrierHardware::newSolid( this ) ); - found = true; - } - } - } - return found; +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(){ -bool FGAICarrier::mark_cat(ssgEntity* e, const list& cat_objects) { - bool found = false; - if (e->isAKindOf(ssgTypeBranch())) { - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - found = mark_cat(kid, cat_objects) || 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 (e->getName() && (*it) == e->getName()) { - e->setTraversalMaskBits(SSGTRAV_HOT); - 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!"); - } - // 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] ); - } + //turn the carrier + FGAIShip::TurnTo(base_course); + FGAIShip::AccelTo(base_speed); + +} + + +void FGAICarrier::ReturnToBox(){ + double course, distance, az2; + + //calculate the bearing and range of the initial position from the carrier + 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 + + +bool FGAICarrier::OutsideBox() { //returns true if the carrier is outside operating box - found = true; - } + if ( max_lat == 0 && min_lat == 0 && max_long == 0 && min_long == 0) { + SG_LOG(SG_GENERAL, SG_DEBUG, "AICarrier: No Operating Box defined" ); + return false; } - } - return found; + + 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 (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_DEBUG, "AICarrier: Inside Operating Box" ); + return false; + +} // end OutsideBox + + +bool FGAICarrier::InToWind() { + in_to_wind = false; + + if ( fabs(rel_wind) < 10 ){ + in_to_wind = true; + return true; + } + return false; } -void FGAICarrier::UpdateFlols( double dt) { -/* 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_x_offset; - in[1] = flols_y_offset; - in[2] = flols_z_offset; - -// pre-process the trig functions - - cosRx = cos(roll * SG_DEGREES_TO_RADIANS); - sinRx = sin(roll * SG_DEGREES_TO_RADIANS); - cosRy = cos(pitch * SG_DEGREES_TO_RADIANS); - sinRy = sin(pitch * SG_DEGREES_TO_RADIANS); - cosRz = cos(hdg * SG_DEGREES_TO_RADIANS); - sinRz = sin(hdg * SG_DEGREES_TO_RADIANS); - -// set up the transform matrix - - trans[0][0] = cosRy * cosRz; - trans[0][1] = -1 * cosRx * sinRz + sinRx * sinRy * cosRz ; - trans[0][2] = sinRx * sinRz + cosRx * sinRy * cosRz; - - trans[1][0] = cosRy * sinRz; - trans[1][1] = cosRx * cosRz + sinRx * sinRy * sinRz; - trans[1][2] = -1 * sinRx * cosRx + cosRx * sinRy * sinRz; - - trans[2][0] = -1 * sinRy; - trans[2][1] = sinRx * cosRy; - trans[2][2] = cosRx * cosRy; - -// 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 ); - - //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 - -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