#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/FGFCS.h>
#include <models/atmosphere/FGWinds.h>
#include <models/propulsion/FGThruster.h>
#include <models/propulsion/FGPropeller.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:

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	feed_jsbsim (double throttle, double aileron, double elevator, double rudder)

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)

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

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

Referenced by fetch_state().

#define DEG2RAD 0.017

Control surface deflections for visualisation.

Definition at line 105 of file nps_fdm_jsbsim.cpp.

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

Definition at line 70 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 MetersOfFeet ( _f ) ((_f)/3.2808399)

Macro to convert from feet to metres.

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

Referenced by nps_fdm_init().

#define NPS_JSBSIM_AILERON_MAX_RAD (20.0*DEG2RAD)

Definition at line 112 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_ELEVATOR_MAX_RAD (20.0*DEG2RAD)

Definition at line 108 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_FLAP_MAX_RAD (20.0*DEG2RAD)

Definition at line 120 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 79 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 91 of file nps_fdm_jsbsim.cpp.

Referenced by feed_jsbsim().

#define NPS_JSBSIM_ROLL_TRIM 0.0

Definition at line 95 of file nps_fdm_jsbsim.cpp.

Referenced by feed_jsbsim().

#define NPS_JSBSIM_RUDDER_MAX_RAD (20.0*DEG2RAD)

Definition at line 116 of file nps_fdm_jsbsim.cpp.

Referenced by fetch_state().

#define NPS_JSBSIM_YAW_TRIM 0.0

Definition at line 99 of file nps_fdm_jsbsim.cpp.

Referenced by feed_jsbsim().

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

Definition at line 72 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 820 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 292 of file nps_fdm_jsbsim.cpp.

References FDMExec.

Referenced by init_jsbsim(), and nps_fdm_run_step().

Here is the caller graph for this function:

static void feed_jsbsim	(	double	throttle,
		double	aileron,
		double	elevator,
		double	rudder
	)

static

Definition at line 315 of file nps_fdm_jsbsim.cpp.

References FDMExec, NPS_JSBSIM_PITCH_TRIM, NPS_JSBSIM_ROLL_TRIM, and NPS_JSBSIM_YAW_TRIM.

static void fetch_state ( void )

static

Populates the NPS fdm struct after a simulation step.

Definition at line 345 of file nps_fdm_jsbsim.cpp.

References NpsFdm::agl, NpsFdm::airspeed, 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::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 504 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_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 637 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 695 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 728 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 744 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 713 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 781 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 798 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 761 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 )

Definition at line 164 of file nps_fdm_jsbsim.cpp.

References NpsFdm::curr_dt, 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
	)

Definition at line 191 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, launch, 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 281 of file nps_fdm_jsbsim.cpp.

References FDMExec.

void nps_fdm_set_turbulence	(	double	wind_speed,
		int	turbulence_severity
	)

Definition at line 273 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 259 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 266 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 148 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_ivy_display(), nps_ivy_send_WORLD_ENV_REQ(), nps_sensor_accel_run_step(), nps_sensor_airspeed_run_step(), nps_sensor_baro_run_step(), nps_sensor_gps_run_step(), nps_sensor_gyro_run_step(), nps_sensor_mag_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 151 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 153 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 162 of file nps_fdm_jsbsim.cpp.

Referenced by nps_fdm_init(), and nps_fdm_run_step().

struct EcefCoor_d offset

static

Definition at line 156 of file nps_fdm_jsbsim.cpp.

Referenced by init_jsbsim(), jsbsimloc_to_loc(), nps_fdm_init(), offset_commands(), parse_xsens700_msg(), parse_xsens_msg(), rtp_frame_send(), and w5100_send().

double vehicle_radius_max

The largest distance between vehicle CG and contact point.

Definition at line 159 of file nps_fdm_jsbsim.cpp.

Referenced by init_jsbsim(), and nps_fdm_run_step().

Macros

Functions

Variables

Macro Definition Documentation

Function Documentation

Variable Documentation