[gazebo] Add actuator dynamics and bebop model (#2219)

* Add gazebo model for Parrot Bebop 1

* Add first-order high pass filter

* First implementation of actuator dynamics and spinup torque

Flies ok with PID control

Does not fly ok with INDI, the required G1 R is 0.159 instead of
0.0022 (estimated by flying in adaptive mode). The other parameters
are close to those specified in the airframe file.

* FIX incorrect yaw behavior

Fixed two bugs:
1. gazebo_actuators.torques was set to NPS_ACTUATOR_THRUSTS
2. spinup torque direction is now also controlled by motor mixing

* Clean up debug code and file logger

* Clean up gazebo example airframes

* Minor cleanup

* FIX warnings about missing initializer in fdm_gazebo

* Update documentation

* Remove modules section, move to firmware

* Minor fixes to fdm_gazebo and high_pass_filter

* Move actuator dynamics to included airframe files

Should prevent duplicate actuator defines spread accross all
airframe files.

* Fix FILTER_ROLL_RATE in bebop2_indi airframe

Moved to firmware targets so it will be disabled in gazebo.
Is this actually working in jsbsim??

* Update documentation

* Fix newlines

* Minor fixes in nps_fdm_gazebo.cpp

* Re-enable filter roll rate

Caution: causes oscillations around roll axis in gazebo

* Disable roll filter and cleanup

conf/airframes/examples/ardrone2_gazebo.xml

@@ -191,12 +191,10 @@
 
   <section name="SIMULATOR" prefix="NPS_">
     <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
-    <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/>
-    <define name="ACTUATOR_TORQUES" value="0.155, -0.155, 0.155, -0.155" type="float[]"/>
     <define name="JSBSIM_MODEL" value="simple_ardrone2" type="string"/>
     <define name="SENSORS_PARAMS" value="nps_sensors_params_ardrone2.h" type="string"/>
-    <define name="BYPASS_AHRS" value="1"/>
   </section>
+  <include href="conf/simulator/gazebo/airframes/ardrone2.xml"/>
 
   <section name="AUTOPILOT">
     <define name="MODE_STARTUP" value="AP_MODE_NAV"/>

conf/airframes/examples/ardrone2_gazebo_cyberzoo.xml

@@ -194,14 +194,10 @@
   <section name="SIMULATOR" prefix="NPS_">
     <define name="GAZEBO_WORLD" value='"ardrone_cyberzoo.world"'/>
     <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
-    <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="double[]"/>
-    <define name="ACTUATOR_TORQUES" value="0.155, -0.155, 0.155, -0.155" type="double[]"/>
     <define name="JSBSIM_MODEL" value="simple_ardrone2" type="string"/>
     <define name="SENSORS_PARAMS" value="nps_sensors_params_ardrone2.h" type="string"/>
-    <define name="BYPASS_INS" value="1"/>
-    <define name="BYPASS_AHRS" value="1"/>
-    <define name="SIMULATE_VIDEO" value="1"/>
   </section>
+  <include href="conf/simulator/gazebo/airframes/ardrone2.xml"/>
 
   <section name="AUTOPILOT">
     <define name="MODE_STARTUP" value="AP_MODE_NAV"/>

conf/airframes/examples/bebop.xml

@@ -33,9 +33,7 @@
     <module name="stabilization" type="int_quat"/>
     <module name="ahrs"          type="float_mlkf"/>
     <module name="ins" type="extended"/>
-  </firmware>
+    
-
-  <modules main_freq="512">
     <module name="geo_mag"/>
     <module name="air_data"/>
     <module name="send_imu_mag_current"/>
@@ -53,8 +51,7 @@
     </module>
 
     <module name="bebop_ae_awb"/>
-
+  </firmware>
-  </modules>
 
   <commands>
     <axis name="PITCH" failsafe_value="0"/>
@@ -196,6 +193,7 @@
     <define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
     <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
   </section>
+  <include href="conf/simulator/gazebo/airframes/bebop.xml"/>
 
   <section name="AUTOPILOT">
     <define name="MODE_STARTUP" value="AP_MODE_NAV"/>

conf/airframes/examples/bebop2_indi.xml

@@ -10,8 +10,8 @@
     <target name="nps" board="pc">
       <module name="fdm" type="jsbsim"/>
       <module name="udp"/>
-      <!-- rotor inertia not modelled in simple_quad JSBSim model, set G2 to zero -->
+      <define name="STABILIZATION_INDI_G2_R" value="0.0"/><!-- for jsbsim (rotor inertia is not modelled) -->
-      <define name="STABILIZATION_INDI_G2_R" value="0.0"/>
+      <!-- <define name="STABILIZATION_INDI_G2_R" value="0.20"/> --><!-- for gazebo -->
     </target>
 
 	  <!-- Subsystem section -->
@@ -27,9 +27,7 @@
       <define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
     </module>
     <module name="ins" type="extended"/>
-  </firmware>
+    
-
-  <modules main_freq="512">
     <module name="geo_mag"/>
     <module name="air_data"/>
     <module name="send_imu_mag_current"/>
@@ -37,8 +35,8 @@
     <module name="logger_file">
       <define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
     </module>
-  </modules>
+  </firmware>
-
+  
   <commands>
     <axis name="PITCH" failsafe_value="0"/>
     <axis name="ROLL" failsafe_value="0"/>
@@ -140,7 +138,7 @@
     <define name="G1_R" value="0.0022"/>
 
     <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="TRUE"/>
+    <define name="FILTER_ROLL_RATE" value="FALSE"/><!-- Set to TRUE when flying without locked dampers -->
     <define name="FILTER_PITCH_RATE" value="FALSE"/>
     <define name="FILTER_YAW_RATE" value="FALSE"/>
 
@@ -195,6 +193,7 @@
     <define name="JSBSIM_MODEL" value="simple_x_quad" type="string"/>
     <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
   </section>
+  <include href="conf/simulator/gazebo/airframes/bebop.xml"/>
 
   <section name="AUTOPILOT">
     <define name="MODE_STARTUP" value="AP_MODE_NAV"/>

conf/modules/fdm_gazebo.xml

@@ -50,50 +50,76 @@
               <module name="fdm" type="gazebo"/>
             </target>
           @endcode
-        2. Add actuator thrusts and torques to the SIMULATOR section:
+        2. Include the gazebo defines for the vehicle:
           @code{.xml}
             <section name="SIMULATOR" prefix="NPS_">
-              <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
-              <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/>
-              <define name="ACTUATOR_TORQUES" value="0.155, -0.155, 0.155, -0.155" type="float[]"/>
-              ...
-            <section>
-          @endcode
-          The thrusts and torques are expressed in SI units (N, Nm) and should
-          be in the same order as the ACTUATOR_NAMES.
-        3. In the same section, bypass the AHRS and INS as these are not
-          supported yet:
-          @code{.xml}
-            <section name="SIMULATOR" prefix="NPS_">
-              ...
-              <define name="BYPASS_AHRS" value="1"/>
-              <define name="BYPASS_INS" value="1"/>
               ...
             </section>
+            <include href="conf/simulator/gazebo/airframes/ardrone2.xml"/>
           @endcode
-        4. If required, enable video thread simulation:
+          - If conf/simulator/gazebo/airframes does not contain an xml for the
-          @code{.xml}
+          vehicle model, it should be created with the following contents:
-            <section name="SIMULATOR" prefix="NPS_">
+            1. Actuator thrusts and torques
-              ...
+              @code{.xml}
-              <define name="SIMULATE_VIDEO" value="1"/>
+              <!DOCTYPE airframe SYSTEM "../../../airframes/airframe.dtd">
-              ...
+  
-            </section>
+                <airframe>
-          @endcode
+                  <section name="SIMULATOR" prefix="NPS_">
-        5. If required, set the aircraft model in the airframe file:
+                    <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/>
-          @code{.xml}
+                    <define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="float[]"/>
-            <section name="SIMULATOR" prefix="NPS_">
+                    ...
-              ...
+                  </section>
-              <define name="GAZEBO_AC_NAME" value="my_uav"/>
+                </airframe>
-            </section>
+              @endcode
-          @endcode
+              The thrusts and torques are expressed in SI units (N, Nm) and should
-        6. Make sure all included modules work with nps. At the moment, most of
+              be in the same order as the ACTUATOR_NAMES defined in the airframe file.
+              The torque direction is determined automatically from the motor mixing.
+            2. (Optional) Add actuator dynamics to the SIMULATOR section:
+              @code{.xml}
+                <section name="SIMULATOR" prefix="NPS_">
+                  ...
+                  <define name="ACTUATOR_TIME_CONSTANTS" value="0.02, 0.02, 0.02, 0.02" type="float[]"/>
+                  <define name="ACTUATOR_MAX_ANGULAR_MOMENTUM" value="0.19, 0.19, 0.19, 0.19" type="float[]"/>
+                  ...
+                </section>
+              @endcode
+              Actuator time constants can be provided without specifying the
+              actuator's maximum angular momentum. If the maximum angular momentum
+              is provided as well, it is used to calculate the rotor spin-up torque.
+            3. In the same section, bypass the AHRS and INS as these are not
+              supported yet:
+              @code{.xml}
+                <section name="SIMULATOR" prefix="NPS_">
+                  ...
+                  <define name="BYPASS_AHRS" value="1"/>
+                  <define name="BYPASS_INS" value="1"/>
+                  ...
+                </section>
+              @endcode
+    
+            4. If required, enable video thread simulation:
+              @code{.xml}
+                <section name="SIMULATOR" prefix="NPS_">
+                  ...
+                  <define name="SIMULATE_VIDEO" value="1"/>
+                  ...
+                </section>
+              @endcode
+            5. Set the aircraft model in the xml file:
+              @code{.xml}
+                <section name="SIMULATOR" prefix="NPS_">
+                  ...
+                  <define name="GAZEBO_AC_NAME" value="my_uav"/>
+                </section>
+              @endcode
+        3. Make sure all included modules work with nps. At the moment, most of
           the modules that depend on video_thread are only built when ap is
           selected as the target. To fix this, add nps to the target attribute
           in the module xml, e.g.:
           @code{.xml}
             <makefile target="ap|nps">
           @endcode
-      4. If required, set the simulation environment in the flight plan file:
+      4. The simulation environment is set in the flight plan file:
         @code{.xml}
           <flight_plan ...>
             <header>
@@ -101,7 +127,7 @@
               #define NPS_GAZEBO_WORLD "my.world"
             </header>
             ...
-          <flight_plan>
+          </flight_plan>
         @endcode
     </description>
     <configure name="NPS_DEBUG_VIDEO" value="0|1" description="show window with video for debugging"/>

conf/simulator/gazebo/airframes/ardrone2.xml

@@ -0,0 +1,10 @@
+<!DOCTYPE airframe SYSTEM "../../../airframes/airframe.dtd">
+
+<airframe>
+  <section name="SIMULATOR" prefix="NPS_">
+    <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/>
+    <define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="float[]"/>
+    <define name="BYPASS_AHRS" value="1"/>
+    <define name="SIMULATE_VIDEO" value="1"/>
+  </section>
+</airframe>

conf/simulator/gazebo/airframes/bebop.xml

@@ -0,0 +1,12 @@
+<!DOCTYPE airframe SYSTEM "../../../airframes/airframe.dtd">
+
+<airframe>
+  <section name="SIMULATOR" prefix="NPS_">
+    <define name="ACTUATOR_THRUSTS" value="2.09, 2.09, 2.09, 2.09" type="float[]"/>
+    <define name="ACTUATOR_TORQUES" value="0.0250, 0.0250, 0.0250, 0.0250" type="float[]"/>
+    <define name="ACTUATOR_TIME_CONSTANTS" value="0.02, 0.02, 0.02, 0.02" type="float[]"/>
+    <define name="ACTUATOR_MAX_ANGULAR_MOMENTUM" value="0.19, 0.19, 0.19, 0.19" type="float[]"/>
+    <define name="GAZEBO_AC_NAME" value="bebop" type="string"/>
+    <define name="BYPASS_AHRS" value="1"/>
+  </section>
+</airframe>

conf/simulator/gazebo/models/bebop/bebop.sdf

@@ -0,0 +1,264 @@
+<?xml version='1.0'?>
+<sdf version='1.4'>
+  <model name="bebop">
+    <pose>0 0 .1 0 0 0</pose>
+
+    <link name="chassis">
+      <velocity_decay>
+        <linear>0.001</linear>
+      </velocity_decay>
+      
+      <inertial>
+        <mass>0.38905</mass>
+        <inertia>
+          <ixx> 0.000906 </ixx>
+          <iyy> 0.001242 </iyy>
+          <izz> 0.002054 </izz>
+          <ixy> 0. </ixy>
+          <ixz> 0.000014 </ixz>
+          <iyz> 0. </iyz>
+        </inertia>
+      </inertial>
+
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.4 0.4 0.05</size>
+          </box>
+        </geometry>
+      </collision>
+
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.15 0.05 0.05</size>
+          </box>
+        </geometry>
+      </visual>
+      
+      <sensor name="contactsensor" type="contact">
+        <contact>
+          <collision>collision</collision>
+        </contact>
+      </sensor>
+    </link>
+
+    <!-- MOTORS -->
+    <link name="nw_motor">
+      <pose>0.077 0.095 0 0 0 0</pose>
+      <inertial>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.0001</ixx>
+          <iyy>0.0001</iyy>
+          <izz>0.0001</izz>
+          <ixy>0</ixy>
+          <iyz>0</iyz>
+          <ixz>0</ixz>
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <geometry>
+          <cylinder>
+            <radius>0.05</radius>
+            <length>0.02</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <diffuse>0.25 0.85 0.95 1</diffuse>
+          <ambient>0.25 0.85 0.95 1</ambient>
+        </material>
+      </visual>
+    </link>
+
+    <joint type="fixed" name="nw_motor_joint">
+      <parent>chassis</parent>
+      <child>nw_motor</child>
+    </joint>
+
+    <link name="se_motor">
+      <pose>-0.077 -0.095 0 0 0 0</pose>
+      <inertial>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.0001</ixx>
+          <iyy>0.0001</iyy>
+          <izz>0.0001</izz>
+          <ixy>0</ixy>
+          <iyz>0</iyz>
+          <ixz>0</ixz>
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <geometry>
+          <cylinder>
+            <radius>0.05</radius>
+            <length>0.02</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <diffuse>0.1 0.1 0.1 1</diffuse>
+          <ambient>0.1 0.1 0.1 1</ambient>
+        </material>
+      </visual>
+    </link>
+
+    <joint type="fixed" name="se_motor_joint">
+      <parent>chassis</parent>
+      <child>se_motor</child>
+    </joint>
+
+    <link name="ne_motor">
+      <pose>0.077 -0.095 0 0 0 0</pose>
+      <inertial>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.0001</ixx>
+          <iyy>0.0001</iyy>
+          <izz>0.0001</izz>
+          <ixy>0</ixy>
+          <iyz>0</iyz>
+          <ixz>0</ixz>
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <geometry>
+          <cylinder>
+            <radius>0.05</radius>
+            <length>0.02</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <diffuse>0.25 0.85 0.95 1</diffuse>
+          <ambient>0.25 0.85 0.95 1</ambient>
+        </material>
+      </visual>
+    </link>
+
+    <joint type="fixed" name="ne_motor_joint">
+      <parent>chassis</parent>
+      <child>ne_motor</child>
+    </joint>
+
+    <link name="sw_motor">
+      <pose>-0.077 0.095 0 0 0 0</pose>
+      <inertial>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.0001</ixx>
+          <iyy>0.0001</iyy>
+          <izz>0.0001</izz>
+          <ixy>0</ixy>
+          <iyz>0</iyz>
+          <ixz>0</ixz>
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <geometry>
+          <cylinder>
+            <radius>0.05</radius>
+            <length>0.02</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <diffuse>0.1 0.1 0.1 1</diffuse>
+          <ambient>0.1 0.1 0.1 1</ambient>
+        </material>
+      </visual>
+    </link>
+
+    <joint type="fixed" name="sw_motor_joint">
+      <parent>chassis</parent>
+      <child>sw_motor</child>
+    </joint>
+
+
+    <!-- CAMERAS -->
+
+    <link name="front_camera">
+      <pose>0.15 0 0 0 0 0</pose>
+      <inertial>
+        <mass>0.001</mass>
+        <inertia>
+          <ixx>0.0001</ixx>
+          <iyy>0.0001</iyy>
+          <izz>0.0001</izz>
+          <ixy>0</ixy>
+          <iyz>0</iyz>
+          <ixz>0</ixz>
+        </inertia>
+      </inertial>
+      <sensor type="camera" name="front_camera">
+        <update_rate>30.0</update_rate>
+        <camera name="front_camera">
+          <horizontal_fov>1.3962634</horizontal_fov>
+          <image>
+            <width>1280</width>
+            <height>720</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>300</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <!-- Noise is sampled independently per pixel on each frame. That pixel's 
+              noise value is added to each of its color channels, which at that point lie 
+              in the range [0,1]. -->
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+      </sensor>
+    </link>
+    
+    <joint name="front_camera_joint" type="fixed">
+      <parent>chassis</parent>
+      <child>front_camera</child>
+    </joint>
+    
+    <link name="bottom_camera">
+      <pose>0 0 -.03 0 1.57 0</pose>
+      <inertial>
+        <mass>0.001</mass>
+        <inertia>
+          <ixx>0.0001</ixx>
+          <iyy>0.0001</iyy>
+          <izz>0.0001</izz>
+          <ixy>0</ixy>
+          <iyz>0</iyz>
+          <ixz>0</ixz>
+        </inertia>
+      </inertial>
+      <sensor type="camera" name="bottom_camera">
+        <update_rate>30.0</update_rate>
+        <camera name="bottom_camera">
+          <horizontal_fov>1.3962634</horizontal_fov>
+          <image>
+            <width>320</width>
+            <height>240</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>300</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <!-- Noise is sampled independently per pixel on each frame. That pixel's 
+              noise value is added to each of its color channels, which at that point lie 
+              in the range [0,1]. -->
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+      </sensor>
+    </link>
+    
+    <joint name="bottom_camera_joint" type="fixed">
+      <parent>chassis</parent>
+      <child>bottom_camera</child>
+    </joint>
+  </model>
+</sdf>

conf/simulator/gazebo/models/bebop/model.config

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>Bebop (Paparazzi)</name>
+  <version>1.0</version>
+  <sdf version='1.4'>bebop.sdf</sdf>
+
+  <author>
+   <name>Tom van Dijk</name>
+   <email>tomvand@users.noreply.github.com</email>
+  </author>
+
+  <description>
+    Simple Bebop model for use with Paparazzi's NPS (http://wiki.paparazziuav.org).
+  </description>
+</model>

sw/airborne/filters/high_pass_filter.h

@@ -30,6 +30,63 @@
 #include "std.h"
 #include "math/pprz_algebra_int.h"
 
+/** First order high pass filter structure.
+ *
+ * using bilinear z transform
+ */
+struct FirstOrderHighPass
+{
+  float time_const;
+  float last_in;
+  float last_out;
+};
+
+/** Init first order high pass filter.
+ *
+ * Laplace transform in continuous time:
+ *          tau*s
+ * H(s) = ---------
+ *        1 + tau*s
+ *
+ * @param filter first order high pass filter structure
+ * @param tau time constant of the first order high pass filter
+ * @param sample_time sampling period of the signal
+ * @param value initial value of the filter
+ */
+static inline void init_first_order_high_pass(struct FirstOrderHighPass *filter,
+    float tau, float sample_time, float value)
+{
+  filter->last_in = value;
+  filter->last_out = value;
+  filter->time_const = 2.0f * tau / sample_time;
+}
+
+/** Update first order high pass filter state with a new value.
+ *
+ * @param filter first order high pass filter structure
+ * @param value new input value of the filter
+ * @return new filtered value
+ */
+static inline float update_first_order_high_pass(
+    struct FirstOrderHighPass *filter, float value)
+{
+  float out = (filter->time_const * (value - filter->last_in)
+      + (filter->time_const - 1.0f) * filter->last_out)
+      / (1.0f + filter->time_const);
+  filter->last_in = value;
+  filter->last_out = out;
+  return out;
+}
+
+/** Get current value of the first order high pass filter.
+ *
+ * @param filter first order high pass filter structure
+ * @return current value of the filter
+ */
+static inline float get_first_order_high_pass(struct FirstOrderHighPass *filter)
+{
+  return filter->last_out;
+}
 
 /** Fourth order filter structure.
  *

sw/simulator/nps/nps_fdm_gazebo.cpp

@@ -56,6 +56,8 @@ extern "C" {
 
 #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"
 }
@@ -102,10 +104,13 @@ struct gazebo_actuators_t {
   string names[NPS_COMMANDS_NB];
   double thrusts[NPS_COMMANDS_NB];
   double torques[NPS_COMMANDS_NB];
+  struct FirstOrderLowPass lowpass[NPS_COMMANDS_NB];
+  struct FirstOrderHighPass highpass[NPS_COMMANDS_NB];
+  double max_ang_momentum[NPS_COMMANDS_NB];
 };
 
-struct gazebo_actuators_t gazebo_actuators = {NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_THRUSTS};
+struct gazebo_actuators_t gazebo_actuators = { NPS_ACTUATOR_NAMES,
-
+    NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { } };
 
 #if NPS_SIMULATE_LASER_RANGE_ARRAY
 extern "C" {
@@ -208,7 +213,7 @@ inline struct DoubleVect3 to_pprz_ltp(gazebo::math::Vector3 xyz)
 // External functions, interface with Paparazzi's NPS as declared in nps_fdm.h
 
 /**
- * Set JSBsim specific fields that are not used for Gazebo.
+ * Initialize actuator dynamics, set unused fields in fdm
  * @param dt
  */
 void nps_fdm_init(double dt)
@@ -216,6 +221,22 @@ void nps_fdm_init(double dt)
   fdm.init_dt = dt; // JSBsim specific
   fdm.curr_dt = dt; // JSBsim specific
   fdm.nan_count = 0; // JSBsim specific
+
+#ifdef NPS_ACTUATOR_TIME_CONSTANTS
+  // Set up low-pass filter to simulate delayed actuator response
+  const float tau[NPS_COMMANDS_NB] = NPS_ACTUATOR_TIME_CONSTANTS;
+  for(int i=0; i<NPS_COMMANDS_NB; i++) {
+    init_first_order_low_pass(&gazebo_actuators.lowpass[i], tau[i], dt, 0.f);
+  }
+#ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
+  // Set up high-pass filter to simulate spinup torque
+  const float Iwmax[NPS_COMMANDS_NB] = NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM;
+  for(int i=0; i<NPS_COMMANDS_NB; i++) {
+    init_first_order_high_pass(&gazebo_actuators.highpass[i], tau[i], dt, 0.f);
+    gazebo_actuators.max_ang_momentum[i] = Iwmax[i];
+  }
+#endif
+#endif
 }
 
 /**
@@ -375,6 +396,7 @@ static void init_gazebo(void)
 
   for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
     gazebo_actuators.torques[i] = -fabs(gazebo_actuators.torques[i]) * yaw_coef[i] / fabs(yaw_coef[i]);
+    gazebo_actuators.max_ang_momentum[i] = -fabs(gazebo_actuators.max_ang_momentum[i]) * yaw_coef[i] / fabs(yaw_coef[i]);
   }
 #endif
   cout << "Gazebo initialized successfully!" << endl;
@@ -536,10 +558,29 @@ static void gazebo_read(void)
  */
 static void gazebo_write(double act_commands[], int commands_nb)
 {
-  // TODO simulte actuator dynamics so indi can work
   for (int i = 0; i < commands_nb; ++i) {
-    double thrust = autopilot.motors_on ? gazebo_actuators.thrusts[i] * act_commands[i] : 0.0;
+    // Thrust setpoint
-    double torque = autopilot.motors_on ? gazebo_actuators.torques[i] * act_commands[i] : 0.0;
+    double sp = autopilot.motors_on ? act_commands[i] : 0.0;  // Normalized thrust setpoint
+
+    // Actuator dynamics, forces and torques
+#ifdef NPS_ACTUATOR_TIME_CONSTANTS
+    // Delayed response from actuator
+    double u = update_first_order_low_pass(&gazebo_actuators.lowpass[i], sp);// Normalized actual thrust
+#else
+    double u = sp;
+#endif
+    double thrust = gazebo_actuators.thrusts[i] * u;
+    double torque = gazebo_actuators.torques[i] * u;
+
+#ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
+    // Spinup torque
+    double udot = update_first_order_high_pass(&gazebo_actuators.highpass[i], sp);
+    double spinup_torque = gazebo_actuators.max_ang_momentum[i] /
+        (2.0 * sqrt(u > 0.05 ? u : 0.05)) * udot;
+    torque += spinup_torque;
+#endif
+
+    // Apply force and torque to gazebo model
     gazebo::physics::LinkPtr link = model->GetLink(gazebo_actuators.names[i]);
     link->AddRelativeForce(gazebo::math::Vector3(0, 0, thrust));
     link->AddRelativeTorque(gazebo::math::Vector3(0, 0, torque));