20#include "generated/airframe.h"
21#include "generated/flight_plan.h"
25#define _WIDEN(x) L ## x
26#define WIDEN(x) _WIDEN(x)
28#ifndef NPS_PYTHON_EXEC
29#define NPS_PYTHON_EXEC "python3"
34#ifndef NPS_PYBULLET_GUI
35#define NPS_PYBULLET_GUI TRUE
38#ifdef NPS_PYBULLET_ACTUATORS_ORDER
39 int actuators_order[ACTUATORS_NB] = NPS_PYBULLET_ACTUATORS_ORDER;
41 #error "[PyBullet] missing NPS_PYBULLET_ACTUATORS_ORDER define!"
44#ifndef NPS_PYBULLET_MODULE
45#define NPS_PYBULLET_MODULE "simple_quad_sim"
46MESSAGE(
"NPS_PYBULLET_MODULE not defined, take 'simple_quad_sim' as default value.")
50#ifndef NPS_PYBULLET_URDF
51#define NPS_PYBULLET_URDF "robobee.urdf"
52MESSAGE(
"NPS_PYBULLET_URDF not defined, take 'robobee.urdf' as default value.")
77static void py_check(
bool exit_on_error,
int line_nb);
83static void get_pos(PyObject* ppos);
84static void get_vel(PyObject* pvel);
85static void get_acc(PyObject* pacc);
102 fdm.total_pressure = -1;
103 fdm.dynamic_pressure = -1;
104 fdm.temperature = -1;
106 fdm.ltpprz_to_body_eulers.psi = 0.0;
115 double dummy_commands[] = {0., 0., 0., 0.};
122 PyObject* pcmd = PyList_New(commands_nb);
124 for (
int i=0; i<commands_nb; i++) {
125 int j = actuators_order[i];
126 PyList_SetItem(pcmd, i, PyFloat_FromDouble(
commands[j]));
133 PyObject* ret = PyObject_CallMethod(
bullet_fdm,
"step",
"(O)", pcmd);
137 PyObject* ppos = PyDict_GetItemString(ret,
"pos");
138 PyObject* pvel = PyDict_GetItemString(ret,
"vel");
139 PyObject* pacc = PyDict_GetItemString(ret,
"accel");
141 PyObject* pquat = PyDict_GetItemString(ret,
"quat");
142 PyObject* pang_v = PyDict_GetItemString(ret,
"ang_v");
143 PyObject* pang_acc = PyDict_GetItemString(ret,
"ang_accel");
161 enu_pos.
x = PyFloat_AsDouble(PyTuple_GetItem(ppos, 0));
162 enu_pos.
y = PyFloat_AsDouble(PyTuple_GetItem(ppos, 1));
163 enu_pos.
z = PyFloat_AsDouble(PyTuple_GetItem(ppos, 2));
170 fdm.hmsl = -
fdm.ltpprz_pos.z;
177 enu_vel.
x = PyFloat_AsDouble(PyTuple_GetItem(pvel, 0));
178 enu_vel.
y = PyFloat_AsDouble(PyTuple_GetItem(pvel, 1));
179 enu_vel.
z = PyFloat_AsDouble(PyTuple_GetItem(pvel, 2));
190 fdm.ltpprz_ecef_vel =
fdm.ltp_ecef_vel;
196 PyFloat_AsDouble(PyTuple_GetItem(pacc, 0)),
197 PyFloat_AsDouble(PyTuple_GetItem(pacc, 1)),
198 PyFloat_AsDouble(PyTuple_GetItem(pacc, 2)),
211 fdm.ltp_ecef_accel.x,
212 fdm.ltp_ecef_accel.y,
213 fdm.ltp_ecef_accel.z - 9.81,
228 PyFloat_AsDouble(PyTuple_GetItem(pquat, 3)),
229 PyFloat_AsDouble(PyTuple_GetItem(pquat, 1)),
230 PyFloat_AsDouble(PyTuple_GetItem(pquat, 0)),
231 -PyFloat_AsDouble(PyTuple_GetItem(pquat, 2)),
245 .x = PyFloat_AsDouble(PyTuple_GetItem(pang_vel, 0)),
246 .y = -PyFloat_AsDouble(PyTuple_GetItem(pang_vel, 1)),
247 .z = -PyFloat_AsDouble(PyTuple_GetItem(pang_vel, 2)),
250 fdm.body_inertial_rotvel.p = body_rates.
x;
251 fdm.body_inertial_rotvel.q = body_rates.
y;
252 fdm.body_inertial_rotvel.r = body_rates.
z;
253 fdm.body_ecef_rotvel =
fdm.body_inertial_rotvel;
260 .x = PyFloat_AsDouble(PyTuple_GetItem(pang_acc, 0)),
261 .y = -PyFloat_AsDouble(PyTuple_GetItem(pang_acc, 1)),
262 .z = -PyFloat_AsDouble(PyTuple_GetItem(pang_acc, 2)),
265 fdm.body_inertial_rotaccel.p = body_rotaccel.
x;
266 fdm.body_inertial_rotaccel.q = body_rotaccel.
y;
267 fdm.body_inertial_rotaccel.r = body_rotaccel.
z;
268 fdm.body_ecef_rotaccel =
fdm.body_inertial_rotaccel;
281 llh_nav0.
lat = RadOfDeg((
double)NAV_LAT0 / 1e7);
282 llh_nav0.
lon = RadOfDeg((
double)NAV_LON0 / 1e7);
283 llh_nav0.
alt = NAV_ALT0 / 1000.0;
290 fdm.ecef_pos = ecef_nav0;
298 fdm.ltp_h.x = AHRS_H_X;
299 fdm.ltp_h.y = AHRS_H_Y;
300 fdm.ltp_h.z = AHRS_H_Z;
314 double dir __attribute__((unused)))
319 double wind_east __attribute__((unused)),
320 double wind_down __attribute__((unused)))
325 int turbulence_severity __attribute__((unused)))
330 double h __attribute__((unused)))
337 PyConfig_InitPythonConfig(&config);
340 config.configure_c_stdio = 1;
341 config.buffered_stdio = 0;
345 status = Py_InitializeFromConfig(&config);
349 PyObject *sys_path = PySys_GetObject(
"path");
350 PyObject *module_path = PyUnicode_FromString( PAPARAZZI_SRC
"/sw/simulator/nps/pybullet");
351 PyList_Append(sys_path, module_path);
352 Py_DECREF(module_path);
357 PyObject* bullet_fdm_class = PyObject_GetAttrString(
fdm_module,
"BulletFDM");
360 PyObject* fdm_ctor_args = Py_BuildValue(
"(d)", dt);
362 bullet_fdm = PyObject_Call(bullet_fdm_class, fdm_ctor_args, fdm_ctor_kwargs);
365 Py_DECREF(fdm_ctor_args);
366 Py_DECREF(bullet_fdm_class);
370 if (PyStatus_Exception(*
status)) {
372 PyConfig_Clear(config);
373 Py_ExitStatusException(*
status);
379static void py_check(
bool exit_on_error,
int line_nb) {
380 if(PyErr_Occurred()) {
381 printf(
"Error at line %d\n", line_nb);
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q)
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
void double_quat_comp(struct DoubleQuat *a2c, struct DoubleQuat *a2b, struct DoubleQuat *b2c)
Composition (multiplication) of two quaternions.
#define EULERS_COPY(_a, _b)
#define QUAT_COPY(_qo, _qi)
#define VECT3_COPY(_a, _b)
double alt
in meters above WGS84 reference ellipsoid
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_enu_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
void ecef_of_enu_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
#define VECT3_NED_OF_ENU(_o, _i)
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
static struct LtpDef_d ltpdef
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
static void python_init(double dt)
void nps_fdm_init(double dt)
NPS FDM rover init.
static void init_ltp(void)
struct DoubleQuat quat_to_pprz
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 c...
#define NPS_PYBULLET_URDF
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
static void get_vel(PyObject *pvel)
static void get_ang_acc(PyObject *pang_acc)
static void get_orient(PyObject *porient)
static void get_ang_vel(PyObject *pang_vel)
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
static void py_check_status(PyConfig *config, PyStatus *status)
#define NPS_PYBULLET_MODULE
static void get_pos(PyObject *ppos)
void nps_fdm_set_wind(double speed, double dir)
void nps_fdm_run_init_step(void)
static void get_acc(PyObject *pacc)
static void py_check(bool exit_on_error, int line_nb)
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
Paparazzi generic macros for geodetic calculations.
Paparazzi double-precision floating point math for geodetic calculations.
Paparazzi floating point math for geodetic calculations.
Paparazzi atmospheric pressure conversion utilities.
static const ShellCommand commands[]
API to get/set the generic vehicle states.