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

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

◆ NPS_GAZEBO_AC_NAME

#define NPS_GAZEBO_AC_NAME "ardrone"

Definition at line 75 of file nps_fdm_gazebo.cpp.

◆ NPS_GAZEBO_WORLD

#define NPS_GAZEBO_WORLD "empty.world"

Definition at line 72 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 505 of file nps_fdm_gazebo.cpp.

References NpsFdm::agl, NpsFdm::airspeed, 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, NpsFdm::ecef_ecef_accel, NpsFdm::ecef_ecef_vel, NpsFdm::ecef_pos, NpsFdm::elevator, fdm, NpsFdm::flap, NpsFdm::hmsl, NpsFdm::left_aileron, NpsFdm::lla_pos, NpsFdm::lla_pos_geoc, NpsFdm::lla_pos_geod, NpsFdm::lla_pos_pprz, 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, model, NpsFdm::num_engines, pprz_isa_pressure_of_height(), NpsFdm::pressure, NpsFdm::pressure_sl, NpsFdm::right_aileron, NpsFdm::rudder, NpsFdm::sideslip, sonar, NpsFdm::temperature, NpsFdm::time, to_global_pprz_eulers(), to_pprz_body(), to_pprz_ecef(), to_pprz_lla(), to_pprz_ltp(), to_pprz_ned(), to_pprz_rates(), and NpsFdm::wind.

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 637 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 333 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.