Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_fdm_gazebo.cpp File Reference

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 "subsystems/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 "subsystems/actuators/motor_mixing_types.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

struct gazebo_actuators_t

Definition at line 110 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

#define NPS_GAZEBO_AC_NAME   "ardrone"

Definition at line 75 of file nps_fdm_gazebo.cpp.

Referenced by init_gazebo().

#define NPS_GAZEBO_WORLD   "empty.world"

Definition at line 72 of file nps_fdm_gazebo.cpp.

Referenced by init_gazebo().

Function Documentation

static void gazebo_read ( void  )
static
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 637 of file nps_fdm_gazebo.cpp.

References autopilot, 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:

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

References ct, 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:

void nps_fdm_init ( double  dt)

Initialize actuator dynamics, set unused fields in fdm.

Parameters
dt

Definition at line 227 of file nps_fdm_gazebo.cpp.

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

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 256 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:

void nps_fdm_set_temperature ( double  temp,
double  h 
)

Set temperature in degrees Celcius at given height h above MSL.

Definition at line 315 of file nps_fdm_gazebo.cpp.

void nps_fdm_set_turbulence ( double  wind_speed,
int  turbulence_severity 
)

Definition at line 308 of file nps_fdm_gazebo.cpp.

void nps_fdm_set_wind ( double  speed,
double  dir 
)

Definition at line 295 of file nps_fdm_gazebo.cpp.

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

Definition at line 301 of file nps_fdm_gazebo.cpp.

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

Definition at line 203 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:

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

Definition at line 176 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:

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

Definition at line 149 of file nps_fdm_gazebo.cpp.

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

Referenced by gazebo_read().

+ Here is the caller graph for this function:

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

Definition at line 194 of file nps_fdm_gazebo.cpp.

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

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

Definition at line 167 of file nps_fdm_gazebo.cpp.

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

Referenced by gazebo_read().

+ Here is the caller graph for this function:

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

Definition at line 212 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:

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

Definition at line 158 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:

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

Definition at line 185 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

gazebo::sensors::ContactSensorPtr ct
static

Definition at line 141 of file nps_fdm_gazebo.cpp.

Referenced by init_gazebo().

struct NpsFdm fdm

Holds all necessary NPS FDM state information.

Definition at line 134 of file nps_fdm_gazebo.cpp.

Referenced by gazebo_read(), and nps_fdm_init().

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

Definition at line 119 of file nps_fdm_gazebo.cpp.

bool gazebo_initialized = false
static

Definition at line 137 of file nps_fdm_gazebo.cpp.

Referenced by nps_fdm_run_step().

gazebo::physics::ModelPtr model = NULL
static

Definition at line 138 of file nps_fdm_gazebo.cpp.

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

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

Definition at line 131 of file nps_fdm_gazebo.cpp.

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