Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nps_fdm_jsbsim.cpp File Reference
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <FGFDMExec.h>
#include <FGJSBBase.h>
#include <initialization/FGInitialCondition.h>
#include <models/FGPropulsion.h>
#include <models/FGGroundReactions.h>
#include <models/FGAccelerations.h>
#include <models/FGAuxiliary.h>
#include <models/FGAtmosphere.h>
#include <models/FGAircraft.h>
#include <models/FGFCS.h>
#include <models/atmosphere/FGWinds.h>
#include <models/propulsion/FGThruster.h>
#include <models/propulsion/FGPropeller.h>
#include "nps_autopilot.h"
#include "nps_fdm.h"
#include "math/pprz_geodetic.h"
#include "math/pprz_geodetic_double.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_wmm2020.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
+ Include dependency graph for nps_fdm_jsbsim.cpp:

Go to the source code of this file.

Macros

#define MetersOfFeet(_f)   ((_f)/3.2808399)
 Macro to convert from feet to metres. More...
 
#define FeetOfMeters(_m)   ((_m)*3.2808399)
 
#define PascalOfPsf(_p)   ((_p) * 47.8802588889)
 
#define CelsiusOfRankine(_r)   (((_r) - 491.67) / 1.8)
 
#define JSBSIM_PATH(_x)   _x
 Macro to build file path according to the lib version. More...
 
#define NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES   FALSE
 
#define NPS_JSBSIM_MODEL   AIRFRAME_NAME
 Name of the JSBSim model. More...
 
#define NPS_JSBSIM_PITCH_TRIM   0.0
 Trim values for the airframe. More...
 
#define NPS_JSBSIM_ROLL_TRIM   0.0
 
#define NPS_JSBSIM_YAW_TRIM   0.0
 
#define DEG2RAD   0.017
 Control surface deflections for visualisation. More...
 
#define NPS_JSBSIM_ELEVATOR_MAX_RAD   (20.0*DEG2RAD)
 
#define NPS_JSBSIM_AILERON_MAX_RAD   (20.0*DEG2RAD)
 
#define NPS_JSBSIM_RUDDER_MAX_RAD   (20.0*DEG2RAD)
 
#define NPS_JSBSIM_FLAP_MAX_RAD   (20.0*DEG2RAD)
 
#define MIN_DT   (1.0/10240.0)
 Minimum JSBSim timestep Around 1/10000 seems to be good for ground impacts. More...
 

Functions

static void feed_jsbsim (double *commands, int commands_nb)
 Feed JSBSim with the latest actuator commands. More...
 
static void fetch_state (void)
 Populates the NPS fdm struct after a simulation step. More...
 
static int check_for_nan (void)
 Checks NpsFdm struct for NaNs. More...
 
static void jsbsimvec_to_vec (DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector)
 Convert JSBSim vector format and struct to NPS vector format and struct. More...
 
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. More...
 
static void jsbsimquat_to_quat (DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat)
 Convert JSBSim quaternion struct to NPS quaternion struct. More...
 
static void jsbsimvec_to_rate (DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector)
 Convert JSBSim rates vector struct to NPS rates struct. More...
 
static void llh_from_jsbsim (LlaCoor_d *fdm_lla, FGPropagate *propagate)
 Convert JSBSim location to NPS LLH. More...
 
static void lla_from_jsbsim_geodetic (LlaCoor_d *fdm_lla, FGPropagate *propagate)
 Convert JSBSim location to NPS LLA. More...
 
static void lla_from_jsbsim_geocentric (LlaCoor_d *fdm_lla, FGPropagate *propagate)
 Convert JSBSim location to NPS LLA. More...
 
static void init_jsbsim (double dt)
 Initializes JSBSim. More...
 
static void init_ltp (void)
 Initialize the ltp from the JSBSim location. More...
 
void nps_fdm_init (double dt)
 NPS FDM rover init. More...
 
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 correspond to the 'sim' target. More...
 
void nps_fdm_set_wind (double speed, double dir)
 
void nps_fdm_set_wind_ned (double wind_north, double wind_east, double wind_down)
 
void nps_fdm_set_turbulence (double wind_speed, int turbulence_severity)
 
void nps_fdm_set_temperature (double temp, double h)
 Set temperature in degrees Celcius at given height h above MSL. More...
 

Variables

struct NpsFdm fdm
 Holds all necessary NPS FDM state information. More...
 
static FGFDMExec * FDMExec
 The JSBSim executive object. More...
 
static struct LtpDef_d ltpdef
 
static struct EcefCoor_d offset
 
double vehicle_radius_max
 The largest distance between vehicle CG and contact point. More...
 
double min_dt
 Timestep used for higher fidelity near the ground. More...
 

Macro Definition Documentation

◆ CelsiusOfRankine

#define CelsiusOfRankine (   _r)    (((_r) - 491.67) / 1.8)

Definition at line 74 of file nps_fdm_jsbsim.cpp.

◆ DEG2RAD

#define DEG2RAD   0.017

Control surface deflections for visualisation.

Definition at line 126 of file nps_fdm_jsbsim.cpp.

◆ FeetOfMeters

#define FeetOfMeters (   _m)    ((_m)*3.2808399)

Definition at line 71 of file nps_fdm_jsbsim.cpp.

◆ JSBSIM_PATH

#define JSBSIM_PATH (   _x)    _x

Macro to build file path according to the lib version.

Definition at line 80 of file nps_fdm_jsbsim.cpp.

◆ MetersOfFeet

#define MetersOfFeet (   _f)    ((_f)/3.2808399)

Macro to convert from feet to metres.

Definition at line 70 of file nps_fdm_jsbsim.cpp.

◆ MIN_DT

#define MIN_DT   (1.0/10240.0)

Minimum JSBSim timestep Around 1/10000 seems to be good for ground impacts.

Definition at line 147 of file nps_fdm_jsbsim.cpp.

◆ NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES

#define NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES   FALSE

Definition at line 93 of file nps_fdm_jsbsim.cpp.

◆ NPS_JSBSIM_AILERON_MAX_RAD

#define NPS_JSBSIM_AILERON_MAX_RAD   (20.0*DEG2RAD)

Definition at line 133 of file nps_fdm_jsbsim.cpp.

◆ NPS_JSBSIM_ELEVATOR_MAX_RAD

#define NPS_JSBSIM_ELEVATOR_MAX_RAD   (20.0*DEG2RAD)

Definition at line 129 of file nps_fdm_jsbsim.cpp.

◆ NPS_JSBSIM_FLAP_MAX_RAD

#define NPS_JSBSIM_FLAP_MAX_RAD   (20.0*DEG2RAD)

Definition at line 141 of file nps_fdm_jsbsim.cpp.

◆ NPS_JSBSIM_MODEL

#define NPS_JSBSIM_MODEL   AIRFRAME_NAME

Name of the JSBSim model.

Defaults to the AIRFRAME_NAME

Definition at line 100 of file nps_fdm_jsbsim.cpp.

◆ NPS_JSBSIM_PITCH_TRIM

#define NPS_JSBSIM_PITCH_TRIM   0.0

Trim values for the airframe.

Definition at line 112 of file nps_fdm_jsbsim.cpp.

◆ NPS_JSBSIM_ROLL_TRIM

#define NPS_JSBSIM_ROLL_TRIM   0.0

Definition at line 116 of file nps_fdm_jsbsim.cpp.

◆ NPS_JSBSIM_RUDDER_MAX_RAD

#define NPS_JSBSIM_RUDDER_MAX_RAD   (20.0*DEG2RAD)

Definition at line 137 of file nps_fdm_jsbsim.cpp.

◆ NPS_JSBSIM_YAW_TRIM

#define NPS_JSBSIM_YAW_TRIM   0.0

Definition at line 120 of file nps_fdm_jsbsim.cpp.

◆ PascalOfPsf

#define PascalOfPsf (   _p)    ((_p) * 47.8802588889)

Definition at line 73 of file nps_fdm_jsbsim.cpp.

Function Documentation

◆ check_for_nan()

◆ feed_jsbsim()

static void feed_jsbsim ( double *  commands,
int  commands_nb 
)
static

Feed JSBSim with the latest actuator commands.

Parameters
commandsPointer to array of doubles holding actuator commands
commands_nbNumber of commands (length of array)

Definition at line 318 of file nps_fdm_jsbsim.cpp.

References commands, FDMExec, NPS_JSBSIM_PITCH_TRIM, NPS_JSBSIM_ROLL_TRIM, and NPS_JSBSIM_YAW_TRIM.

Referenced by init_jsbsim(), and nps_fdm_run_step().

+ Here is the caller graph for this function:

◆ fetch_state()

static void fetch_state ( void  )
static

Populates the NPS fdm struct after a simulation step.

Definition at line 379 of file nps_fdm_jsbsim.cpp.

References NpsFdm::agl, NpsFdm::airspeed, NpsFdm::aoa, NpsFdm::body_accel, NpsFdm::body_ecef_accel, NpsFdm::body_ecef_rotaccel, NpsFdm::body_ecef_rotvel, NpsFdm::body_ecef_vel, NpsFdm::body_inertial_accel, NpsFdm::body_inertial_rotaccel, NpsFdm::body_inertial_rotvel, CelsiusOfRankine, double_eulers_of_quat(), NpsFdm::dynamic_pressure, NpsFdm::ecef_ecef_accel, NpsFdm::ecef_ecef_vel, NpsFdm::ecef_pos, NpsFdm::elevator, NpsFdm::eng_state, EULERS_COPY, fdm, FDMExec, NpsFdm::flap, NpsFdm::hmsl, jsbsimloc_to_loc(), jsbsimquat_to_quat(), jsbsimvec_to_rate(), jsbsimvec_to_vec(), NpsFdm::left_aileron, lla_from_jsbsim_geocentric(), lla_from_jsbsim_geodetic(), lla_of_ecef_d(), NpsFdm::lla_pos, NpsFdm::lla_pos_geoc, NpsFdm::lla_pos_geod, NpsFdm::lla_pos_pprz, llh_from_jsbsim(), NpsFdm::ltp_ecef_accel, NpsFdm::ltp_ecef_vel, NpsFdm::ltp_to_body_eulers, NpsFdm::ltp_to_body_quat, ltpdef, NpsFdm::ltpprz_ecef_accel, NpsFdm::ltpprz_ecef_vel, NpsFdm::ltpprz_pos, NpsFdm::ltpprz_to_body_eulers, NpsFdm::ltpprz_to_body_quat, MetersOfFeet, ned_of_ecef_point_d(), ned_of_ecef_vect_d(), NPS_JSBSIM_AILERON_MAX_RAD, NPS_JSBSIM_ELEVATOR_MAX_RAD, NPS_JSBSIM_FLAP_MAX_RAD, NPS_JSBSIM_RUDDER_MAX_RAD, NpsFdm::num_engines, NpsFdm::on_ground, PascalOfPsf, NpsFdm::pressure, NpsFdm::pressure_sl, QUAT_COPY, NpsFdm::right_aileron, NpsFdm::rpm, NpsFdm::rudder, NpsFdm::sideslip, NpsFdm::temperature, NpsFdm::time, NpsFdm::total_pressure, NpsFdm::wind, DoubleVect3::x, EcefCoor_d::x, NedCoor_d::x, DoubleVect3::y, EcefCoor_d::y, NedCoor_d::y, DoubleVect3::z, EcefCoor_d::z, and NedCoor_d::z.

Referenced by nps_fdm_init(), and nps_fdm_run_step().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ init_jsbsim()

static void init_jsbsim ( double  dt)
static

Initializes JSBSim.

Sets up the JSBSim executive and loads initial conditions Exits NPS with -1 if models or ICs fail to load

Parameters
dtThe desired simulation timestep
Warning
Needs PAPARAZZI_HOME defined to find the config files

Definition at line 544 of file nps_fdm_jsbsim.cpp.

References LlaCoor_d::alt, ecef_of_lla_d(), FDMExec, feed_jsbsim(), FeetOfMeters, gc_of_gd_lat_d(), JSBSIM_PATH, LlaCoor_d::lat, llh_from_jsbsim(), LlaCoor_d::lon, MetersOfFeet, NPS_COMMANDS_NB, NPS_JSBSIM_MODEL, offset, VECT3_DIFF, and vehicle_radius_max.

Referenced by nps_fdm_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ init_ltp()

static void init_ltp ( void  )
static

Initialize the ltp from the JSBSim location.

@FIXME properly get current time

Definition at line 679 of file nps_fdm_jsbsim.cpp.

References LlaCoor_d::alt, double_vect3_normalize(), NpsFdm::ecef_pos, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3, extrapsh(), fdm, FDMExec, GEO_EPOCH, IEXT, jsbsimloc_to_loc(), LlaCoor_d::lat, NpsFdm::lla_pos, llh_from_jsbsim(), LlaCoor_d::lon, ltp_def_from_ecef_d(), NpsFdm::ltp_g, NpsFdm::ltp_h, ltpdef, mag_calc(), MAXCOEFF, NMAX_1, NMAX_2, PRINT_CONFIG_MSG(), DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

Referenced by nps_fdm_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ jsbsimloc_to_loc()

static void jsbsimloc_to_loc ( EcefCoor_d fdm_location,
const FGLocation *  jsb_location 
)
static

Convert JSBSim location format and struct to NPS location format and struct.

JSBSim is in feet by default, NPS in metres

Parameters
fdm_locationPointer to EcefCoor_d struct
jsb_locationPointer to FGLocation struct

Definition at line 737 of file nps_fdm_jsbsim.cpp.

References MetersOfFeet, offset, VECT3_ADD, EcefCoor_d::x, EcefCoor_d::y, and EcefCoor_d::z.

Referenced by fetch_state(), and init_ltp().

+ Here is the caller graph for this function:

◆ jsbsimquat_to_quat()

static void jsbsimquat_to_quat ( DoubleQuat fdm_quat,
const FGQuaternion *  jsb_quat 
)
static

Convert JSBSim quaternion struct to NPS quaternion struct.

Parameters
fdm_quatPointer to DoubleQuat struct
jsb_quatPointer to FGQuaternion struct

Definition at line 770 of file nps_fdm_jsbsim.cpp.

References DoubleQuat::qi, DoubleQuat::qx, DoubleQuat::qy, and DoubleQuat::qz.

Referenced by fetch_state().

+ Here is the caller graph for this function:

◆ jsbsimvec_to_rate()

static void jsbsimvec_to_rate ( DoubleRates fdm_rate,
const FGColumnVector3 *  jsb_vector 
)
static

Convert JSBSim rates vector struct to NPS rates struct.

Parameters
fdm_ratePointer to DoubleRates struct
jsb_vectorPointer to FGColumnVector3 struct

Definition at line 786 of file nps_fdm_jsbsim.cpp.

References DoubleRates::p, DoubleRates::q, and DoubleRates::r.

Referenced by fetch_state().

+ Here is the caller graph for this function:

◆ jsbsimvec_to_vec()

static void jsbsimvec_to_vec ( DoubleVect3 fdm_vector,
const FGColumnVector3 *  jsb_vector 
)
static

Convert JSBSim vector format and struct to NPS vector format and struct.

JSBSim is in feet by default, NPS in metres

Parameters
fdm_vectorPointer to DoubleVect3 struct
jsb_vectorPointer to FGColumnVector3 struct

Definition at line 755 of file nps_fdm_jsbsim.cpp.

References MetersOfFeet, DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

Referenced by fetch_state().

+ Here is the caller graph for this function:

◆ lla_from_jsbsim_geocentric()

void lla_from_jsbsim_geocentric ( LlaCoor_d fdm_lla,
FGPropagate *  propagate 
)
static

Convert JSBSim location to NPS LLA.

Gets geocentric latitude, longitude and geocentric radius

Parameters
fdm_llaPointer to LlaCoor_d struct
propagatePointer to JSBSim FGPropagate object

Definition at line 823 of file nps_fdm_jsbsim.cpp.

References LlaCoor_d::alt, LlaCoor_d::lat, LlaCoor_d::lon, and MetersOfFeet.

Referenced by fetch_state().

+ Here is the caller graph for this function:

◆ lla_from_jsbsim_geodetic()

void lla_from_jsbsim_geodetic ( LlaCoor_d fdm_lla,
FGPropagate *  propagate 
)
static

Convert JSBSim location to NPS LLA.

Gets geodetic latitude, longitude and geodetic altitude in metres

Parameters
fdm_llaPointer to LlaCoor_d struct
propagatePointer to JSBSim FGPropagate object

Definition at line 840 of file nps_fdm_jsbsim.cpp.

References LlaCoor_d::alt, LlaCoor_d::lat, LlaCoor_d::lon, and MetersOfFeet.

Referenced by fetch_state().

+ Here is the caller graph for this function:

◆ llh_from_jsbsim()

void llh_from_jsbsim ( LlaCoor_d fdm_lla,
FGPropagate *  propagate 
)
static

Convert JSBSim location to NPS LLH.

Gets geodetic latitude, longitude and height above sea level in metres

Parameters
fdm_llaPointer to LlaCoor_d struct
propagatePointer to JSBSim FGPropagate object

Definition at line 803 of file nps_fdm_jsbsim.cpp.

References LlaCoor_d::alt, LlaCoor_d::lat, and LlaCoor_d::lon.

Referenced by fetch_state(), init_jsbsim(), and init_ltp().

+ Here is the caller graph for this function:

◆ nps_fdm_init()

void nps_fdm_init ( double  dt)

NPS FDM rover init.

NPS FDM rover init.

Parameters
dt

Definition at line 186 of file nps_fdm_jsbsim.cpp.

References NpsFdm::curr_dt, fdm, FDMExec, fetch_state(), NpsFdm::init_dt, init_jsbsim(), init_ltp(), MIN_DT, min_dt, NpsFdm::nan_count, offset, and VECT3_ASSIGN.

+ Here is the call graph for this function:

◆ nps_fdm_run_step()

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 correspond to the 'sim' target.

Johnson, E.N., Fontaine, S.G., and Kahn, A.D., “Minimum Complexity Uninhabited Air Vehicle Guidance And Flight Control System,” Proceedings of the 20th Digital Avionics Systems Conference, 2001. http://www.ae.gatech.edu/~ejohnson/JohnsonFontaineKahn.pdf

Johnson, E.N. and Fontaine, S.G., “Use Of Flight Simulation To Complement Flight Testing Of Low-Cost UAVs,” Proceedings of the AIAA Modeling and Simulation Technology Conference, 2001. http://www.ae.gatech.edu/~ejohnson/AIAA%202001-4059.pdf

Minimum complexity flight dynamic model In legacy Paparazzi simulator, was implemented in OCaml and correspond to the 'sim' target.

Parameters
launch
act_commands
commands_nb

Definition at line 217 of file nps_fdm_jsbsim.cpp.

References NpsFdm::agl, check_for_nan(), commands, NpsFdm::curr_dt, FALSE, fdm, FDMExec, feed_jsbsim(), FeetOfMeters, fetch_state(), NpsFdm::init_dt, NpsFdm::ltp_ecef_vel, min_dt, NpsFdm::nan_count, NpsFdm::time, TRUE, vehicle_radius_max, and NedCoor_d::z.

+ Here is the call graph for this function:

◆ 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 at line 307 of file nps_fdm_jsbsim.cpp.

References FDMExec, and h().

+ Here is the call graph for this function:

◆ nps_fdm_set_turbulence()

void nps_fdm_set_turbulence ( double  wind_speed,
int  turbulence_severity 
)

Definition at line 299 of file nps_fdm_jsbsim.cpp.

References FDMExec, and FeetOfMeters.

◆ nps_fdm_set_wind()

void nps_fdm_set_wind ( double  speed,
double  dir 
)

Definition at line 285 of file nps_fdm_jsbsim.cpp.

References dir, FDMExec, and FeetOfMeters.

◆ nps_fdm_set_wind_ned()

void nps_fdm_set_wind_ned ( double  wind_north,
double  wind_east,
double  wind_down 
)

Definition at line 292 of file nps_fdm_jsbsim.cpp.

References FDMExec, and FeetOfMeters.

Variable Documentation

◆ fdm

struct NpsFdm fdm

Holds all necessary NPS FDM state information.

Definition at line 167 of file nps_fdm_jsbsim.cpp.

Referenced by check_for_nan(), fetch_state(), init_ltp(), nps_fdm_init(), and nps_fdm_run_step().

◆ FDMExec

◆ ltpdef

struct LtpDef_d ltpdef
static

Definition at line 173 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state(), and init_ltp().

◆ min_dt

double min_dt

Timestep used for higher fidelity near the ground.

Definition at line 184 of file nps_fdm_jsbsim.cpp.

Referenced by nps_fdm_init(), and nps_fdm_run_step().

◆ offset

struct EcefCoor_d offset
static

Definition at line 173 of file nps_fdm_jsbsim.cpp.

Referenced by init_jsbsim(), jsbsimloc_to_loc(), and nps_fdm_init().

◆ vehicle_radius_max

double vehicle_radius_max

The largest distance between vehicle CG and contact point.

Definition at line 181 of file nps_fdm_jsbsim.cpp.

Referenced by init_jsbsim(), and nps_fdm_run_step().