Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
nps_fdm_gazebo.cpp File Reference
#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_f::alt, LlaCoor_d::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_f::ecef, LtpDef_d::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(), NpsFdm::hmsl, LtpDef_f::hmsl, LtpDef_d::hmsl, LlaCoor_f::lat, LlaCoor_d::lat, NpsFdm::left_aileron, LtpDef_f::lla, LtpDef_d::lla, lla_of_ecef_d(), NpsFdm::lla_pos, NpsFdm::lla_pos_geoc, NpsFdm::lla_pos_geod, NpsFdm::lla_pos_pprz, LlaCoor_f::lon, LlaCoor_d::lon, NpsFdm::ltp_ecef_accel, NpsFdm::ltp_ecef_vel, NpsFdm::ltp_g, NpsFdm::ltp_h, LtpDef_f::ltp_of_ecef, LtpDef_d::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_f::x, EcefCoor_d::x, EcefCoor_f::y, EcefCoor_d::y, EcefCoor_f::z, EcefCoor_d::z, and NedCoor_d::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.

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.

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

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

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

References DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

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

References EcefCoor_d::x, EcefCoor_d::y, and EcefCoor_d::z.

◆ to_pprz_eulers()

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

Definition at line 198 of file nps_fdm_gazebo.cpp.

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

◆ to_pprz_lla()

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

Definition at line 171 of file nps_fdm_gazebo.cpp.

References LlaCoor_d::alt, LlaCoor_d::lat, and LlaCoor_d::lon.

◆ to_pprz_ltp()

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

Definition at line 216 of file nps_fdm_gazebo.cpp.

References DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

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

References NedCoor_d::x, NedCoor_d::y, and NedCoor_d::z.

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

References DoubleRates::p, DoubleRates::q, and DoubleRates::r.

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 138 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 123 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