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

#include <cstdio>
#include <cstdlib>
#include <string>
#include <iostream>
#include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/gazebo_config.h>
#include <sys/time.h>
#include "nps_fdm.h"
#include "math/pprz_algebra_double.h"
#include "generated/airframe.h"
#include "autopilot.h"

Include dependency graph for nps_fdm_gazebo.cpp:

Go to the source code of this file.

Macros
#define	NPS_GAZEBO_WORLD "ardrone.world"

#define	NPS_GAZEBO_AC_NAME "paparazzi_uav"

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 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 (gazebo::math::Vector3 body_g)

struct DoubleRates	to_pprz_rates (gazebo::math::Vector3 body_g)

struct DoubleEulers	to_pprz_eulers (gazebo::math::Quaternion quat)

struct DoubleVect3	to_pprz_ltp (gazebo::math::Vector3 xyz)

void	nps_fdm_init (double dt)
	Set JSBsim specific fields that are not used for Gazebo. More...

void	nps_fdm_run_step (bool launch, double *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 NpsFdm	fdm
	Holds all necessary NPS FDM state information. More...

static bool	gazebo_initialized = false

static gazebo::physics::ModelPtr	model = NULL

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.

Macro Definition Documentation

#define NPS_GAZEBO_AC_NAME "paparazzi_uav"

Definition at line 68 of file nps_fdm_gazebo.cpp.

Referenced by init_gazebo().

#define NPS_GAZEBO_WORLD "ardrone.world"

Definition at line 65 of file nps_fdm_gazebo.cpp.

Referenced by init_gazebo().

Function Documentation

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 300 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, NpsFdm::pressure, NpsFdm::pressure_sl, NpsFdm::right_aileron, NpsFdm::rudder, NpsFdm::sideslip, NpsFdm::temperature, NpsFdm::time, to_pprz_body(), to_pprz_ecef(), to_pprz_eulers(), to_pprz_lla(), to_pprz_ltp(), to_pprz_ned(), to_pprz_rates(), NpsFdm::wind, and DoubleVect3::x.

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

static void gazebo_write	(	double	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

commands
commands_nb

Definition at line 434 of file nps_fdm_gazebo.cpp.

References autopilot, model, and pprz_autopilot::motors_on.

Referenced by nps_fdm_run_step().

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

References model, NPS_GAZEBO_AC_NAME, and NPS_GAZEBO_WORLD.

Referenced by nps_fdm_run_step().

Here is the caller graph for this function: