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_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)
	Initialize actuator dynamics, set unused fields in fdm. 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 113 of file nps_fdm_jsbsim.cpp.

#define FeetOfMeters

(

_m

)

((_m)*3.2808399)

Definition at line 71 of file nps_fdm_jsbsim.cpp.

Referenced by init_jsbsim(), nps_fdm_run_step(), nps_fdm_set_turbulence(), nps_fdm_set_wind(), and nps_fdm_set_wind_ned().

#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.

Referenced by init_jsbsim().

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

Referenced by nps_fdm_init().

#define NPS_JSBSIM_AILERON_MAX_RAD   (20.0*DEG2RAD)

Definition at line 120 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_ELEVATOR_MAX_RAD   (20.0*DEG2RAD)

Definition at line 116 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_FLAP_MAX_RAD   (20.0*DEG2RAD)

Definition at line 128 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 87 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 99 of file nps_fdm_jsbsim.cpp.

Referenced by feed_jsbsim().

#define NPS_JSBSIM_ROLL_TRIM   0.0

Definition at line 103 of file nps_fdm_jsbsim.cpp.

Referenced by feed_jsbsim().

#define NPS_JSBSIM_RUDDER_MAX_RAD   (20.0*DEG2RAD)

Definition at line 124 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_YAW_TRIM   0.0

Definition at line 107 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 int check_for_nan ( void  )

static

Checks NpsFdm struct for NaNs.

Increments the NaN count on each new NaN

Returns

Count of new NaNs. 0 for no new NaNs.

Definition at line 846 of file nps_fdm_jsbsim.cpp.

References LlaCoor_d::alt, NpsFdm::body_ecef_accel, NpsFdm::body_ecef_rotaccel, NpsFdm::body_ecef_rotvel, NpsFdm::body_ecef_vel, NpsFdm::ecef_ecef_accel, NpsFdm::ecef_ecef_vel, NpsFdm::ecef_pos, NpsFdm::ecef_to_body_quat, fdm, NpsFdm::hmsl, LlaCoor_d::lat, NpsFdm::lla_pos, LlaCoor_d::lon, NpsFdm::ltp_ecef_accel, NpsFdm::ltp_ecef_vel, NpsFdm::ltp_g, NpsFdm::ltp_h, NpsFdm::ltp_to_body_eulers, NpsFdm::ltp_to_body_quat, NpsFdm::ltpprz_ecef_accel, NpsFdm::ltpprz_ecef_vel, NpsFdm::ltpprz_pos, NpsFdm::ltpprz_to_body_eulers, NpsFdm::ltpprz_to_body_quat, NpsFdm::nan_count, DoubleRates::p, DoubleEulers::phi, DoubleEulers::psi, DoubleRates::q, DoubleQuat::qi, DoubleQuat::qx, DoubleQuat::qy, DoubleQuat::qz, DoubleRates::r, DoubleEulers::theta, 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_run_step().

Here is the caller graph for this function:

static void feed_jsbsim ( double *  commands, int  commands_nb  )

static

Feed JSBSim with the latest actuator commands.

Parameters

commands	Pointer to array of doubles holding actuator commands
commands_nb	Number of commands (length of array)

Definition at line 305 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 364 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

dt	The desired simulation timestep

Warning

Needs PAPARAZZI_HOME defined to find the config files

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

static void init_ltp ( void  )

static

Initialize the ltp from the JSBSim location.

properly get current time

Definition at line 663 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_location	Pointer to EcefCoor_d struct
jsb_location	Pointer to FGLocation struct

Definition at line 721 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_quat	Pointer to DoubleQuat struct
jsb_quat	Pointer to FGQuaternion struct

Definition at line 754 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_rate	Pointer to DoubleRates struct
jsb_vector	Pointer to FGColumnVector3 struct

Definition at line 770 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_vector	Pointer to DoubleVect3 struct
jsb_vector	Pointer to FGColumnVector3 struct

Definition at line 739 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_lla	Pointer to LlaCoor_d struct
propagate	Pointer to JSBSim FGPropagate object

Definition at line 807 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_lla	Pointer to LlaCoor_d struct
propagate	Pointer to JSBSim FGPropagate object

Definition at line 824 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_lla	Pointer to LlaCoor_d struct
propagate	Pointer to JSBSim FGPropagate object

Definition at line 787 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

)

Initialize actuator dynamics, set unused fields in fdm.

Parameters

dt

Definition at line 173 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
act_commands
commands_nb

Definition at line 204 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 294 of file nps_fdm_jsbsim.cpp.

References FDMExec.

void nps_fdm_set_turbulence	(	double	wind_speed,
		int	turbulence_severity
	)

Definition at line 286 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 272 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 279 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 NpsFdm fdm

Holds all necessary NPS FDM state information.

Definition at line 157 of file nps_fdm_jsbsim.cpp.

Referenced by check_for_nan(), fetch_state(), gps_feed_value(), init_ltp(), nps_fdm_init(), nps_fdm_run_step(), nps_flightgear_send(), nps_flightgear_send_fdm(), nps_ins_data_loop(), nps_ivy_send_WORLD_ENV_REQ(), nps_main_display(), nps_main_loop(), nps_sensor_accel_run_step(), nps_sensor_airspeed_run_step(), nps_sensor_aoa_run_step(), nps_sensor_baro_run_step(), nps_sensor_gps_run_step(), nps_sensor_gyro_run_step(), nps_sensor_mag_run_step(), nps_sensor_sideslip_run_step(), nps_sensor_sonar_run_step(), nps_sensor_temperature_run_step(), sim_overwrite_ahrs(), and sim_overwrite_ins().

FGFDMExec* FDMExec

static

The JSBSim executive object.

Definition at line 160 of file nps_fdm_jsbsim.cpp.

Referenced by feed_jsbsim(), fetch_state(), init_jsbsim(), init_ltp(), nps_fdm_init(), nps_fdm_run_step(), nps_fdm_set_temperature(), nps_fdm_set_turbulence(), nps_fdm_set_wind(), and nps_fdm_set_wind_ned().

struct LtpDef_d ltpdef

static

Definition at line 162 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 171 of file nps_fdm_jsbsim.cpp.

Referenced by nps_fdm_init(), and nps_fdm_run_step().

struct EcefCoor_d offset

static

Definition at line 165 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 168 of file nps_fdm_jsbsim.cpp.

Referenced by init_jsbsim(), and nps_fdm_run_step().