Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_fdm_jsbsim.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #include <iostream>
30 #include <stdlib.h>
31 #include <stdio.h>
32 
33 // ignore stupid warnings in JSBSim
34 #pragma GCC diagnostic push
35 #pragma GCC diagnostic ignored "-Wunused-parameter"
36 
37 #include <FGFDMExec.h>
38 #include <FGJSBBase.h>
39 #include <initialization/FGInitialCondition.h>
40 #include <models/FGPropulsion.h>
41 #include <models/FGGroundReactions.h>
42 #include <models/FGAccelerations.h>
43 #include <models/FGAuxiliary.h>
44 #include <models/FGAtmosphere.h>
45 #include <models/FGAircraft.h>
46 #include <models/FGFCS.h>
47 #include <models/atmosphere/FGWinds.h>
48 
49 // Thrusters
50 #include <models/propulsion/FGThruster.h>
51 #include <models/propulsion/FGPropeller.h>
52 
53 // end ignore unused param warnings in JSBSim
54 #pragma GCC diagnostic pop
55 
56 #include "nps_autopilot.h"
57 #include "nps_fdm.h"
58 #include "math/pprz_geodetic.h"
61 #include "math/pprz_algebra.h"
63 
65 
66 #include "generated/airframe.h"
67 #include "generated/flight_plan.h"
68 
70 #define MetersOfFeet(_f) ((_f)/3.2808399)
71 #define FeetOfMeters(_m) ((_m)*3.2808399)
72 
73 #define PascalOfPsf(_p) ((_p) * 47.8802588889)
74 #define CelsiusOfRankine(_r) (((_r) - 491.67) / 1.8)
75 
79 #ifndef NPS_JSBSIM_MODEL
80 #define NPS_JSBSIM_MODEL AIRFRAME_NAME
81 #endif
82 
83 #ifdef NPS_INITIAL_CONDITITONS
84 #warning NPS_INITIAL_CONDITITONS was replaced by NPS_JSBSIM_INIT!
85 #warning Defaulting to flight plan location.
86 #endif
87 
91 #ifndef NPS_JSBSIM_PITCH_TRIM
92 #define NPS_JSBSIM_PITCH_TRIM 0.0
93 #endif
94 
95 #ifndef NPS_JSBSIM_ROLL_TRIM
96 #define NPS_JSBSIM_ROLL_TRIM 0.0
97 #endif
98 
99 #ifndef NPS_JSBSIM_YAW_TRIM
100 #define NPS_JSBSIM_YAW_TRIM 0.0
101 #endif
102 
106 #define DEG2RAD 0.017
107 
108 #ifndef NPS_JSBSIM_ELEVATOR_MAX_RAD
109 #define NPS_JSBSIM_ELEVATOR_MAX_RAD (20.0*DEG2RAD)
110 #endif
111 
112 #ifndef NPS_JSBSIM_AILERON_MAX_RAD
113 #define NPS_JSBSIM_AILERON_MAX_RAD (20.0*DEG2RAD)
114 #endif
115 
116 #ifndef NPS_JSBSIM_RUDDER_MAX_RAD
117 #define NPS_JSBSIM_RUDDER_MAX_RAD (20.0*DEG2RAD)
118 #endif
119 
120 #ifndef NPS_JSBSIM_FLAP_MAX_RAD
121 #define NPS_JSBSIM_FLAP_MAX_RAD (20.0*DEG2RAD)
122 #endif
123 
127 #define MIN_DT (1.0/10240.0)
128 // TODO: maybe lower for slower CPUs & HITL?
129 //#define MIN_DT (1.0/1000.0)
130 
131 using namespace JSBSim;
132 using namespace std;
133 
134 static void feed_jsbsim(double *commands, int commands_nb);
135 static void fetch_state(void);
136 static int check_for_nan(void);
137 
138 static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector);
139 static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location);
140 static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat);
141 static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector);
142 static void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate);
143 static void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate);
144 static void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate);
145 
146 static void init_jsbsim(double dt);
147 static void init_ltp(void);
148 
150 struct NpsFdm fdm;
151 
153 static FGFDMExec *FDMExec;
154 
155 static struct LtpDef_d ltpdef;
156 
157 // Offset between ecef in geodetic and geocentric coordinates
158 static struct EcefCoor_d offset;
159 
162 
164 double min_dt;
165 
166 void nps_fdm_init(double dt)
167 {
168 
169  fdm.init_dt = dt;
170  fdm.curr_dt = dt;
171  //Sets up the high fidelity timestep as a multiple of the normal timestep
172  for (min_dt = (1.0 / dt); min_dt < (1 / MIN_DT); min_dt += (1 / dt)) {}
173  min_dt = (1 / min_dt);
174 
175  fdm.nan_count = 0;
176 
177  VECT3_ASSIGN(offset, 0., 0., 0.);
178 
179  init_jsbsim(dt);
180 
181  FDMExec->RunIC();
182 
183  // Fix getting initial incorrect accel measurements
184  for(uint16_t i = 0; i < 1500; i++)
185  FDMExec->Run();
186 
187  init_ltp();
188 
189 #if DEBUG_NPS_JSBSIM
190  printf("fdm.time,fdm.body_ecef_accel.x,fdm.body_ecef_accel.y,fdm.body_ecef_accel.z,fdm.ltp_ecef_accel.x,fdm.ltp_ecef_accel.y,fdm.ltp_ecef_accel.z,fdm.ecef_ecef_accel.x,fdm.ecef_ecef_accel.y,fdm.ecef_ecef_accel.z,fdm.ltpprz_ecef_accel.z,fdm.ltpprz_ecef_accel.y,fdm.ltpprz_ecef_accel.z,fdm.agl\n");
191 #endif
192 
193  fetch_state();
194 
195 }
196 
197 void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb)
198 {
199 
200 #ifdef NPS_JSBSIM_LAUNCHSPEED
201  static bool already_launched = FALSE;
202 
203  if (launch && !already_launched) {
204  printf("Launching with speed of %.1f m/s!\n", (float)NPS_JSBSIM_LAUNCHSPEED);
205  FDMExec->GetIC()->SetUBodyFpsIC(FeetOfMeters(NPS_JSBSIM_LAUNCHSPEED));
206  FDMExec->RunIC();
207  already_launched = TRUE;
208  }
209 #endif
210 
211  feed_jsbsim(commands, commands_nb);
212 
213  /* To deal with ground interaction issues, we decrease the time
214  step as the vehicle is close to the ground. This is done predictively
215  to ensure no weird accelerations or oscillations. From tests with a bouncing
216  ball model in JSBSim, it seems that 10k steps per second is reasonable to capture
217  all the dynamics. Higher might be a bit more stable, but really starting to push
218  the simulation CPU requirements, especially for more complex models.
219  - at init: get the largest radius from CG to any contact point (landing gear)
220  - if descending...
221  - find current number of timesteps to impact
222  - if impact imminent, calculate a new timestep to use (with limit)
223  - if ascending...
224  - change timestep back to init value
225  - run sim for as many steps as needed to reach init_dt amount of time
226 
227  Of course, could probably be improved...
228  */
229  // If the vehicle has a downwards velocity
230  if (fdm.ltp_ecef_vel.z > 0) {
231  // Get the current number of timesteps until impact at current velocity
232  double numDT_to_impact = (fdm.agl - vehicle_radius_max) / (fdm.curr_dt * fdm.ltp_ecef_vel.z);
233  // If impact imminent within next timestep, use high sim rate
234  if (numDT_to_impact <= 1.0) {
235  fdm.curr_dt = min_dt;
236  }
237  }
238  // If the vehicle is moving upwards and out of the ground, reset timestep
239  else if ((fdm.ltp_ecef_vel.z <= 0) && ((fdm.agl + vehicle_radius_max) > 0)) {
241  }
242 
243  // Calculate the number of sim steps for correct amount of time elapsed
244  int num_steps = int(fdm.init_dt / fdm.curr_dt);
245 
246  // Set the timestep then run sim
247  FDMExec->Setdt(fdm.curr_dt);
248  int i;
249  for (i = 0; i < num_steps; i++) {
250  FDMExec->Run();
251  }
252 
253  fetch_state();
254 
255  /* Check the current state to make sure it is valid (no NaNs) */
256  if (check_for_nan()) {
257  printf("Error: FDM simulation encountered a total of %i NaN values at simulation time %f.\n", fdm.nan_count, fdm.time);
258  printf("It is likely the simulation diverged and gave non-physical results. If you did\n");
259  printf("not crash, check your model and/or initial conditions. Exiting with status 1.\n");
260  exit(1);
261  }
262 
263 }
264 
265 void nps_fdm_set_wind(double speed, double dir)
266 {
267  FGWinds *Winds = FDMExec->GetWinds();
268  Winds->SetWindspeed(FeetOfMeters(speed));
269  Winds->SetWindPsi(dir);
270 }
271 
272 void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
273 {
274  FGWinds *Winds = FDMExec->GetWinds();
275  Winds->SetWindNED(FeetOfMeters(wind_north), FeetOfMeters(wind_east),
276  FeetOfMeters(wind_down));
277 }
278 
279 void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
280 {
281  FGWinds *Winds = FDMExec->GetWinds();
282  /* wind speed used for turbulence */
283  Winds->SetWindspeed20ft(FeetOfMeters(wind_speed) / 2);
284  Winds->SetProbabilityOfExceedence(turbulence_severity);
285 }
286 
287 void nps_fdm_set_temperature(double temp, double h)
288 {
289  FDMExec->GetAtmosphere()->SetTemperature(temp, h, FGAtmosphere::eCelsius);
290 }
291 
298 static void feed_jsbsim(double *commands, int commands_nb __attribute__((unused)))
299 {
300 #ifdef NPS_ACTUATOR_NAMES
301  char buf[64];
302  const char *names[] = NPS_ACTUATOR_NAMES;
303  string property;
304 
305  int i;
306  for (i = 0; i < commands_nb; i++) {
307  sprintf(buf, "fcs/%s", names[i]);
308  property = string(buf);
309  FDMExec->GetPropertyManager()->GetNode(property)->SetDouble("", commands[i]);
310  }
311 #else /* use COMMAND names */
312 
313  // get FGFCS instance
314  FGFCS *FCS = FDMExec->GetFCS();
315 
316  // Set trims
317  FCS->SetPitchTrimCmd(NPS_JSBSIM_PITCH_TRIM);
318  FCS->SetRollTrimCmd(NPS_JSBSIM_ROLL_TRIM);
319  FCS->SetYawTrimCmd(NPS_JSBSIM_YAW_TRIM);
320 
321 #ifdef COMMAND_THROTTLE
322  FGPropulsion *FProp = FDMExec->GetPropulsion();
323  for (unsigned int i = 0; i < FDMExec->GetPropulsion()->GetNumEngines(); i++) {
324  FCS->SetThrottleCmd(i, commands[COMMAND_THROTTLE]);
325 
326  // Hack to show spinning propellers in flight gear models
327  if (commands[COMMAND_THROTTLE] > 0.01) {
328  FProp->SetStarter(1);
329  } else {
330  FProp->SetStarter(0);
331  }
332  }
333 #endif /* COMMAND_THROTTLE */
334 
335 #ifdef COMMAND_ROLL
336  FCS->SetDaCmd(commands[COMMAND_ROLL]);
337 #endif /* COMMAND_ROLL */
338 
339 #ifdef COMMAND_PITCH
340  FCS->SetDeCmd(commands[COMMAND_PITCH]);
341 #endif /* COMMAND_PITCH */
342 
343 #ifdef COMMAND_YAW
344  FCS->SetDrCmd(commands[COMMAND_YAW]);
345 #endif /* COMMAND_YAW */
346 
347 #ifdef COMMAND_FLAP
348  FCS->SetDfCmd(commands[COMMAND_FLAP]);
349 #endif /* COMMAND_FLAP */
350 
351 #endif /* NPS_ACTUATOR_NAMES */
352 }
353 
357 static void fetch_state(void)
358 {
359 
360  fdm.time = FDMExec->GetPropertyManager()->GetNode("simulation/sim-time-sec")->getDoubleValue();
361 
362 #if DEBUG_NPS_JSBSIM
363  printf("%f,", fdm.time);
364 #endif
365 
366  FGPropagate *propagate = FDMExec->GetPropagate();
367  FGAccelerations *accelerations = FDMExec->GetAccelerations();
368 
369  fdm.on_ground = FDMExec->GetGroundReactions()->GetWOW();
370 
371  /*
372  * position
373  */
374  jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation());
375  fdm.hmsl = propagate->GetAltitudeASLmeters();
376 
377  /*
378  * linear speed and accelerations
379  */
380 
381  /* in body frame */
382  jsbsimvec_to_vec(&fdm.body_ecef_vel, &propagate->GetUVW());
383  jsbsimvec_to_vec(&fdm.body_ecef_accel, &accelerations->GetUVWdot());
384  jsbsimvec_to_vec(&fdm.body_inertial_accel, &accelerations->GetUVWidot());
385  jsbsimvec_to_vec(&fdm.body_accel, &accelerations->GetBodyAccel());
386 
387 #if DEBUG_NPS_JSBSIM
388  printf("%f,%f,%f,", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);
389 #endif
390 
391  /* in LTP frame */
392  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_vel, &propagate->GetVel());
393  const FGColumnVector3 &fg_ltp_ecef_accel = propagate->GetTb2l() * accelerations->GetUVWdot();
394  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_accel, &fg_ltp_ecef_accel);
395 
396 #if DEBUG_NPS_JSBSIM
397  printf("%f,%f,%f,", fdm.ltp_ecef_accel.x, fdm.ltp_ecef_accel.y, fdm.ltp_ecef_accel.z);
398 #endif
399 
400  /* in ECEF frame */
401  const FGColumnVector3 &fg_ecef_ecef_vel = propagate->GetECEFVelocity();
402  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel);
403 
404  const FGColumnVector3 &fg_ecef_ecef_accel = propagate->GetTb2ec() * accelerations->GetUVWdot();
405  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel);
406 
407 #if DEBUG_NPS_JSBSIM
408  printf("%f,%f,%f,", fdm.ecef_ecef_accel.x, fdm.ecef_ecef_accel.y, fdm.ecef_ecef_accel.z);
409 #endif
410 
411  /* in LTP pprz */
415 
416 #if DEBUG_NPS_JSBSIM
418 #endif
419 
420  /* llh */
421  llh_from_jsbsim(&fdm.lla_pos, propagate);
422 
423  //for debug
427  fdm.agl = MetersOfFeet(propagate->GetDistanceAGL());
428 
429 #if DEBUG_NPS_JSBSIM
430  printf("%f\n", fdm.agl);
431 #endif
432 
433  /*
434  * attitude
435  */
436  const FGQuaternion jsb_quat = propagate->GetQuaternion();
438  /* convert to eulers */
440  /* the "false" pprz lpt */
441  /* FIXME: use jsbsim ltp for now */
444 
445  /*
446  * rotational speed and accelerations
447  */
448  jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR());
449  jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot());
450 
451  jsbsimvec_to_rate(&fdm.body_inertial_rotvel, &propagate->GetPQRi());
452  jsbsimvec_to_rate(&fdm.body_inertial_rotaccel, &accelerations->GetPQRidot());
453 
454 
455  /*
456  * wind
457  */
458  const FGColumnVector3 &fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED();
459  jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned);
460 
461  /*
462  * Equivalent Airspeed, atmospheric pressure and temperature.
463  */
464  fdm.airspeed = MetersOfFeet(FDMExec->GetAuxiliary()->GetVequivalentFPS());
465  fdm.pressure = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressure());
466  fdm.pressure_sl = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressureSL());
467  fdm.total_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->GetTotalPressure());
468  fdm.dynamic_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->Getqbar());
469  fdm.temperature = CelsiusOfRankine(FDMExec->GetAtmosphere()->GetTemperature());
470 
471  /*
472  * angle of attack and SlideSlip.
473  */
474  fdm.aoa= FDMExec->GetPropertyManager()->GetNode("aero/alpha-rad")->getDoubleValue();
475  fdm.sideslip = FDMExec->GetPropertyManager()->GetNode("aero/beta-rad")->getDoubleValue();
476 
477  /*
478  * Control surface positions
479  *
480  */
481  fdm.rudder = (FDMExec->GetPropertyManager()->GetNode("fcs/rudder-pos-rad")->getDoubleValue()) /
483  fdm.left_aileron = (-1 * FDMExec->GetPropertyManager()->GetNode("fcs/left-aileron-pos-rad")->getDoubleValue()) /
485  fdm.right_aileron = (FDMExec->GetPropertyManager()->GetNode("fcs/right-aileron-pos-rad")->getDoubleValue()) /
487  fdm.elevator = (FDMExec->GetPropertyManager()->GetNode("fcs/elevator-pos-rad")->getDoubleValue()) /
489  fdm.flap = (FDMExec->GetPropertyManager()->GetNode("fcs/flap-pos-rad")->getDoubleValue()) / NPS_JSBSIM_FLAP_MAX_RAD;
490 
491  /*
492  * Propulsion
493  */
494  FGPropulsion *FGProp = FDMExec->GetPropulsion();
495  fdm.num_engines = FGProp->GetNumEngines();
496 
497  /*
498  * Note that JSBSim for some reason has very high momentum for the propeller
499  * (even when the moment of inertia of the propeller has the right value)
500  * As a result after switching the motor off
501  */
502  for (uint32_t k = 0; k < fdm.num_engines; k++) {
503  FGEngine *FGEng = FGProp->GetEngine(k);
504  FGThruster *FGThrst = FGEng->GetThruster();
505  fdm.eng_state[k] = FGEng->GetStarter();
506  fdm.rpm[k] = (float) FGThrst->GetRPM();
507  //printf("RPM: %f\n", fdm.rpm[k]);
508  //printf("STATE: %u\n", fdm.eng_state[k]);
509  }
510 }
511 
522 static void init_jsbsim(double dt)
523 {
524 
525  char buf[1024];
526  string rootdir;
527  string jsbsim_home = "/conf/simulator/jsbsim/";
528  string jsbsim_ic_name;
529 
530  char* pprz_home = getenv("PAPARAZZI_HOME");
531 
532  int cnt = -1;
533  if (strlen(pprz_home) < sizeof(buf)) {
534  cnt = snprintf(buf, strlen(pprz_home) + 1, "%s", pprz_home);
535  rootdir = string(buf) + jsbsim_home;
536  }
537 
538  // check the results
539  if (cnt < 0){
540  // Either pprz_home path too long for the buffer
541  // or writing the string was not successful.
542  cout << "PPRZ_HOME not set correctly, exiting..." << endl;
543  exit(-1);
544  }
545 
546  /* if jsbsim initial conditions are defined, use them
547  * otherwise use flightplan location
548  */
549 #ifdef NPS_JSBSIM_INIT
550  jsbsim_ic_name = NPS_JSBSIM_INIT;
551 #endif
552 
553  FDMExec = new FGFDMExec();
554 
555  FDMExec->Setsim_time(0.);
556  FDMExec->Setdt(dt);
557 
558  FDMExec->DisableOutput();
559  FDMExec->SetDebugLevel(0); // No DEBUG messages
560 
561  if (! FDMExec->LoadModel(rootdir + "aircraft",
562  rootdir + "engine",
563  rootdir + "systems",
565  false)) {
566 #ifdef DEBUG
567  cerr << " JSBSim could not be started" << endl << endl;
568 #endif
569  delete FDMExec;
570  exit(-1);
571  }
572 
573 #ifdef DEBUG
574  cerr << "NumEngines: " << FDMExec->GetPropulsion()->GetNumEngines() << endl;
575  cerr << "NumGearUnits: " << FDMExec->GetGroundReactions()->GetNumGearUnits() << endl;
576 #endif
577 
578  // LLA initial coordinates (geodetic lat, geoid alt)
579  struct LlaCoor_d lla0;
580 
581  FGInitialCondition *IC = FDMExec->GetIC();
582  if (!jsbsim_ic_name.empty()) {
583  if (! IC->Load(jsbsim_ic_name)) {
584 #ifdef DEBUG
585  cerr << "Initialization unsuccessful" << endl;
586 #endif
587  delete FDMExec;
588  exit(-1);
589  }
590 
591  llh_from_jsbsim(&lla0, FDMExec->GetPropagate());
592  cout << "JSBSim initial conditions loaded from " << jsbsim_ic_name << endl;
593  } else {
594  // FGInitialCondition::SetAltitudeASLFtIC
595  // requires this function to be called
596  // before itself
597  IC->SetVgroundFpsIC(0.);
598 
599  // Use flight plan initial conditions
600  // convert geodetic lat from flight plan to geocentric
601  double gd_lat = RadOfDeg(NAV_LAT0 / 1e7);
602  double gc_lat = gc_of_gd_lat_d(gd_lat, GROUND_ALT);
603  IC->SetLatitudeDegIC(DegOfRad(gc_lat));
604  IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
605 
606  IC->SetWindNEDFpsIC(0.0, 0.0, 0.0);
607  IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0));
608  IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT));
609  IC->SetPsiDegIC(QFU);
610  IC->SetVgroundFpsIC(0.);
611 
612  lla0.lon = RadOfDeg(NAV_LON0 / 1e7);
613  lla0.lat = gd_lat;
614  lla0.alt = (double)(NAV_ALT0 + NAV_MSL0) / 1000.0;
615  }
616 
617  // initial commands to zero
618  double init_commands[NPS_COMMANDS_NB] = {0.0};
619  feed_jsbsim(init_commands, NPS_COMMANDS_NB);
620 
621  //loop JSBSim once w/o integrating
622  if (!FDMExec->RunIC()) {
623  cerr << "Initialization unsuccessful" << endl;
624  exit(-1);
625  }
626 
627  //initRunning for all engines
628  FDMExec->GetPropulsion()->InitRunning(-1);
629 
630 
631  // compute offset between geocentric and geodetic ecef
632  ecef_of_lla_d(&offset, &lla0);
633  struct EcefCoor_d ecef0 = {
634  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(1)),
635  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(2)),
636  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(3))
637  };
638  VECT3_DIFF(offset, offset, ecef0);
639 
640  // calculate vehicle max radius in m
641  vehicle_radius_max = 0.01; // specify not 0.0 in case no gear
642  int num_gear = FDMExec->GetGroundReactions()->GetNumGearUnits();
643  int i;
644  for (i = 0; i < num_gear; i++) {
645  FGColumnVector3 gear_location = FDMExec->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation();
646  double radius = MetersOfFeet(gear_location.Magnitude());
647  if (radius > vehicle_radius_max) { vehicle_radius_max = radius; }
648  }
649 
650 }
651 
656 static void init_ltp(void)
657 {
658 
659  FGPropagate *propagate = FDMExec->GetPropagate();
660 
661  jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation());
663 
664  fdm.ltp_g.x = 0.;
665  fdm.ltp_g.y = 0.;
666  fdm.ltp_g.z = 9.81;
667 
668 
669 #if !NPS_CALC_GEO_MAG && defined(AHRS_H_X)
670  PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (AHRS section).")
671  fdm.ltp_h.x = AHRS_H_X;
672  fdm.ltp_h.y = AHRS_H_Y;
673  fdm.ltp_h.z = AHRS_H_Z;
674 #elif !NPS_CALC_GEO_MAG && defined(INS_H_X)
675  PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (INS section).")
676  fdm.ltp_h.x = INS_H_X;
677  fdm.ltp_h.y = INS_H_Y;
678  fdm.ltp_h.z = INS_H_Z;
679 #else
680  PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated location.")
681  /* calculation of magnetic field according to WMM2010 model */
682  double gha[MAXCOEFF];
683 
684  /* Current date in decimal year, for example 2012.68 */
686  double sdate = 2019.0;
687 
688  llh_from_jsbsim(&fdm.lla_pos, propagate);
689  /* LLA Position in decimal degrees and altitude in km */
690  double latitude = DegOfRad(fdm.lla_pos.lat);
691  double longitude = DegOfRad(fdm.lla_pos.lon);
692  double alt = fdm.lla_pos.alt / 1e3;
693 
694  // Calculates additional coeffs
695  int32_t nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
696  // Calculates absolute magnetic field
697  mag_calc(1, latitude, longitude, alt, nmax, gha,
698  &fdm.ltp_h.x, &fdm.ltp_h.y, &fdm.ltp_h.z,
701  printf("normalized magnetic field: %.4f %.4f %.4f\n", fdm.ltp_h.x, fdm.ltp_h.y, fdm.ltp_h.z);
702 #endif
703 
704 }
705 
714 static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location)
715 {
716 
717  fdm_location->x = MetersOfFeet(jsb_location->Entry(1));
718  fdm_location->y = MetersOfFeet(jsb_location->Entry(2));
719  fdm_location->z = MetersOfFeet(jsb_location->Entry(3));
720 
721  VECT3_ADD(*fdm_location, offset);
722 }
723 
732 static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector)
733 {
734 
735  fdm_vector->x = MetersOfFeet(jsb_vector->Entry(1));
736  fdm_vector->y = MetersOfFeet(jsb_vector->Entry(2));
737  fdm_vector->z = MetersOfFeet(jsb_vector->Entry(3));
738 
739 }
740 
747 static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat)
748 {
749 
750  fdm_quat->qi = jsb_quat->Entry(1);
751  fdm_quat->qx = jsb_quat->Entry(2);
752  fdm_quat->qy = jsb_quat->Entry(3);
753  fdm_quat->qz = jsb_quat->Entry(4);
754 
755 }
756 
763 static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector)
764 {
765 
766  fdm_rate->p = jsb_vector->Entry(1);
767  fdm_rate->q = jsb_vector->Entry(2);
768  fdm_rate->r = jsb_vector->Entry(3);
769 
770 }
771 
780 void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate)
781 {
782 
783  fdm_lla->lat = propagate->GetGeodLatitudeRad();
784  fdm_lla->lon = propagate->GetLongitude();
785  fdm_lla->alt = propagate->GetAltitudeASLmeters();
786  //printf("geodetic alt: %f\n", MetersOfFeet(propagate->GetGeodeticAltitude()));
787  //printf("ground alt: %f\n", MetersOfFeet(propagate->GetDistanceAGL()));
788  //printf("ASL alt: %f\n", propagate->GetAltitudeASLmeters());
789 
790 }
791 
800 void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate)
801 {
802 
803  fdm_lla->lat = propagate->GetLatitude();
804  fdm_lla->lon = propagate->GetLongitude();
805  fdm_lla->alt = MetersOfFeet(propagate->GetRadius());
806 
807 }
808 
817 void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate)
818 {
819 
820  fdm_lla->lat = propagate->GetGeodLatitudeRad();
821  fdm_lla->lon = propagate->GetLongitude();
822  fdm_lla->alt = MetersOfFeet(propagate->GetGeodeticAltitude());
823 
824 }
825 
826 #ifdef __APPLE__
827 /* Why isn't this there when we include math.h (on osx with clang)? */
829 static int isnan(double f) { return (f != f); }
830 #endif
831 
839 static int check_for_nan(void)
840 {
841  int orig_nan_count = fdm.nan_count;
842  /* Check all elements for nans */
843  if (isnan(fdm.ecef_pos.x)) { fdm.nan_count++; }
844  if (isnan(fdm.ecef_pos.y)) { fdm.nan_count++; }
845  if (isnan(fdm.ecef_pos.z)) { fdm.nan_count++; }
846  if (isnan(fdm.ltpprz_pos.x)) { fdm.nan_count++; }
847  if (isnan(fdm.ltpprz_pos.y)) { fdm.nan_count++; }
848  if (isnan(fdm.ltpprz_pos.z)) { fdm.nan_count++; }
849  if (isnan(fdm.lla_pos.lon)) { fdm.nan_count++; }
850  if (isnan(fdm.lla_pos.lat)) { fdm.nan_count++; }
851  if (isnan(fdm.lla_pos.alt)) { fdm.nan_count++; }
852  if (isnan(fdm.hmsl)) { fdm.nan_count++; }
853  // Skip debugging elements
854  if (isnan(fdm.ecef_ecef_vel.x)) { fdm.nan_count++; }
855  if (isnan(fdm.ecef_ecef_vel.y)) { fdm.nan_count++; }
856  if (isnan(fdm.ecef_ecef_vel.z)) { fdm.nan_count++; }
857  if (isnan(fdm.ecef_ecef_accel.x)) { fdm.nan_count++; }
858  if (isnan(fdm.ecef_ecef_accel.y)) { fdm.nan_count++; }
859  if (isnan(fdm.ecef_ecef_accel.z)) { fdm.nan_count++; }
860  if (isnan(fdm.body_ecef_vel.x)) { fdm.nan_count++; }
861  if (isnan(fdm.body_ecef_vel.y)) { fdm.nan_count++; }
862  if (isnan(fdm.body_ecef_vel.z)) { fdm.nan_count++; }
863  if (isnan(fdm.body_ecef_accel.x)) { fdm.nan_count++; }
864  if (isnan(fdm.body_ecef_accel.y)) { fdm.nan_count++; }
865  if (isnan(fdm.body_ecef_accel.z)) { fdm.nan_count++; }
866  if (isnan(fdm.ltp_ecef_vel.x)) { fdm.nan_count++; }
867  if (isnan(fdm.ltp_ecef_vel.y)) { fdm.nan_count++; }
868  if (isnan(fdm.ltp_ecef_vel.z)) { fdm.nan_count++; }
869  if (isnan(fdm.ltp_ecef_accel.x)) { fdm.nan_count++; }
870  if (isnan(fdm.ltp_ecef_accel.y)) { fdm.nan_count++; }
871  if (isnan(fdm.ltp_ecef_accel.z)) { fdm.nan_count++; }
872  if (isnan(fdm.ltpprz_ecef_vel.x)) { fdm.nan_count++; }
873  if (isnan(fdm.ltpprz_ecef_vel.y)) { fdm.nan_count++; }
874  if (isnan(fdm.ltpprz_ecef_vel.z)) { fdm.nan_count++; }
875  if (isnan(fdm.ltpprz_ecef_accel.x)) { fdm.nan_count++; }
876  if (isnan(fdm.ltpprz_ecef_accel.y)) { fdm.nan_count++; }
877  if (isnan(fdm.ltpprz_ecef_accel.z)) { fdm.nan_count++; }
878  if (isnan(fdm.ecef_to_body_quat.qi)) { fdm.nan_count++; }
879  if (isnan(fdm.ecef_to_body_quat.qx)) { fdm.nan_count++; }
880  if (isnan(fdm.ecef_to_body_quat.qy)) { fdm.nan_count++; }
881  if (isnan(fdm.ecef_to_body_quat.qz)) { fdm.nan_count++; }
882  if (isnan(fdm.ltp_to_body_quat.qi)) { fdm.nan_count++; }
883  if (isnan(fdm.ltp_to_body_quat.qx)) { fdm.nan_count++; }
884  if (isnan(fdm.ltp_to_body_quat.qy)) { fdm.nan_count++; }
885  if (isnan(fdm.ltp_to_body_quat.qz)) { fdm.nan_count++; }
886  if (isnan(fdm.ltp_to_body_eulers.phi)) { fdm.nan_count++; }
887  if (isnan(fdm.ltp_to_body_eulers.theta)) { fdm.nan_count++; }
888  if (isnan(fdm.ltp_to_body_eulers.psi)) { fdm.nan_count++; }
889  if (isnan(fdm.ltpprz_to_body_quat.qi)) { fdm.nan_count++; }
890  if (isnan(fdm.ltpprz_to_body_quat.qx)) { fdm.nan_count++; }
891  if (isnan(fdm.ltpprz_to_body_quat.qy)) { fdm.nan_count++; }
892  if (isnan(fdm.ltpprz_to_body_quat.qz)) { fdm.nan_count++; }
893  if (isnan(fdm.ltpprz_to_body_eulers.phi)) { fdm.nan_count++; }
894  if (isnan(fdm.ltpprz_to_body_eulers.theta)) { fdm.nan_count++; }
895  if (isnan(fdm.ltpprz_to_body_eulers.psi)) { fdm.nan_count++; }
896  if (isnan(fdm.body_ecef_rotvel.p)) { fdm.nan_count++; }
897  if (isnan(fdm.body_ecef_rotvel.q)) { fdm.nan_count++; }
898  if (isnan(fdm.body_ecef_rotvel.r)) { fdm.nan_count++; }
899  if (isnan(fdm.body_ecef_rotaccel.p)) { fdm.nan_count++; }
900  if (isnan(fdm.body_ecef_rotaccel.q)) { fdm.nan_count++; }
901  if (isnan(fdm.body_ecef_rotaccel.r)) { fdm.nan_count++; }
902  if (isnan(fdm.ltp_g.x)) { fdm.nan_count++; }
903  if (isnan(fdm.ltp_g.y)) { fdm.nan_count++; }
904  if (isnan(fdm.ltp_g.z)) { fdm.nan_count++; }
905  if (isnan(fdm.ltp_h.x)) { fdm.nan_count++; }
906  if (isnan(fdm.ltp_h.y)) { fdm.nan_count++; }
907  if (isnan(fdm.ltp_h.z)) { fdm.nan_count++; }
908 
909  return (fdm.nan_count - orig_nan_count);
910 }
#define NPS_JSBSIM_ROLL_TRIM
Roation quaternion.
double agl
Definition: nps_fdm.h:61
unsigned short uint16_t
Definition: types.h:16
double z
in meters
#define NMAX_1
double total_pressure
total atmospheric pressure in Pascal
Definition: nps_fdm.h:111
uint32_t eng_state[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:127
WMM2015 Geomagnetic field model.
struct DoubleQuat ecef_to_body_quat
Definition: nps_fdm.h:90
#define MAXCOEFF
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
double time
Definition: nps_fdm.h:46
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
struct LlaCoor_d lla_pos_geoc
Definition: nps_fdm.h:60
double vehicle_radius_max
The largest distance between vehicle CG and contact point.
static struct EcefCoor_d offset
struct DoubleVect3 body_inertial_accel
acceleration in body frame, wrt ECI inertial frame
Definition: nps_fdm.h:84
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
static void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate)
Convert JSBSim location to NPS LLA.
#define NMAX_2
double phi
in radians
float left_aileron
Definition: nps_fdm.h:121
#define NPS_JSBSIM_YAW_TRIM
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
Definition: nps_autopilot.h:42
#define NPS_JSBSIM_AILERON_MAX_RAD
void nps_fdm_set_wind(double speed, double dir)
double gc_of_gd_lat_d(double gd_lat, double hmsl)
struct NedCoor_d ltp_ecef_accel
acceleration in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:76
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
double psi
in radians
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
angular rates
bool on_ground
Definition: nps_fdm.h:49
struct DoubleRates body_ecef_rotaccel
Definition: nps_fdm.h:98
double q
in rad/s^2
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
static void init_ltp(void)
Initialize the ltp from the JSBSim location.
double theta
in radians
struct LlaCoor_d lla_pos_pprz
Definition: nps_fdm.h:58
vector in Latitude, Longitude and Altitude
struct DoubleRates body_inertial_rotvel
Definition: nps_fdm.h:101
double r
in rad/s^2
#define NPS_JSBSIM_PITCH_TRIM
Trim values for the airframe.
static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector)
Convert JSBSim vector format and struct to NPS vector format and struct.
#define MetersOfFeet(_f)
Macro to convert from feet to metres.
int nan_count
Definition: nps_fdm.h:50
#define NPS_JSBSIM_ELEVATOR_MAX_RAD
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
struct NedCoor_d ltp_ecef_vel
velocity in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:74
double pressure_sl
pressure at sea level in Pascal
Definition: nps_fdm.h:114
Definition: nps_fdm.h:44
double x
in meters
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
static void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate)
Convert JSBSim location to NPS LLA.
#define FALSE
Definition: std.h:5
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
double hmsl
Definition: nps_fdm.h:56
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
Paparazzi double-precision floating point math for geodetic calculations.
Paparazzi floating point math for geodetic calculations.
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:64
double airspeed
equivalent airspeed in m/s
Definition: nps_fdm.h:109
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Update the simulation state.
#define TRUE
Definition: std.h:4
static void double_vect3_normalize(struct DoubleVect3 *v)
normalize 3D vector in place
static struct LtpDef_d ltpdef
double y
in meters
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition: nps_fdm.h:87
double x
in meters
double pressure
current (static) atmospheric pressure in Pascal
Definition: nps_fdm.h:110
Paparazzi floating point algebra.
float elevator
Definition: nps_fdm.h:119
float right_aileron
Definition: nps_fdm.h:122
double init_dt
Definition: nps_fdm.h:47
Paparazzi generic algebra macros.
uint32_t num_engines
Definition: nps_fdm.h:126
unsigned long uint32_t
Definition: types.h:18
double min_dt
Timestep used for higher fidelity near the ground.
void nps_fdm_init(double dt)
Initialize actuator dynamics, set unused fields in fdm.
static const float dir[]
struct DoubleVect3 wind
velocity in m/s in NED
Definition: nps_fdm.h:107
static void feed_jsbsim(double *commands, int commands_nb)
Feed JSBSim with the latest actuator commands.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
definition of the local (flat earth) coordinate system
#define IEXT
#define NPS_JSBSIM_FLAP_MAX_RAD
struct LlaCoor_d lla_pos_geod
Definition: nps_fdm.h:59
static void fetch_state(void)
Populates the NPS fdm struct after a simulation step.
static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector)
Convert JSBSim rates vector struct to NPS rates struct.
#define NPS_JSBSIM_MODEL
Name of the JSBSim model.
double dynamic_pressure
dynamic pressure in Pascal
Definition: nps_fdm.h:112
vector in EarthCenteredEarthFixed coordinates
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
double curr_dt
Definition: nps_fdm.h:48
signed long int32_t
Definition: types.h:19
void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q)
struct LlaCoor_d lla_pos
Definition: nps_fdm.h:55
#define EXT_COEFF2
struct DoubleVect3 body_ecef_vel
velocity in body frame, wrt ECEF frame
Definition: nps_fdm.h:69
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Paparazzi generic macros for geodetic calculations.
#define FeetOfMeters(_m)
#define NPS_JSBSIM_RUDDER_MAX_RAD
double temperature
current temperature in degrees Celcius
Definition: nps_fdm.h:113
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
float rpm[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:128
static void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate)
Convert JSBSim location to NPS LLH.
struct DoubleQuat ltpprz_to_body_quat
Definition: nps_fdm.h:93
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
double lon
in radians
int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t nmax, double *gh, double *geo_mag_x, double *geo_mag_y, double *geo_mag_z, int16_t iext, double ext1, double ext2, double ext3)
static void init_jsbsim(double dt)
Initializes JSBSim.
struct EcefCoor_d ecef_pos
Definition: nps_fdm.h:53
struct DoubleVect3 body_ecef_accel
acceleration in body frame, wrt ECEF frame
Definition: nps_fdm.h:71
#define GEO_EPOCH
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:268
struct DoubleVect3 ltp_g
Definition: nps_fdm.h:104
#define EXT_COEFF1
double z
in meters
static FGFDMExec * FDMExec
The JSBSim executive object.
float rudder
Definition: nps_fdm.h:123
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
#define CelsiusOfRankine(_r)
double p
in rad/s^2
#define EXT_COEFF3
float flap
Definition: nps_fdm.h:120
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
static int check_for_nan(void)
Checks NpsFdm struct for NaNs.
static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location)
Convert JSBSim location format and struct to NPS location format and struct.
#define MIN_DT
Minimum JSBSim timestep Around 1/10000 seems to be good for ground impacts.
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
double sideslip
sideslip angle in rad
Definition: nps_fdm.h:116
double y
in meters
struct EcefCoor_d ecef_ecef_accel
acceleration in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:66
struct DoubleEulers ltpprz_to_body_eulers
Definition: nps_fdm.h:94
static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat)
Convert JSBSim quaternion struct to NPS quaternion struct.
#define PascalOfPsf(_p)
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
struct DoubleRates body_inertial_rotaccel
Definition: nps_fdm.h:102
double aoa
angle of attack in rad
Definition: nps_fdm.h:115
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105