Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#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"
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... | |
#define CelsiusOfRankine | ( | _r | ) | (((_r) - 491.67) / 1.8) |
Definition at line 74 of file nps_fdm_jsbsim.cpp.
#define DEG2RAD 0.017 |
Control surface deflections for visualisation.
Definition at line 126 of file nps_fdm_jsbsim.cpp.
#define FeetOfMeters | ( | _m | ) | ((_m)*3.2808399) |
Definition at line 71 of file nps_fdm_jsbsim.cpp.
#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.
#define MetersOfFeet | ( | _f | ) | ((_f)/3.2808399) |
Macro to convert from feet to metres.
Definition at line 70 of file nps_fdm_jsbsim.cpp.
#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.
#define NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES FALSE |
Definition at line 93 of file nps_fdm_jsbsim.cpp.
#define NPS_JSBSIM_AILERON_MAX_RAD (20.0*DEG2RAD) |
Definition at line 133 of file nps_fdm_jsbsim.cpp.
#define NPS_JSBSIM_ELEVATOR_MAX_RAD (20.0*DEG2RAD) |
Definition at line 129 of file nps_fdm_jsbsim.cpp.
#define NPS_JSBSIM_FLAP_MAX_RAD (20.0*DEG2RAD) |
Definition at line 141 of file nps_fdm_jsbsim.cpp.
#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.
#define NPS_JSBSIM_PITCH_TRIM 0.0 |
Trim values for the airframe.
Definition at line 112 of file nps_fdm_jsbsim.cpp.
#define NPS_JSBSIM_ROLL_TRIM 0.0 |
Definition at line 116 of file nps_fdm_jsbsim.cpp.
#define NPS_JSBSIM_RUDDER_MAX_RAD (20.0*DEG2RAD) |
Definition at line 137 of file nps_fdm_jsbsim.cpp.
#define NPS_JSBSIM_YAW_TRIM 0.0 |
Definition at line 120 of file nps_fdm_jsbsim.cpp.
#define PascalOfPsf | ( | _p | ) | ((_p) * 47.8802588889) |
Definition at line 73 of file nps_fdm_jsbsim.cpp.
|
static |
Checks NpsFdm struct for NaNs.
Increments the NaN count on each new NaN
Definition at line 862 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().
|
static |
Feed JSBSim with the latest actuator commands.
commands | Pointer to array of doubles holding actuator commands |
commands_nb | Number 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().
|
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().
|
static |
Initializes JSBSim.
Sets up the JSBSim executive and loads initial conditions Exits NPS with -1 if models or ICs fail to load
dt | The desired simulation timestep |
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().
|
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().
|
static |
Convert JSBSim location format and struct to NPS location format and struct.
JSBSim is in feet by default, NPS in metres
fdm_location | Pointer to EcefCoor_d struct |
jsb_location | Pointer 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().
|
static |
Convert JSBSim quaternion struct to NPS quaternion struct.
fdm_quat | Pointer to DoubleQuat struct |
jsb_quat | Pointer 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().
|
static |
Convert JSBSim rates vector struct to NPS rates struct.
fdm_rate | Pointer to DoubleRates struct |
jsb_vector | Pointer 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().
|
static |
Convert JSBSim vector format and struct to NPS vector format and struct.
JSBSim is in feet by default, NPS in metres
fdm_vector | Pointer to DoubleVect3 struct |
jsb_vector | Pointer 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().
|
static |
Convert JSBSim location to NPS LLA.
Gets geocentric latitude, longitude and geocentric radius
fdm_lla | Pointer to LlaCoor_d struct |
propagate | Pointer 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().
|
static |
Convert JSBSim location to NPS LLA.
Gets geodetic latitude, longitude and geodetic altitude in metres
fdm_lla | Pointer to LlaCoor_d struct |
propagate | Pointer 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().
|
static |
Convert JSBSim location to NPS LLH.
Gets geodetic latitude, longitude and height above sea level in metres
fdm_lla | Pointer to LlaCoor_d struct |
propagate | Pointer 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().
void nps_fdm_init | ( | double | dt | ) |
NPS FDM rover init.
NPS FDM rover init.
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.
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.
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.
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.
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.
void nps_fdm_set_wind | ( | double | speed, |
double | dir | ||
) |
Definition at line 285 of file nps_fdm_jsbsim.cpp.
References dir, FDMExec, and FeetOfMeters.
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.
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().
|
static |
The JSBSim executive object.
Definition at line 173 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().
|
static |
Definition at line 173 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 184 of file nps_fdm_jsbsim.cpp.
Referenced by nps_fdm_init(), and nps_fdm_run_step().
|
static |
Definition at line 173 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 181 of file nps_fdm_jsbsim.cpp.
Referenced by init_jsbsim(), and nps_fdm_run_step().