diff --git a/conf/airframes/ENAC/conf_enac.xml b/conf/airframes/ENAC/conf_enac.xml
index 149d98a161..a7099b7526 100644
--- a/conf/airframes/ENAC/conf_enac.xml
+++ b/conf/airframes/ENAC/conf_enac.xml
@@ -10,28 +10,6 @@
settings_modules="modules/ahrs_float_dcm.xml modules/gps.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_attitude_fw.xml"
gui_color="blue"
/>
-
-
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
new file mode 100644
index 0000000000..269bf3c89a
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
@@ -0,0 +1,272 @@
+
+
+
+
+
+ * Autopilot: Tawaki
+ * Actuators: 4 in 4 Holybro BLHELI ESC
+ * Telemetry: XBee
+ * GPS: ublox
+ * RC: FrSky XM+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/robobee.xml b/conf/airframes/ENAC/quadrotor/robobee.xml
new file mode 100644
index 0000000000..369e7b6ea8
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/robobee.xml
@@ -0,0 +1,277 @@
+
+
+
+
+
+ * Autopilot: Tawaki
+ * Actuators: 4 in 1
+ * Telemetry: XBee
+ * GPS: datalink
+ * RC: SBUS
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/ulysse_indi.xml b/conf/airframes/ENAC/quadrotor/ulysse_indi.xml
new file mode 100644
index 0000000000..02e4e98b5b
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/ulysse_indi.xml
@@ -0,0 +1,261 @@
+
+
+
+
+
+ * Autopilot: Tawaki
+ * Actuators: 4 in 4 Holybro BLHELI ESC
+ * Telemetry: XBee
+ * GPS: ublox
+ * RC: FrSky XM+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/fdm_pybullet.xml b/conf/modules/fdm_pybullet.xml
new file mode 100644
index 0000000000..33b1eb2909
--- /dev/null
+++ b/conf/modules/fdm_pybullet.xml
@@ -0,0 +1,24 @@
+
+
+
+
+
+ PyBullet based FDM for NPS simulator
+
+
+
+
+
+
+
+
+
+
+ nps.CFLAGS += $(shell python3-config --cflags) -fPIE
+ nps.LDFLAGS += $(shell python3-config --ldflags) -lpython3.8
+ nps.CFLAGS += -DPAPARAZZI_SRC=\"$(PAPARAZZI_SRC)\"
+
+
+
+
+
diff --git a/conf/simulator/pybullet/robobee.obj b/conf/simulator/pybullet/robobee.obj
new file mode 100644
index 0000000000..bfe0e08edd
--- /dev/null
+++ b/conf/simulator/pybullet/robobee.obj
@@ -0,0 +1,776 @@
+# Blender v2.92.0 OBJ File: ''
+# www.blender.org
+mtllib robobee.mtl
+o RoboBee_fuse_visual
+v -0.056852 0.000000 0.025734
+v -0.067472 0.000000 0.030623
+v -0.066000 0.000000 0.020309
+v -0.066000 0.000000 0.000000
+v -0.054786 0.000000 -0.045999
+v -0.101985 0.000000 -0.098521
+v -0.097933 0.000000 -0.102901
+v 0.071770 0.000000 -0.040114
+v 0.046782 0.000000 -0.039374
+v 0.054786 0.000000 -0.045999
+v 0.067472 0.000000 -0.030623
+v 0.066000 0.000000 -0.020309
+v 0.056852 0.000000 -0.025734
+v -0.056852 0.000000 -0.025734
+v -0.066000 0.000000 -0.020309
+v -0.067472 0.000000 -0.030623
+v 0.000000 0.000000 -0.037000
+v 0.000000 0.000000 0.037000
+v -0.036667 0.000000 0.037000
+v 0.056852 0.000000 0.025734
+v 0.067472 0.000000 0.030623
+v 0.036667 0.000000 0.037000
+v -0.071770 0.000000 -0.040114
+v -0.046782 0.000000 -0.039374
+v -0.036667 0.000000 -0.037000
+v 0.066000 0.000000 -0.000000
+v 0.066000 0.000000 0.020309
+v -0.107492 0.000000 -0.096226
+v -0.071770 0.000000 0.040114
+v -0.046782 0.000000 0.039374
+v -0.054786 0.000000 0.045999
+v -0.107492 0.000000 0.096226
+v -0.101985 0.000000 0.098521
+v -0.097933 0.000000 0.102901
+v 0.107492 0.000000 -0.096226
+v 0.101985 0.000000 -0.098521
+v 0.097933 0.000000 -0.102901
+v 0.036667 0.000000 -0.037000
+v 0.046782 0.000000 0.039374
+v 0.071770 0.000000 0.040114
+v 0.054786 0.000000 0.045999
+v 0.107492 0.000000 0.096226
+v 0.101985 0.000000 0.098521
+v 0.097933 0.000000 0.102901
+v -0.121959 0.000000 -0.113639
+v -0.117159 0.000000 -0.120247
+v -0.121326 0.000000 -0.118229
+v -0.114326 0.000000 -0.123315
+v -0.109302 0.000000 -0.122481
+v -0.105674 0.000000 -0.123315
+v -0.121653 0.000000 -0.105477
+v -0.121683 0.000000 -0.102285
+v -0.116372 0.000000 -0.099246
+v -0.115497 0.000000 -0.097125
+v -0.108371 0.000000 -0.097607
+v -0.124000 0.000000 -0.110000
+v -0.098674 0.000000 -0.118229
+v -0.101744 0.000000 -0.119386
+v -0.096000 0.000000 -0.110000
+v -0.097710 0.000000 -0.112283
+v -0.098924 0.000000 -0.104207
+v -0.121959 0.000000 0.113639
+v -0.121326 0.000000 0.118229
+v -0.117159 0.000000 0.120247
+v -0.114326 0.000000 0.123315
+v -0.109302 0.000000 0.122481
+v -0.098924 0.000000 0.104207
+v -0.096000 0.000000 0.110000
+v -0.097710 0.000000 0.112283
+v -0.098674 0.000000 0.118229
+v -0.101744 0.000000 0.119386
+v -0.105674 0.000000 0.123315
+v -0.108371 0.000000 0.097607
+v -0.116372 0.000000 0.099246
+v -0.115497 0.000000 0.097125
+v -0.121653 0.000000 0.105477
+v -0.121683 0.000000 0.102285
+v -0.124000 0.000000 0.110000
+v 0.114326 0.000000 -0.123315
+v 0.109302 0.000000 -0.122481
+v 0.117159 0.000000 -0.120247
+v 0.121326 0.000000 -0.118229
+v 0.121959 0.000000 -0.113639
+v 0.121653 0.000000 -0.105477
+v 0.116372 0.000000 -0.099246
+v 0.121683 0.000000 -0.102285
+v 0.108371 0.000000 -0.097607
+v 0.115497 0.000000 -0.097125
+v 0.098924 0.000000 -0.104207
+v 0.096000 0.000000 -0.110000
+v 0.097710 0.000000 -0.112283
+v 0.098674 0.000000 -0.118229
+v 0.101744 0.000000 -0.119386
+v 0.105674 0.000000 -0.123315
+v 0.124000 0.000000 -0.110000
+v 0.121959 0.000000 0.113639
+v 0.117159 0.000000 0.120247
+v 0.121326 0.000000 0.118229
+v 0.114326 0.000000 0.123315
+v 0.109302 0.000000 0.122481
+v 0.124000 0.000000 0.110000
+v 0.121683 0.000000 0.102285
+v 0.121653 0.000000 0.105477
+v 0.108371 0.000000 0.097607
+v 0.115497 0.000000 0.097125
+v 0.116372 0.000000 0.099246
+v 0.098924 0.000000 0.104207
+v 0.097710 0.000000 0.112283
+v 0.096000 0.000000 0.110000
+v 0.101744 0.000000 0.119386
+v 0.098674 0.000000 0.118229
+v 0.105674 0.000000 0.123315
+v -0.121326 -0.015000 -0.101771
+v -0.114326 -0.015000 -0.096685
+v -0.105674 -0.015000 -0.096685
+v -0.098674 -0.015000 -0.101771
+v -0.124000 -0.015000 -0.110000
+v -0.096000 -0.015000 -0.110000
+v -0.098674 -0.015000 0.101771
+v -0.105674 -0.015000 0.096685
+v -0.114326 -0.015000 0.096685
+v -0.121326 -0.015000 0.101771
+v -0.096000 -0.015000 0.110000
+v -0.124000 -0.015000 0.110000
+v 0.098674 -0.015000 -0.101771
+v 0.105674 -0.015000 -0.096685
+v 0.114326 -0.015000 -0.096685
+v 0.121326 -0.015000 -0.101771
+v 0.096000 -0.015000 -0.110000
+v 0.124000 -0.015000 -0.110000
+v 0.121326 -0.015000 0.101771
+v 0.114326 -0.015000 0.096685
+v 0.105674 -0.015000 0.096685
+v 0.098674 -0.015000 0.101771
+v 0.124000 -0.015000 0.110000
+v 0.096000 -0.015000 0.110000
+v -0.098924 0.008000 -0.104207
+v -0.054786 0.008000 -0.045999
+v -0.108371 0.008000 -0.097607
+v -0.116372 0.008000 -0.099246
+v -0.121653 0.008000 -0.105477
+v -0.121959 0.008000 -0.113639
+v -0.117159 0.008000 -0.120247
+v -0.109302 0.008000 -0.122481
+v -0.101744 0.008000 -0.119386
+v -0.097710 0.008000 -0.112283
+v -0.071770 0.008000 -0.040114
+v -0.071770 0.008000 0.040114
+v -0.108371 0.008000 0.097607
+v -0.098924 0.008000 0.104207
+v -0.097710 0.008000 0.112283
+v -0.101744 0.008000 0.119386
+v -0.109302 0.008000 0.122481
+v -0.117159 0.008000 0.120247
+v -0.121959 0.008000 0.113639
+v -0.121653 0.008000 0.105477
+v -0.116372 0.008000 0.099246
+v -0.054786 0.008000 0.045999
+v 0.098924 0.008000 0.104207
+v 0.054786 0.008000 0.045999
+v 0.108371 0.008000 0.097607
+v 0.116372 0.008000 0.099246
+v 0.121653 0.008000 0.105477
+v 0.121959 0.008000 0.113639
+v 0.117159 0.008000 0.120247
+v 0.109302 0.008000 0.122481
+v 0.101744 0.008000 0.119386
+v 0.097710 0.008000 0.112283
+v 0.071770 0.008000 0.040114
+v 0.071770 0.008000 -0.040114
+v 0.108371 0.008000 -0.097607
+v 0.098924 0.008000 -0.104207
+v 0.097710 0.008000 -0.112283
+v 0.101744 0.008000 -0.119386
+v 0.109302 0.008000 -0.122481
+v 0.117159 0.008000 -0.120247
+v 0.121959 0.008000 -0.113639
+v 0.121653 0.008000 -0.105477
+v 0.116372 0.008000 -0.099246
+v 0.054786 0.008000 -0.045999
+v -0.098674 -0.015000 -0.118229
+v -0.121326 -0.015000 -0.118229
+v -0.105674 -0.015000 -0.123315
+v -0.114326 -0.015000 -0.123315
+v -0.105674 -0.015000 0.123315
+v -0.114326 -0.015000 0.123315
+v -0.098674 -0.015000 0.118229
+v -0.121326 -0.015000 0.118229
+v 0.121326 -0.015000 -0.118229
+v 0.098674 -0.015000 -0.118229
+v 0.114326 -0.015000 -0.123315
+v 0.105674 -0.015000 -0.123315
+v 0.114326 -0.015000 0.123315
+v 0.105674 -0.015000 0.123315
+v 0.121326 -0.015000 0.118229
+v 0.098674 -0.015000 0.118229
+v -0.056852 -0.035000 0.025734
+v 0.056852 -0.035000 0.025734
+v 0.056852 -0.035000 -0.025734
+v -0.056852 -0.035000 -0.025734
+v -0.046782 0.008000 0.039374
+v -0.036667 0.008000 0.037000
+v 0.046782 0.008000 -0.039374
+v 0.036667 0.008000 -0.037000
+v 0.067472 0.008000 -0.030623
+v -0.046782 0.008000 -0.039374
+v -0.067472 0.008000 -0.030623
+v -0.066000 0.008000 -0.020309
+v 0.066000 0.008000 -0.020309
+v 0.000000 0.008000 -0.037000
+v -0.036667 0.008000 -0.037000
+v -0.067472 0.008000 0.030623
+v 0.000000 0.008000 0.037000
+v 0.036667 0.008000 0.037000
+v 0.067472 0.008000 0.030623
+v 0.066000 0.008000 0.020309
+v -0.066000 0.008000 0.020309
+v 0.066000 0.008000 -0.000000
+v -0.066000 0.008000 0.000000
+v 0.046782 0.008000 0.039374
+vn 0.0000 -1.0000 0.0000
+vn 0.0000 1.0000 0.0000
+vn -0.5876 -0.0222 0.8088
+vn 0.5872 -0.0455 0.8082
+vn -0.1115 0.0204 0.9936
+vn 0.0000 -0.0306 0.9995
+vn 0.3846 0.0184 0.9229
+vn -0.9577 0.0000 0.2877
+vn -0.9510 -0.0120 0.3090
+vn -0.6406 0.0111 0.7678
+vn 0.9511 0.0000 0.3090
+vn 0.9645 -0.0278 0.2627
+vn 0.7340 0.0149 0.6790
+vn 0.5872 -0.0455 -0.8082
+vn -0.5876 -0.0222 -0.8088
+vn 0.3846 0.0184 -0.9229
+vn 0.0000 -0.0306 -0.9995
+vn -0.1115 0.0204 -0.9936
+vn 0.9649 0.0000 -0.2628
+vn 0.9508 -0.0237 -0.3089
+vn 0.7340 0.0149 -0.6790
+vn -0.9511 0.0000 -0.3090
+vn -0.9576 -0.0129 -0.2877
+vn -0.6406 0.0111 -0.7678
+vn -0.5872 -0.0455 0.8082
+vn 0.5876 -0.0222 0.8088
+vn -0.3846 0.0184 0.9229
+vn 0.1115 0.0204 0.9936
+vn -0.9649 0.0000 0.2628
+vn -0.9508 -0.0237 0.3089
+vn -0.7340 0.0149 0.6790
+vn 0.9576 -0.0129 0.2877
+vn 0.6406 0.0111 0.7678
+vn 0.5876 -0.0222 -0.8088
+vn -0.5872 -0.0455 -0.8082
+vn 0.1115 0.0204 -0.9936
+vn -0.3846 0.0184 -0.9229
+vn 0.9577 0.0000 -0.2877
+vn 0.9510 -0.0120 -0.3090
+vn 0.6406 0.0111 -0.7678
+vn -0.9645 -0.0278 -0.2627
+vn -0.7340 0.0149 -0.6790
+vn 0.7968 0.0000 -0.6042
+vn -0.2008 0.0000 0.9796
+vn -0.7629 0.0000 0.6465
+vn -0.9993 0.0000 0.0374
+vn -0.8091 0.0000 -0.5877
+vn -0.2734 0.0000 -0.9619
+vn 0.3789 0.0000 -0.9254
+vn 0.8695 0.0000 -0.4939
+vn 0.9889 0.0000 0.1486
+vn -0.8436 0.0000 0.5370
+vn -0.8435 0.0000 0.5371
+vn -0.8436 0.0000 -0.5370
+vn -0.8435 0.0000 -0.5371
+vn 0.9889 0.0000 -0.1486
+vn 0.8695 0.0000 0.4939
+vn 0.3789 0.0000 0.9254
+vn -0.2734 0.0000 0.9619
+vn -0.8091 0.0000 0.5877
+vn -0.9993 0.0000 -0.0374
+vn -0.7629 0.0000 -0.6465
+vn -0.2008 0.0000 -0.9796
+vn 0.7968 0.0000 0.6042
+vn 0.7968 -0.0001 0.6042
+vn -0.7968 0.0000 0.6042
+vn 0.2008 0.0000 -0.9796
+vn 0.7629 0.0000 -0.6465
+vn 0.9993 0.0000 -0.0374
+vn 0.8091 0.0000 0.5877
+vn 0.2734 0.0000 0.9619
+vn -0.3789 0.0000 0.9254
+vn -0.8695 0.0000 0.4939
+vn -0.9889 0.0000 -0.1486
+vn 0.8436 0.0000 -0.5370
+vn 0.8435 0.0000 -0.5371
+vn 0.8436 0.0000 0.5370
+vn 0.8435 0.0000 0.5371
+vn -0.9889 0.0000 0.1486
+vn -0.8695 0.0000 -0.4939
+vn -0.3789 0.0000 -0.9254
+vn 0.2734 0.0000 -0.9619
+vn 0.8091 0.0000 -0.5877
+vn 0.9993 0.0000 0.0374
+vn 0.7629 0.0000 0.6465
+vn 0.2008 0.0000 0.9796
+vn -0.7968 0.0000 -0.6042
+vn -0.7968 -0.0001 -0.6042
+vn 0.9511 0.0000 -0.3090
+vn 0.5878 0.0000 -0.8090
+vn 0.0000 0.0000 -1.0000
+vn -0.5878 0.0000 -0.8090
+vn -0.9511 0.0000 0.3090
+vn -0.5878 0.0000 0.8090
+vn 0.0000 0.0000 1.0000
+vn 0.5878 0.0000 0.8090
+vn 1.0000 0.0000 0.0000
+vn -1.0000 0.0000 0.0000
+vn -0.6377 0.0000 -0.7703
+vn -0.2285 0.0000 -0.9736
+vn 0.9900 0.0000 0.1413
+vn 0.9110 0.0000 0.4125
+vn 0.9110 0.0000 -0.4125
+vn 0.9900 0.0000 -0.1413
+vn -0.2285 0.0000 0.9736
+vn -0.6377 0.0000 0.7703
+vn 0.6377 0.0000 0.7703
+vn 0.2285 0.0000 0.9736
+vn -0.9900 0.0000 -0.1413
+vn -0.9110 0.0000 -0.4125
+vn -0.9110 0.0000 0.4125
+vn -0.9900 0.0000 0.1413
+vn 0.2285 0.0000 -0.9736
+vn 0.6377 0.0000 -0.7703
+usemtl None
+s off
+f 1//1 2//1 3//1
+f 4//1 1//1 3//1
+f 5//1 6//1 7//1
+f 8//1 9//1 10//1
+f 11//1 12//1 13//1
+f 14//1 1//1 4//1
+f 14//1 4//1 15//1
+f 14//1 15//1 16//1
+f 14//1 16//1 17//1
+f 14//1 17//1 11//1
+f 14//1 11//1 13//1
+f 18//1 19//1 2//1
+f 18//1 2//1 1//1
+f 18//1 1//1 20//1
+f 18//1 20//1 21//1
+f 18//1 21//1 22//1
+f 16//1 23//1 24//1
+f 16//1 24//1 25//1
+f 16//1 25//1 17//1
+f 20//1 13//1 12//1
+f 20//1 12//1 26//1
+f 20//1 26//1 27//1
+f 20//1 27//1 21//1
+f 28//1 6//1 23//1
+f 23//1 6//1 5//1
+f 23//1 5//1 24//1
+f 2//1 19//1 29//1
+f 29//1 19//1 30//1
+f 29//1 30//1 31//1
+f 32//1 29//1 33//1
+f 33//1 29//1 31//1
+f 33//1 31//1 34//1
+f 35//1 8//1 36//1
+f 36//1 8//1 10//1
+f 36//1 10//1 37//1
+f 9//1 8//1 38//1
+f 38//1 8//1 11//1
+f 38//1 11//1 17//1
+f 22//1 21//1 39//1
+f 39//1 21//1 40//1
+f 39//1 40//1 41//1
+f 40//1 42//1 41//1
+f 41//1 42//1 43//1
+f 41//1 43//1 44//1
+f 45//2 46//2 47//2
+f 48//2 47//2 46//2
+f 48//2 46//2 49//2
+f 48//2 49//2 50//2
+f 51//2 52//2 53//2
+f 53//2 52//2 54//2
+f 53//2 54//2 55//2
+f 55//2 54//2 28//2
+f 52//2 51//2 56//2
+f 56//2 51//2 45//2
+f 56//2 45//2 47//2
+f 50//2 49//2 57//2
+f 57//2 49//2 58//2
+f 57//2 58//2 59//2
+f 59//2 58//2 60//2
+f 59//2 60//2 7//2
+f 7//2 60//2 61//2
+f 62//2 63//2 64//2
+f 64//2 63//2 65//2
+f 66//2 64//2 65//2
+f 34//2 67//2 68//2
+f 68//2 67//2 69//2
+f 68//2 69//2 70//2
+f 70//2 69//2 71//2
+f 70//2 71//2 72//2
+f 72//2 71//2 66//2
+f 72//2 66//2 65//2
+f 73//2 32//2 74//2
+f 74//2 32//2 75//2
+f 74//2 75//2 76//2
+f 76//2 75//2 77//2
+f 76//2 77//2 62//2
+f 62//2 77//2 78//2
+f 62//2 78//2 63//2
+f 79//2 80//2 81//2
+f 79//2 81//2 82//2
+f 82//2 81//2 83//2
+f 84//2 85//2 86//2
+f 87//2 35//2 85//2
+f 85//2 35//2 88//2
+f 85//2 88//2 86//2
+f 37//2 89//2 90//2
+f 90//2 89//2 91//2
+f 90//2 91//2 92//2
+f 92//2 91//2 93//2
+f 92//2 93//2 94//2
+f 94//2 93//2 80//2
+f 94//2 80//2 79//2
+f 84//2 86//2 83//2
+f 83//2 86//2 95//2
+f 83//2 95//2 82//2
+f 96//2 97//2 98//2
+f 99//2 98//2 97//2
+f 99//2 97//2 100//2
+f 101//2 102//2 103//2
+f 101//2 103//2 96//2
+f 101//2 96//2 98//2
+f 42//2 104//2 105//2
+f 105//2 104//2 106//2
+f 105//2 106//2 102//2
+f 102//2 106//2 103//2
+f 107//2 44//2 108//2
+f 108//2 44//2 109//2
+f 108//2 109//2 110//2
+f 110//2 109//2 111//2
+f 110//2 111//2 100//2
+f 100//2 111//2 112//2
+f 100//2 112//2 99//2
+f 113//3 114//3 54//3
+f 115//4 116//4 6//4
+f 28//5 54//5 114//5
+f 28//6 114//6 115//6
+f 28//7 115//7 6//7
+f 56//8 117//8 52//8
+f 52//9 117//9 113//9
+f 52//10 113//10 54//10
+f 118//11 59//11 116//11
+f 116//12 59//12 7//12
+f 116//13 7//13 6//13
+f 119//14 120//14 33//14
+f 75//15 121//15 122//15
+f 32//16 33//16 120//16
+f 32//17 120//17 121//17
+f 32//18 121//18 75//18
+f 68//19 123//19 34//19
+f 34//20 123//20 119//20
+f 34//21 119//21 33//21
+f 124//22 78//22 122//22
+f 122//23 78//23 77//23
+f 122//24 77//24 75//24
+f 125//25 126//25 36//25
+f 88//26 127//26 128//26
+f 35//27 36//27 126//27
+f 35//6 126//6 127//6
+f 35//28 127//28 88//28
+f 90//29 129//29 37//29
+f 37//30 129//30 125//30
+f 37//31 125//31 36//31
+f 130//11 95//11 128//11
+f 128//32 95//32 86//32
+f 128//33 86//33 88//33
+f 131//34 132//34 105//34
+f 133//35 134//35 43//35
+f 42//36 105//36 132//36
+f 42//17 132//17 133//17
+f 42//37 133//37 43//37
+f 101//38 135//38 102//38
+f 102//39 135//39 131//39
+f 102//40 131//40 105//40
+f 136//22 109//22 134//22
+f 134//41 109//41 44//41
+f 134//42 44//42 43//42
+f 61//43 137//43 7//43
+f 7//43 137//43 138//43
+f 7//43 138//43 5//43
+f 55//44 139//44 53//44
+f 53//44 139//44 140//44
+f 53//45 140//45 51//45
+f 51//45 140//45 141//45
+f 51//46 141//46 45//46
+f 45//46 141//46 142//46
+f 45//47 142//47 46//47
+f 46//47 142//47 143//47
+f 46//48 143//48 49//48
+f 49//48 143//48 144//48
+f 49//49 144//49 58//49
+f 58//49 144//49 145//49
+f 58//50 145//50 60//50
+f 60//50 145//50 146//50
+f 60//51 146//51 61//51
+f 61//51 146//51 137//51
+f 23//52 147//52 28//52
+f 28//52 147//52 139//52
+f 28//53 139//53 55//53
+f 148//54 29//54 149//54
+f 149//54 29//54 32//54
+f 149//55 32//55 73//55
+f 67//56 150//56 69//56
+f 69//56 150//56 151//56
+f 69//57 151//57 71//57
+f 71//57 151//57 152//57
+f 71//58 152//58 66//58
+f 66//58 152//58 153//58
+f 66//59 153//59 64//59
+f 64//59 153//59 154//59
+f 64//60 154//60 62//60
+f 62//60 154//60 155//60
+f 62//61 155//61 76//61
+f 76//61 155//61 156//61
+f 76//62 156//62 74//62
+f 74//62 156//62 157//62
+f 74//63 157//63 73//63
+f 73//63 157//63 149//63
+f 150//64 67//64 158//64
+f 158//65 67//65 34//65
+f 158//64 34//64 31//64
+f 107//66 159//66 44//66
+f 44//66 159//66 160//66
+f 44//66 160//66 41//66
+f 104//67 161//67 106//67
+f 106//67 161//67 162//67
+f 106//68 162//68 103//68
+f 103//68 162//68 163//68
+f 103//69 163//69 96//69
+f 96//69 163//69 164//69
+f 96//70 164//70 97//70
+f 97//70 164//70 165//70
+f 97//71 165//71 100//71
+f 100//71 165//71 166//71
+f 100//72 166//72 110//72
+f 110//72 166//72 167//72
+f 110//73 167//73 108//73
+f 108//73 167//73 168//73
+f 108//74 168//74 107//74
+f 107//74 168//74 159//74
+f 40//75 169//75 42//75
+f 42//75 169//75 161//75
+f 42//76 161//76 104//76
+f 170//77 8//77 171//77
+f 171//77 8//77 35//77
+f 171//78 35//78 87//78
+f 89//79 172//79 91//79
+f 91//79 172//79 173//79
+f 91//80 173//80 93//80
+f 93//80 173//80 174//80
+f 93//81 174//81 80//81
+f 80//81 174//81 175//81
+f 80//82 175//82 81//82
+f 81//82 175//82 176//82
+f 81//83 176//83 83//83
+f 83//83 176//83 177//83
+f 83//84 177//84 84//84
+f 84//84 177//84 178//84
+f 84//85 178//85 85//85
+f 85//85 178//85 179//85
+f 85//86 179//86 87//86
+f 87//86 179//86 171//86
+f 172//87 89//87 180//87
+f 180//88 89//88 37//88
+f 180//87 37//87 10//87
+f 115//1 114//1 116//1
+f 116//1 114//1 113//1
+f 116//1 113//1 118//1
+f 118//1 113//1 117//1
+f 118//1 117//1 181//1
+f 181//1 117//1 182//1
+f 181//1 182//1 183//1
+f 183//1 182//1 184//1
+f 59//89 118//89 57//89
+f 57//89 118//89 181//89
+f 57//90 181//90 50//90
+f 50//90 181//90 183//90
+f 50//91 183//91 48//91
+f 48//91 183//91 184//91
+f 48//92 184//92 47//92
+f 47//92 184//92 182//92
+f 47//22 182//22 56//22
+f 56//22 182//22 117//22
+f 185//1 186//1 187//1
+f 187//1 186//1 188//1
+f 187//1 188//1 123//1
+f 123//1 188//1 124//1
+f 123//1 124//1 119//1
+f 119//1 124//1 122//1
+f 119//1 122//1 120//1
+f 120//1 122//1 121//1
+f 78//93 124//93 63//93
+f 63//93 124//93 188//93
+f 63//94 188//94 65//94
+f 65//94 188//94 186//94
+f 65//95 186//95 72//95
+f 72//95 186//95 185//95
+f 72//96 185//96 70//96
+f 70//96 185//96 187//96
+f 70//11 187//11 68//11
+f 68//11 187//11 123//11
+f 127//1 126//1 128//1
+f 128//1 126//1 125//1
+f 128//1 125//1 130//1
+f 130//1 125//1 129//1
+f 130//1 129//1 189//1
+f 189//1 129//1 190//1
+f 189//1 190//1 191//1
+f 191//1 190//1 192//1
+f 95//89 130//89 82//89
+f 82//89 130//89 189//89
+f 82//90 189//90 79//90
+f 79//90 189//90 191//90
+f 79//91 191//91 94//91
+f 94//91 191//91 192//91
+f 94//92 192//92 92//92
+f 92//92 192//92 190//92
+f 92//22 190//22 90//22
+f 90//22 190//22 129//22
+f 193//1 194//1 195//1
+f 195//1 194//1 196//1
+f 195//1 196//1 135//1
+f 135//1 196//1 136//1
+f 135//1 136//1 131//1
+f 131//1 136//1 134//1
+f 131//1 134//1 132//1
+f 132//1 134//1 133//1
+f 109//93 136//93 111//93
+f 111//93 136//93 196//93
+f 111//94 196//94 112//94
+f 112//94 196//94 194//94
+f 112//95 194//95 99//95
+f 99//95 194//95 193//95
+f 99//96 193//96 98//96
+f 98//96 193//96 195//96
+f 98//11 195//11 101//11
+f 101//11 195//11 135//11
+f 1//95 197//95 20//95
+f 20//95 197//95 198//95
+f 199//97 13//97 198//97
+f 198//97 13//97 20//97
+f 13//91 199//91 14//91
+f 14//91 199//91 200//91
+f 197//98 1//98 200//98
+f 200//98 1//98 14//98
+f 200//1 199//1 197//1
+f 197//1 199//1 198//1
+f 148//2 158//2 201//2
+f 148//2 201//2 202//2
+f 170//2 171//2 180//2
+f 170//2 180//2 203//2
+f 170//2 203//2 204//2
+f 170//2 204//2 205//2
+f 150//2 158//2 148//2
+f 150//2 148//2 149//2
+f 150//2 149//2 157//2
+f 150//2 157//2 156//2
+f 150//2 156//2 155//2
+f 150//2 155//2 151//2
+f 138//2 137//2 139//2
+f 138//2 139//2 147//2
+f 138//2 147//2 206//2
+f 172//2 180//2 171//2
+f 172//2 171//2 179//2
+f 172//2 179//2 178//2
+f 172//2 178//2 177//2
+f 172//2 177//2 173//2
+f 207//2 208//2 209//2
+f 207//2 209//2 205//2
+f 207//2 205//2 204//2
+f 207//2 204//2 210//2
+f 207//2 210//2 211//2
+f 207//2 211//2 206//2
+f 207//2 206//2 147//2
+f 212//2 148//2 202//2
+f 212//2 202//2 213//2
+f 212//2 213//2 214//2
+f 212//2 214//2 215//2
+f 212//2 215//2 216//2
+f 212//2 216//2 217//2
+f 173//2 177//2 174//2
+f 174//2 177//2 176//2
+f 174//2 176//2 175//2
+f 140//2 139//2 141//2
+f 141//2 139//2 137//2
+f 141//2 137//2 142//2
+f 142//2 137//2 146//2
+f 142//2 146//2 143//2
+f 143//2 146//2 145//2
+f 143//2 145//2 144//2
+f 209//2 208//2 218//2
+f 218//2 208//2 219//2
+f 218//2 219//2 216//2
+f 216//2 219//2 217//2
+f 151//2 155//2 152//2
+f 152//2 155//2 154//2
+f 152//2 154//2 153//2
+f 215//2 214//2 169//2
+f 169//2 214//2 220//2
+f 169//2 220//2 160//2
+f 169//2 160//2 161//2
+f 161//2 160//2 159//2
+f 161//2 159//2 162//2
+f 162//2 159//2 163//2
+f 163//2 159//2 164//2
+f 164//2 159//2 168//2
+f 164//2 168//2 165//2
+f 165//2 168//2 167//2
+f 165//2 167//2 166//2
+f 204//91 38//91 210//91
+f 210//91 38//91 17//91
+f 180//99 10//99 203//99
+f 203//99 10//99 9//99
+f 203//100 9//100 204//100
+f 204//100 9//100 38//100
+f 209//101 12//101 205//101
+f 205//101 12//101 11//101
+f 205//102 11//102 170//102
+f 170//102 11//102 8//102
+f 218//97 26//97 209//97
+f 209//97 26//97 12//97
+f 216//97 27//97 218//97
+f 218//97 27//97 26//97
+f 169//103 40//103 215//103
+f 215//103 40//103 21//103
+f 215//104 21//104 216//104
+f 216//104 21//104 27//104
+f 214//105 22//105 220//105
+f 220//105 22//105 39//105
+f 220//106 39//106 160//106
+f 160//106 39//106 41//106
+f 22//95 214//95 18//95
+f 18//95 214//95 213//95
+f 18//95 213//95 19//95
+f 19//95 213//95 202//95
+f 158//107 31//107 201//107
+f 201//107 31//107 30//107
+f 201//108 30//108 202//108
+f 202//108 30//108 19//108
+f 217//109 3//109 212//109
+f 212//109 3//109 2//109
+f 212//110 2//110 148//110
+f 148//110 2//110 29//110
+f 219//98 4//98 217//98
+f 217//98 4//98 3//98
+f 208//98 15//98 219//98
+f 219//98 15//98 4//98
+f 147//111 23//111 207//111
+f 207//111 23//111 16//111
+f 207//112 16//112 208//112
+f 208//112 16//112 15//112
+f 211//113 25//113 206//113
+f 206//113 25//113 24//113
+f 206//114 24//114 138//114
+f 138//114 24//114 5//114
+f 210//91 17//91 211//91
+f 211//91 17//91 25//91
diff --git a/conf/simulator/pybullet/robobee.urdf b/conf/simulator/pybullet/robobee.urdf
new file mode 100644
index 0000000000..89af5b9e54
--- /dev/null
+++ b/conf/simulator/pybullet/robobee.urdf
@@ -0,0 +1,170 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/math/pprz_algebra_double.c b/sw/airborne/math/pprz_algebra_double.c
index 5a98368c91..5d1a34bc68 100644
--- a/sw/airborne/math/pprz_algebra_double.c
+++ b/sw/airborne/math/pprz_algebra_double.c
@@ -111,6 +111,15 @@ void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct D
v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
}
+void double_quat_comp(struct DoubleQuat *a2c, struct DoubleQuat *a2b, struct DoubleQuat *b2c)
+{
+ a2c->qi = a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz;
+ a2c->qx = a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy;
+ a2c->qy = a2b->qi * b2c->qy - a2b->qx * b2c->qz + a2b->qy * b2c->qi + a2b->qz * b2c->qx;
+ a2c->qz = a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi;
+}
+
+
void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
{
/*RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);*/
diff --git a/sw/airborne/math/pprz_algebra_double.h b/sw/airborne/math/pprz_algebra_double.h
index 900581c331..a25c399393 100644
--- a/sw/airborne/math/pprz_algebra_double.h
+++ b/sw/airborne/math/pprz_algebra_double.h
@@ -157,6 +157,11 @@ extern void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e);
extern void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q);
extern void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in);
+/** Composition (multiplication) of two quaternions.
+ * a2c = a2b comp b2c , aka a2c = a2b * b2c
+ */
+extern void double_quat_comp(struct DoubleQuat *a2c, struct DoubleQuat *a2b, struct DoubleQuat *b2c);
+
/** initialises a rotation matrix to identity */
static inline void double_rmat_identity(struct DoubleRMat *rm)
{
diff --git a/sw/simulator/nps/nps_fdm_pybullet.c b/sw/simulator/nps/nps_fdm_pybullet.c
new file mode 100644
index 0000000000..b7fe7631ff
--- /dev/null
+++ b/sw/simulator/nps/nps_fdm_pybullet.c
@@ -0,0 +1,387 @@
+/*
+ * Copyright (C) 2021 Fabien-B
+ *
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+#include "nps_fdm.h"
+
+#include
+#include
+#include
+
+#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"
+
+#define _WIDEN(x) L ## x
+#define WIDEN(x) _WIDEN(x)
+
+#ifndef PYTHON_EXEC
+#define PYTHON_EXEC "python3"
+#endif
+MESSAGE("PyBullet using" VALUE(PYTHON_EXEC))
+
+#ifndef PYBULLET_GUI
+#define PYBULLET_GUI TRUE
+#endif
+
+#ifdef NPS_ACTUATORS_ORDER
+ int actuators_order[ACTUATORS_NB] = NPS_ACTUATORS_ORDER;
+#else
+ #error "[PyBullet] missing NPS_ACTUATORS_ORDER define!"
+#endif
+
+#ifndef NPS_PYBULLET_MODULE
+#define NPS_PYBULLET_MODULE "simple_quad_sim"
+MESSAGE("NPS_PYBULLET_MODULE not defined, take 'simple_quad_sim' as default value.")
+#endif
+
+
+#ifndef NPS_PYBULLET_URDF
+#define NPS_PYBULLET_URDF "robobee.urdf"
+MESSAGE("NPS_PYBULLET_URDF not defined, take 'robobee.urdf' as default value.")
+#endif
+
+
+// NpsFdm structure
+struct NpsFdm fdm;
+
+// Reference point
+static struct LtpDef_d ltpdef;
+
+//rotation from pybullet to pprz
+struct DoubleQuat quat_to_pprz = {
+ 1/sqrt(2),
+ 0,
+ 0,
+ 1/sqrt(2),
+};
+
+// python object declarations
+PyObject *fdm_module = NULL;
+PyObject *bullet_fdm = NULL;
+
+// static python related function declaration
+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 functions declaration
+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)
+{
+ python_init(dt);
+
+ fdm.init_dt = dt; // (1 / simulation freq)
+ fdm.curr_dt = dt;
+ fdm.time = dt;
+
+ fdm.on_ground = TRUE;
+
+ fdm.nan_count = 0;
+ fdm.pressure = -1;
+ fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE;
+ fdm.total_pressure = -1;
+ fdm.dynamic_pressure = -1;
+ fdm.temperature = -1;
+
+ fdm.ltpprz_to_body_eulers.psi = 0.0;
+ init_ltp();
+
+ // run a first step to initialize all fdm fields
+ double dummy_commands[] = {1, 2, 3, 4};
+ nps_fdm_run_step(false, dummy_commands, 4);
+}
+
+void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb __attribute__((unused)))
+{
+ // TODO create a np.array instead ?
+ PyObject* pcmd = PyList_New(commands_nb);
+
+ for(int i=0; i 0.5:
+ # self.last_time_print = t
+ # print(f"{commands[0]:.2f} {commands[1]:.2f} {commands[2]:.2f} {commands[3]:.2f}")
+
+ self.apply_force_and_moments(commands)
+ p.stepSimulation(physicsClientId=self.physicsClient)
+ return self.get_observation()
+
+ def step_debug(self,commands):
+ # commands are not used...
+ roll_cmd = p.readUserDebugParameter(self.roll_Id)
+ pitch_cmd = p.readUserDebugParameter(self.pitch_Id)
+ yaw_cmd = p.readUserDebugParameter(self.yaw_Id)
+
+ # p.applyExternalForce(self.vehicle,
+ # -1,
+ # forceObj=[0, 0, 9 + yaw_cmd*100],
+ # posObj=[0, 0, 0],
+ # flags=p.LINK_FRAME,
+ # physicsClientId=self.physicsClient
+ # )
+
+ p.applyExternalTorque(self.vehicle,
+ -1, # FIXME : this is not correct , use center of mass !!!
+ torqueObj=[roll_cmd, pitch_cmd, yaw_cmd],
+ flags=p.LINK_FRAME,
+ physicsClientId=self.physicsClient
+ )
+
+ p.stepSimulation(physicsClientId=self.physicsClient)
+ return self.get_observation()
+
+ def get_observation(self):
+ # Get observation
+ v_pos, v_quat = p.getBasePositionAndOrientation(self.vehicle)
+ # print(v_Pos, type(v_Pos))
+ v_rpy = p.getEulerFromQuaternion(v_quat)
+ v_vel, v_ang_v = p.getBaseVelocity(self.vehicle)
+
+ self.accel = (np.array(v_vel) - self.vel)/self.dt
+ self.vel = np.array(v_vel)
+ self.ang_accel = (np.array(v_ang_v) - self.ang_vel)/self.dt
+ self.ang_vel = np.array(v_ang_v)
+
+ self.observation = {'pos':v_pos,
+ 'quat':v_quat,
+ 'rpy':v_rpy,
+ 'vel':tuple(self.vel),
+ 'ang_v':tuple(self.ang_vel),
+ 'accel':tuple(self.accel),
+ 'ang_accel':tuple(self.ang_accel)
+ }
+ return self.observation
+
+ def reset(self):
+ # Reset the simulation and the state
+ # p.resetSimulation(physicsClientId=self.physicsClient) # FIXME : this is not the correct way to reset the simulation
+ p.resetBasePositionAndOrientation(self.vehicle, self.vehicle_start_pos, self.vehicle_start_orientation)
+ p.resetBaseVelocity(self.vehicle, [0, 0, 0], [0, 0, 0])
+
+ return self.get_observation()
+
+
+
+if __name__ == "__main__":
+ from time import sleep
+ debug = True
+ m = BulletFDM(GUI=True, debug=debug)
+
+ # An example simulation loop with random commands generation
+ while 1:
+ for i in range(200):
+ # commands = np.array([10., 10., 10., 10.]) # fixed rpms
+ commands = np.random.normal(13., 0.5, 4) # random rpms
+ if debug:
+ m.step_debug(commands)
+ else:
+ m.step(commands)
+ sleep(0.05) # FIXME : checck the computation time and sleep to sync realtime...
+ m.reset()