[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
2026-05-09 22:49:53 +08:00 · 2018-01-30 16:39:18 +01:00
parent 2d86a0b60a
commit cfabb51eed
11 changed files with 477 additions and 61 deletions
@@ -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"/>
@@ -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"/>
@@ -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"/>
-
-  </modules>
+  </firmware>

  <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"/>
@@ -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"/>
+      <define name="STABILIZATION_INDI_G2_R" value="0.0"/><!-- for jsbsim (rotor inertia is not modelled) -->
+      <!-- <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"/>
@@ -50,50 +50,76 @@
              &lt;module name=&quot;fdm&quot; type=&quot;gazebo&quot;/&gt;
            &lt;/target&gt;
          @endcode
-        2. Add actuator thrusts and torques to the SIMULATOR section:
+        2. Include the gazebo defines for the vehicle:
          @code{.xml}
            &lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
-              &lt;define name=&quot;ACTUATOR_NAMES&quot; value=&quot;nw_motor, ne_motor, se_motor, sw_motor&quot; type=&quot;string[]&quot;/&gt;
-              &lt;define name=&quot;ACTUATOR_THRUSTS&quot; value=&quot;1.55, 1.55, 1.55, 1.55&quot; type=&quot;float[]&quot;/&gt;
-              &lt;define name=&quot;ACTUATOR_TORQUES&quot; value=&quot;0.155, -0.155, 0.155, -0.155&quot; type=&quot;float[]&quot;/&gt;
-              ...
-            &lt;section&gt;
-          @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}
-            &lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
-              ...
-              &lt;define name=&quot;BYPASS_AHRS&quot; value=&quot;1&quot;/&gt;
-              &lt;define name=&quot;BYPASS_INS&quot; value=&quot;1&quot;/&gt;
              ...
            &lt;/section&gt;
+            &lt;include href=&quot;conf/simulator/gazebo/airframes/ardrone2.xml&quot;/&gt;
          @endcode
-        4. If required, enable video thread simulation:
-          @code{.xml}
-            &lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
-              ...
-              &lt;define name=&quot;SIMULATE_VIDEO&quot; value=&quot;1&quot;/&gt;
-              ...
-            &lt;/section&gt;
-          @endcode
-        5. If required, set the aircraft model in the airframe file:
-          @code{.xml}
-            &lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
-              ...
-              &lt;define name=&quot;GAZEBO_AC_NAME&quot; value=&quot;my_uav&quot;/&gt;
-            &lt;/section&gt;
-          @endcode
-        6. Make sure all included modules work with nps. At the moment, most of
+          - If conf/simulator/gazebo/airframes does not contain an xml for the
+          vehicle model, it should be created with the following contents:
+            1. Actuator thrusts and torques
+              @code{.xml}
+              &lt;!DOCTYPE airframe SYSTEM &quot;../../../airframes/airframe.dtd&quot;&gt;
+  
+                &lt;airframe&gt;
+                  &lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
+                    &lt;define name=&quot;ACTUATOR_THRUSTS&quot; value=&quot;1.55, 1.55, 1.55, 1.55&quot; type=&quot;float[]&quot;/&gt;
+                    &lt;define name=&quot;ACTUATOR_TORQUES&quot; value=&quot;0.155, 0.155, 0.155, 0.155&quot; type=&quot;float[]&quot;/&gt;
+                    ...
+                  &lt;/section&gt;
+                &lt;/airframe&gt;
+              @endcode
+              The thrusts and torques are expressed in SI units (N, Nm) and should
+              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}
+                &lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
+                  ...
+                  &lt;define name=&quot;ACTUATOR_TIME_CONSTANTS&quot; value=&quot;0.02, 0.02, 0.02, 0.02&quot; type=&quot;float[]&quot;/&gt;
+                  &lt;define name=&quot;ACTUATOR_MAX_ANGULAR_MOMENTUM&quot; value=&quot;0.19, 0.19, 0.19, 0.19&quot; type=&quot;float[]&quot;/&gt;
+                  ...
+                &lt;/section&gt;
+              @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}
+                &lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
+                  ...
+                  &lt;define name=&quot;BYPASS_AHRS&quot; value=&quot;1&quot;/&gt;
+                  &lt;define name=&quot;BYPASS_INS&quot; value=&quot;1&quot;/&gt;
+                  ...
+                &lt;/section&gt;
+              @endcode
+    
+            4. If required, enable video thread simulation:
+              @code{.xml}
+                &lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
+                  ...
+                  &lt;define name=&quot;SIMULATE_VIDEO&quot; value=&quot;1&quot;/&gt;
+                  ...
+                &lt;/section&gt;
+              @endcode
+            5. Set the aircraft model in the xml file:
+              @code{.xml}
+                &lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
+                  ...
+                  &lt;define name=&quot;GAZEBO_AC_NAME&quot; value=&quot;my_uav&quot;/&gt;
+                &lt;/section&gt;
+              @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}
            &lt;makefile target=&quot;ap|nps&quot;&gt;
          @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}
          &lt;flight_plan ...&gt;
            &lt;header&gt;
@@ -101,7 +127,7 @@
              #define NPS_GAZEBO_WORLD &quot;my.world&quot;
            &lt;/header&gt;
            ...
-          &lt;flight_plan&gt;
+          &lt;/flight_plan&gt;
        @endcode
    </description>
    <configure name="NPS_DEBUG_VIDEO" value="0|1" description="show window with video for debugging"/>
@@ -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>
@@ -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>
@@ -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>
@@ -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>
@@ -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.
 *
@@ -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;
-    double torque = autopilot.motors_on ? gazebo_actuators.torques[i] * act_commands[i] : 0.0;
+    // Thrust setpoint
+    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));