Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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  cout << "JSBSim model loaded from " << NPS_JSBSIM_MODEL << endl;
580 
581 #ifdef DEBUG
582  cerr << "NumEngines: " << FDMExec->GetPropulsion()->GetNumEngines() << endl;
583  cerr << "NumGearUnits: " << FDMExec->GetGroundReactions()->GetNumGearUnits() << endl;
584 #endif
585 
586  // LLA initial coordinates (geodetic lat, geoid alt)
587  struct LlaCoor_d lla0;
588 
589  FGInitialCondition *IC = FDMExec->GetIC();
590  if (!jsbsim_ic_name.empty()) {
591  if (! IC->Load(JSBSIM_PATH(jsbsim_ic_name))) {
592 #ifdef DEBUG
593  cerr << "Initialization unsuccessful" << endl;
594 #endif
595  delete FDMExec;
596  exit(-1);
597  }
598 
599  llh_from_jsbsim(&lla0, FDMExec->GetPropagate());
600  cout << "JSBSim initial conditions loaded from " << jsbsim_ic_name << endl;
601  } else {
602  // FGInitialCondition::SetAltitudeASLFtIC
603  // requires this function to be called
604  // before itself
605  IC->SetVgroundFpsIC(0.);
606 
607  // Use flight plan initial conditions
608  // convert geodetic lat from flight plan to geocentric
609  double gd_lat = RadOfDeg(NAV_LAT0 / 1e7);
610  double gc_lat = gc_of_gd_lat_d(gd_lat, GROUND_ALT);
611  IC->SetLatitudeDegIC(DegOfRad(gc_lat));
612  IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
613 
614  IC->SetWindNEDFpsIC(0.0, 0.0, 0.0);
615  IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0));
616  IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT));
617  IC->SetPsiDegIC(QFU);
618  IC->SetVgroundFpsIC(0.);
619 
620  lla0.lon = RadOfDeg(NAV_LON0 / 1e7);
621  lla0.lat = gd_lat;
622  lla0.alt = (double)(NAV_ALT0 + NAV_MSL0) / 1000.0;
623  }
624 
625  // initial commands to zero
626  double init_commands[NPS_COMMANDS_NB] = {0.0};
627  feed_jsbsim(init_commands, NPS_COMMANDS_NB);
628 
629  //loop JSBSim once w/o integrating
630  if (!FDMExec->RunIC()) {
631  cerr << "Initialization unsuccessful" << endl;
632  exit(-1);
633  }
634 
635  //initRunning for all engines
636  FDMExec->GetPropulsion()->InitRunning(-1);
637 
638 
639  // compute offset between geocentric and geodetic ecef
640  ecef_of_lla_d(&offset, &lla0);
641  struct EcefCoor_d ecef0 = {
642  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(1)),
643  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(2)),
644  MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(3))
645  };
646  VECT3_DIFF(offset, offset, ecef0);
647 
648  // calculate vehicle max radius in m
649  vehicle_radius_max = 0.01; // specify not 0.0 in case no gear
650  int num_gear = FDMExec->GetGroundReactions()->GetNumGearUnits();
651  int i;
652  for (i = 0; i < num_gear; i++) {
653  FGColumnVector3 gear_location = FDMExec->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation();
654  double radius = MetersOfFeet(gear_location.Magnitude());
655  if (radius > vehicle_radius_max) { vehicle_radius_max = radius; }
656  }
657 
658 }
659 
664 static void init_ltp(void)
665 {
666 
667  FGPropagate *propagate = FDMExec->GetPropagate();
668 
669  jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation());
671 
672  fdm.ltp_g.x = 0.;
673  fdm.ltp_g.y = 0.;
674  fdm.ltp_g.z = 9.81;
675 
676 
677 #if !NPS_CALC_GEO_MAG && defined(AHRS_H_X)
678  PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (AHRS section).")
679  fdm.ltp_h.x = AHRS_H_X;
680  fdm.ltp_h.y = AHRS_H_Y;
681  fdm.ltp_h.z = AHRS_H_Z;
682 #elif !NPS_CALC_GEO_MAG && defined(INS_H_X)
683  PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (INS section).")
684  fdm.ltp_h.x = INS_H_X;
685  fdm.ltp_h.y = INS_H_Y;
686  fdm.ltp_h.z = INS_H_Z;
687 #else
688  PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated location.")
689  /* calculation of magnetic field according to WMM2010 model */
690  double gha[MAXCOEFF];
691 
692  /* Current date in decimal year, for example 2012.68 */
694  double sdate = 2019.0;
695 
696  llh_from_jsbsim(&fdm.lla_pos, propagate);
697  /* LLA Position in decimal degrees and altitude in km */
698  double latitude = DegOfRad(fdm.lla_pos.lat);
699  double longitude = DegOfRad(fdm.lla_pos.lon);
700  double alt = fdm.lla_pos.alt / 1e3;
701 
702  // Calculates additional coeffs
703  int32_t nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
704  // Calculates absolute magnetic field
705  mag_calc(1, latitude, longitude, alt, nmax, gha,
706  &fdm.ltp_h.x, &fdm.ltp_h.y, &fdm.ltp_h.z,
709  printf("normalized magnetic field: %.4f %.4f %.4f\n", fdm.ltp_h.x, fdm.ltp_h.y, fdm.ltp_h.z);
710 #endif
711 
712 }
713 
722 static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location)
723 {
724 
725  fdm_location->x = MetersOfFeet(jsb_location->Entry(1));
726  fdm_location->y = MetersOfFeet(jsb_location->Entry(2));
727  fdm_location->z = MetersOfFeet(jsb_location->Entry(3));
728 
729  VECT3_ADD(*fdm_location, offset);
730 }
731 
740 static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector)
741 {
742 
743  fdm_vector->x = MetersOfFeet(jsb_vector->Entry(1));
744  fdm_vector->y = MetersOfFeet(jsb_vector->Entry(2));
745  fdm_vector->z = MetersOfFeet(jsb_vector->Entry(3));
746 
747 }
748 
755 static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat)
756 {
757 
758  fdm_quat->qi = jsb_quat->Entry(1);
759  fdm_quat->qx = jsb_quat->Entry(2);
760  fdm_quat->qy = jsb_quat->Entry(3);
761  fdm_quat->qz = jsb_quat->Entry(4);
762 
763 }
764 
771 static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector)
772 {
773 
774  fdm_rate->p = jsb_vector->Entry(1);
775  fdm_rate->q = jsb_vector->Entry(2);
776  fdm_rate->r = jsb_vector->Entry(3);
777 
778 }
779 
788 void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate)
789 {
790 
791  fdm_lla->lat = propagate->GetGeodLatitudeRad();
792  fdm_lla->lon = propagate->GetLongitude();
793  fdm_lla->alt = propagate->GetAltitudeASLmeters();
794  //printf("geodetic alt: %f\n", MetersOfFeet(propagate->GetGeodeticAltitude()));
795  //printf("ground alt: %f\n", MetersOfFeet(propagate->GetDistanceAGL()));
796  //printf("ASL alt: %f\n", propagate->GetAltitudeASLmeters());
797 
798 }
799 
808 void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate)
809 {
810 
811  fdm_lla->lat = propagate->GetLatitude();
812  fdm_lla->lon = propagate->GetLongitude();
813  fdm_lla->alt = MetersOfFeet(propagate->GetRadius());
814 
815 }
816 
825 void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate)
826 {
827 
828  fdm_lla->lat = propagate->GetGeodLatitudeRad();
829  fdm_lla->lon = propagate->GetLongitude();
830  fdm_lla->alt = MetersOfFeet(propagate->GetGeodeticAltitude());
831 
832 }
833 
834 #ifdef __APPLE__
835 /* Why isn't this there when we include math.h (on osx with clang)? */
837 static int isnan(double f) { return (f != f); }
838 #endif
839 
847 static int check_for_nan(void)
848 {
849  int orig_nan_count = fdm.nan_count;
850  /* Check all elements for nans */
851  if (isnan(fdm.ecef_pos.x)) { fdm.nan_count++; }
852  if (isnan(fdm.ecef_pos.y)) { fdm.nan_count++; }
853  if (isnan(fdm.ecef_pos.z)) { fdm.nan_count++; }
854  if (isnan(fdm.ltpprz_pos.x)) { fdm.nan_count++; }
855  if (isnan(fdm.ltpprz_pos.y)) { fdm.nan_count++; }
856  if (isnan(fdm.ltpprz_pos.z)) { fdm.nan_count++; }
857  if (isnan(fdm.lla_pos.lon)) { fdm.nan_count++; }
858  if (isnan(fdm.lla_pos.lat)) { fdm.nan_count++; }
859  if (isnan(fdm.lla_pos.alt)) { fdm.nan_count++; }
860  if (isnan(fdm.hmsl)) { fdm.nan_count++; }
861  // Skip debugging elements
862  if (isnan(fdm.ecef_ecef_vel.x)) { fdm.nan_count++; }
863  if (isnan(fdm.ecef_ecef_vel.y)) { fdm.nan_count++; }
864  if (isnan(fdm.ecef_ecef_vel.z)) { fdm.nan_count++; }
865  if (isnan(fdm.ecef_ecef_accel.x)) { fdm.nan_count++; }
866  if (isnan(fdm.ecef_ecef_accel.y)) { fdm.nan_count++; }
867  if (isnan(fdm.ecef_ecef_accel.z)) { fdm.nan_count++; }
868  if (isnan(fdm.body_ecef_vel.x)) { fdm.nan_count++; }
869  if (isnan(fdm.body_ecef_vel.y)) { fdm.nan_count++; }
870  if (isnan(fdm.body_ecef_vel.z)) { fdm.nan_count++; }
871  if (isnan(fdm.body_ecef_accel.x)) { fdm.nan_count++; }
872  if (isnan(fdm.body_ecef_accel.y)) { fdm.nan_count++; }
873  if (isnan(fdm.body_ecef_accel.z)) { fdm.nan_count++; }
874  if (isnan(fdm.ltp_ecef_vel.x)) { fdm.nan_count++; }
875  if (isnan(fdm.ltp_ecef_vel.y)) { fdm.nan_count++; }
876  if (isnan(fdm.ltp_ecef_vel.z)) { fdm.nan_count++; }
877  if (isnan(fdm.ltp_ecef_accel.x)) { fdm.nan_count++; }
878  if (isnan(fdm.ltp_ecef_accel.y)) { fdm.nan_count++; }
879  if (isnan(fdm.ltp_ecef_accel.z)) { fdm.nan_count++; }
880  if (isnan(fdm.ltpprz_ecef_vel.x)) { fdm.nan_count++; }
881  if (isnan(fdm.ltpprz_ecef_vel.y)) { fdm.nan_count++; }
882  if (isnan(fdm.ltpprz_ecef_vel.z)) { fdm.nan_count++; }
883  if (isnan(fdm.ltpprz_ecef_accel.x)) { fdm.nan_count++; }
884  if (isnan(fdm.ltpprz_ecef_accel.y)) { fdm.nan_count++; }
885  if (isnan(fdm.ltpprz_ecef_accel.z)) { fdm.nan_count++; }
886  if (isnan(fdm.ecef_to_body_quat.qi)) { fdm.nan_count++; }
887  if (isnan(fdm.ecef_to_body_quat.qx)) { fdm.nan_count++; }
888  if (isnan(fdm.ecef_to_body_quat.qy)) { fdm.nan_count++; }
889  if (isnan(fdm.ecef_to_body_quat.qz)) { fdm.nan_count++; }
890  if (isnan(fdm.ltp_to_body_quat.qi)) { fdm.nan_count++; }
891  if (isnan(fdm.ltp_to_body_quat.qx)) { fdm.nan_count++; }
892  if (isnan(fdm.ltp_to_body_quat.qy)) { fdm.nan_count++; }
893  if (isnan(fdm.ltp_to_body_quat.qz)) { fdm.nan_count++; }
894  if (isnan(fdm.ltp_to_body_eulers.phi)) { fdm.nan_count++; }
895  if (isnan(fdm.ltp_to_body_eulers.theta)) { fdm.nan_count++; }
896  if (isnan(fdm.ltp_to_body_eulers.psi)) { fdm.nan_count++; }
897  if (isnan(fdm.ltpprz_to_body_quat.qi)) { fdm.nan_count++; }
898  if (isnan(fdm.ltpprz_to_body_quat.qx)) { fdm.nan_count++; }
899  if (isnan(fdm.ltpprz_to_body_quat.qy)) { fdm.nan_count++; }
900  if (isnan(fdm.ltpprz_to_body_quat.qz)) { fdm.nan_count++; }
901  if (isnan(fdm.ltpprz_to_body_eulers.phi)) { fdm.nan_count++; }
902  if (isnan(fdm.ltpprz_to_body_eulers.theta)) { fdm.nan_count++; }
903  if (isnan(fdm.ltpprz_to_body_eulers.psi)) { fdm.nan_count++; }
904  if (isnan(fdm.body_ecef_rotvel.p)) { fdm.nan_count++; }
905  if (isnan(fdm.body_ecef_rotvel.q)) { fdm.nan_count++; }
906  if (isnan(fdm.body_ecef_rotvel.r)) { fdm.nan_count++; }
907  if (isnan(fdm.body_ecef_rotaccel.p)) { fdm.nan_count++; }
908  if (isnan(fdm.body_ecef_rotaccel.q)) { fdm.nan_count++; }
909  if (isnan(fdm.body_ecef_rotaccel.r)) { fdm.nan_count++; }
910  if (isnan(fdm.ltp_g.x)) { fdm.nan_count++; }
911  if (isnan(fdm.ltp_g.y)) { fdm.nan_count++; }
912  if (isnan(fdm.ltp_g.z)) { fdm.nan_count++; }
913  if (isnan(fdm.ltp_h.x)) { fdm.nan_count++; }
914  if (isnan(fdm.ltp_h.y)) { fdm.nan_count++; }
915  if (isnan(fdm.ltp_h.z)) { fdm.nan_count++; }
916 
917  return (fdm.nan_count - orig_nan_count);
918 }
PascalOfPsf
#define PascalOfPsf(_p)
Definition: nps_fdm_jsbsim.cpp:73
vehicle_radius_max
double vehicle_radius_max
The largest distance between vehicle CG and contact point.
Definition: nps_fdm_jsbsim.cpp:168
DoubleQuat
Roation quaternion.
Definition: pprz_algebra_double.h:55
NPS_COMMANDS_NB
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
Definition: nps_autopilot.h:42
NedCoor_d::y
double y
in meters
Definition: pprz_geodetic_double.h:69
jsbsimvec_to_vec
static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector)
Convert JSBSim vector format and struct to NPS vector format and struct.
Definition: nps_fdm_jsbsim.cpp:740
NpsFdm::ltp_h
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
NpsFdm::eng_state
uint32_t eng_state[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:127
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
ltp_def_from_ecef_d
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:34
jsbsimquat_to_quat
static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat)
Convert JSBSim quaternion struct to NPS quaternion struct.
Definition: nps_fdm_jsbsim.cpp:755
offset
static struct EcefCoor_d offset
Definition: nps_fdm_jsbsim.cpp:165
NPS_JSBSIM_YAW_TRIM
#define NPS_JSBSIM_YAW_TRIM
Definition: nps_fdm_jsbsim.cpp:107
lla_from_jsbsim_geocentric
static void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate)
Convert JSBSim location to NPS LLA.
Definition: nps_fdm_jsbsim.cpp:808
DoubleQuat::qz
double qz
Definition: pprz_algebra_double.h:59
NpsFdm::left_aileron
float left_aileron
Definition: nps_fdm.h:121
NpsFdm::airspeed
double airspeed
equivalent airspeed in m/s
Definition: nps_fdm.h:109
pprz_geodetic.h
Paparazzi generic macros for geodetic calculations.
check_for_nan
static int check_for_nan(void)
Checks NpsFdm struct for NaNs.
Definition: nps_fdm_jsbsim.cpp:847
NpsFdm::elevator
float elevator
Definition: nps_fdm.h:119
NpsFdm::num_engines
uint32_t num_engines
Definition: nps_fdm.h:126
NpsFdm::body_ecef_accel
struct DoubleVect3 body_ecef_accel
acceleration in body frame, wrt ECEF frame
Definition: nps_fdm.h:71
DoubleVect3::z
double z
Definition: pprz_algebra_double.h:49
pprz_geodetic_wmm2020.h
WMM2020 Geomagnetic field model.
LlaCoor_d
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_double.h:58
NpsFdm
Definition: nps_fdm.h:44
NpsFdm::right_aileron
float right_aileron
Definition: nps_fdm.h:122
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
ecef_of_lla_d
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
Definition: pprz_geodetic_double.c:120
LlaCoor_d::alt
double alt
in meters above WGS84 reference ellipsoid
Definition: pprz_geodetic_double.h:61
NpsFdm::ltpprz_ecef_vel
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
NedCoor_d::x
double x
in meters
Definition: pprz_geodetic_double.h:68
init_ltp
static void init_ltp(void)
Initialize the ltp from the JSBSim location.
Definition: nps_fdm_jsbsim.cpp:664
NpsFdm::wind
struct DoubleVect3 wind
velocity in m/s in NED
Definition: nps_fdm.h:107
DoubleEulers::phi
double phi
in radians
Definition: pprz_algebra_double.h:77
DoubleRates::q
double q
in rad/s^2
Definition: pprz_algebra_double.h:87
DoubleEulers::theta
double theta
in radians
Definition: pprz_algebra_double.h:78
NMAX_1
#define NMAX_1
Definition: pprz_geodetic_wmm2020.h:48
DoubleRates::r
double r
in rad/s^2
Definition: pprz_algebra_double.h:88
NPS_JSBSIM_AILERON_MAX_RAD
#define NPS_JSBSIM_AILERON_MAX_RAD
Definition: nps_fdm_jsbsim.cpp:120
nps_fdm_set_wind
void nps_fdm_set_wind(double speed, double dir)
Definition: nps_fdm_jsbsim.cpp:272
NpsFdm::body_accel
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition: nps_fdm.h:87
MAXCOEFF
#define MAXCOEFF
Definition: pprz_geodetic_wmm2020.h:67
DoubleEulers::psi
double psi
in radians
Definition: pprz_algebra_double.h:79
VECT3_DIFF
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
double_eulers_of_quat
void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q)
Definition: pprz_algebra_double.c:68
NpsFdm::lla_pos_pprz
struct LlaCoor_d lla_pos_pprz
Definition: nps_fdm.h:58
extrapsh
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
Definition: pprz_geodetic_wmm2020.c:64
NpsFdm::sideslip
double sideslip
sideslip angle in rad
Definition: nps_fdm.h:116
NpsFdm::on_ground
bool on_ground
Definition: nps_fdm.h:49
feed_jsbsim
static void feed_jsbsim(double *commands, int commands_nb)
Feed JSBSim with the latest actuator commands.
Definition: nps_fdm_jsbsim.cpp:305
MetersOfFeet
#define MetersOfFeet(_f)
Macro to convert from feet to metres.
Definition: nps_fdm_jsbsim.cpp:70
NMAX_2
#define NMAX_2
Definition: pprz_geodetic_wmm2020.h:49
commands
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
NpsFdm::ltpprz_ecef_accel
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
nps_fdm.h
pprz_algebra_float.h
Paparazzi floating point algebra.
NPS_JSBSIM_ELEVATOR_MAX_RAD
#define NPS_JSBSIM_ELEVATOR_MAX_RAD
Definition: nps_fdm_jsbsim.cpp:116
nps_fdm_set_turbulence
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
Definition: nps_fdm_jsbsim.cpp:286
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
NpsFdm::ltp_to_body_quat
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
nps_autopilot.h
nps_fdm_set_temperature
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
Definition: nps_fdm_jsbsim.cpp:294
NpsFdm::ltp_ecef_vel
struct NedCoor_d ltp_ecef_vel
velocity in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:74
NpsFdm::ltpprz_to_body_eulers
struct DoubleEulers ltpprz_to_body_eulers
Definition: nps_fdm.h:94
DoubleQuat::qy
double qy
Definition: pprz_algebra_double.h:58
NpsFdm::body_inertial_rotaccel
struct DoubleRates body_inertial_rotaccel
Definition: nps_fdm.h:102
NpsFdm::body_inertial_accel
struct DoubleVect3 body_inertial_accel
acceleration in body frame, wrt ECI inertial frame
Definition: nps_fdm.h:84
double_vect3_normalize
static void double_vect3_normalize(struct DoubleVect3 *v)
normalize 3D vector in place
Definition: pprz_algebra_double.h:106
NpsFdm::agl
double agl
Definition: nps_fdm.h:61
nps_fdm_set_wind_ned
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
Definition: nps_fdm_jsbsim.cpp:279
NpsFdm::body_ecef_rotvel
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
NpsFdm::ecef_ecef_vel
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:64
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
NpsFdm::time
double time
Definition: nps_fdm.h:46
DoubleRates
angular rates
Definition: pprz_algebra_double.h:85
DoubleVect3::x
double x
Definition: pprz_algebra_double.h:47
NpsFdm::ltp_g
struct DoubleVect3 ltp_g
Definition: nps_fdm.h:104
NpsFdm::body_ecef_vel
struct DoubleVect3 body_ecef_vel
velocity in body frame, wrt ECEF frame
Definition: nps_fdm.h:69
dir
static const float dir[]
Definition: shift_tracking.c:91
NpsFdm::ltp_ecef_accel
struct NedCoor_d ltp_ecef_accel
acceleration in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:76
NpsFdm::total_pressure
double total_pressure
total atmospheric pressure in Pascal
Definition: nps_fdm.h:111
JSBSIM_PATH
#define JSBSIM_PATH(_x)
Macro to build file path according to the lib version.
Definition: nps_fdm_jsbsim.cpp:80
NpsFdm::lla_pos_geoc
struct LlaCoor_d lla_pos_geoc
Definition: nps_fdm.h:60
EULERS_COPY
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:268
ltpdef
static struct LtpDef_d ltpdef
Definition: nps_fdm_jsbsim.cpp:162
min_dt
double min_dt
Timestep used for higher fidelity near the ground.
Definition: nps_fdm_jsbsim.cpp:171
jsbsimvec_to_rate
static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector)
Convert JSBSim rates vector struct to NPS rates struct.
Definition: nps_fdm_jsbsim.cpp:771
NedCoor_d::z
double z
in meters
Definition: pprz_geodetic_double.h:70
NPS_JSBSIM_PITCH_TRIM
#define NPS_JSBSIM_PITCH_TRIM
Trim values for the airframe.
Definition: nps_fdm_jsbsim.cpp:99
NpsFdm::body_ecef_rotaccel
struct DoubleRates body_ecef_rotaccel
Definition: nps_fdm.h:98
nps_fdm_run_step
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Update the simulation state.
Definition: nps_fdm_jsbsim.cpp:204
EXT_COEFF2
#define EXT_COEFF2
Definition: pprz_geodetic_wmm2020.h:53
LlaCoor_d::lat
double lat
in radians
Definition: pprz_geodetic_double.h:59
nps_fdm_init
void nps_fdm_init(double dt)
Initialize actuator dynamics, set unused fields in fdm.
Definition: nps_fdm_jsbsim.cpp:173
NpsFdm::rudder
float rudder
Definition: nps_fdm.h:123
lla_from_jsbsim_geodetic
static void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate)
Convert JSBSim location to NPS LLA.
Definition: nps_fdm_jsbsim.cpp:825
pprz_geodetic_double.h
Paparazzi double-precision floating point math for geodetic calculations.
NPS_JSBSIM_FLAP_MAX_RAD
#define NPS_JSBSIM_FLAP_MAX_RAD
Definition: nps_fdm_jsbsim.cpp:128
IEXT
#define IEXT
Definition: pprz_geodetic_wmm2020.h:51
LtpDef_d
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_double.h:97
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FeetOfMeters
#define FeetOfMeters(_m)
Definition: nps_fdm_jsbsim.cpp:71
NpsFdm::ecef_ecef_accel
struct EcefCoor_d ecef_ecef_accel
acceleration in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:66
NpsFdm::flap
float flap
Definition: nps_fdm.h:120
DoubleQuat::qx
double qx
Definition: pprz_algebra_double.h:57
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
NpsFdm::lla_pos
struct LlaCoor_d lla_pos
Definition: nps_fdm.h:55
NPS_JSBSIM_MODEL
#define NPS_JSBSIM_MODEL
Name of the JSBSim model.
Definition: nps_fdm_jsbsim.cpp:87
NpsFdm::aoa
double aoa
angle of attack in rad
Definition: nps_fdm.h:115
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_jsbsim.cpp:157
NpsFdm::hmsl
double hmsl
Definition: nps_fdm.h:56
NpsFdm::ecef_to_body_quat
struct DoubleQuat ecef_to_body_quat
Definition: nps_fdm.h:90
QUAT_COPY
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
NpsFdm::ltp_to_body_eulers
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
NpsFdm::init_dt
double init_dt
Definition: nps_fdm.h:47
NpsFdm::ltpprz_pos
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
init_jsbsim
static void init_jsbsim(double dt)
Initializes JSBSim.
Definition: nps_fdm_jsbsim.cpp:529
ned_of_ecef_point_d
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:147
NpsFdm::ltpprz_to_body_quat
struct DoubleQuat ltpprz_to_body_quat
Definition: nps_fdm.h:93
EXT_COEFF1
#define EXT_COEFF1
Definition: pprz_geodetic_wmm2020.h:52
EcefCoor_d
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_double.h:49
NpsFdm::ecef_pos
struct EcefCoor_d ecef_pos
Definition: nps_fdm.h:53
DoubleQuat::qi
double qi
Definition: pprz_algebra_double.h:56
EcefCoor_d::y
double y
in meters
Definition: pprz_geodetic_double.h:51
FDMExec
static FGFDMExec * FDMExec
The JSBSim executive object.
Definition: nps_fdm_jsbsim.cpp:160
NpsFdm::temperature
double temperature
current temperature in degrees Celcius
Definition: nps_fdm.h:113
fetch_state
static void fetch_state(void)
Populates the NPS fdm struct after a simulation step.
Definition: nps_fdm_jsbsim.cpp:364
lla_of_ecef_d
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:83
llh_from_jsbsim
static void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate)
Convert JSBSim location to NPS LLH.
Definition: nps_fdm_jsbsim.cpp:788
gc_of_gd_lat_d
double gc_of_gd_lat_d(double gd_lat, double hmsl)
Definition: pprz_geodetic_double.c:210
ned_of_ecef_vect_d
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:159
EcefCoor_d::x
double x
in meters
Definition: pprz_geodetic_double.h:50
NpsFdm::pressure
double pressure
current (static) atmospheric pressure in Pascal
Definition: nps_fdm.h:110
mag_calc
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)
Definition: pprz_geodetic_wmm2020.c:105
NpsFdm::dynamic_pressure
double dynamic_pressure
dynamic pressure in Pascal
Definition: nps_fdm.h:112
DoubleVect3
Definition: pprz_algebra_double.h:46
CelsiusOfRankine
#define CelsiusOfRankine(_r)
Definition: nps_fdm_jsbsim.cpp:74
pprz_algebra.h
Paparazzi generic algebra macros.
EcefCoor_d::z
double z
in meters
Definition: pprz_geodetic_double.h:52
DoubleVect3::y
double y
Definition: pprz_algebra_double.h:48
FALSE
#define FALSE
Definition: std.h:5
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
NpsFdm::lla_pos_geod
struct LlaCoor_d lla_pos_geod
Definition: nps_fdm.h:59
LlaCoor_d::lon
double lon
in radians
Definition: pprz_geodetic_double.h:60
TRUE
#define TRUE
Definition: std.h:4
NpsFdm::curr_dt
double curr_dt
Definition: nps_fdm.h:48
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
NpsFdm::nan_count
int nan_count
Definition: nps_fdm.h:50
NpsFdm::rpm
float rpm[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:128
DoubleRates::p
double p
in rad/s^2
Definition: pprz_algebra_double.h:86
GEO_EPOCH
#define GEO_EPOCH
Definition: pprz_geodetic_wmm2020.h:45
EXT_COEFF3
#define EXT_COEFF3
Definition: pprz_geodetic_wmm2020.h:54
NPS_JSBSIM_RUDDER_MAX_RAD
#define NPS_JSBSIM_RUDDER_MAX_RAD
Definition: nps_fdm_jsbsim.cpp:124
NpsFdm::body_inertial_rotvel
struct DoubleRates body_inertial_rotvel
Definition: nps_fdm.h:101
NPS_JSBSIM_ROLL_TRIM
#define NPS_JSBSIM_ROLL_TRIM
Definition: nps_fdm_jsbsim.cpp:103
NpsFdm::pressure_sl
double pressure_sl
pressure at sea level in Pascal
Definition: nps_fdm.h:114
jsbsimloc_to_loc
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.
Definition: nps_fdm_jsbsim.cpp:722
MIN_DT
#define MIN_DT
Minimum JSBSim timestep Around 1/10000 seems to be good for ground impacts.
Definition: nps_fdm_jsbsim.cpp:134