Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
28
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
83/* Actuator naming*/
84#ifdef NPS_USE_COMMANDS
85#define NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES TRUE
86#endif
87
88#ifdef NPS_ACTUATOR_NAMES
89#define NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES TRUE
90#endif
91
92#ifndef NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES
93#define NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES FALSE
94#endif
95
99#ifndef NPS_JSBSIM_MODEL
100#define NPS_JSBSIM_MODEL AIRFRAME_NAME
101#endif
102
103#ifdef NPS_INITIAL_CONDITITONS
104#warning NPS_INITIAL_CONDITITONS was replaced by NPS_JSBSIM_INIT!
105#warning Defaulting to flight plan location.
106#endif
107
111#ifndef NPS_JSBSIM_PITCH_TRIM
112#define NPS_JSBSIM_PITCH_TRIM 0.0
113#endif
114
115#ifndef NPS_JSBSIM_ROLL_TRIM
116#define NPS_JSBSIM_ROLL_TRIM 0.0
117#endif
118
119#ifndef NPS_JSBSIM_YAW_TRIM
120#define NPS_JSBSIM_YAW_TRIM 0.0
121#endif
122
126#define DEG2RAD 0.017
127
128#ifndef NPS_JSBSIM_ELEVATOR_MAX_RAD
129#define NPS_JSBSIM_ELEVATOR_MAX_RAD (20.0*DEG2RAD)
130#endif
131
132#ifndef NPS_JSBSIM_AILERON_MAX_RAD
133#define NPS_JSBSIM_AILERON_MAX_RAD (20.0*DEG2RAD)
134#endif
135
136#ifndef NPS_JSBSIM_RUDDER_MAX_RAD
137#define NPS_JSBSIM_RUDDER_MAX_RAD (20.0*DEG2RAD)
138#endif
139
140#ifndef NPS_JSBSIM_FLAP_MAX_RAD
141#define NPS_JSBSIM_FLAP_MAX_RAD (20.0*DEG2RAD)
142#endif
143
147#define MIN_DT (1.0/10240.0)
148// TODO: maybe lower for slower CPUs & HITL?
149//#define MIN_DT (1.0/1000.0)
150
151using namespace JSBSim;
152using namespace std;
153
154static void feed_jsbsim(double *commands, int commands_nb);
155static void fetch_state(void);
156static int check_for_nan(void);
157
158static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector);
159static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location);
160static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat);
161static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector);
162static void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate);
163static void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate);
164static void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate);
165
166static void init_jsbsim(double dt);
167static void init_ltp(void);
168
170struct NpsFdm fdm;
171
173static FGFDMExec *FDMExec;
174
175static struct LtpDef_d ltpdef;
176
177// Offset between ecef in geodetic and geocentric coordinates
178static struct EcefCoor_d offset;
179
182
184double min_dt;
185
186void nps_fdm_init(double dt)
187{
188
189 fdm.init_dt = dt;
190 fdm.curr_dt = dt;
191 //Sets up the high fidelity timestep as a multiple of the normal timestep
192 for (min_dt = (1.0 / dt); min_dt < (1 / MIN_DT); min_dt += (1 / dt)) {}
193 min_dt = (1 / min_dt);
194
195 fdm.nan_count = 0;
196
197 VECT3_ASSIGN(offset, 0., 0., 0.);
198
199 init_jsbsim(dt);
200
201 FDMExec->RunIC();
202
203 // Fix getting initial incorrect accel measurements
204 for(uint16_t i = 0; i < 1500; i++)
205 FDMExec->Run();
206
207 init_ltp();
208
209#if DEBUG_NPS_JSBSIM
210 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");
211#endif
212
213 fetch_state();
214
215}
216
218{
219 // nothing to do
220}
221
222void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb)
223{
224
225#ifdef NPS_JSBSIM_LAUNCHSPEED
226 static bool already_launched = FALSE;
227
228 if (launch && !already_launched) {
229 printf("Launching with speed of %.1f m/s!\n", (float)NPS_JSBSIM_LAUNCHSPEED);
230 FDMExec->GetIC()->SetUBodyFpsIC(FeetOfMeters(NPS_JSBSIM_LAUNCHSPEED));
231 FDMExec->RunIC();
232#ifdef NPS_JSBSIM_LAUNCH_RPM
233 // Model a motor already running at hand launch. Starting an electric
234 // propeller from rest at full power can produce a large torque transient.
235 FGPropulsion *propulsion = FDMExec->GetPropulsion();
236 for (uint32_t i = 0; i < propulsion->GetNumEngines(); i++) {
237 FGThruster *thruster = propulsion->GetEngine(i)->GetThruster();
238 if (thruster->GetRPM() < NPS_JSBSIM_LAUNCH_RPM) {
239 thruster->SetRPM(NPS_JSBSIM_LAUNCH_RPM);
240 }
241 }
242#endif
243 already_launched = TRUE;
244 }
245#endif
246
247 feed_jsbsim(commands, commands_nb);
248
249 /* To deal with ground interaction issues, we decrease the time
250 step as the vehicle is close to the ground. This is done predictively
251 to ensure no weird accelerations or oscillations. From tests with a bouncing
252 ball model in JSBSim, it seems that 10k steps per second is reasonable to capture
253 all the dynamics. Higher might be a bit more stable, but really starting to push
254 the simulation CPU requirements, especially for more complex models.
255 - at init: get the largest radius from CG to any contact point (landing gear)
256 - if descending...
257 - find current number of timesteps to impact
258 - if impact imminent, calculate a new timestep to use (with limit)
259 - if ascending...
260 - change timestep back to init value
261 - run sim for as many steps as needed to reach init_dt amount of time
262
263 Of course, could probably be improved...
264 */
265 // If the vehicle has a downwards velocity
266 if (fdm.ltp_ecef_vel.z > 0) {
267 // Get the current number of timesteps until impact at current velocity
268 double numDT_to_impact = (fdm.agl - vehicle_radius_max) / (fdm.curr_dt * fdm.ltp_ecef_vel.z);
269 // If impact imminent within next timestep, use high sim rate
270 if (numDT_to_impact <= 1.0) {
271 fdm.curr_dt = min_dt;
272 }
273 }
274 // If the vehicle is moving upwards and out of the ground, reset timestep
275 else if ((fdm.ltp_ecef_vel.z <= 0) && ((fdm.agl + vehicle_radius_max) > 0)) {
276 fdm.curr_dt = fdm.init_dt;
277 }
278
279 // Calculate the number of sim steps for correct amount of time elapsed
280 int num_steps = int(fdm.init_dt / fdm.curr_dt);
281
282 // Set the timestep then run sim
283 FDMExec->Setdt(fdm.curr_dt);
284 int i;
285 for (i = 0; i < num_steps; i++) {
286 FDMExec->Run();
287 }
288
289 fetch_state();
290
291 /* Check the current state to make sure it is valid (no NaNs) */
292 if (check_for_nan()) {
293 printf("Error: FDM simulation encountered a total of %i NaN values at simulation time %f.\n", fdm.nan_count, fdm.time);
294 printf("It is likely the simulation diverged and gave non-physical results. If you did\n");
295 printf("not crash, check your model and/or initial conditions. Exiting with status 1.\n");
296 exit(1);
297 }
298
299}
300
301void nps_fdm_set_wind(double speed, double dir)
302{
303 FGWinds *Winds = FDMExec->GetWinds();
304 Winds->SetWindspeed(FeetOfMeters(speed));
305 Winds->SetWindPsi(dir);
306}
307
308void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
309{
310 FGWinds *Winds = FDMExec->GetWinds();
311 Winds->SetWindNED(FeetOfMeters(wind_north), FeetOfMeters(wind_east),
312 FeetOfMeters(wind_down));
313}
314
315void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
316{
317 FGWinds *Winds = FDMExec->GetWinds();
318 /* wind speed used for turbulence */
319 Winds->SetWindspeed20ft(FeetOfMeters(wind_speed) / 2);
320 Winds->SetProbabilityOfExceedence(turbulence_severity);
321}
322
323void nps_fdm_set_temperature(double temp, double h)
324{
325 FDMExec->GetAtmosphere()->SetTemperature(temp, h, FGAtmosphere::eCelsius);
326}
327
334static void feed_jsbsim(double *commands, int commands_nb __attribute__((unused)))
335{
336#if NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES
337 char buf[64];
338#if defined(NPS_USE_COMMANDS)
339 const char *names[] = COMMAND_NAMES;
340#elif defined(NPS_ACTUATOR_NAMES)
341 const char *names[] = NPS_ACTUATOR_NAMES;
342#endif
343 string property;
344 int i;
345 for (i = 0; i < commands_nb; i++) {
346 sprintf(buf, "fcs/%s", names[i]);
347 property = string(buf);
348 FDMExec->GetPropertyManager()->GetNode(property)->SetDouble("", commands[i]);
349 }
350#else /*Use manually defined commands*/
351 // get FGFCS instance
352 FGFCS *FCS = FDMExec->GetFCS();
353
354 // Set trims
355 FCS->SetPitchTrimCmd(NPS_JSBSIM_PITCH_TRIM);
356 FCS->SetRollTrimCmd(NPS_JSBSIM_ROLL_TRIM);
357 FCS->SetYawTrimCmd(NPS_JSBSIM_YAW_TRIM);
358
359#ifdef COMMAND_THROTTLE
360 FGPropulsion *FProp = FDMExec->GetPropulsion();
361 for (unsigned int i = 0; i < FDMExec->GetPropulsion()->GetNumEngines(); i++) {
362 FCS->SetThrottleCmd(i, commands[COMMAND_THROTTLE]);
363
364 // Hack to show spinning propellers in flight gear models
365 if (commands[COMMAND_THROTTLE] > 0.01) {
366 FProp->SetStarter(1);
367 } else {
368 FProp->SetStarter(0);
369 }
370 }
371#endif /* COMMAND_THROTTLE */
372
373#ifdef COMMAND_ROLL
374 FCS->SetDaCmd(commands[COMMAND_ROLL]);
375#endif /* COMMAND_ROLL */
376
377#ifdef COMMAND_PITCH
378 FCS->SetDeCmd(commands[COMMAND_PITCH]);
379#endif /* COMMAND_PITCH */
380
381#ifdef COMMAND_YAW
382 FCS->SetDrCmd(commands[COMMAND_YAW]);
383#endif /* COMMAND_YAW */
384
385#ifdef COMMAND_FLAP
386 FCS->SetDfCmd(commands[COMMAND_FLAP]);
387#endif /* COMMAND_FLAP */
388
389#endif /* NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES */
390}
391
395static void fetch_state(void)
396{
397
398 fdm.time = FDMExec->GetPropertyManager()->GetNode("simulation/sim-time-sec")->getDoubleValue();
399
400#if DEBUG_NPS_JSBSIM
401 printf("%f,", fdm.time);
402#endif
403
404 FGPropagate *propagate = FDMExec->GetPropagate();
405 FGAccelerations *accelerations = FDMExec->GetAccelerations();
406
407 fdm.on_ground = FDMExec->GetGroundReactions()->GetWOW();
408
409 /*
410 * position
411 */
412 jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation());
413 fdm.hmsl = propagate->GetAltitudeASLmeters();
414
415 /*
416 * linear speed and accelerations
417 */
418
419 /* in body frame */
420 jsbsimvec_to_vec(&fdm.body_ecef_vel, &propagate->GetUVW());
421 jsbsimvec_to_vec(&fdm.body_ecef_accel, &accelerations->GetUVWdot());
422 jsbsimvec_to_vec(&fdm.body_inertial_accel, &accelerations->GetUVWidot());
423 jsbsimvec_to_vec(&fdm.body_accel, &accelerations->GetBodyAccel());
424
425#if DEBUG_NPS_JSBSIM
426 printf("%f,%f,%f,", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);
427#endif
428
429 /* in LTP frame */
430 jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_vel, &propagate->GetVel());
431 const FGColumnVector3 &fg_ltp_ecef_accel = propagate->GetTb2l() * accelerations->GetUVWdot();
432 jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_accel, &fg_ltp_ecef_accel);
433
434#if DEBUG_NPS_JSBSIM
435 printf("%f,%f,%f,", fdm.ltp_ecef_accel.x, fdm.ltp_ecef_accel.y, fdm.ltp_ecef_accel.z);
436#endif
437
438 /* in ECEF frame */
439 const FGColumnVector3 &fg_ecef_ecef_vel = propagate->GetECEFVelocity();
440 jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel);
441
442 const FGColumnVector3 &fg_ecef_ecef_accel = propagate->GetTb2ec() * accelerations->GetUVWdot();
443 jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel);
444
445#if DEBUG_NPS_JSBSIM
446 printf("%f,%f,%f,", fdm.ecef_ecef_accel.x, fdm.ecef_ecef_accel.y, fdm.ecef_ecef_accel.z);
447#endif
448
449 /* in LTP pprz */
450 ned_of_ecef_point_d(&fdm.ltpprz_pos, &ltpdef, &fdm.ecef_pos);
451 ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, &ltpdef, &fdm.ecef_ecef_vel);
452 ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, &ltpdef, &fdm.ecef_ecef_accel);
453
454#if DEBUG_NPS_JSBSIM
455 printf("%f,%f,%f,", fdm.ltpprz_ecef_accel.z, fdm.ltpprz_ecef_accel.y, fdm.ltpprz_ecef_accel.z);
456#endif
457
458 /* llh */
459 llh_from_jsbsim(&fdm.lla_pos, propagate);
460
461 //for debug
462 lla_from_jsbsim_geodetic(&fdm.lla_pos_geod, propagate);
463 lla_from_jsbsim_geocentric(&fdm.lla_pos_geoc, propagate);
464 lla_of_ecef_d(&fdm.lla_pos_pprz, &fdm.ecef_pos);
465 fdm.agl = MetersOfFeet(propagate->GetDistanceAGL());
466
467#if DEBUG_NPS_JSBSIM
468 printf("%f\n", fdm.agl);
469#endif
470
471 /*
472 * attitude
473 */
474 const FGQuaternion jsb_quat = propagate->GetQuaternion();
475 jsbsimquat_to_quat(&fdm.ltp_to_body_quat, &jsb_quat);
476 /* convert to eulers */
477 double_eulers_of_quat(&fdm.ltp_to_body_eulers, &fdm.ltp_to_body_quat);
478 /* the "false" pprz lpt */
479 /* FIXME: use jsbsim ltp for now */
480 EULERS_COPY(fdm.ltpprz_to_body_eulers, fdm.ltp_to_body_eulers);
481 QUAT_COPY(fdm.ltpprz_to_body_quat, fdm.ltp_to_body_quat);
482
483 /*
484 * rotational speed and accelerations
485 */
486 jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR());
487 jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot());
488
489 jsbsimvec_to_rate(&fdm.body_inertial_rotvel, &propagate->GetPQRi());
490 jsbsimvec_to_rate(&fdm.body_inertial_rotaccel, &accelerations->GetPQRidot());
491
492
493 /*
494 * wind
495 */
496 const FGColumnVector3 &fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED();
497 jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned);
498
499 /*
500 * Equivalent Airspeed, atmospheric pressure and temperature.
501 */
502 fdm.airspeed = MetersOfFeet(FDMExec->GetAuxiliary()->GetVequivalentFPS());
503 fdm.pressure = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressure());
504 fdm.pressure_sl = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressureSL());
505 fdm.total_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->GetTotalPressure());
506 fdm.dynamic_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->Getqbar());
507 fdm.temperature = CelsiusOfRankine(FDMExec->GetAtmosphere()->GetTemperature());
508
509 /*
510 * angle of attack and SlideSlip.
511 */
512 fdm.aoa= FDMExec->GetPropertyManager()->GetNode("aero/alpha-rad")->getDoubleValue();
513 fdm.sideslip = FDMExec->GetPropertyManager()->GetNode("aero/beta-rad")->getDoubleValue();
514
515 /*
516 * Control surface positions
517 *
518 */
519 fdm.rudder = (FDMExec->GetPropertyManager()->GetNode("fcs/rudder-pos-rad")->getDoubleValue()) /
521 fdm.left_aileron = (-1 * FDMExec->GetPropertyManager()->GetNode("fcs/left-aileron-pos-rad")->getDoubleValue()) /
523 fdm.right_aileron = (FDMExec->GetPropertyManager()->GetNode("fcs/right-aileron-pos-rad")->getDoubleValue()) /
525 fdm.elevator = (FDMExec->GetPropertyManager()->GetNode("fcs/elevator-pos-rad")->getDoubleValue()) /
527 fdm.flap = (FDMExec->GetPropertyManager()->GetNode("fcs/flap-pos-rad")->getDoubleValue()) / NPS_JSBSIM_FLAP_MAX_RAD;
528
529 /*
530 * Propulsion
531 */
532 FGPropulsion *FGProp = FDMExec->GetPropulsion();
533 fdm.num_engines = FGProp->GetNumEngines();
534
535 /*
536 * Note that JSBSim for some reason has very high momentum for the propeller
537 * (even when the moment of inertia of the propeller has the right value)
538 * As a result after switching the motor off
539 */
540 for (uint32_t k = 0; k < fdm.num_engines; k++) {
541 FGEngine *FGEng = FGProp->GetEngine(k);
542 FGThruster *FGThrst = FGEng->GetThruster();
543 fdm.eng_state[k] = FGEng->GetStarter();
544 fdm.rpm[k] = (float) FGThrst->GetRPM();
545 //printf("RPM: %f\n", fdm.rpm[k]);
546 //printf("STATE: %u\n", fdm.eng_state[k]);
547 }
548}
549
560static void init_jsbsim(double dt)
561{
562
563 char buf[1024];
564 string rootdir;
565 string jsbsim_home = "/conf/simulator/jsbsim/";
566 string jsbsim_ic_name;
567
568 char* pprz_home = getenv("PAPARAZZI_HOME");
569
570 int cnt = -1;
571 if (strlen(pprz_home) < sizeof(buf)) {
572 cnt = snprintf(buf, strlen(pprz_home) + 1, "%s", pprz_home);
573 rootdir = string(buf) + jsbsim_home;
574 }
575
576 // check the results
577 if (cnt < 0){
578 // Either pprz_home path too long for the buffer
579 // or writing the string was not successful.
580 cout << "PPRZ_HOME not set correctly, exiting..." << endl;
581 exit(-1);
582 }
583
584 /* if jsbsim initial conditions are defined, use them
585 * otherwise use flightplan location
586 */
587#ifdef NPS_JSBSIM_INIT
588 jsbsim_ic_name = NPS_JSBSIM_INIT;
589#endif
590
591 FDMExec = new FGFDMExec();
592
593 FDMExec->Setsim_time(0.);
594 FDMExec->Setdt(dt);
595
596 FDMExec->DisableOutput();
597 FDMExec->SetDebugLevel(0); // No DEBUG messages
598
599 if (! FDMExec->LoadModel(JSBSIM_PATH(rootdir + "aircraft"),
600 JSBSIM_PATH(rootdir + "engine"),
601 JSBSIM_PATH(rootdir + "systems"),
603 false)) {
604#ifdef DEBUG
605 cerr << " JSBSim could not be started" << endl << endl;
606#endif
607 delete FDMExec;
608 exit(-1);
609 }
610 cout << "JSBSim model loaded from " << NPS_JSBSIM_MODEL << endl;
611
612#ifdef DEBUG
613 cerr << "NumEngines: " << FDMExec->GetPropulsion()->GetNumEngines() << endl;
614 cerr << "NumGearUnits: " << FDMExec->GetGroundReactions()->GetNumGearUnits() << endl;
615#endif
616
617 // LLA initial coordinates (geodetic lat, geoid alt)
618 struct LlaCoor_d lla0;
619
620 FGInitialCondition *IC = FDMExec->GetIC();
621 if (!jsbsim_ic_name.empty()) {
622 if (! IC->Load(JSBSIM_PATH(jsbsim_ic_name))) {
623#ifdef DEBUG
624 cerr << "Initialization unsuccessful" << endl;
625#endif
626 delete FDMExec;
627 exit(-1);
628 }
629
630 llh_from_jsbsim(&lla0, FDMExec->GetPropagate());
631 cout << "JSBSim initial conditions loaded from " << jsbsim_ic_name << endl;
632 } else {
633 // FGInitialCondition::SetAltitudeASLFtIC
634 // requires this function to be called
635 // before itself
636 IC->SetVgroundFpsIC(0.);
637
638 // Use flight plan initial conditions
639 // convert geodetic lat from flight plan to geocentric
640 double gd_lat = RadOfDeg(NAV_LAT0 / 1e7);
641 double gc_lat = gc_of_gd_lat_d(gd_lat, GROUND_ALT);
642 IC->SetLatitudeDegIC(DegOfRad(gc_lat));
643 IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
644
645 IC->SetWindNEDFpsIC(0.0, 0.0, 0.0);
646 IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0));
647 IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT));
648 IC->SetPsiDegIC(QFU);
649 IC->SetVgroundFpsIC(0.);
650
651 lla0.lon = RadOfDeg(NAV_LON0 / 1e7);
652 lla0.lat = gd_lat;
653 lla0.alt = (double)(NAV_ALT0 + NAV_MSL0) / 1000.0;
654 }
655
656 // initial commands to zero
657 double init_commands[NPS_COMMANDS_NB] = {0.0};
658 feed_jsbsim(init_commands, NPS_COMMANDS_NB);
659
660 //loop JSBSim once w/o integrating
661 if (!FDMExec->RunIC()) {
662 cerr << "Initialization unsuccessful" << endl;
663 exit(-1);
664 }
665
666 //initRunning for all engines
667 FDMExec->GetPropulsion()->InitRunning(-1);
668
669
670 // compute offset between geocentric and geodetic ecef
671 ecef_of_lla_d(&offset, &lla0);
672 struct EcefCoor_d ecef0 = {
673 MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(1)),
674 MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(2)),
675 MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(3))
676 };
677 VECT3_DIFF(offset, offset, ecef0);
678
679 // calculate vehicle max radius in m
680 vehicle_radius_max = 0.01; // specify not 0.0 in case no gear
681 int num_gear = FDMExec->GetGroundReactions()->GetNumGearUnits();
682 int i;
683 for (i = 0; i < num_gear; i++) {
684 FGColumnVector3 gear_location = FDMExec->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation();
685 double radius = MetersOfFeet(gear_location.Magnitude());
686 if (radius > vehicle_radius_max) { vehicle_radius_max = radius; }
687 }
688
689}
690
695static void init_ltp(void)
696{
697
698 FGPropagate *propagate = FDMExec->GetPropagate();
699
700 jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation());
701 ltp_def_from_ecef_d(&ltpdef, &fdm.ecef_pos);
702
703 fdm.ltp_g.x = 0.;
704 fdm.ltp_g.y = 0.;
705 fdm.ltp_g.z = 9.81;
706
707
708#if !NPS_CALC_GEO_MAG && defined(AHRS_H_X)
709 PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (AHRS section).")
710 fdm.ltp_h.x = AHRS_H_X;
711 fdm.ltp_h.y = AHRS_H_Y;
712 fdm.ltp_h.z = AHRS_H_Z;
713#elif !NPS_CALC_GEO_MAG && defined(INS_H_X)
714 PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (INS section).")
715 fdm.ltp_h.x = INS_H_X;
716 fdm.ltp_h.y = INS_H_Y;
717 fdm.ltp_h.z = INS_H_Z;
718#else
719 PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated location.")
720 /* calculation of magnetic field according to WMM2010 model */
721 double gha[MAXCOEFF];
722
723 /* Current date in decimal year, for example 2012.68 */
725 double sdate = 2019.0;
726
727 llh_from_jsbsim(&fdm.lla_pos, propagate);
728 /* LLA Position in decimal degrees and altitude in km */
729 double latitude = DegOfRad(fdm.lla_pos.lat);
730 double longitude = DegOfRad(fdm.lla_pos.lon);
731 double alt = fdm.lla_pos.alt / 1e3;
732
733 // Calculates additional coeffs
734 int32_t nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
735 // Calculates absolute magnetic field
736 mag_calc(1, latitude, longitude, alt, nmax, gha,
737 &fdm.ltp_h.x, &fdm.ltp_h.y, &fdm.ltp_h.z,
740 printf("normalized magnetic field: %.4f %.4f %.4f\n", fdm.ltp_h.x, fdm.ltp_h.y, fdm.ltp_h.z);
741#endif
742
743}
744
753static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location)
754{
755
756 fdm_location->x = MetersOfFeet(jsb_location->Entry(1));
757 fdm_location->y = MetersOfFeet(jsb_location->Entry(2));
758 fdm_location->z = MetersOfFeet(jsb_location->Entry(3));
759
760 VECT3_ADD(*fdm_location, offset);
761}
762
771static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector)
772{
773
774 fdm_vector->x = MetersOfFeet(jsb_vector->Entry(1));
775 fdm_vector->y = MetersOfFeet(jsb_vector->Entry(2));
776 fdm_vector->z = MetersOfFeet(jsb_vector->Entry(3));
777
778}
779
786static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat)
787{
788
789 fdm_quat->qi = jsb_quat->Entry(1);
790 fdm_quat->qx = jsb_quat->Entry(2);
791 fdm_quat->qy = jsb_quat->Entry(3);
792 fdm_quat->qz = jsb_quat->Entry(4);
793
794}
795
802static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector)
803{
804
805 fdm_rate->p = jsb_vector->Entry(1);
806 fdm_rate->q = jsb_vector->Entry(2);
807 fdm_rate->r = jsb_vector->Entry(3);
808
809}
810
819void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate)
820{
821
822 fdm_lla->lat = propagate->GetGeodLatitudeRad();
823 fdm_lla->lon = propagate->GetLongitude();
824 fdm_lla->alt = propagate->GetAltitudeASLmeters();
825 //printf("geodetic alt: %f\n", MetersOfFeet(propagate->GetGeodeticAltitude()));
826 //printf("ground alt: %f\n", MetersOfFeet(propagate->GetDistanceAGL()));
827 //printf("ASL alt: %f\n", propagate->GetAltitudeASLmeters());
828
829}
830
839void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate)
840{
841
842 fdm_lla->lat = propagate->GetLatitude();
843 fdm_lla->lon = propagate->GetLongitude();
844 fdm_lla->alt = MetersOfFeet(propagate->GetRadius());
845
846}
847
856void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate)
857{
858
859 fdm_lla->lat = propagate->GetGeodLatitudeRad();
860 fdm_lla->lon = propagate->GetLongitude();
861 fdm_lla->alt = MetersOfFeet(propagate->GetGeodeticAltitude());
862
863}
864
865#ifdef __APPLE__
866/* Why isn't this there when we include math.h (on osx with clang)? */
868static int isnan(double f) { return (f != f); }
869#endif
870
878static int check_for_nan(void)
879{
880 int orig_nan_count = fdm.nan_count;
881 /* Check all elements for nans */
882 if (isnan(fdm.ecef_pos.x)) { fdm.nan_count++; }
883 if (isnan(fdm.ecef_pos.y)) { fdm.nan_count++; }
884 if (isnan(fdm.ecef_pos.z)) { fdm.nan_count++; }
885 if (isnan(fdm.ltpprz_pos.x)) { fdm.nan_count++; }
886 if (isnan(fdm.ltpprz_pos.y)) { fdm.nan_count++; }
887 if (isnan(fdm.ltpprz_pos.z)) { fdm.nan_count++; }
888 if (isnan(fdm.lla_pos.lon)) { fdm.nan_count++; }
889 if (isnan(fdm.lla_pos.lat)) { fdm.nan_count++; }
890 if (isnan(fdm.lla_pos.alt)) { fdm.nan_count++; }
891 if (isnan(fdm.hmsl)) { fdm.nan_count++; }
892 // Skip debugging elements
893 if (isnan(fdm.ecef_ecef_vel.x)) { fdm.nan_count++; }
894 if (isnan(fdm.ecef_ecef_vel.y)) { fdm.nan_count++; }
895 if (isnan(fdm.ecef_ecef_vel.z)) { fdm.nan_count++; }
896 if (isnan(fdm.ecef_ecef_accel.x)) { fdm.nan_count++; }
897 if (isnan(fdm.ecef_ecef_accel.y)) { fdm.nan_count++; }
898 if (isnan(fdm.ecef_ecef_accel.z)) { fdm.nan_count++; }
899 if (isnan(fdm.body_ecef_vel.x)) { fdm.nan_count++; }
900 if (isnan(fdm.body_ecef_vel.y)) { fdm.nan_count++; }
901 if (isnan(fdm.body_ecef_vel.z)) { fdm.nan_count++; }
902 if (isnan(fdm.body_ecef_accel.x)) { fdm.nan_count++; }
903 if (isnan(fdm.body_ecef_accel.y)) { fdm.nan_count++; }
904 if (isnan(fdm.body_ecef_accel.z)) { fdm.nan_count++; }
905 if (isnan(fdm.ltp_ecef_vel.x)) { fdm.nan_count++; }
906 if (isnan(fdm.ltp_ecef_vel.y)) { fdm.nan_count++; }
907 if (isnan(fdm.ltp_ecef_vel.z)) { fdm.nan_count++; }
908 if (isnan(fdm.ltp_ecef_accel.x)) { fdm.nan_count++; }
909 if (isnan(fdm.ltp_ecef_accel.y)) { fdm.nan_count++; }
910 if (isnan(fdm.ltp_ecef_accel.z)) { fdm.nan_count++; }
911 if (isnan(fdm.ltpprz_ecef_vel.x)) { fdm.nan_count++; }
912 if (isnan(fdm.ltpprz_ecef_vel.y)) { fdm.nan_count++; }
913 if (isnan(fdm.ltpprz_ecef_vel.z)) { fdm.nan_count++; }
914 if (isnan(fdm.ltpprz_ecef_accel.x)) { fdm.nan_count++; }
915 if (isnan(fdm.ltpprz_ecef_accel.y)) { fdm.nan_count++; }
916 if (isnan(fdm.ltpprz_ecef_accel.z)) { fdm.nan_count++; }
917 if (isnan(fdm.ecef_to_body_quat.qi)) { fdm.nan_count++; }
918 if (isnan(fdm.ecef_to_body_quat.qx)) { fdm.nan_count++; }
919 if (isnan(fdm.ecef_to_body_quat.qy)) { fdm.nan_count++; }
920 if (isnan(fdm.ecef_to_body_quat.qz)) { fdm.nan_count++; }
921 if (isnan(fdm.ltp_to_body_quat.qi)) { fdm.nan_count++; }
922 if (isnan(fdm.ltp_to_body_quat.qx)) { fdm.nan_count++; }
923 if (isnan(fdm.ltp_to_body_quat.qy)) { fdm.nan_count++; }
924 if (isnan(fdm.ltp_to_body_quat.qz)) { fdm.nan_count++; }
925 if (isnan(fdm.ltp_to_body_eulers.phi)) { fdm.nan_count++; }
926 if (isnan(fdm.ltp_to_body_eulers.theta)) { fdm.nan_count++; }
927 if (isnan(fdm.ltp_to_body_eulers.psi)) { fdm.nan_count++; }
928 if (isnan(fdm.ltpprz_to_body_quat.qi)) { fdm.nan_count++; }
929 if (isnan(fdm.ltpprz_to_body_quat.qx)) { fdm.nan_count++; }
930 if (isnan(fdm.ltpprz_to_body_quat.qy)) { fdm.nan_count++; }
931 if (isnan(fdm.ltpprz_to_body_quat.qz)) { fdm.nan_count++; }
932 if (isnan(fdm.ltpprz_to_body_eulers.phi)) { fdm.nan_count++; }
933 if (isnan(fdm.ltpprz_to_body_eulers.theta)) { fdm.nan_count++; }
934 if (isnan(fdm.ltpprz_to_body_eulers.psi)) { fdm.nan_count++; }
935 if (isnan(fdm.body_ecef_rotvel.p)) { fdm.nan_count++; }
936 if (isnan(fdm.body_ecef_rotvel.q)) { fdm.nan_count++; }
937 if (isnan(fdm.body_ecef_rotvel.r)) { fdm.nan_count++; }
938 if (isnan(fdm.body_ecef_rotaccel.p)) { fdm.nan_count++; }
939 if (isnan(fdm.body_ecef_rotaccel.q)) { fdm.nan_count++; }
940 if (isnan(fdm.body_ecef_rotaccel.r)) { fdm.nan_count++; }
941 if (isnan(fdm.ltp_g.x)) { fdm.nan_count++; }
942 if (isnan(fdm.ltp_g.y)) { fdm.nan_count++; }
943 if (isnan(fdm.ltp_g.z)) { fdm.nan_count++; }
944 if (isnan(fdm.ltp_h.x)) { fdm.nan_count++; }
945 if (isnan(fdm.ltp_h.y)) { fdm.nan_count++; }
946 if (isnan(fdm.ltp_h.z)) { fdm.nan_count++; }
947
948 return (fdm.nan_count - orig_nan_count);
949}
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
#define COMMAND_PITCH
#define COMMAND_ROLL
double p
in rad/s^2
double q
in rad/s^2
double r
in rad/s^2
void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q)
static void double_vect3_normalize(struct DoubleVect3 *v)
normalize 3D vector in place
Roation quaternion.
angular rates
#define EULERS_COPY(_a, _b)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define QUAT_COPY(_qo, _qi)
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_ADD(_a, _b)
double x
in meters
double y
in meters
double z
in meters
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
double lon
in radians
double gc_of_gd_lat_d(double gd_lat, double hmsl)
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
#define EXT_COEFF2
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)
#define NMAX_2
int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh)
#define EXT_COEFF3
#define IEXT
#define MAXCOEFF
#define GEO_EPOCH
#define EXT_COEFF1
#define NMAX_1
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
static struct LtpDef_d ltpdef
#define CelsiusOfRankine(_r)
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
#define NPS_JSBSIM_RUDDER_MAX_RAD
double vehicle_radius_max
The largest distance between vehicle CG and contact point.
#define NPS_JSBSIM_PITCH_TRIM
Trim values for the airframe.
#define NPS_JSBSIM_MODEL
Name of the JSBSim model.
static FGFDMExec * FDMExec
The JSBSim executive object.
void nps_fdm_init(double dt)
NPS FDM rover init.
#define FeetOfMeters(_m)
static void init_jsbsim(double dt)
Initializes JSBSim.
static void fetch_state(void)
Populates the NPS fdm struct after a simulation step.
static void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate)
Convert JSBSim location to NPS LLA.
static void init_ltp(void)
Initialize the ltp from the JSBSim location.
#define MIN_DT
Minimum JSBSim timestep Around 1/10000 seems to be good for ground impacts.
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Minimum complexity flight dynamic model In legacy Paparazzi simulator, was implemented in OCaml and c...
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 NPS_JSBSIM_ELEVATOR_MAX_RAD
#define NPS_JSBSIM_FLAP_MAX_RAD
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat)
Convert JSBSim quaternion struct to NPS quaternion struct.
#define PascalOfPsf(_p)
static void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate)
Convert JSBSim location to NPS LLA.
#define NPS_JSBSIM_YAW_TRIM
static int check_for_nan(void)
Checks NpsFdm struct for NaNs.
#define JSBSIM_PATH(_x)
Macro to build file path according to the lib version.
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
static void feed_jsbsim(double *commands, int commands_nb)
Feed JSBSim with the latest actuator commands.
static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector)
Convert JSBSim vector format and struct to NPS vector format and struct.
static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector)
Convert JSBSim rates vector struct to NPS rates struct.
#define NPS_JSBSIM_AILERON_MAX_RAD
void nps_fdm_set_wind(double speed, double dir)
void nps_fdm_run_init_step(void)
double min_dt
Timestep used for higher fidelity near the ground.
#define MetersOfFeet(_f)
Macro to convert from feet to metres.
static void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate)
Convert JSBSim location to NPS LLH.
#define NPS_JSBSIM_ROLL_TRIM
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
Paparazzi generic macros for geodetic calculations.
Paparazzi double-precision floating point math for geodetic calculations.
Paparazzi floating point math for geodetic calculations.
static const ShellCommand commands[]
Definition shell_arch.c:77
static const float dir[]
#define TRUE
Definition std.h:4
#define FALSE
Definition std.h:5
static const float offset[]
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup).
Definition wedgebug.c:204