-
- // 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<float>::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;