Exemplo n.º 1
0
/**
 * 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, &ltpdef, &fdm.ecef_pos);
  ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, &ltpdef, &fdm.ecef_ecef_vel);
  ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, &ltpdef, &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);
}
Exemplo n.º 2
0
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));
}