[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
paparazzi · Jan 30, 2018 · cfabb51 · cfabb51
1 parent 2d86a0b
commit cfabb51
Show file tree

Hide file tree

Showing 11 changed files with 477 additions and 61 deletions.
diff --git a/conf/airframes/examples/ardrone2_gazebo.xml b/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"/>

diff --git a/conf/airframes/examples/ardrone2_gazebo_cyberzoo.xml b/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"/>

diff --git a/conf/airframes/examples/bebop.xml b/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"/>
-
-  </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"/>

diff --git a/conf/airframes/examples/bebop2_indi.xml b/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"/>
+      <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,18 +27,16 @@
       <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"/>
     <module name="gps" type="ubx_ucenter"/>
     <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"/>

diff --git a/conf/modules/fdm_gazebo.xml b/conf/modules/fdm_gazebo.xml
@@ -50,58 +50,84 @@
               &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;
-          @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;
+            &lt;include href=&quot;conf/simulator/gazebo/airframes/ardrone2.xml&quot;/&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;
               ...
               #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"/>

diff --git a/conf/simulator/gazebo/airframes/ardrone2.xml b/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>
diff --git a/conf/simulator/gazebo/airframes/bebop.xml b/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>