Add AHRS to Tethys equipped model (#158)

Combining upstream IMU and magnetometer sensors, configured after Sparton AHRS-M2 datasheet. Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
osrf · Mar 18, 2022 · b2c9936 · b2c9936
1 parent e388214
commit b2c9936
Show file tree

Hide file tree

Showing 4 changed files with 646 additions and 1 deletion.
diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf
@@ -7,7 +7,7 @@
   <model name="tethys">
     <include merge="true">
       <!-- align with NED (face North) -->
-      <pose degrees="true">0 0 0   0 0 -90</pose>
+      <pose degrees="true">0 0 0 0 0 -90</pose>
       <uri>tethys</uri>
 
       <!-- Sensors -->
@@ -76,6 +76,120 @@
             </noise>
           </ignition:current>
         </sensor>
+        <!--
+            Sparton AHRS-M2 arrangement of IMU + Magnetometer
+
+            See https://www.spartonnavex.com/product/ahrs-m2 for
+            AHRS specifications and additional documentation.
+        -->
+        <sensor
+            element_id="base_link"
+            action="add"
+            name="sparton_ahrs_m2_imu"
+            type="imu">
+          <!--IMU in a Sparton AHRS-M2 -->
+          <always_on>1</always_on>
+          <!-- Flip body frame to match FSK frame convention -->
+          <!--
+              Also account for model being rotated 180 degrees
+              See https://github.com/osrf/lrauv/issues/80.
+          -->
+          <pose degrees="true">0 0 0 180 0 180</pose>
+          <orientation_reference_frame>
+            <localization>NED</localization>
+          </orientation_reference_frame>
+          <enable_orientation>1</enable_orientation>
+          <update_rate>10</update_rate>
+          <linear_acceleration>
+            <!--
+                Use zero-mean gaussian distributions for each
+                linear acceleration component and its (turn on)
+                bias.
+
+                Standard deviation is assumed to be the same for all
+                linear acceleration components. It is computed from
+                reported noise density ND = 250 ug/√Hz and chosen
+                sample rate SR = 10 Hz as ND √SR.
+
+                Standard deviation is assumed to be the same for all
+                linear acceleration component (turn on) biases. It is
+                made equal to the reported bias stability BS = 0.07441 mg.
+            -->
+            <x>
+              <noise type="gaussian">
+                <stddev>0.00775283755402256</stddev>
+                <bias_stddev>0.007297128265</bias_stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <stddev>0.00775283755402256</stddev>
+                <bias_stddev>0.007297128265</bias_stddev>
+              </noise>
+            </y>
+            <z>
+              <noise tzpe="gaussian">
+                <stddev>0.00775283755402256</stddev>
+                <bias_stddev>0.007297128265</bias_stddev>
+              </noise>
+            </z>
+          </linear_acceleration>
+          <angular_velocity>
+            <!--
+                Use zero-mean gaussian distributions for each
+                angular velocity component and its (turn on)
+                bias.
+
+                Standard deviation is assumed to be the same for all
+                angular velocity components. It is computed from
+                reported noise density ND = 0.04 °/s/√Hz and chosen
+                sample rate SR = 10 Hz as ND √SR.
+
+                Standard deviation is assumed to be the same for all
+                angular velocity component (turn on) biases. It is made
+                equal to the reported bias stability BS = 6.415 °/hr.
+            -->
+            <x>
+              <noise type="gaussian">
+                <stddev>0.0022076862812880228</stddev>
+                <bias_stddev>0.0017819444444444445</bias_stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <stddev>0.0022076862812880228</stddev>
+                <bias_stddev>0.0017819444444444445</bias_stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <stddev>0.0022076862812880228</stddev>
+                <bias_stddev>0.0017819444444444445</bias_stddev>
+              </noise>
+            </z>
+          </angular_velocity>
+          <topic>/tethys/ahrs/imu</topic>
+          <plugin filename="ignition-gazebo-imu-system"
+                  name="ignition::gazebo::systems::Imu"/>
+        </sensor>
+        <!--Magnetometer in a Sparton AHRS-M2 -->
+        <sensor
+            element_id="base_link"
+            action="add"
+            name="ahrs_magnetometer"
+            type="magnetometer">
+          <always_on>1</always_on>
+          <update_rate>10</update_rate>
+          <!-- Flip body frame to match FSK frame convention -->
+          <!--
+              Also account for model being rotated 180 degrees
+              See https://github.com/osrf/lrauv/issues/80.
+          -->
+          <pose degrees="true">0 0 0 180 0 180</pose>
+          <topic>/tethys/ahrs/magnetometer</topic>
+          <plugin filename="ignition-gazebo-magnetometer-system"
+                  name="ignition::gazebo::systems::Magnetometer"/>
+        </sensor>
       </experimental:params>
       <!-- Joint controllers -->
       <plugin

diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt
@@ -261,6 +261,7 @@ if(BUILD_TESTING)
 
   # Tests
   foreach(TEST_TARGET
+    test_ahrs
     test_buoyancy_action
     test_controller
     test_drop_weight