Flight Dynamics Model (FDM) for NPS using Gazebo. More...

#include <cstdio>
#include <cstdlib>
#include <string>
#include <iostream>
#include <sys/time.h>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh>
#include <sdf/sdf.hh>
#include <gazebo/gazebo_config.h>
#include "nps_fdm.h"
#include "nps_autopilot.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "autopilot.h"
#include "modules/core/abi.h"
#include "math/pprz_isa.h"
#include "math/pprz_algebra_double.h"
#include "filters/low_pass_filter.h"
#include "filters/high_pass_filter.h"
#include "modules/actuators/motor_mixing_types.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_geodetic_double.h"
#include "state.h"

Include dependency graph for nps_fdm_gazebo.cpp:

Go to the source code of this file.

Data Structures
struct	gazebo_actuators_t

Macros
#define	NPS_GAZEBO_WORLD "empty.world"

#define	NPS_GAZEBO_AC_NAME "ardrone"

Functions
static void	init_gazebo (void)
	Set up a Gazebo server. More...

static void	gazebo_read (void)
	Read Gazebo's simulation state and store the results in the fdm struct used by NPS. More...

static void	gazebo_write (double act_commands[], int commands_nb)
	Write actuator commands to Gazebo. More...

struct EcefCoor_d	to_pprz_ecef (ignition::math::Vector3d ecef_i)

struct NedCoor_d	to_pprz_ned (ignition::math::Vector3d global)

struct LlaCoor_d	to_pprz_lla (ignition::math::Vector3d lla_i)

struct DoubleVect3	to_pprz_body (ignition::math::Vector3d body_g)

struct DoubleRates	to_pprz_rates (ignition::math::Vector3d body_g)

struct DoubleEulers	to_pprz_eulers (ignition::math::Quaterniond quat)

struct DoubleEulers	to_global_pprz_eulers (ignition::math::Quaterniond quat)

struct DoubleVect3	to_pprz_ltp (ignition::math::Vector3d xyz)

void	nps_fdm_init (double dt)
	Initialize actuator dynamics, set unused fields in fdm. More...

void	nps_fdm_run_step (bool launch, double *act_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 gazebo_actuators_t	gazebo_actuators = { NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { }}

std::shared_ptr< gazebo::sensors::SonarSensor >	sonar = NULL

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

static bool	gazebo_initialized = false

static gazebo::physics::ModelPtr	model = NULL

static gazebo::sensors::ContactSensorPtr	ct

Detailed Description

Flight Dynamics Model (FDM) for NPS using Gazebo.

This is an FDM for NPS that uses Gazebo as the simulation engine.

Definition in file nps_fdm_gazebo.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]
double	thrusts[NPS_COMMANDS_NB]
double	torques[NPS_COMMANDS_NB]

Macro Definition Documentation

◆ NPS_GAZEBO_AC_NAME

#define NPS_GAZEBO_AC_NAME "ardrone"

Definition at line 79 of file nps_fdm_gazebo.cpp.

◆ NPS_GAZEBO_WORLD

#define NPS_GAZEBO_WORLD "empty.world"

Definition at line 76 of file nps_fdm_gazebo.cpp.

Function Documentation

◆ gazebo_read()

static void gazebo_read ( void )

static

Read Gazebo's simulation state and store the results in the fdm struct used by NPS.

Not all fields are filled at the moment, as some of them are unused by paparazzi (see comments) and others are not available in Gazebo 7 (atmosphere).

Definition at line 509 of file nps_fdm_gazebo.cpp.

References NpsFdm::agl, NpsFdm::airspeed, LlaCoor_d::alt, 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, fdm, NpsFdm::flap, gc_of_gd_lat_d(), LtpDef_d::hmsl, LtpDef_f::hmsl, NpsFdm::hmsl, LlaCoor_d::lat, LlaCoor_f::lat, NpsFdm::left_aileron, LtpDef_d::lla, LtpDef_f::lla, lla_of_ecef_d(), NpsFdm::lla_pos, NpsFdm::lla_pos_geoc, NpsFdm::lla_pos_geod, NpsFdm::lla_pos_pprz, LlaCoor_d::lon, LlaCoor_f::lon, NpsFdm::ltp_ecef_accel, NpsFdm::ltp_ecef_vel, NpsFdm::ltp_g, NpsFdm::ltp_h, LtpDef_d::ltp_of_ecef, 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, DoubleRMat::m, FloatRMat::m, model, State::ned_origin_f, NpsFdm::num_engines, pprz_isa_pressure_of_height(), NpsFdm::pressure, NpsFdm::pressure_sl, NpsFdm::right_aileron, NpsFdm::rudder, NpsFdm::sideslip, sonar, state, NpsFdm::temperature, NpsFdm::time, to_global_pprz_eulers(), to_pprz_body(), to_pprz_ltp(), to_pprz_ned(), to_pprz_rates(), NpsFdm::wind, EcefCoor_d::x, EcefCoor_f::x, EcefCoor_d::y, EcefCoor_f::y, EcefCoor_d::z, 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

Write actuator commands to Gazebo.

This function takes the normalized commands and applies them as forces and torques in Gazebo. Since the commands are normalized in [0,1], their thrusts (NPS_ACTUATOR_THRUSTS) and torques (NPS_ACTUATOR_TORQUES) need to be specified in the airframe file. Their values need to be specified in the same order as the NPS_ACTUATOR_NAMES and should be provided in SI units. See conf/airframes/examples/ardrone2_gazebo.xml for an example.

The forces and torques are applied to (the origin of) the links named in NPS_ACTUATOR_NAMES. See conf/simulator/gazebo/models/ardrone/ardrone.sdf for an example.

Parameters

act_commands
commands_nb

Definition at line 651 of file nps_fdm_gazebo.cpp.

References autopilot, gazebo_actuators, gazebo_actuators_t::highpass, gazebo_actuators_t::lowpass, gazebo_actuators_t::max_ang_momentum, model, pprz_autopilot::motors_on, gazebo_actuators_t::names, 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

Set up a Gazebo server.

Starts a Gazebo server, adds conf/simulator/gazebo/models to its model path and loads the world specified by NPS_GAZEBO_WORLD.

This function also obtaines a pointer to the aircraft model, named NPS_GAZEBO_AC_NAME ('paparazzi_uav' by default). This pointer, 'model', is used to read the state and write actuator commands in gazebo_read and _write.

Definition at line 337 of file nps_fdm_gazebo.cpp.

References ct, gazebo_actuators, h(), init, gazebo_actuators_t::max_ang_momentum, Min, model, MT9F002_TARGET_FPS, MT9V117_TARGET_FPS, NPS_COMMANDS_NB, NPS_GAZEBO_AC_NAME, NPS_GAZEBO_WORLD, sonar, 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:

◆ nps_fdm_init()

void nps_fdm_init ( double dt )

Initialize actuator dynamics, set unused fields in fdm.

NPS FDM rover init.

Parameters

dt

Definition at line 231 of file nps_fdm_gazebo.cpp.

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

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

Update the simulation state.

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 260 of file nps_fdm_gazebo.cpp.

References gazebo_initialized, gazebo_read(), gazebo_write(), init_gazebo(), and model.

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 319 of file nps_fdm_gazebo.cpp.

◆ nps_fdm_set_turbulence()

void nps_fdm_set_turbulence	(	double	wind_speed,
		int	turbulence_severity
	)

Definition at line 312 of file nps_fdm_gazebo.cpp.

◆ nps_fdm_set_wind()

void nps_fdm_set_wind	(	double	speed,
		double	dir
	)

Definition at line 299 of file nps_fdm_gazebo.cpp.

◆ nps_fdm_set_wind_ned()

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

Definition at line 305 of file nps_fdm_gazebo.cpp.

◆ to_global_pprz_eulers()

struct DoubleEulers to_global_pprz_eulers ( ignition::math::Quaterniond quat )

inline

Definition at line 150 of file nps_fdm_gazebo.cpp.

Referenced by gazebo_read().

Here is the caller graph for this function:

◆ to_pprz_body()

struct DoubleVect3 to_pprz_body ( ignition::math::Vector3d body_g )

inline

Definition at line 150 of file nps_fdm_gazebo.cpp.

Referenced by gazebo_read().

Here is the caller graph for this function:

◆ to_pprz_ecef()

struct EcefCoor_d to_pprz_ecef ( ignition::math::Vector3d ecef_i )

inline

Definition at line 150 of file nps_fdm_gazebo.cpp.

◆ to_pprz_eulers()

struct DoubleEulers to_pprz_eulers ( ignition::math::Quaterniond quat )

inline

Definition at line 150 of file nps_fdm_gazebo.cpp.

◆ to_pprz_lla()

struct LlaCoor_d to_pprz_lla ( ignition::math::Vector3d lla_i )

inline

Definition at line 150 of file nps_fdm_gazebo.cpp.

◆ to_pprz_ltp()

struct DoubleVect3 to_pprz_ltp ( ignition::math::Vector3d xyz )

inline

Definition at line 150 of file nps_fdm_gazebo.cpp.

Referenced by gazebo_read().

Here is the caller graph for this function:

◆ to_pprz_ned()

struct NedCoor_d to_pprz_ned ( ignition::math::Vector3d global )

inline

Definition at line 150 of file nps_fdm_gazebo.cpp.

Referenced by gazebo_read().

Here is the caller graph for this function:

◆ to_pprz_rates()

struct DoubleRates to_pprz_rates ( ignition::math::Vector3d body_g )

inline

Definition at line 150 of file nps_fdm_gazebo.cpp.

Referenced by gazebo_read().

Here is the caller graph for this function:

Variable Documentation

◆ ct

gazebo::sensors::ContactSensorPtr ct

static

Definition at line 145 of file nps_fdm_gazebo.cpp.

Referenced by init_gazebo().

◆ fdm

struct NpsFdm fdm

Holds all necessary NPS FDM state information.

Definition at line 135 of file nps_fdm_gazebo.cpp.

Referenced by gazebo_read(), and nps_fdm_init().

◆ gazebo_actuators

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

Definition at line 1 of file nps_fdm_gazebo.cpp.

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

◆ gazebo_initialized

bool gazebo_initialized = false

static

Definition at line 141 of file nps_fdm_gazebo.cpp.

Referenced by nps_fdm_run_step().

◆ model

gazebo::physics::ModelPtr model = NULL

static

Definition at line 142 of file nps_fdm_gazebo.cpp.

Referenced by gazebo_read(), gazebo_write(), init_gazebo(), and nps_fdm_run_step().

◆ sonar

std::shared_ptr<gazebo::sensors::SonarSensor> sonar = NULL

Definition at line 135 of file nps_fdm_gazebo.cpp.

Referenced by gazebo_read(), init_gazebo(), nps_sensor_sonar_init(), nps_sensor_sonar_run_step(), and save_shot_on_disk().

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

◆ gazebo_read()

◆ gazebo_write()

◆ init_gazebo()

◆ nps_fdm_init()

◆ nps_fdm_run_step()

◆ nps_fdm_set_temperature()

◆ nps_fdm_set_turbulence()

◆ nps_fdm_set_wind()

◆ nps_fdm_set_wind_ned()

◆ to_global_pprz_eulers()

◆ to_pprz_body()

◆ to_pprz_ecef()

◆ to_pprz_eulers()

◆ to_pprz_lla()

◆ to_pprz_ltp()

◆ to_pprz_ned()

◆ to_pprz_rates()

Variable Documentation

◆ ct

◆ fdm

◆ gazebo_actuators

◆ gazebo_initialized

◆ model

◆ sonar