/**
* Populates the NPS fdm struct after a simulation step.
*/
static void fetch_state(void) {
FGPropertyManager* node = FDMExec->GetPropertyManager()->GetNode("simulation/sim-time-sec");
fdm.time = node->getDoubleValue();
#if DEBUG_NPS_JSBSIM
printf("%f,",fdm.time);
#endif
FGPropagate* propagate = FDMExec->GetPropagate();
FGAccelerations* accelerations = FDMExec->GetAccelerations();
fdm.on_ground = FDMExec->GetGroundReactions()->GetWOW();
/*
* position
*/
jsbsimloc_to_loc(&fdm.ecef_pos,&propagate->GetLocation());
fdm.hmsl = propagate->GetAltitudeASLmeters();
/*
* linear speed and accelerations
*/
/* in body frame */
const FGColumnVector3& fg_body_ecef_vel = propagate->GetUVW();
jsbsimvec_to_vec(&fdm.body_ecef_vel, &fg_body_ecef_vel);
const FGColumnVector3& fg_body_ecef_accel = accelerations->GetUVWdot();
jsbsimvec_to_vec(&fdm.body_ecef_accel,&fg_body_ecef_accel);
#if DEBUG_NPS_JSBSIM
printf("%f,%f,%f,%f,%f,%f,",(&fg_body_ecef_accel)->Entry(1),(&fg_body_ecef_accel)->Entry(2),(&fg_body_ecef_accel)->Entry(3),fdm.body_ecef_accel.x,fdm.body_ecef_accel.y,fdm.body_ecef_accel.z);
#endif
/* in LTP frame */
const FGMatrix33& body_to_ltp = propagate->GetTb2l();
const FGColumnVector3& fg_ltp_ecef_vel = body_to_ltp * fg_body_ecef_vel;
jsbsimvec_to_vec((DoubleVect3*)&fdm.ltp_ecef_vel, &fg_ltp_ecef_vel);
const FGColumnVector3& fg_ltp_ecef_accel = body_to_ltp * fg_body_ecef_accel;
jsbsimvec_to_vec((DoubleVect3*)&fdm.ltp_ecef_accel, &fg_ltp_ecef_accel);
#if DEBUG_NPS_JSBSIM
printf("%f,%f,%f,%f,%f,%f,",(&fg_ltp_ecef_accel)->Entry(1),(&fg_ltp_ecef_accel)->Entry(2),(&fg_ltp_ecef_accel)->Entry(3),fdm.ltp_ecef_accel.x,fdm.ltp_ecef_accel.y,fdm.ltp_ecef_accel.z);
#endif
/* in ECEF frame */
const FGMatrix33& body_to_ecef = propagate->GetTb2ec();
const FGColumnVector3& fg_ecef_ecef_vel = body_to_ecef * fg_body_ecef_vel;
jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel);
const FGColumnVector3& fg_ecef_ecef_accel = body_to_ecef * fg_body_ecef_accel;
jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel);
#if DEBUG_NPS_JSBSIM
printf("%f,%f,%f,%f,%f,%f,",(&fg_ecef_ecef_accel)->Entry(1),(&fg_ecef_ecef_accel)->Entry(2),(&fg_ecef_ecef_accel)->Entry(3),fdm.ecef_ecef_accel.x,fdm.ecef_ecef_accel.y,fdm.ecef_ecef_accel.z);
#endif
/* in LTP pprz */
ned_of_ecef_point_d(&fdm.ltpprz_pos, <pdef, &fdm.ecef_pos);
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, <pdef, &fdm.ecef_ecef_vel);
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, <pdef, &fdm.ecef_ecef_accel);
#if DEBUG_NPS_JSBSIM
printf("%f,%f,%f,",fdm.ltpprz_ecef_accel.z,fdm.ltpprz_ecef_accel.y,fdm.ltpprz_ecef_accel.z);
#endif
/* llh */
llh_from_jsbsim(&fdm.lla_pos, propagate);
//for debug
lla_from_jsbsim_geodetic(&fdm.lla_pos_geod, propagate);
lla_from_jsbsim_geocentric(&fdm.lla_pos_geoc, propagate);
lla_of_ecef_d(&fdm.lla_pos_pprz, &fdm.ecef_pos);
fdm.agl = MetersOfFeet(propagate->GetDistanceAGL());
#if DEBUG_NPS_JSBSIM
printf("%f\n",fdm.agl);
#endif
/*
* attitude
*/
const FGQuaternion jsb_quat = propagate->GetQuaternion();
jsbsimquat_to_quat(&fdm.ltp_to_body_quat, &jsb_quat);
/* convert to eulers */
DOUBLE_EULERS_OF_QUAT(fdm.ltp_to_body_eulers, fdm.ltp_to_body_quat);
/* the "false" pprz lpt */
/* FIXME: use jsbsim ltp for now */
EULERS_COPY(fdm.ltpprz_to_body_eulers, fdm.ltp_to_body_eulers);
QUAT_COPY(fdm.ltpprz_to_body_quat, fdm.ltp_to_body_quat);
/*
* rotational speed and accelerations
*/
jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR());
jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot());
/*
* wind
*/
const FGColumnVector3& fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED();
jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned);
}
void FGTrim::trimOnGround(void)
{
FGGroundReactions* GroundReactions = fdmex->GetGroundReactions();
FGPropagate* Propagate = fdmex->GetPropagate();
FGMassBalance* MassBalance = fdmex->GetMassBalance();
FGAccelerations* Accelerations = fdmex->GetAccelerations();
vector<ContactPoints> contacts;
FGLocation CGLocation = Propagate->GetLocation();
FGMatrix33 Tec2b = Propagate->GetTec2b();
FGMatrix33 Tl2b = Propagate->GetTl2b();
double hmin = 1E+10;
int contactRef = -1;
// Build the list of the aircraft contact points and take opportunity of the
// loop to find which one is closer to (or deeper into) the ground.
for (int i = 0; i < GroundReactions->GetNumGearUnits(); i++) {
ContactPoints c;
FGLGear* gear = GroundReactions->GetGearUnit(i);
c.location = gear->GetLocalGear();
FGLocation gearLoc = CGLocation.LocalToLocation(c.location);
c.location = Tl2b * c.location;
FGColumnVector3 normal, vDummy;
FGLocation lDummy;
double height = gearLoc.GetContactPoint(lDummy, normal, vDummy, vDummy);
c.normal = Tec2b * normal;
contacts.push_back(c);
if (height < hmin) {
hmin = height;
contactRef = i;
}
}
// Remove the contact point that is closest to the ground from the list:
// the rotation axis will be going thru this point so we need to remove it
// to avoid divisions by zero that could result from the computation of
// the rotations.
FGColumnVector3 contact0 = contacts[contactRef].location;
contacts.erase(contacts.begin() + contactRef);
// Update the initial conditions: this should remove the forces generated
// by overcompressed landing gears
fgic.SetAltitudeASLFtIC(fgic.GetAltitudeASLFtIC() - hmin);
fdmex->Initialize(&fgic);
fdmex->Run();
// Compute the rotation axis: it is obtained from the direction of the
// moment measured at the contact point 'contact0'
FGColumnVector3 force = MassBalance->GetMass() * Accelerations->GetUVWdot();
FGColumnVector3 moment = MassBalance->GetJ() * Accelerations->GetPQRdot()
+ force * contact0;
FGColumnVector3 rotationAxis = moment.Normalize();
// Compute the rotation parameters: angle and the first point to come into
// contact with the ground when the rotation is applied.
RotationParameters rParam = calcRotation(contacts, rotationAxis, contact0);
FGQuaternion q0(rParam.angleMin, rotationAxis);
// Apply the computed rotation to all the contact points
FGMatrix33 rot = q0.GetTInv();
vector<ContactPoints>::iterator iter;
for (iter = contacts.begin(); iter != contacts.end(); iter++)
iter->location = contact0 + rot * (iter->location - contact0);
// Remove the second point to come in contact with the ground from the list.
// The reason is the same than above: avoid divisions by zero when the next
// rotation will be computed.
FGColumnVector3 contact1 = rParam.contactRef->location;
contacts.erase(rParam.contactRef);
// Compute the rotation axis: now there are 2 points in contact with the
// ground so the only option for the aircraft is to rotate around the axis
// generated by these 2 points.
rotationAxis = contact1 - contact0;
// Make sure that the rotation orientation is consistent with the moment.
if (DotProduct(rotationAxis, moment) < 0.0)
rotationAxis = contact0 - contact1;
rotationAxis.Normalize();
// Compute the rotation parameters
rParam = calcRotation(contacts, rotationAxis, contact0);
FGQuaternion q1(rParam.angleMin, rotationAxis);
// Update the aircraft orientation
FGColumnVector3 euler = (q0 * q1 * fgic.GetOrientation()).GetEuler();
fgic.SetPhiRadIC(euler(1));
fgic.SetThetaRadIC(euler(2));
fgic.SetPsiRadIC(euler(3));
}
