+ using namespace simgear;
+ if (get_direction() == SG_IO_OUT) {
+
+ // check if we have left initialization phase. That will not provide
+ // interresting data, also the freeze in simulation time hurts the
+ // multiplayer clients
+ double sim_time = globals->get_sim_time_sec();
+// if (sim_time < 20)
+// return true;
+
+ FGInterface *ifce = cur_fdm_state;
+
+ // put together a motion info struct, you will get that later
+ // from FGInterface directly ...
+ FGExternalMotionData motionInfo;
+
+ // The current simulation time we need to update for,
+ // note that the simulation time is updated before calling all the
+ // update methods. Thus it contains the time intervals *end* time.
+ // The FDM is already run, so the states belong to that time.
+ motionInfo.time = sim_time;
+
+ // The typical lag will be the reciprocal of the output frequency
+ double hz = get_hz();
+ if (hz != 0) // I guess we can test a double for exact zero in this case
+ motionInfo.lag = 1/get_hz();
+ else
+ motionInfo.lag = 0.1; //??
+
+ // These are for now converted from lat/lon/alt and euler angles.
+ // But this should change in FGInterface ...
+ double lon = ifce->get_Longitude();
+ double lat = ifce->get_Latitude();
+ // first the aprioriate structure for the geodetic one
+ SGGeod geod = SGGeod::fromRadFt(lon, lat, ifce->get_Altitude());
+ // Convert to cartesion coordinate
+ motionInfo.position = SGVec3d::fromGeod(geod);
+
+ // The quaternion rotating from the earth centered frame to the
+ // horizontal local frame
+ SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)lon, (float)lat);
+ // The orientation wrt the horizontal local frame
+ float heading = ifce->get_Psi();
+ float pitch = ifce->get_Theta();
+ float roll = ifce->get_Phi();
+ SGQuatf hlOr = SGQuatf::fromYawPitchRoll(heading, pitch, roll);
+ // The orientation of the vehicle wrt the earth centered frame
+ motionInfo.orientation = qEc2Hl*hlOr;
+
+ if (!ifce->is_suspended()) {
+ // velocities
+ motionInfo.linearVel = SG_FEET_TO_METER*SGVec3f(ifce->get_U_body(),
+ ifce->get_V_body(),
+ ifce->get_W_body());
+ motionInfo.angularVel = SGVec3f(ifce->get_P_body(),
+ ifce->get_Q_body(),
+ ifce->get_R_body());
+
+ // accels, set that to zero for now.
+ // Angular accelerations are missing from the interface anyway,
+ // linear accelerations are screwed up at least for JSBSim.
+// motionInfo.linearAccel = SG_FEET_TO_METER*SGVec3f(ifce->get_U_dot_body(),
+// ifce->get_V_dot_body(),
+// ifce->get_W_dot_body());
+ motionInfo.linearAccel = SGVec3f::zeros();
+ motionInfo.angularAccel = SGVec3f::zeros();
+ } else {
+ // if the interface is suspendend, prevent the client from
+ // wild extrapolations
+ motionInfo.linearVel = SGVec3f::zeros();
+ motionInfo.angularVel = SGVec3f::zeros();
+ motionInfo.linearAccel = SGVec3f::zeros();
+ motionInfo.angularAccel = SGVec3f::zeros();
+ }