Paparazzi UAS  v5.17_devel-14-g4575375
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 
77 #if NPS_JSBSIM_USE_SGPATH
78 #define JSBSIM_PATH(_x) SGPath(_x)
79 #else
80 #define JSBSIM_PATH(_x) _x
81 #endif
82 
86 #ifndef NPS_JSBSIM_MODEL
87 #define NPS_JSBSIM_MODEL AIRFRAME_NAME
88 #endif
89 
90 #ifdef NPS_INITIAL_CONDITITONS
91 #warning NPS_INITIAL_CONDITITONS was replaced by NPS_JSBSIM_INIT!
92 #warning Defaulting to flight plan location.
93 #endif
94 
98 #ifndef NPS_JSBSIM_PITCH_TRIM
99 #define NPS_JSBSIM_PITCH_TRIM 0.0
100 #endif
101 
102 #ifndef NPS_JSBSIM_ROLL_TRIM
103 #define NPS_JSBSIM_ROLL_TRIM 0.0
104 #endif
105 
106 #ifndef NPS_JSBSIM_YAW_TRIM
107 #define NPS_JSBSIM_YAW_TRIM 0.0
108 #endif
109 
113 #define DEG2RAD 0.017
114 
115 #ifndef NPS_JSBSIM_ELEVATOR_MAX_RAD
116 #define NPS_JSBSIM_ELEVATOR_MAX_RAD (20.0*DEG2RAD)
117 #endif
118 
119 #ifndef NPS_JSBSIM_AILERON_MAX_RAD
120 #define NPS_JSBSIM_AILERON_MAX_RAD (20.0*DEG2RAD)
121 #endif
122 
123 #ifndef NPS_JSBSIM_RUDDER_MAX_RAD
124 #define NPS_JSBSIM_RUDDER_MAX_RAD (20.0*DEG2RAD)
125 #endif
126 
127 #ifndef NPS_JSBSIM_FLAP_MAX_RAD
128 #define NPS_JSBSIM_FLAP_MAX_RAD (20.0*DEG2RAD)
129 #endif
130 
134 #define MIN_DT (1.0/10240.0)
135 // TODO: maybe lower for slower CPUs & HITL?
136 //#define MIN_DT (1.0/1000.0)
137 
138 using namespace JSBSim;
139 using namespace std;
140 
141 static void feed_jsbsim(double *commands, int commands_nb);
142 static void fetch_state(void);
143 static int check_for_nan(void);
144 
145 static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector);
146 static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location);
147 static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat);
148 static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector);
149 static void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate);
150 static void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate);
151 static void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate);
152 
153 static void init_jsbsim(double dt);
154 static void init_ltp(void);
155 
157 struct NpsFdm fdm;
158 
160 static FGFDMExec *FDMExec;
161 
162 static struct LtpDef_d ltpdef;
163 
164 // Offset between ecef in geodetic and geocentric coordinates
165 static struct EcefCoor_d offset;
166 
169 
171 double min_dt;
172 
173 void nps_fdm_init(double dt)
174 {
175 
176  fdm.init_dt = dt;
177  fdm.curr_dt = dt;
178  //Sets up the high fidelity timestep as a multiple of the normal timestep
179  for (min_dt = (1.0 / dt); min_dt < (1 / MIN_DT); min_dt += (1 / dt)) {}
180  min_dt = (1 / min_dt);
181 
182  fdm.nan_count = 0;
183 
184  VECT3_ASSIGN(offset, 0., 0., 0.);
185 
186  init_jsbsim(dt);
187 
188  FDMExec->RunIC();
189 
190  // Fix getting initial incorrect accel measurements
191  for(uint16_t i = 0; i < 1500; i++)
192  FDMExec->Run();
193 
194  init_ltp();
195 
196 #if DEBUG_NPS_JSBSIM
197  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");
198 #endif
199 
200  fetch_state();
201 
202 }
203 
204 void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb)
205 {
206 
207 #ifdef NPS_JSBSIM_LAUNCHSPEED
208  static bool already_launched = FALSE;
209 
210  if (launch && !already_launched) {
211  printf("Launching with speed of %.1f m/s!\n", (float)NPS_JSBSIM_LAUNCHSPEED);
212  FDMExec->GetIC()->SetUBodyFpsIC(FeetOfMeters(NPS_JSBSIM_LAUNCHSPEED));
213  FDMExec->RunIC();
214  already_launched = TRUE;
215  }
216 #endif
217 
218  feed_jsbsim(commands, commands_nb);
219 
220  /* To deal with ground interaction issues, we decrease the time
221  step as the vehicle is close to the ground. This is done predictively
222  to ensure no weird accelerations or oscillations. From tests with a bouncing
223  ball model in JSBSim, it seems that 10k steps per second is reasonable to capture
224  all the dynamics. Higher might be a bit more stable, but really starting to push
225  the simulation CPU requirements, especially for more complex models.
226  - at init: get the largest radius from CG to any contact point (landing gear)
227  - if descending...
228  - find current number of timesteps to impact
229  - if impact imminent, calculate a new timestep to use (with limit)
230  - if ascending...
231  - change timestep back to init value
232  - run sim for as many steps as needed to reach init_dt amount of time
233 
234  Of course, could probably be improved...
235  */
236  // If the vehicle has a downwards velocity
237  if (fdm.ltp_ecef_vel.z > 0) {
238  // Get the current number of timesteps until impact at current velocity
239  double numDT_to_impact = (fdm.agl - vehicle_radius_max) / (fdm.curr_dt * fdm.ltp_ecef_vel.z);
240  // If impact imminent within next timestep, use high sim rate
241  if (numDT_to_impact <= 1.0) {
242  fdm.curr_dt = min_dt;
243  }
244  }
245  // If the vehicle is moving upwards and out of the ground, reset timestep
246  else if ((fdm.ltp_ecef_vel.z <= 0) && ((fdm.agl + vehicle_radius_max) > 0)) {
248  }
249 
250  // Calculate the number of sim steps for correct amount of time elapsed
251  int num_steps = int(fdm.init_dt / fdm.curr_dt);
252 
253  // Set the timestep then run sim
254  FDMExec->Setdt(fdm.curr_dt);
255  int i;
256  for (i = 0; i < num_steps; i++) {
257  FDMExec->Run();
258  }
259 
260  fetch_state();
261 
262  /* Check the current state to make sure it is valid (no NaNs) */
263  if (check_for_nan()) {
264  printf("Error: FDM simulation encountered a total of %i NaN values at simulation time %f.\n", fdm.nan_count, fdm.time);
265  printf("It is likely the simulation diverged and gave non-physical results. If you did\n");
266  printf("not crash, check your model and/or initial conditions. Exiting with status 1.\n");
267  exit(1);
268  }
269 
270 }
271 
272 void nps_fdm_set_wind(double speed, double dir)
273 {
274  FGWinds *Winds = FDMExec->GetWinds();
275  Winds->SetWindspeed(FeetOfMeters(speed));
276  Winds->SetWindPsi(dir);
277 }
278 
279 void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
280 {
281  FGWinds *Winds = FDMExec->GetWinds();
282  Winds->SetWindNED(FeetOfMeters(wind_north), FeetOfMeters(wind_east),
283  FeetOfMeters(wind_down));
284 }
285 
286 void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
287 {
288  FGWinds *Winds = FDMExec->GetWinds();
289  /* wind speed used for turbulence */
290  Winds->SetWindspeed20ft(FeetOfMeters(wind_speed) / 2);
291  Winds->SetProbabilityOfExceedence(turbulence_severity);
292 }
293 
294 void nps_fdm_set_temperature(double temp, double h)
295 {
296  FDMExec->GetAtmosphere()->SetTemperature(temp, h, FGAtmosphere::eCelsius);
297 }
298 
305 static void feed_jsbsim(double *commands, int commands_nb __attribute__((unused)))
306 {
307 #ifdef NPS_ACTUATOR_NAMES
308  char buf[64];
309  const char *names[] = NPS_ACTUATOR_NAMES;
310  string property;
311 
312  int i;
313  for (i = 0; i < commands_nb; i++) {
314  sprintf(buf, "fcs/%s", names[i]);
315  property = string(buf);
316  FDMExec->GetPropertyManager()->GetNode(property)->SetDouble("", commands[i]);
317  }
318 #else /* use COMMAND names */
319 
320  // get FGFCS instance
321  FGFCS *FCS = FDMExec->GetFCS();
322 
323  // Set trims
324  FCS->SetPitchTrimCmd(NPS_JSBSIM_PITCH_TRIM);
325  FCS->SetRollTrimCmd(NPS_JSBSIM_ROLL_TRIM);
326  FCS->SetYawTrimCmd(NPS_JSBSIM_YAW_TRIM);
327 
328 #ifdef COMMAND_THROTTLE
329  FGPropulsion *FProp = FDMExec->GetPropulsion();
330  for (unsigned int i = 0; i < FDMExec->GetPropulsion()->GetNumEngines(); i++) {
331  FCS->SetThrottleCmd(i, commands[COMMAND_THROTTLE]);
332 
333  // Hack to show spinning propellers in flight gear models
334  if (commands[COMMAND_THROTTLE] > 0.01) {
335  FProp->SetStarter(1);
336  } else {
337  FProp->SetStarter(0);
338  }
339  }
340 #endif /* COMMAND_THROTTLE */
341 
342 #ifdef COMMAND_ROLL
343  FCS->SetDaCmd(commands[COMMAND_ROLL]);
344 #endif /* COMMAND_ROLL */
345 
346 #ifdef COMMAND_PITCH
347  FCS->SetDeCmd(commands[COMMAND_PITCH]);
348 #endif /* COMMAND_PITCH */
349 
350 #ifdef COMMAND_YAW
351  FCS->SetDrCmd(commands[COMMAND_YAW]);
352 #endif /* COMMAND_YAW */
353 
354 #ifdef COMMAND_FLAP
355  FCS->SetDfCmd(commands[COMMAND_FLAP]);
356 #endif /* COMMAND_FLAP */
357 
358 #endif /* NPS_ACTUATOR_NAMES */
359 }
360 
364 static void fetch_state(void)
365 {
366 
367  fdm.time = FDMExec->GetPropertyManager()->GetNode("simulation/sim-time-sec")->getDoubleValue();
368 
369 #if DEBUG_NPS_JSBSIM
370  printf("%f,", fdm.time);
371 #endif
372 
373  FGPropagate *propagate = FDMExec->GetPropagate();
374  FGAccelerations *accelerations = FDMExec->GetAccelerations();
375 
376  fdm.on_ground = FDMExec->GetGroundReactions()->GetWOW();
377 
378  /*
379  * position
380  */
381  jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation());
382  fdm.hmsl = propagate->GetAltitudeASLmeters();
383 
384  /*
385  * linear speed and accelerations
386  */
387 
388  /* in body frame */
389  jsbsimvec_to_vec(&fdm.body_ecef_vel, &propagate->GetUVW());
390  jsbsimvec_to_vec(&fdm.body_ecef_accel, &accelerations->GetUVWdot());
391  jsbsimvec_to_vec(&fdm.body_inertial_accel, &accelerations->GetUVWidot());
392  jsbsimvec_to_vec(&fdm.body_accel, &accelerations->GetBodyAccel());
393 
394 #if DEBUG_NPS_JSBSIM
395  printf("%f,%f,%f,", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);
396 #endif
397 
398  /* in LTP frame */
399  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_vel, &propagate->GetVel());
400  const FGColumnVector3 &fg_ltp_ecef_accel = propagate->GetTb2l() * accelerations->GetUVWdot();
401  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_accel, &fg_ltp_ecef_accel);
402 
403 #if DEBUG_NPS_JSBSIM
404  printf("%f,%f,%f,", fdm.ltp_ecef_accel.x, fdm.ltp_ecef_accel.y, fdm.ltp_ecef_accel.z);
405 #endif
406 
407  /* in ECEF frame */
408  const FGColumnVector3 &fg_ecef_ecef_vel = propagate->GetECEFVelocity();
409  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel);
410 
411  const FGColumnVector3 &fg_ecef_ecef_accel = propagate->GetTb2ec() * accelerations->GetUVWdot();
412  jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel);
413 
414 #if DEBUG_NPS_JSBSIM
415  printf("%f,%f,%f,", fdm.ecef_ecef_accel.x, fdm.ecef_ecef_accel.y, fdm.ecef_ecef_accel.z);
416 #endif
417 
418  /* in LTP pprz */
422 
423 #if DEBUG_NPS_JSBSIM
425 #endif
426 
427  /* llh */
428  llh_from_jsbsim(&fdm.lla_pos, propagate);
429 
430  //for debug
434  fdm.agl = MetersOfFeet(propagate->GetDistanceAGL());
435 
436 #if DEBUG_NPS_JSBSIM
437  printf("%f\n", fdm.agl);
438 #endif
439 
440  /*
441  * attitude
442  */
443  const FGQuaternion jsb_quat = propagate->GetQuaternion();
445  /* convert to eulers */
447  /* the "false" pprz lpt */
448  /* FIXME: use jsbsim ltp for now */
451 
452  /*
453  * rotational speed and accelerations
454  */
455  jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR());
456  jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot());
457 
458  jsbsimvec_to_rate(&fdm.body_inertial_rotvel, &propagate->GetPQRi());
459  jsbsimvec_to_rate(&fdm.body_inertial_rotaccel, &accelerations->GetPQRidot());
460 
461 
462  /*
463  * wind
464  */
465  const FGColumnVector3 &fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED();
466  jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned);
467 
468  /*
469  * Equivalent Airspeed, atmospheric pressure and temperature.
470  */
471  fdm.airspeed = MetersOfFeet(FDMExec->GetAuxiliary()->GetVequivalentFPS());
472  fdm.pressure = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressure());
473  fdm.pressure_sl = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressureSL());
474  fdm.total_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->GetTotalPressure());
475  fdm.dynamic_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->Getqbar());
476  fdm.temperature = CelsiusOfRankine(FDMExec->GetAtmosphere()->GetTemperature());
477 
478  /*
479  * angle of attack and SlideSlip.
480  */
481  fdm.aoa= FDMExec->GetPropertyManager()->GetNode("aero/alpha-rad")->getDoubleValue();
482  fdm.sideslip = FDMExec->GetPropertyManager()->GetNode("aero/beta-rad")->getDoubleValue();
483 
484  /*
485  * Control surface positions
486  *
487  */
488  fdm.rudder = (FDMExec->GetPropertyManager()->GetNode("fcs/rudder-pos-rad")->getDoubleValue()) /
490  fdm.left_aileron = (-1 * FDMExec->GetPropertyManager()->GetNode("fcs/left-aileron-pos-rad")->getDoubleValue()) /
492  fdm.right_aileron = (FDMExec->GetPropertyManager()->GetNode("fcs/right-aileron-pos-rad")->getDoubleValue()) /
494  fdm.elevator = (FDMExec->GetPropertyManager()->GetNode("fcs/elevator-pos-rad")->getDoubleValue()) /
496  fdm.flap = (FDMExec->GetPropertyManager()->GetNode("fcs/flap-pos-rad")->getDoubleValue()) / NPS_JSBSIM_FLAP_MAX_RAD;
497 
498  /*
499  * Propulsion
500  */
501  FGPropulsion *FGProp = FDMExec->GetPropulsion();
502  fdm.num_engines = FGProp->GetNumEngines();
503 
504  /*
505  * Note that JSBSim for some reason has very high momentum for the propeller
506  * (even when the moment of inertia of the propeller has the right value)
507  * As a result after switching the motor off
508  */
509  for (uint32_t k = 0; k < fdm.num_engines; k++) {
510  FGEngine *FGEng = FGProp->GetEngine(k);
511  FGThruster *FGThrst = FGEng->GetThruster();
512  fdm.eng_state[k] = FGEng->GetStarter();
513  fdm.rpm[k] = (float) FGThrst->GetRPM();
514  //printf("RPM: %f\n", fdm.rpm[k]);
515  //printf("STATE: %u\n", fdm.eng_state[k]);
516  }
517 }
518 
529 static void init_jsbsim(double dt)
530 {
531 
532  char buf[1024];
533  string rootdir;
534  string jsbsim_home = "/conf/simulator/jsbsim/";
535  string jsbsim_ic_name;
536 
537  char* pprz_home = getenv("PAPARAZZI_HOME");
538 
539  int cnt = -1;
540  if (strlen(pprz_home) < sizeof(buf)) {
541  cnt = snprintf(buf, strlen(pprz_home) + 1, "%s", pprz_home);
542  rootdir = string(buf) + jsbsim_home;
543  }
544 
545  // check the results
546  if (cnt < 0){
547  // Either pprz_home path too long for the buffer
548  // or writing the string was not successful.
549  cout << "PPRZ_HOME not set correctly, exiting..." << endl;
550  exit(-1);
551  }
552 
553  /* if jsbsim initial conditions are defined, use them
554  * otherwise use flightplan location
555  */
556 #ifdef NPS_JSBSIM_INIT
557  jsbsim_ic_name = NPS_JSBSIM_INIT;
558 #endif
559 
560  FDMExec = new FGFDMExec();
561 
562  FDMExec->Setsim_time(0.);
563  FDMExec->Setdt(dt);
564 
565  FDMExec->DisableOutput();
566  FDMExec->SetDebugLevel(0); // No DEBUG messages
567 
568  if (! FDMExec->LoadModel(JSBSIM_PATH(rootdir + "aircraft"),
569  JSBSIM_PATH(rootdir + "engine"),
570  JSBSIM_PATH(rootdir + "systems"),
572  false)) {
573 #ifdef DEBUG
574  cerr << " JSBSim could not be started" << endl << endl;
575 #endif
576  delete FDMExec;
577  exit(-1);
578  }
579 
580 #ifdef DEBUG
581  cerr << "NumEngines: " << FDMExec->GetPropulsion()->GetNumEngines() << endl;
582  cerr << "NumGearUnits: " << FDMExec->GetGroundReactions()->GetNumGearUnits() << endl;
583 #endif
584 
585  // LLA initial coordinates (geodetic lat, geoid alt)
586  struct LlaCoor_d lla0;
587 
588  FGInitialCondition *IC = FDMExec->GetIC();
589  if (!jsbsim_ic_name.empty()) {
590  if (! IC->Load(JSBSIM_PATH(jsbsim_ic_name))) {
591 #ifdef DEBUG
592  cerr << "Initialization unsuccessful" << endl;
593 #endif
594  delete FDMExec;
595  exit(-1);
596  }
597 
598  llh_from_jsbsim(&lla0, FDMExec->GetPropagate());
599  cout << "JSBSim initial conditions loaded from " << jsbsim_ic_name << endl;
600  } else {
601  // FGInitialCondition::SetAltitudeASLFtIC
602  // requires this function to be called
603  // before itself
604  IC->SetVgroundFpsIC(0.);
605 
606  // Use flight plan initial conditions
607  // convert geodetic lat from flight plan to geocentric
608  double gd_lat = RadOfDeg(NAV_LAT0 / 1e7);
609  double gc_lat = gc_of_gd_lat_d(gd_lat, GROUND_ALT);
610  IC->SetLatitudeDegIC(DegOfRad(gc_lat));
611  IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
612 
613  IC->SetWindNEDFpsIC(0.0, 0.0, 0.0);
614  IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0));
615  IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT));
616  IC->SetPsiDegIC(QFU);
617  IC->SetVgroundFpsIC(0.);
618 
619  lla0.lon = RadOfDeg(NAV_LON0 / 1e7);
620  lla0.lat = gd_lat;
621  lla0.alt = (double)(NAV_ALT0 + NAV_MSL0) / 1000.0;
622  }
623 
624  // initial commands to zero
625  double init_commands[NPS_COMMANDS_NB] = {0.0};
626  feed_jsbsim(init_commands, NPS_COMMANDS_NB);
627 
628  //loop JSBSim once w/o integrating
629  if (!FDMExec->RunIC()) {
630  cerr << "Initialization unsuccessful" << endl;
631  exit(-1);
632  }
633 
634  //initRunning for all engines
635  FDMExec->GetPropulsion()->InitRunning(-1);
636 
637 
638  // compute offset between geocentric and geodetic ecef
639  ecef_of_lla_d(&offset, &lla0);
640  struct EcefCoor_d ecef0 = {
641  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(1)),
642  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(2)),
643  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(3))
644  };
645  VECT3_DIFF(offset, offset, ecef0);
646 
647  // calculate vehicle max radius in m
648  vehicle_radius_max = 0.01; // specify not 0.0 in case no gear
649  int num_gear = FDMExec->GetGroundReactions()->GetNumGearUnits();
650  int i;
651  for (i = 0; i < num_gear; i++) {
652  FGColumnVector3 gear_location = FDMExec->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation();
653  double radius = MetersOfFeet(gear_location.Magnitude());
654  if (radius > vehicle_radius_max) { vehicle_radius_max = radius; }
655  }
656 
657 }
658 
663 static void init_ltp(void)
664 {
665 
666  FGPropagate *propagate = FDMExec->GetPropagate();
667 
668  jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation());
670 
671  fdm.ltp_g.x = 0.;
672  fdm.ltp_g.y = 0.;
673  fdm.ltp_g.z = 9.81;
674 
675 
676 #if !NPS_CALC_GEO_MAG && defined(AHRS_H_X)
677  PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (AHRS section).")
678  fdm.ltp_h.x = AHRS_H_X;
679  fdm.ltp_h.y = AHRS_H_Y;
680  fdm.ltp_h.z = AHRS_H_Z;
681 #elif !NPS_CALC_GEO_MAG && defined(INS_H_X)
682  PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (INS section).")
683  fdm.ltp_h.x = INS_H_X;
684  fdm.ltp_h.y = INS_H_Y;
685  fdm.ltp_h.z = INS_H_Z;
686 #else
687  PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated location.")
688  /* calculation of magnetic field according to WMM2010 model */
689  double gha[MAXCOEFF];
690 
691  /* Current date in decimal year, for example 2012.68 */
693  double sdate = 2019.0;
694 
695  llh_from_jsbsim(&fdm.lla_pos, propagate);
696  /* LLA Position in decimal degrees and altitude in km */
697  double latitude = DegOfRad(fdm.lla_pos.lat);
698  double longitude = DegOfRad(fdm.lla_pos.lon);
699  double alt = fdm.lla_pos.alt / 1e3;
700 
701  // Calculates additional coeffs
702  int32_t nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
703  // Calculates absolute magnetic field
704  mag_calc(1, latitude, longitude, alt, nmax, gha,
705  &fdm.ltp_h.x, &fdm.ltp_h.y, &fdm.ltp_h.z,
708  printf("normalized magnetic field: %.4f %.4f %.4f\n", fdm.ltp_h.x, fdm.ltp_h.y, fdm.ltp_h.z);
709 #endif
710 
711 }
712 
721 static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location)
722 {
723 
724  fdm_location->x = MetersOfFeet(jsb_location->Entry(1));
725  fdm_location->y = MetersOfFeet(jsb_location->Entry(2));
726  fdm_location->z = MetersOfFeet(jsb_location->Entry(3));
727 
728  VECT3_ADD(*fdm_location, offset);
729 }
730 
739 static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector)
740 {
741 
742  fdm_vector->x = MetersOfFeet(jsb_vector->Entry(1));
743  fdm_vector->y = MetersOfFeet(jsb_vector->Entry(2));
744  fdm_vector->z = MetersOfFeet(jsb_vector->Entry(3));
745 
746 }
747 
754 static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat)
755 {
756 
757  fdm_quat->qi = jsb_quat->Entry(1);
758  fdm_quat->qx = jsb_quat->Entry(2);
759  fdm_quat->qy = jsb_quat->Entry(3);
760  fdm_quat->qz = jsb_quat->Entry(4);
761 
762 }
763 
770 static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector)
771 {
772 
773  fdm_rate->p = jsb_vector->Entry(1);
774  fdm_rate->q = jsb_vector->Entry(2);
775  fdm_rate->r = jsb_vector->Entry(3);
776 
777 }
778 
787 void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate)
788 {
789 
790  fdm_lla->lat = propagate->GetGeodLatitudeRad();
791  fdm_lla->lon = propagate->GetLongitude();
792  fdm_lla->alt = propagate->GetAltitudeASLmeters();
793  //printf("geodetic alt: %f\n", MetersOfFeet(propagate->GetGeodeticAltitude()));
794  //printf("ground alt: %f\n", MetersOfFeet(propagate->GetDistanceAGL()));
795  //printf("ASL alt: %f\n", propagate->GetAltitudeASLmeters());
796 
797 }
798 
807 void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate)
808 {
809 
810  fdm_lla->lat = propagate->GetLatitude();
811  fdm_lla->lon = propagate->GetLongitude();
812  fdm_lla->alt = MetersOfFeet(propagate->GetRadius());
813 
814 }
815 
824 void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate)
825 {
826 
827  fdm_lla->lat = propagate->GetGeodLatitudeRad();
828  fdm_lla->lon = propagate->GetLongitude();
829  fdm_lla->alt = MetersOfFeet(propagate->GetGeodeticAltitude());
830 
831 }
832 
833 #ifdef __APPLE__
834 /* Why isn't this there when we include math.h (on osx with clang)? */
836 static int isnan(double f) { return (f != f); }
837 #endif
838 
846 static int check_for_nan(void)
847 {
848  int orig_nan_count = fdm.nan_count;
849  /* Check all elements for nans */
850  if (isnan(fdm.ecef_pos.x)) { fdm.nan_count++; }
851  if (isnan(fdm.ecef_pos.y)) { fdm.nan_count++; }
852  if (isnan(fdm.ecef_pos.z)) { fdm.nan_count++; }
853  if (isnan(fdm.ltpprz_pos.x)) { fdm.nan_count++; }
854  if (isnan(fdm.ltpprz_pos.y)) { fdm.nan_count++; }
855  if (isnan(fdm.ltpprz_pos.z)) { fdm.nan_count++; }
856  if (isnan(fdm.lla_pos.lon)) { fdm.nan_count++; }
857  if (isnan(fdm.lla_pos.lat)) { fdm.nan_count++; }
858  if (isnan(fdm.lla_pos.alt)) { fdm.nan_count++; }
859  if (isnan(fdm.hmsl)) { fdm.nan_count++; }
860  // Skip debugging elements
861  if (isnan(fdm.ecef_ecef_vel.x)) { fdm.nan_count++; }
862  if (isnan(fdm.ecef_ecef_vel.y)) { fdm.nan_count++; }
863  if (isnan(fdm.ecef_ecef_vel.z)) { fdm.nan_count++; }
864  if (isnan(fdm.ecef_ecef_accel.x)) { fdm.nan_count++; }
865  if (isnan(fdm.ecef_ecef_accel.y)) { fdm.nan_count++; }
866  if (isnan(fdm.ecef_ecef_accel.z)) { fdm.nan_count++; }
867  if (isnan(fdm.body_ecef_vel.x)) { fdm.nan_count++; }
868  if (isnan(fdm.body_ecef_vel.y)) { fdm.nan_count++; }
869  if (isnan(fdm.body_ecef_vel.z)) { fdm.nan_count++; }
870  if (isnan(fdm.body_ecef_accel.x)) { fdm.nan_count++; }
871  if (isnan(fdm.body_ecef_accel.y)) { fdm.nan_count++; }
872  if (isnan(fdm.body_ecef_accel.z)) { fdm.nan_count++; }
873  if (isnan(fdm.ltp_ecef_vel.x)) { fdm.nan_count++; }
874  if (isnan(fdm.ltp_ecef_vel.y)) { fdm.nan_count++; }
875  if (isnan(fdm.ltp_ecef_vel.z)) { fdm.nan_count++; }
876  if (isnan(fdm.ltp_ecef_accel.x)) { fdm.nan_count++; }
877  if (isnan(fdm.ltp_ecef_accel.y)) { fdm.nan_count++; }
878  if (isnan(fdm.ltp_ecef_accel.z)) { fdm.nan_count++; }
879  if (isnan(fdm.ltpprz_ecef_vel.x)) { fdm.nan_count++; }
880  if (isnan(fdm.ltpprz_ecef_vel.y)) { fdm.nan_count++; }
881  if (isnan(fdm.ltpprz_ecef_vel.z)) { fdm.nan_count++; }
882  if (isnan(fdm.ltpprz_ecef_accel.x)) { fdm.nan_count++; }
883  if (isnan(fdm.ltpprz_ecef_accel.y)) { fdm.nan_count++; }
884  if (isnan(fdm.ltpprz_ecef_accel.z)) { fdm.nan_count++; }
885  if (isnan(fdm.ecef_to_body_quat.qi)) { fdm.nan_count++; }
886  if (isnan(fdm.ecef_to_body_quat.qx)) { fdm.nan_count++; }
887  if (isnan(fdm.ecef_to_body_quat.qy)) { fdm.nan_count++; }
888  if (isnan(fdm.ecef_to_body_quat.qz)) { fdm.nan_count++; }
889  if (isnan(fdm.ltp_to_body_quat.qi)) { fdm.nan_count++; }
890  if (isnan(fdm.ltp_to_body_quat.qx)) { fdm.nan_count++; }
891  if (isnan(fdm.ltp_to_body_quat.qy)) { fdm.nan_count++; }
892  if (isnan(fdm.ltp_to_body_quat.qz)) { fdm.nan_count++; }
893  if (isnan(fdm.ltp_to_body_eulers.phi)) { fdm.nan_count++; }
894  if (isnan(fdm.ltp_to_body_eulers.theta)) { fdm.nan_count++; }
895  if (isnan(fdm.ltp_to_body_eulers.psi)) { fdm.nan_count++; }
896  if (isnan(fdm.ltpprz_to_body_quat.qi)) { fdm.nan_count++; }
897  if (isnan(fdm.ltpprz_to_body_quat.qx)) { fdm.nan_count++; }
898  if (isnan(fdm.ltpprz_to_body_quat.qy)) { fdm.nan_count++; }
899  if (isnan(fdm.ltpprz_to_body_quat.qz)) { fdm.nan_count++; }
900  if (isnan(fdm.ltpprz_to_body_eulers.phi)) { fdm.nan_count++; }
901  if (isnan(fdm.ltpprz_to_body_eulers.theta)) { fdm.nan_count++; }
902  if (isnan(fdm.ltpprz_to_body_eulers.psi)) { fdm.nan_count++; }
903  if (isnan(fdm.body_ecef_rotvel.p)) { fdm.nan_count++; }
904  if (isnan(fdm.body_ecef_rotvel.q)) { fdm.nan_count++; }
905  if (isnan(fdm.body_ecef_rotvel.r)) { fdm.nan_count++; }
906  if (isnan(fdm.body_ecef_rotaccel.p)) { fdm.nan_count++; }
907  if (isnan(fdm.body_ecef_rotaccel.q)) { fdm.nan_count++; }
908  if (isnan(fdm.body_ecef_rotaccel.r)) { fdm.nan_count++; }
909  if (isnan(fdm.ltp_g.x)) { fdm.nan_count++; }
910  if (isnan(fdm.ltp_g.y)) { fdm.nan_count++; }
911  if (isnan(fdm.ltp_g.z)) { fdm.nan_count++; }
912  if (isnan(fdm.ltp_h.x)) { fdm.nan_count++; }
913  if (isnan(fdm.ltp_h.y)) { fdm.nan_count++; }
914  if (isnan(fdm.ltp_h.z)) { fdm.nan_count++; }
915 
916  return (fdm.nan_count - orig_nan_count);
917 }
#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
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.
WMM2020 Geomagnetic field model.
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
#define JSBSIM_PATH(_x)
Macro to build file path according to the lib version.
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