#include "nps_fdm.h"
#include <stdlib.h>
#include <stdio.h>
#include <Python.h>
#include "math/pprz_geodetic.h"
#include "math/pprz_geodetic_double.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_isa.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "state.h"

Include dependency graph for nps_fdm_pybullet.c:

Macros
#define	_WIDEN(x) L ## x

#define	WIDEN(x) _WIDEN(x)

#define	PYTHON_EXEC "python3"

#define	PYBULLET_GUI TRUE

#define	NPS_PYBULLET_MODULE "simple_quad_sim"

#define	NPS_PYBULLET_URDF "robobee.urdf"

Functions
static void	py_check_status (PyConfig config, PyStatus status)

static void	py_check (bool exit_on_error, int line_nb)

static void	python_init (double dt)

static void	init_ltp (void)

static void	get_pos (PyObject *ppos)

static void	get_vel (PyObject *pvel)

static void	get_acc (PyObject *pacc)

static void	get_orient (PyObject *porient)

static void	get_ang_vel (PyObject *pang_vel)

static void	get_ang_acc (PyObject *pang_acc)

void	nps_fdm_init (double dt)
	NPS FDM rover init. More...

void	nps_fdm_run_step (bool launch, double *commands, int commands_nb)
	Minimum complexity flight dynamic model In legacy Paparazzi simulator, was implemented in OCaml and correspond to the 'sim' target. 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 struct LtpDef_d	ltpdef

struct DoubleQuat	quat_to_pprz

PyObject *	fdm_module = NULL

PyObject *	bullet_fdm = NULL

Macro Definition Documentation

◆ _WIDEN

#define _WIDEN ( x ) L ## x

Definition at line 25 of file nps_fdm_pybullet.c.

◆ NPS_PYBULLET_MODULE

#define NPS_PYBULLET_MODULE "simple_quad_sim"

Definition at line 44 of file nps_fdm_pybullet.c.

◆ NPS_PYBULLET_URDF

#define NPS_PYBULLET_URDF "robobee.urdf"

Definition at line 50 of file nps_fdm_pybullet.c.

◆ PYBULLET_GUI

#define PYBULLET_GUI TRUE

Definition at line 34 of file nps_fdm_pybullet.c.

◆ PYTHON_EXEC

#define PYTHON_EXEC "python3"

Definition at line 29 of file nps_fdm_pybullet.c.

◆ WIDEN

#define WIDEN ( x ) _WIDEN(x)

Definition at line 26 of file nps_fdm_pybullet.c.

Function Documentation

◆ get_acc()

static void get_acc ( PyObject * pacc )

static

accel in ltppprz frame, wrt ECEF frame

acceleration in LTP frame, wrt ECEF frame

acceleration in ECEF frame, wrt ECEF frame

acceleration in body frame as measured by an accelerometer (incl. gravity)

Definition at line 189 of file nps_fdm_pybullet.c.

References NpsFdm::body_accel, double_quat_vmult(), NpsFdm::ecef_ecef_accel, ecef_of_enu_vect_d(), fdm, NpsFdm::ltp_ecef_accel, NpsFdm::ltp_to_body_quat, ltpdef, NpsFdm::ltpprz_ecef_accel, VECT3_COPY, VECT3_NED_OF_ENU, NedCoor_d::x, NedCoor_d::y, 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:

◆ get_ang_acc()

static void get_ang_acc ( PyObject * pang_acc )

static

Definition at line 255 of file nps_fdm_pybullet.c.

References NpsFdm::body_ecef_rotaccel, NpsFdm::body_inertial_rotaccel, double_quat_vmult(), fdm, NpsFdm::ltp_to_body_quat, DoubleRates::p, DoubleRates::q, DoubleRates::r, DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_ang_vel()

static void get_ang_vel ( PyObject * pang_vel )

static

Definition at line 237 of file nps_fdm_pybullet.c.

References NpsFdm::body_ecef_rotvel, NpsFdm::body_inertial_rotvel, double_quat_vmult(), fdm, NpsFdm::ltp_to_body_quat, DoubleRates::p, DoubleRates::q, DoubleRates::r, DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_orient()

static void get_orient ( PyObject * porient )

static

Definition at line 221 of file nps_fdm_pybullet.c.

References double_eulers_of_quat(), double_quat_comp(), EULERS_COPY, fdm, NpsFdm::ltp_to_body_eulers, NpsFdm::ltp_to_body_quat, NpsFdm::ltpprz_to_body_eulers, NpsFdm::ltpprz_to_body_quat, QUAT_COPY, and quat_to_pprz.

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_pos()

static void get_pos ( PyObject * ppos )

static

Definition at line 154 of file nps_fdm_pybullet.c.

References NpsFdm::agl, ecef_of_enu_point_d(), NpsFdm::ecef_pos, fdm, NpsFdm::hmsl, lla_of_ecef_d(), NpsFdm::lla_pos, ltpdef, NpsFdm::ltpprz_pos, py_check(), VECT3_NED_OF_ENU, EnuCoor_d::x, EnuCoor_d::y, NedCoor_d::z, and EnuCoor_d::z.

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_vel()

static void get_vel ( PyObject * pvel )

static

velocity in LTP frame, wrt ECEF frame (NedCoor_d)

velocity in ECEF frame, wrt ECEF frame

Definition at line 170 of file nps_fdm_pybullet.c.

References NpsFdm::ecef_ecef_vel, ecef_of_enu_vect_d(), fdm, NpsFdm::ltp_ecef_vel, ltpdef, NpsFdm::ltpprz_ecef_vel, py_check(), VECT3_NED_OF_ENU, EnuCoor_d::x, EnuCoor_d::y, and EnuCoor_d::z.

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_ltp()

static void init_ltp ( void )

static

Definition at line 276 of file nps_fdm_pybullet.c.

References LlaCoor_d::alt, ecef_of_lla_d(), NpsFdm::ecef_pos, fdm, LlaCoor_d::lat, LlaCoor_d::lon, ltp_def_from_ecef_d(), NpsFdm::ltp_g, NpsFdm::ltp_h, ltpdef, PRINT_CONFIG_MSG(), DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

Referenced by nps_fdm_init().

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 )

NPS FDM rover init.

Parameters

dt

Definition at line 89 of file nps_fdm_pybullet.c.

References NpsFdm::curr_dt, NpsFdm::dynamic_pressure, fdm, NpsFdm::init_dt, init_ltp(), NpsFdm::ltpprz_to_body_eulers, NpsFdm::nan_count, nps_fdm_run_step(), NpsFdm::on_ground, PPRZ_ISA_SEA_LEVEL_PRESSURE, NpsFdm::pressure, NpsFdm::pressure_sl, DoubleEulers::psi, python_init(), NpsFdm::temperature, NpsFdm::time, NpsFdm::total_pressure, and TRUE.

Here is the call graph for this function:

◆ nps_fdm_run_step()

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

Minimum complexity flight dynamic model In legacy Paparazzi simulator, was implemented in OCaml and correspond to the 'sim' target.

Johnson, E.N., Fontaine, S.G., and Kahn, A.D., “Minimum Complexity Uninhabited Air Vehicle Guidance And Flight Control System,” Proceedings of the 20th Digital Avionics Systems Conference, 2001. http://www.ae.gatech.edu/~ejohnson/JohnsonFontaineKahn.pdf

Johnson, E.N. and Fontaine, S.G., “Use Of Flight Simulation To Complement Flight Testing Of Low-Cost UAVs,” Proceedings of the AIAA Modeling and Simulation Technology Conference, 2001. http://www.ae.gatech.edu/~ejohnson/AIAA%202001-4059.pdf

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 114 of file nps_fdm_pybullet.c.

References bullet_fdm, commands, get_acc(), get_ang_acc(), get_ang_vel(), get_orient(), get_pos(), get_vel(), and py_check().

Referenced by nps_fdm_init().

Here is the call graph for this function:

Here is the caller 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 329 of file nps_fdm_pybullet.c.

◆ nps_fdm_set_turbulence()

void nps_fdm_set_turbulence	(	double	wind_speed,
		int	turbulence_severity
	)

Definition at line 324 of file nps_fdm_pybullet.c.

◆ nps_fdm_set_wind()

void nps_fdm_set_wind	(	double	speed,
		double	dir
	)

Definition at line 313 of file nps_fdm_pybullet.c.

◆ nps_fdm_set_wind_ned()

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

Definition at line 318 of file nps_fdm_pybullet.c.

◆ py_check()

static void py_check	(	bool	exit_on_error,
		int	line_nb
	)

static

Definition at line 379 of file nps_fdm_pybullet.c.

Referenced by get_pos(), get_vel(), nps_fdm_run_step(), and python_init().

Here is the caller graph for this function:

◆ py_check_status()

static void py_check_status	(	PyConfig *	config,
		PyStatus *	status
	)

static

Definition at line 369 of file nps_fdm_pybullet.c.

References config, and status.

Referenced by python_init().

Here is the caller graph for this function:

◆ python_init()

static void python_init ( double dt )

static

Definition at line 334 of file nps_fdm_pybullet.c.

References bullet_fdm, config, fdm_module, NPS_PYBULLET_MODULE, NPS_PYBULLET_URDF, py_check(), py_check_status(), PYBULLET_GUI, PYTHON_EXEC, status, and WIDEN.

Referenced by nps_fdm_init().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ bullet_fdm

PyObject* bullet_fdm = NULL

Definition at line 71 of file nps_fdm_pybullet.c.

Referenced by nps_fdm_run_step(), and python_init().

◆ fdm

struct NpsFdm fdm

Holds all necessary NPS FDM state information.

Definition at line 1 of file nps_fdm_pybullet.c.

Referenced by get_acc(), get_ang_acc(), get_ang_vel(), get_orient(), get_pos(), get_vel(), init_ltp(), and nps_fdm_init().

◆ fdm_module

PyObject* fdm_module = NULL

Definition at line 70 of file nps_fdm_pybullet.c.

Referenced by python_init().

◆ ltpdef

struct LtpDef_d ltpdef

static

Definition at line 1 of file nps_fdm_pybullet.c.

Referenced by get_acc(), get_pos(), get_vel(), and init_ltp().

◆ quat_to_pprz

struct DoubleQuat quat_to_pprz

Initial value:

= {
  1/sqrt(2),
  0,
  0,
  1/sqrt(2),
}

Definition at line 1 of file nps_fdm_pybullet.c.

Referenced by get_orient().

Macros

Functions

Variables

Macro Definition Documentation

◆ _WIDEN

◆ NPS_PYBULLET_MODULE

◆ NPS_PYBULLET_URDF

◆ PYBULLET_GUI

◆ PYTHON_EXEC

◆ WIDEN

Function Documentation

◆ get_acc()

◆ get_ang_acc()

◆ get_ang_vel()

◆ get_orient()

◆ get_pos()

◆ get_vel()

◆ init_ltp()

◆ nps_fdm_init()

◆ nps_fdm_run_step()

◆ nps_fdm_set_temperature()

◆ nps_fdm_set_turbulence()

◆ nps_fdm_set_wind()

◆ nps_fdm_set_wind_ned()

◆ py_check()

◆ py_check_status()

◆ python_init()

Variable Documentation

◆ bullet_fdm

◆ fdm

◆ fdm_module

◆ ltpdef

◆ quat_to_pprz