Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_fdm_jsbsim.cpp 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_wmm2015.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 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)
 Set JSBsim specific fields that are not used for Gazebo. More...
 
void nps_fdm_run_step (bool launch, double *commands, int commands_nb)
 Update the simulation state. 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

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

Definition at line 74 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define DEG2RAD   0.017

Control surface deflections for visualisation.

Definition at line 106 of file nps_fdm_jsbsim.cpp.

#define FeetOfMeters (   _m)    ((_m)*3.2808399)
#define MetersOfFeet (   _f)    ((_f)/3.2808399)

Macro to convert from feet to metres.

Definition at line 70 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state(), init_jsbsim(), jsbsimloc_to_loc(), jsbsimvec_to_vec(), lla_from_jsbsim_geocentric(), and lla_from_jsbsim_geodetic().

#define MIN_DT   (1.0/10240.0)

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

Definition at line 127 of file nps_fdm_jsbsim.cpp.

Referenced by nps_fdm_init().

#define NPS_JSBSIM_AILERON_MAX_RAD   (20.0*DEG2RAD)

Definition at line 113 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_ELEVATOR_MAX_RAD   (20.0*DEG2RAD)

Definition at line 109 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_FLAP_MAX_RAD   (20.0*DEG2RAD)

Definition at line 121 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_MODEL   AIRFRAME_NAME

Name of the JSBSim model.

Defaults to the AIRFRAME_NAME

Definition at line 80 of file nps_fdm_jsbsim.cpp.

Referenced by init_jsbsim().

#define NPS_JSBSIM_PITCH_TRIM   0.0

Trim values for the airframe.

Definition at line 92 of file nps_fdm_jsbsim.cpp.

Referenced by feed_jsbsim().

#define NPS_JSBSIM_ROLL_TRIM   0.0

Definition at line 96 of file nps_fdm_jsbsim.cpp.

Referenced by feed_jsbsim().

#define NPS_JSBSIM_RUDDER_MAX_RAD   (20.0*DEG2RAD)

Definition at line 117 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_YAW_TRIM   0.0

Definition at line 100 of file nps_fdm_jsbsim.cpp.

Referenced by feed_jsbsim().

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

Definition at line 73 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

Function Documentation

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 294 of file nps_fdm_jsbsim.cpp.

References 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:

static void fetch_state ( void  )
static

Populates the NPS fdm struct after a simulation step.

Definition at line 353 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:

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 518 of file nps_fdm_jsbsim.cpp.

References LlaCoor_d::alt, ecef_of_lla_d(), FDMExec, feed_jsbsim(), FeetOfMeters, gc_of_gd_lat_d(), LlaCoor_d::lat, llh_from_jsbsim(), LlaCoor_d::lon, MetersOfFeet, NPS_COMMANDS_NB, NPS_JSBSIM_MODEL, offset, radius, 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:

static void init_ltp ( void  )
static

Initialize the ltp from the JSBSim location.

properly get current time

Definition at line 652 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:

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 710 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:

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 743 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:

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 759 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:

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 728 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:

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 796 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:

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 813 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:

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 776 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:

void nps_fdm_init ( double  dt)

Set JSBsim specific fields that are not used for Gazebo.

Parameters
dt

Definition at line 166 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.

Referenced by nps_main_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nps_fdm_run_step ( bool  launch,
double *  commands,
int  commands_nb 
)

Update the simulation state.

Parameters
launch
commands
commands_nb

Definition at line 193 of file nps_fdm_jsbsim.cpp.

References NpsFdm::agl, check_for_nan(), 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.

Referenced by nps_main_run_sim_step().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nps_fdm_set_temperature ( double  temp,
double  h 
)

Set temperature in degrees Celcius at given height h above MSL.

Definition at line 283 of file nps_fdm_jsbsim.cpp.

References FDMExec.

void nps_fdm_set_turbulence ( double  wind_speed,
int  turbulence_severity 
)

Definition at line 275 of file nps_fdm_jsbsim.cpp.

References FDMExec, and FeetOfMeters.

Referenced by nps_atmosphere_update().

+ Here is the caller graph for this function:

void nps_fdm_set_wind ( double  speed,
double  dir 
)

Definition at line 261 of file nps_fdm_jsbsim.cpp.

References FDMExec, and FeetOfMeters.

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

Definition at line 268 of file nps_fdm_jsbsim.cpp.

References FDMExec, and FeetOfMeters.

Referenced by nps_atmosphere_update().

+ Here is the caller graph for this function:

Variable Documentation

struct LtpDef_d ltpdef
static

Definition at line 155 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state(), and init_ltp().

double min_dt

Timestep used for higher fidelity near the ground.

Definition at line 164 of file nps_fdm_jsbsim.cpp.

Referenced by nps_fdm_init(), and nps_fdm_run_step().

struct EcefCoor_d offset
static

Definition at line 158 of file nps_fdm_jsbsim.cpp.

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

double vehicle_radius_max

The largest distance between vehicle CG and contact point.

Definition at line 161 of file nps_fdm_jsbsim.cpp.

Referenced by init_jsbsim(), and nps_fdm_run_step().