Paparazzi UAS  v5.12_stable-4-g9b43e9b
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  init_ltp();
184 
185 #if DEBUG_NPS_JSBSIM
186  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");
187 #endif
188 
189  fetch_state();
190 
191 }
192 
193 void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb)
194 {
195 
196 #ifdef NPS_JSBSIM_LAUNCHSPEED
197  static bool already_launched = FALSE;
198 
199  if (launch && !already_launched) {
200  printf("Launching with speed of %.1f m/s!\n", (float)NPS_JSBSIM_LAUNCHSPEED);
201  FDMExec->GetIC()->SetUBodyFpsIC(FeetOfMeters(NPS_JSBSIM_LAUNCHSPEED));
202  FDMExec->RunIC();
203  already_launched = TRUE;
204  }
205 #endif
206 
207  feed_jsbsim(commands, commands_nb);
208 
209  /* To deal with ground interaction issues, we decrease the time
210  step as the vehicle is close to the ground. This is done predictively
211  to ensure no weird accelerations or oscillations. From tests with a bouncing
212  ball model in JSBSim, it seems that 10k steps per second is reasonable to capture
213  all the dynamics. Higher might be a bit more stable, but really starting to push
214  the simulation CPU requirements, especially for more complex models.
215  - at init: get the largest radius from CG to any contact point (landing gear)
216  - if descending...
217  - find current number of timesteps to impact
218  - if impact imminent, calculate a new timestep to use (with limit)
219  - if ascending...
220  - change timestep back to init value
221  - run sim for as many steps as needed to reach init_dt amount of time
222 
223  Of course, could probably be improved...
224  */
225  // If the vehicle has a downwards velocity
226  if (fdm.ltp_ecef_vel.z > 0) {
227  // Get the current number of timesteps until impact at current velocity
228  double numDT_to_impact = (fdm.agl - vehicle_radius_max) / (fdm.curr_dt * fdm.ltp_ecef_vel.z);
229  // If impact imminent within next timestep, use high sim rate
230  if (numDT_to_impact <= 1.0) {
231  fdm.curr_dt = min_dt;
232  }
233  }
234  // If the vehicle is moving upwards and out of the ground, reset timestep
235  else if ((fdm.ltp_ecef_vel.z <= 0) && ((fdm.agl + vehicle_radius_max) > 0)) {
237  }
238 
239  // Calculate the number of sim steps for correct amount of time elapsed
240  int num_steps = int(fdm.init_dt / fdm.curr_dt);
241 
242  // Set the timestep then run sim
243  FDMExec->Setdt(fdm.curr_dt);
244  int i;
245  for (i = 0; i < num_steps; i++) {
246  FDMExec->Run();
247  }
248 
249  fetch_state();
250 
251  /* Check the current state to make sure it is valid (no NaNs) */
252  if (check_for_nan()) {
253  printf("Error: FDM simulation encountered a total of %i NaN values at simulation time %f.\n", fdm.nan_count, fdm.time);
254  printf("It is likely the simulation diverged and gave non-physical results. If you did\n");
255  printf("not crash, check your model and/or initial conditions. Exiting with status 1.\n");
256  exit(1);
257  }
258 
259 }
260 
261 void nps_fdm_set_wind(double speed, double dir)
262 {
263  FGWinds *Winds = FDMExec->GetWinds();
264  Winds->SetWindspeed(FeetOfMeters(speed));
265  Winds->SetWindPsi(dir);
266 }
267 
268 void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
269 {
270  FGWinds *Winds = FDMExec->GetWinds();
271  Winds->SetWindNED(FeetOfMeters(wind_north), FeetOfMeters(wind_east),
272  FeetOfMeters(wind_down));
273 }
274 
275 void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
276 {
277  FGWinds *Winds = FDMExec->GetWinds();
278  /* wind speed used for turbulence */
279  Winds->SetWindspeed20ft(FeetOfMeters(wind_speed) / 2);
280  Winds->SetProbabilityOfExceedence(turbulence_severity);
281 }
282 
283 void nps_fdm_set_temperature(double temp, double h)
284 {
285  FDMExec->GetAtmosphere()->SetTemperature(temp, h, FGAtmosphere::eCelsius);
286 }
287 
294 static void feed_jsbsim(double *commands, int commands_nb __attribute__((unused)))
295 {
296 #ifdef NPS_ACTUATOR_NAMES
297  char buf[64];
298  const char *names[] = NPS_ACTUATOR_NAMES;
299  string property;
300 
301  int i;
302  for (i = 0; i < commands_nb; i++) {
303  sprintf(buf, "fcs/%s", names[i]);
304  property = string(buf);
305  FDMExec->GetPropertyManager()->GetNode(property)->SetDouble("", commands[i]);
306  }
307 #else /* use COMMAND names */
308 
309  // get FGFCS instance
310  FGFCS *FCS = FDMExec->GetFCS();
311 
312  // Set trims
313  FCS->SetPitchTrimCmd(NPS_JSBSIM_PITCH_TRIM);
314  FCS->SetRollTrimCmd(NPS_JSBSIM_ROLL_TRIM);
315  FCS->SetYawTrimCmd(NPS_JSBSIM_YAW_TRIM);
316 
317 #ifdef COMMAND_THROTTLE
318  FGPropulsion *FProp = FDMExec->GetPropulsion();
319  for (unsigned int i = 0; i < FDMExec->GetPropulsion()->GetNumEngines(); i++) {
320  FCS->SetThrottleCmd(i, commands[COMMAND_THROTTLE]);
321 
322  // Hack to show spinning propellers in flight gear models
323  if (commands[COMMAND_THROTTLE] > 0.01) {
324  FProp->SetStarter(1);
325  } else {
326  FProp->SetStarter(0);
327  }
328  }
329 #endif /* COMMAND_THROTTLE */
330 
331 #ifdef COMMAND_ROLL
332  FCS->SetDaCmd(commands[COMMAND_ROLL]);
333 #endif /* COMMAND_ROLL */
334 
335 #ifdef COMMAND_PITCH
336  FCS->SetDeCmd(commands[COMMAND_PITCH]);
337 #endif /* COMMAND_PITCH */
338 
339 #ifdef COMMAND_YAW
340  FCS->SetDrCmd(commands[COMMAND_YAW]);
341 #endif /* COMMAND_YAW */
342 
343 #ifdef COMMAND_FLAP
344  FCS->SetDfCmd(commands[COMMAND_FLAP]);
345 #endif /* COMMAND_FLAP */
346 
347 #endif /* NPS_ACTUATOR_NAMES */
348 }
349 
353 static void fetch_state(void)
354 {
355 
356  fdm.time = FDMExec->GetPropertyManager()->GetNode("simulation/sim-time-sec")->getDoubleValue();
357 
358 #if DEBUG_NPS_JSBSIM
359  printf("%f,", fdm.time);
360 #endif
361 
362  FGPropagate *propagate = FDMExec->GetPropagate();
363  FGAccelerations *accelerations = FDMExec->GetAccelerations();
364 
365  fdm.on_ground = FDMExec->GetGroundReactions()->GetWOW();
366 
367  /*
368  * position
369  */
370  jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation());
371  fdm.hmsl = propagate->GetAltitudeASLmeters();
372 
373  /*
374  * linear speed and accelerations
375  */
376 
377  /* in body frame */
378  jsbsimvec_to_vec(&fdm.body_ecef_vel, &propagate->GetUVW());
379  jsbsimvec_to_vec(&fdm.body_ecef_accel, &accelerations->GetUVWdot());
380  jsbsimvec_to_vec(&fdm.body_inertial_accel, &accelerations->GetUVWidot());
381  jsbsimvec_to_vec(&fdm.body_accel, &accelerations->GetBodyAccel());
382 
383 #if DEBUG_NPS_JSBSIM
384  printf("%f,%f,%f,", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);
385 #endif
386 
387  /* in LTP frame */
388  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_vel, &propagate->GetVel());
389  const FGColumnVector3 &fg_ltp_ecef_accel = propagate->GetTb2l() * accelerations->GetUVWdot();
390  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_accel, &fg_ltp_ecef_accel);
391 
392 #if DEBUG_NPS_JSBSIM
393  printf("%f,%f,%f,", fdm.ltp_ecef_accel.x, fdm.ltp_ecef_accel.y, fdm.ltp_ecef_accel.z);
394 #endif
395 
396  /* in ECEF frame */
397  const FGColumnVector3 &fg_ecef_ecef_vel = propagate->GetECEFVelocity();
398  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel);
399 
400  const FGColumnVector3 &fg_ecef_ecef_accel = propagate->GetTb2ec() * accelerations->GetUVWdot();
401  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel);
402 
403 #if DEBUG_NPS_JSBSIM
404  printf("%f,%f,%f,", fdm.ecef_ecef_accel.x, fdm.ecef_ecef_accel.y, fdm.ecef_ecef_accel.z);
405 #endif
406 
407  /* in LTP pprz */
411 
412 #if DEBUG_NPS_JSBSIM
414 #endif
415 
416  /* llh */
417  llh_from_jsbsim(&fdm.lla_pos, propagate);
418 
419  //for debug
423  fdm.agl = MetersOfFeet(propagate->GetDistanceAGL());
424 
425 #if DEBUG_NPS_JSBSIM
426  printf("%f\n", fdm.agl);
427 #endif
428 
429  /*
430  * attitude
431  */
432  const FGQuaternion jsb_quat = propagate->GetQuaternion();
434  /* convert to eulers */
436  /* the "false" pprz lpt */
437  /* FIXME: use jsbsim ltp for now */
440 
441  /*
442  * rotational speed and accelerations
443  */
444  jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR());
445  jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot());
446 
447  jsbsimvec_to_rate(&fdm.body_inertial_rotvel, &propagate->GetPQRi());
448  jsbsimvec_to_rate(&fdm.body_inertial_rotaccel, &accelerations->GetPQRidot());
449 
450 
451  /*
452  * wind
453  */
454  const FGColumnVector3 &fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED();
455  jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned);
456 
457  /*
458  * Equivalent Airspeed, atmospheric pressure and temperature.
459  */
460  fdm.airspeed = MetersOfFeet(FDMExec->GetAuxiliary()->GetVequivalentFPS());
461  fdm.pressure = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressure());
462  fdm.pressure_sl = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressureSL());
463  fdm.total_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->GetTotalPressure());
464  fdm.dynamic_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->Getqbar());
465  fdm.temperature = CelsiusOfRankine(FDMExec->GetAtmosphere()->GetTemperature());
466 
467  /*
468  * angle of attack and SlideSlip.
469  */
470  fdm.aoa= FDMExec->GetPropertyManager()->GetNode("aero/alpha-rad")->getDoubleValue();
471  fdm.sideslip = FDMExec->GetPropertyManager()->GetNode("aero/beta-rad")->getDoubleValue();
472 
473  /*
474  * Control surface positions
475  *
476  */
477  fdm.rudder = (FDMExec->GetPropertyManager()->GetNode("fcs/rudder-pos-rad")->getDoubleValue()) /
479  fdm.left_aileron = (-1 * FDMExec->GetPropertyManager()->GetNode("fcs/left-aileron-pos-rad")->getDoubleValue()) /
481  fdm.right_aileron = (FDMExec->GetPropertyManager()->GetNode("fcs/right-aileron-pos-rad")->getDoubleValue()) /
483  fdm.elevator = (FDMExec->GetPropertyManager()->GetNode("fcs/elevator-pos-rad")->getDoubleValue()) /
485  fdm.flap = (FDMExec->GetPropertyManager()->GetNode("fcs/flap-pos-rad")->getDoubleValue()) / NPS_JSBSIM_FLAP_MAX_RAD;
486 
487  /*
488  * Propulsion
489  */
490  FGPropulsion *FGProp = FDMExec->GetPropulsion();
491  fdm.num_engines = FGProp->GetNumEngines();
492 
493  /*
494  * Note that JSBSim for some reason has very high momentum for the propeller
495  * (even when the moment of inertia of the propeller has the right value)
496  * As a result after switching the motor off
497  */
498  for (uint32_t k = 0; k < fdm.num_engines; k++) {
499  FGEngine *FGEng = FGProp->GetEngine(k);
500  FGThruster *FGThrst = FGEng->GetThruster();
501  fdm.eng_state[k] = FGEng->GetStarter();
502  fdm.rpm[k] = (float) FGThrst->GetRPM();
503  //printf("RPM: %f\n", fdm.rpm[k]);
504  //printf("STATE: %u\n", fdm.eng_state[k]);
505  }
506 }
507 
518 static void init_jsbsim(double dt)
519 {
520 
521  char buf[1024];
522  string rootdir;
523  string jsbsim_home = "/conf/simulator/jsbsim/";
524  string jsbsim_ic_name;
525 
526  char* pprz_home = getenv("PAPARAZZI_HOME");
527 
528  int cnt = -1;
529  if (strlen(pprz_home) < sizeof(buf)) {
530  cnt = snprintf(buf, strlen(pprz_home) + 1, "%s", pprz_home);
531  rootdir = string(buf) + jsbsim_home;
532  }
533 
534  // check the results
535  if (cnt < 0){
536  // Either pprz_home path too long for the buffer
537  // or writing the string was not successful.
538  cout << "PPRZ_HOME not set correctly, exiting..." << endl;
539  exit(-1);
540  }
541 
542  /* if jsbsim initial conditions are defined, use them
543  * otherwise use flightplan location
544  */
545 #ifdef NPS_JSBSIM_INIT
546  jsbsim_ic_name = NPS_JSBSIM_INIT;
547 #endif
548 
549  FDMExec = new FGFDMExec();
550 
551  FDMExec->Setsim_time(0.);
552  FDMExec->Setdt(dt);
553 
554  FDMExec->DisableOutput();
555  FDMExec->SetDebugLevel(0); // No DEBUG messages
556 
557  if (! FDMExec->LoadModel(rootdir + "aircraft",
558  rootdir + "engine",
559  rootdir + "systems",
561  false)) {
562 #ifdef DEBUG
563  cerr << " JSBSim could not be started" << endl << endl;
564 #endif
565  delete FDMExec;
566  exit(-1);
567  }
568 
569 #ifdef DEBUG
570  cerr << "NumEngines: " << FDMExec->GetPropulsion()->GetNumEngines() << endl;
571  cerr << "NumGearUnits: " << FDMExec->GetGroundReactions()->GetNumGearUnits() << endl;
572 #endif
573 
574  // LLA initial coordinates (geodetic lat, geoid alt)
575  struct LlaCoor_d lla0;
576 
577  FGInitialCondition *IC = FDMExec->GetIC();
578  if (!jsbsim_ic_name.empty()) {
579  if (! IC->Load(jsbsim_ic_name)) {
580 #ifdef DEBUG
581  cerr << "Initialization unsuccessful" << endl;
582 #endif
583  delete FDMExec;
584  exit(-1);
585  }
586 
587  llh_from_jsbsim(&lla0, FDMExec->GetPropagate());
588  cout << "JSBSim initial conditions loaded from " << jsbsim_ic_name << endl;
589  } else {
590  // FGInitialCondition::SetAltitudeASLFtIC
591  // requires this function to be called
592  // before itself
593  IC->SetVgroundFpsIC(0.);
594 
595  // Use flight plan initial conditions
596  // convert geodetic lat from flight plan to geocentric
597  double gd_lat = RadOfDeg(NAV_LAT0 / 1e7);
598  double gc_lat = gc_of_gd_lat_d(gd_lat, GROUND_ALT);
599  IC->SetLatitudeDegIC(DegOfRad(gc_lat));
600  IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
601 
602  IC->SetWindNEDFpsIC(0.0, 0.0, 0.0);
603  IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0));
604  IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT));
605  IC->SetPsiDegIC(QFU);
606  IC->SetVgroundFpsIC(0.);
607 
608  lla0.lon = RadOfDeg(NAV_LON0 / 1e7);
609  lla0.lat = gd_lat;
610  lla0.alt = (double)(NAV_ALT0 + NAV_MSL0) / 1000.0;
611  }
612 
613  // initial commands to zero
614  double init_commands[NPS_COMMANDS_NB] = {0.0};
615  feed_jsbsim(init_commands, NPS_COMMANDS_NB);
616 
617  //loop JSBSim once w/o integrating
618  if (!FDMExec->RunIC()) {
619  cerr << "Initialization unsuccessful" << endl;
620  exit(-1);
621  }
622 
623  //initRunning for all engines
624  FDMExec->GetPropulsion()->InitRunning(-1);
625 
626 
627  // compute offset between geocentric and geodetic ecef
628  ecef_of_lla_d(&offset, &lla0);
629  struct EcefCoor_d ecef0 = {
630  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(1)),
631  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(2)),
632  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(3))
633  };
634  VECT3_DIFF(offset, offset, ecef0);
635 
636  // calculate vehicle max radius in m
637  vehicle_radius_max = 0.01; // specify not 0.0 in case no gear
638  int num_gear = FDMExec->GetGroundReactions()->GetNumGearUnits();
639  int i;
640  for (i = 0; i < num_gear; i++) {
641  FGColumnVector3 gear_location = FDMExec->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation();
642  double radius = MetersOfFeet(gear_location.Magnitude());
643  if (radius > vehicle_radius_max) { vehicle_radius_max = radius; }
644  }
645 
646 }
647 
652 static void init_ltp(void)
653 {
654 
655  FGPropagate *propagate = FDMExec->GetPropagate();
656 
657  jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation());
659 
660  fdm.ltp_g.x = 0.;
661  fdm.ltp_g.y = 0.;
662  fdm.ltp_g.z = 9.81;
663 
664 
665 #if !NPS_CALC_GEO_MAG && defined(AHRS_H_X)
666  PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (AHRS section).")
667  fdm.ltp_h.x = AHRS_H_X;
668  fdm.ltp_h.y = AHRS_H_Y;
669  fdm.ltp_h.z = AHRS_H_Z;
670 #elif !NPS_CALC_GEO_MAG && defined(INS_H_X)
671  PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (INS section).")
672  fdm.ltp_h.x = INS_H_X;
673  fdm.ltp_h.y = INS_H_Y;
674  fdm.ltp_h.z = INS_H_Z;
675 #else
676  PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated location.")
677  /* calculation of magnetic field according to WMM2010 model */
678  double gha[MAXCOEFF];
679 
680  /* Current date in decimal year, for example 2012.68 */
682  double sdate = 2014.5;
683 
684  llh_from_jsbsim(&fdm.lla_pos, propagate);
685  /* LLA Position in decimal degrees and altitude in km */
686  double latitude = DegOfRad(fdm.lla_pos.lat);
687  double longitude = DegOfRad(fdm.lla_pos.lon);
688  double alt = fdm.lla_pos.alt / 1e3;
689 
690  // Calculates additional coeffs
691  int32_t nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
692  // Calculates absolute magnetic field
693  mag_calc(1, latitude, longitude, alt, nmax, gha,
694  &fdm.ltp_h.x, &fdm.ltp_h.y, &fdm.ltp_h.z,
697  printf("normalized magnetic field: %.4f %.4f %.4f\n", fdm.ltp_h.x, fdm.ltp_h.y, fdm.ltp_h.z);
698 #endif
699 
700 }
701 
710 static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location)
711 {
712 
713  fdm_location->x = MetersOfFeet(jsb_location->Entry(1));
714  fdm_location->y = MetersOfFeet(jsb_location->Entry(2));
715  fdm_location->z = MetersOfFeet(jsb_location->Entry(3));
716 
717  VECT3_ADD(*fdm_location, offset);
718 }
719 
728 static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector)
729 {
730 
731  fdm_vector->x = MetersOfFeet(jsb_vector->Entry(1));
732  fdm_vector->y = MetersOfFeet(jsb_vector->Entry(2));
733  fdm_vector->z = MetersOfFeet(jsb_vector->Entry(3));
734 
735 }
736 
743 static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat)
744 {
745 
746  fdm_quat->qi = jsb_quat->Entry(1);
747  fdm_quat->qx = jsb_quat->Entry(2);
748  fdm_quat->qy = jsb_quat->Entry(3);
749  fdm_quat->qz = jsb_quat->Entry(4);
750 
751 }
752 
759 static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector)
760 {
761 
762  fdm_rate->p = jsb_vector->Entry(1);
763  fdm_rate->q = jsb_vector->Entry(2);
764  fdm_rate->r = jsb_vector->Entry(3);
765 
766 }
767 
776 void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate)
777 {
778 
779  fdm_lla->lat = propagate->GetGeodLatitudeRad();
780  fdm_lla->lon = propagate->GetLongitude();
781  fdm_lla->alt = propagate->GetAltitudeASLmeters();
782  //printf("geodetic alt: %f\n", MetersOfFeet(propagate->GetGeodeticAltitude()));
783  //printf("ground alt: %f\n", MetersOfFeet(propagate->GetDistanceAGL()));
784  //printf("ASL alt: %f\n", propagate->GetAltitudeASLmeters());
785 
786 }
787 
796 void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate)
797 {
798 
799  fdm_lla->lat = propagate->GetLatitude();
800  fdm_lla->lon = propagate->GetLongitude();
801  fdm_lla->alt = MetersOfFeet(propagate->GetRadius());
802 
803 }
804 
813 void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate)
814 {
815 
816  fdm_lla->lat = propagate->GetGeodLatitudeRad();
817  fdm_lla->lon = propagate->GetLongitude();
818  fdm_lla->alt = MetersOfFeet(propagate->GetGeodeticAltitude());
819 
820 }
821 
822 #ifdef __APPLE__
823 /* Why isn't this there when we include math.h (on osx with clang)? */
825 static int isnan(double f) { return (f != f); }
826 #endif
827 
835 static int check_for_nan(void)
836 {
837  int orig_nan_count = fdm.nan_count;
838  /* Check all elements for nans */
839  if (isnan(fdm.ecef_pos.x)) { fdm.nan_count++; }
840  if (isnan(fdm.ecef_pos.y)) { fdm.nan_count++; }
841  if (isnan(fdm.ecef_pos.z)) { fdm.nan_count++; }
842  if (isnan(fdm.ltpprz_pos.x)) { fdm.nan_count++; }
843  if (isnan(fdm.ltpprz_pos.y)) { fdm.nan_count++; }
844  if (isnan(fdm.ltpprz_pos.z)) { fdm.nan_count++; }
845  if (isnan(fdm.lla_pos.lon)) { fdm.nan_count++; }
846  if (isnan(fdm.lla_pos.lat)) { fdm.nan_count++; }
847  if (isnan(fdm.lla_pos.alt)) { fdm.nan_count++; }
848  if (isnan(fdm.hmsl)) { fdm.nan_count++; }
849  // Skip debugging elements
850  if (isnan(fdm.ecef_ecef_vel.x)) { fdm.nan_count++; }
851  if (isnan(fdm.ecef_ecef_vel.y)) { fdm.nan_count++; }
852  if (isnan(fdm.ecef_ecef_vel.z)) { fdm.nan_count++; }
853  if (isnan(fdm.ecef_ecef_accel.x)) { fdm.nan_count++; }
854  if (isnan(fdm.ecef_ecef_accel.y)) { fdm.nan_count++; }
855  if (isnan(fdm.ecef_ecef_accel.z)) { fdm.nan_count++; }
856  if (isnan(fdm.body_ecef_vel.x)) { fdm.nan_count++; }
857  if (isnan(fdm.body_ecef_vel.y)) { fdm.nan_count++; }
858  if (isnan(fdm.body_ecef_vel.z)) { fdm.nan_count++; }
859  if (isnan(fdm.body_ecef_accel.x)) { fdm.nan_count++; }
860  if (isnan(fdm.body_ecef_accel.y)) { fdm.nan_count++; }
861  if (isnan(fdm.body_ecef_accel.z)) { fdm.nan_count++; }
862  if (isnan(fdm.ltp_ecef_vel.x)) { fdm.nan_count++; }
863  if (isnan(fdm.ltp_ecef_vel.y)) { fdm.nan_count++; }
864  if (isnan(fdm.ltp_ecef_vel.z)) { fdm.nan_count++; }
865  if (isnan(fdm.ltp_ecef_accel.x)) { fdm.nan_count++; }
866  if (isnan(fdm.ltp_ecef_accel.y)) { fdm.nan_count++; }
867  if (isnan(fdm.ltp_ecef_accel.z)) { fdm.nan_count++; }
868  if (isnan(fdm.ltpprz_ecef_vel.x)) { fdm.nan_count++; }
869  if (isnan(fdm.ltpprz_ecef_vel.y)) { fdm.nan_count++; }
870  if (isnan(fdm.ltpprz_ecef_vel.z)) { fdm.nan_count++; }
871  if (isnan(fdm.ltpprz_ecef_accel.x)) { fdm.nan_count++; }
872  if (isnan(fdm.ltpprz_ecef_accel.y)) { fdm.nan_count++; }
873  if (isnan(fdm.ltpprz_ecef_accel.z)) { fdm.nan_count++; }
874  if (isnan(fdm.ecef_to_body_quat.qi)) { fdm.nan_count++; }
875  if (isnan(fdm.ecef_to_body_quat.qx)) { fdm.nan_count++; }
876  if (isnan(fdm.ecef_to_body_quat.qy)) { fdm.nan_count++; }
877  if (isnan(fdm.ecef_to_body_quat.qz)) { fdm.nan_count++; }
878  if (isnan(fdm.ltp_to_body_quat.qi)) { fdm.nan_count++; }
879  if (isnan(fdm.ltp_to_body_quat.qx)) { fdm.nan_count++; }
880  if (isnan(fdm.ltp_to_body_quat.qy)) { fdm.nan_count++; }
881  if (isnan(fdm.ltp_to_body_quat.qz)) { fdm.nan_count++; }
882  if (isnan(fdm.ltp_to_body_eulers.phi)) { fdm.nan_count++; }
883  if (isnan(fdm.ltp_to_body_eulers.theta)) { fdm.nan_count++; }
884  if (isnan(fdm.ltp_to_body_eulers.psi)) { fdm.nan_count++; }
885  if (isnan(fdm.ltpprz_to_body_quat.qi)) { fdm.nan_count++; }
886  if (isnan(fdm.ltpprz_to_body_quat.qx)) { fdm.nan_count++; }
887  if (isnan(fdm.ltpprz_to_body_quat.qy)) { fdm.nan_count++; }
888  if (isnan(fdm.ltpprz_to_body_quat.qz)) { fdm.nan_count++; }
889  if (isnan(fdm.ltpprz_to_body_eulers.phi)) { fdm.nan_count++; }
890  if (isnan(fdm.ltpprz_to_body_eulers.theta)) { fdm.nan_count++; }
891  if (isnan(fdm.ltpprz_to_body_eulers.psi)) { fdm.nan_count++; }
892  if (isnan(fdm.body_ecef_rotvel.p)) { fdm.nan_count++; }
893  if (isnan(fdm.body_ecef_rotvel.q)) { fdm.nan_count++; }
894  if (isnan(fdm.body_ecef_rotvel.r)) { fdm.nan_count++; }
895  if (isnan(fdm.body_ecef_rotaccel.p)) { fdm.nan_count++; }
896  if (isnan(fdm.body_ecef_rotaccel.q)) { fdm.nan_count++; }
897  if (isnan(fdm.body_ecef_rotaccel.r)) { fdm.nan_count++; }
898  if (isnan(fdm.ltp_g.x)) { fdm.nan_count++; }
899  if (isnan(fdm.ltp_g.y)) { fdm.nan_count++; }
900  if (isnan(fdm.ltp_g.z)) { fdm.nan_count++; }
901  if (isnan(fdm.ltp_h.x)) { fdm.nan_count++; }
902  if (isnan(fdm.ltp_h.y)) { fdm.nan_count++; }
903  if (isnan(fdm.ltp_h.z)) { fdm.nan_count++; }
904 
905  return (fdm.nan_count - orig_nan_count);
906 }
#define NPS_JSBSIM_ROLL_TRIM
Roation quaternion.
double agl
Definition: nps_fdm.h:61
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:146
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:18
#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:181
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:124
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
angular rates
bool on_ground
Definition: nps_fdm.h:49
static float radius
Definition: chemotaxis.c:15
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:543
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)
Set JSBsim specific fields that are not used for Gazebo.
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:267
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