Flight Dynamics Model (FDM) for NPS using the new Gazebo (gz-sim). More...

#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <memory>
#include <csignal>
#include <chrono>
#include <functional>
#include <mutex>
#include <gz/sim/Server.hh>
#include <gz/sim/ServerConfig.hh>
#include <gz/sim/System.hh>
#include <gz/sim/Model.hh>
#include <gz/sim/Link.hh>
#include <gz/sim/World.hh>
#include <gz/sim/components/Gravity.hh>
#include <gz/sim/components/MagneticField.hh>
#include <gz/sim/components/SphericalCoordinates.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/Quaternion.hh>
#include <gz/math/Vector3.hh>
#include <gz/msgs/image.pb.h>
#include <gz/transport/Node.hh>
#include <sdf/sdf.hh>
#include "nps_fdm.h"
#include "nps_autopilot.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "autopilot.h"
#include "math/pprz_isa.h"
#include "math/pprz_algebra_double.h"
#include "math/pprz_geodetic_double.h"
#include "filters/low_pass_filter.h"
#include "filters/high_pass_filter.h"
#include "state.h"

Include dependency graph for nps_fdm_gz.cpp:

Go to the source code of this file.

Data Structures
struct	gazebo_actuators_t

class	PaparazziSystem
	gz-sim System that connects NPS with the simulation. More...

Macros
#define	NPS_GAZEBO_WORLD "empty.world"

#define	NPS_GAZEBO_AC_NAME "bebop"

Functions
static void	init_gazebo (void)
	Start an embedded gz-sim server.

static void	gazebo_read (void)
	Fill the fdm struct from the gz state.

static void	gazebo_write (double act_commands[], int commands_nb)
	Compute the motor thrusts and torques from the normalized commands [0, 1], with NPS_ACTUATOR_THRUSTS [N] and NPS_ACTUATOR_TORQUES [Nm] from the airframe.

static gz::math::Quaterniond	local_to_enu (void)

static struct NedCoor_d	ned_of_enu (const gz::math::Vector3d &enu)

static struct DoubleVect3	vect_ned_of_enu (const gz::math::Vector3d &enu)

static struct DoubleVect3	frd_of_flu (const gz::math::Vector3d &flu)

static struct DoubleRates	rates_frd_of_flu (const gz::math::Vector3d &flu)

static struct DoubleEulers	eulers_ned_of_enu (const gz::math::Quaterniond &q_enu)
	NED to FRD euler angles from the ENU to FLU attitude.

void	nps_fdm_init (double dt)
	NPS FDM rover init.

void	nps_fdm_run_init_step (void)

void	nps_fdm_run_step (bool launch, double *act_commands, int commands_nb)
	Minimum complexity flight dynamic model In legacy Paparazzi simulator, was implemented in OCaml and correspond to the 'sim' target.

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.

Variables
struct NpsFdm	fdm
	Holds all necessary NPS FDM state information.

static struct gazebo_actuators_t	gazebo_actuators = { NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { } }

struct {

gz::math::Pose3d	pose

gz::math::Vector3d	linear_vel

gz::math::Vector3d	angular_vel

gz::math::Vector3d	gravity

gz::math::Vector3d	magnetic_field

double	heading_offset
	heading of <spherical_coordinates> [rad]

double	sim_time
	[s]

bool	valid

}	gz_state
	Simulation state, written in PaparazziSystem::PostUpdate (gz local frame, FLU body)

struct {

double	thrusts [NPS_COMMANDS_NB]

double	torques [NPS_COMMANDS_NB]

}	gz_cmd
	Actuator commands in the body frame of each motor link, read in PaparazziSystem::PreUpdate.

static std::unique_ptr< gz::sim::Server >	server

static std::shared_ptr< PaparazziSystem >	pprz_system

Detailed Description

Flight Dynamics Model (FDM) for NPS using the new Gazebo (gz-sim).

Port of the Gazebo Classic FDM (nps_fdm_gazebo.cpp) to gz-sim (Harmonic and newer). It uses the same models, worlds and airframe defines.

gz-sim only gives access to the simulation state from a System plugin. The PaparazziSystem below is added to an embedded gz::sim::Server, which is stepped from nps_fdm_run_step with a blocking RunOnce call in the same thread, so the shared structs need no locking.

Frames: the worlds keep the Gazebo Classic workaround of a 180 degree heading offset in <spherical_coordinates> (Classic used a WSU frame). With that offset the local x, y, z axes of a world are East, North, Up, as in Gazebo Classic. The heading offset is applied here directly, so the result does not depend on the (changing) frame conventions of gz-math.

Cameras (NPS_SIMULATE_VIDEO) are rendered by the gz Sensors system, which NPS adds to the world. Their images are received over gz-transport and passed to the video thread as with Gazebo Classic.

Not ported yet: the laser range array (NPS_SIMULATE_LASER_RANGE_ARRAY) and the sonar.

Definition in file nps_fdm_gz.cpp.

Data Structure Documentation

◆ gazebo_actuators_t

struct gazebo_actuators_t

Definition at line 114 of file nps_fdm_gazebo.cpp.

Collaboration diagram for gazebo_actuators_t:

Data Fields
struct FirstOrderHighPass	highpass[NPS_COMMANDS_NB]
struct FirstOrderLowPass	lowpass[NPS_COMMANDS_NB]
double	max_ang_momentum[NPS_COMMANDS_NB]
string	names[NPS_COMMANDS_NB]
string	names[NPS_COMMANDS_NB]
double	thrusts[NPS_COMMANDS_NB]
double	torques[NPS_COMMANDS_NB]

Macro Definition Documentation

◆ NPS_GAZEBO_AC_NAME

#define NPS_GAZEBO_AC_NAME "bebop"

Definition at line 126 of file nps_fdm_gz.cpp.

◆ NPS_GAZEBO_WORLD

#define NPS_GAZEBO_WORLD "empty.world"

Definition at line 123 of file nps_fdm_gz.cpp.

Function Documentation

◆ eulers_ned_of_enu()

static struct DoubleEulers eulers_ned_of_enu ( const gz::math::Quaterniond & q_enu )

static

NED to FRD euler angles from the ENU to FLU attitude.

Definition at line 303 of file nps_fdm_gz.cpp.

References foo, DoubleEulers::phi, DoubleEulers::psi, and DoubleEulers::theta.

Referenced by gazebo_read().

Here is the caller graph for this function:

◆ frd_of_flu()

static struct DoubleVect3 frd_of_flu ( const gz::math::Vector3d & flu )

static

Definition at line 290 of file nps_fdm_gz.cpp.

References foo.

Referenced by gazebo_read().

Here is the caller graph for this function:

◆ gazebo_read()

static void gazebo_read ( void )

static

Fill the fdm struct from the gz state.

As with Gazebo Classic, the atmosphere is not simulated and the AHRS and INS should be bypassed (NPS_BYPASS_AHRS, NPS_BYPASS_INS).

Definition at line 496 of file nps_fdm_gz.cpp.

References NpsFdm::agl, NpsFdm::airspeed, LlaCoor_f::alt, NpsFdm::aoa, NpsFdm::body_accel, NpsFdm::body_ecef_accel, NpsFdm::body_ecef_rotvel, NpsFdm::body_ecef_vel, NpsFdm::body_inertial_accel, NpsFdm::body_inertial_rotvel, double_quat_of_eulers(), NpsFdm::dynamic_pressure, LtpDef_d::ecef, LtpDef_f::ecef, NpsFdm::ecef_ecef_accel, NpsFdm::ecef_ecef_vel, ecef_of_ned_point_d(), ecef_of_ned_vect_d(), NpsFdm::ecef_pos, NpsFdm::elevator, eulers_ned_of_enu(), fdm, NpsFdm::flap, foo, frd_of_flu(), gc_of_gd_lat_d(), gz_state, LtpDef_f::hmsl, NpsFdm::hmsl, LlaCoor_d::lat, LlaCoor_f::lat, NpsFdm::left_aileron, LtpDef_f::lla, lla_of_ecef_d(), NpsFdm::lla_pos, NpsFdm::lla_pos_geoc, NpsFdm::lla_pos_geod, NpsFdm::lla_pos_pprz, local_to_enu(), LlaCoor_f::lon, NpsFdm::ltp_ecef_accel, NpsFdm::ltp_ecef_vel, NpsFdm::ltp_g, NpsFdm::ltp_h, LtpDef_f::ltp_of_ecef, 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, FloatRMat::m, ned_of_enu(), State::ned_origin_f, NpsFdm::num_engines, pprz_isa_pressure_of_height(), PPRZ_ISA_SEA_LEVEL_PRESSURE, NpsFdm::pressure, NpsFdm::pressure_sl, rates_frd_of_flu(), NpsFdm::right_aileron, NpsFdm::rudder, NpsFdm::sideslip, state, NpsFdm::temperature, NpsFdm::time, vect_ned_of_enu(), NpsFdm::wind, EcefCoor_d::x, EcefCoor_f::x, EcefCoor_f::y, NedCoor_d::z, and EcefCoor_f::z.

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ gazebo_write()

static void gazebo_write	(	double	act_commands[],
		int	commands_nb
	)

static

Compute the motor thrusts and torques from the normalized commands [0, 1], with NPS_ACTUATOR_THRUSTS [N] and NPS_ACTUATOR_TORQUES [Nm] from the airframe.

Definition at line 591 of file nps_fdm_gz.cpp.

References autopilot, foo, gazebo_actuators, gz_cmd, gazebo_actuators_t::highpass, gazebo_actuators_t::lowpass, gazebo_actuators_t::max_ang_momentum, pprz_autopilot::motors_on, NPS_COMMANDS_NB, gazebo_actuators_t::thrusts, gazebo_actuators_t::torques, update_first_order_high_pass(), and update_first_order_low_pass().

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_gazebo()

static void init_gazebo ( void )

static

Start an embedded gz-sim server.

The vehicle NPS_GAZEBO_AC_NAME is inserted in the world NPS_GAZEBO_WORLD, both found in conf/simulator/gazebo and sw/ext/tudelft_gazebo_models, and the merged world is written to var/gazebo.world (as with Gazebo Classic).

Definition at line 371 of file nps_fdm_gz.cpp.

References dir, fdm, foo, gazebo_actuators, NpsFdm::init_dt, gazebo_actuators_t::max_ang_momentum, MOTOR_MIXING_YAW_COEF, NPS_COMMANDS_NB, NPS_GAZEBO_AC_NAME, NPS_GAZEBO_WORLD, pprz_system, server, sign(), gazebo_actuators_t::torques, and yaw_coef.

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ local_to_enu()

static gz::math::Quaterniond local_to_enu ( void )

static

Definition at line 273 of file nps_fdm_gz.cpp.

References foo, and gz_state.

Referenced by gazebo_read().

Here is the caller graph for this function:

◆ ned_of_enu()

static struct NedCoor_d ned_of_enu ( const gz::math::Vector3d & enu )

static

Definition at line 278 of file nps_fdm_gz.cpp.

References foo.

Referenced by gazebo_read().

Here is the caller graph for this function:

◆ nps_fdm_init()

void nps_fdm_init ( double dt )

NPS FDM rover init.

Parameters

dt

Definition at line 315 of file nps_fdm_gz.cpp.

References NpsFdm::curr_dt, fdm, foo, gazebo_actuators, gazebo_actuators_t::highpass, NpsFdm::init_dt, init_first_order_high_pass(), init_first_order_low_pass(), gazebo_actuators_t::lowpass, gazebo_actuators_t::max_ang_momentum, NpsFdm::nan_count, and NPS_COMMANDS_NB.

Here is the call graph for this function:

◆ nps_fdm_run_init_step()

void nps_fdm_run_init_step ( void )

Definition at line 338 of file nps_fdm_gz.cpp.

◆ nps_fdm_run_step()

void nps_fdm_run_step	(	bool	launch,
		double *	act_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 343 of file nps_fdm_gz.cpp.

References foo, gazebo_read(), gazebo_write(), init_gazebo(), and server.

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 362 of file nps_fdm_gz.cpp.

◆ nps_fdm_set_turbulence()

void nps_fdm_set_turbulence	(	double	wind_speed,
		int	turbulence_severity
	)

Definition at line 360 of file nps_fdm_gz.cpp.

◆ nps_fdm_set_wind()

void nps_fdm_set_wind	(	double	speed,
		double	dir
	)

Definition at line 357 of file nps_fdm_gz.cpp.

◆ nps_fdm_set_wind_ned()

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

Definition at line 358 of file nps_fdm_gz.cpp.

◆ rates_frd_of_flu()

static struct DoubleRates rates_frd_of_flu ( const gz::math::Vector3d & flu )

static

Definition at line 296 of file nps_fdm_gz.cpp.

References foo.

Referenced by gazebo_read().

Here is the caller graph for this function:

◆ vect_ned_of_enu()

static struct DoubleVect3 vect_ned_of_enu ( const gz::math::Vector3d & enu )

static

Definition at line 284 of file nps_fdm_gz.cpp.

References foo.

Referenced by gazebo_read().

Here is the caller graph for this function:

Variable Documentation

◆ fdm

struct NpsFdm fdm

Holds all necessary NPS FDM state information.

Definition at line 130 of file nps_fdm_gz.cpp.

Referenced by gazebo_read(), init_gazebo(), and nps_fdm_init().

◆ gazebo_actuators

struct gazebo_actuators_t gazebo_actuators = { NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { } }

static

Definition at line 141 of file nps_fdm_gz.cpp.

Referenced by PaparazziSystem::FindModel(), gazebo_write(), init_gazebo(), and nps_fdm_init().

◆ [struct]

struct { ... } gz_cmd

Actuator commands in the body frame of each motor link, read in PaparazziSystem::PreUpdate.

Referenced by gazebo_write(), and PaparazziSystem::PreUpdate().

◆ [struct]

struct { ... } gz_state

Simulation state, written in PaparazziSystem::PostUpdate (gz local frame, FLU body)

Referenced by gazebo_read(), local_to_enu(), and PaparazziSystem::PostUpdate().

◆ pprz_system

std::shared_ptr<PaparazziSystem> pprz_system

static

Definition at line 264 of file nps_fdm_gz.cpp.

Referenced by init_gazebo().

◆ server

std::unique_ptr<gz::sim::Server> server

static

Definition at line 263 of file nps_fdm_gz.cpp.

Referenced by init_gazebo(), and nps_fdm_run_step().

Data Structures

Macros

Functions

Variables

Detailed Description

Data Structure Documentation

◆ gazebo_actuators_t

Macro Definition Documentation

◆ NPS_GAZEBO_AC_NAME

◆ NPS_GAZEBO_WORLD

Function Documentation

◆ eulers_ned_of_enu()

◆ frd_of_flu()

◆ gazebo_read()

◆ gazebo_write()

◆ init_gazebo()

◆ local_to_enu()

◆ ned_of_enu()

◆ nps_fdm_init()

◆ nps_fdm_run_init_step()

◆ nps_fdm_run_step()

◆ nps_fdm_set_temperature()

◆ nps_fdm_set_turbulence()

◆ nps_fdm_set_wind()

◆ nps_fdm_set_wind_ned()

◆ rates_frd_of_flu()

◆ vect_ned_of_enu()

Variable Documentation

◆ fdm

◆ gazebo_actuators

◆ [struct]

◆ [struct]

◆ pprz_system

◆ server