Fix AHRS config for Tethys (#218)

Signed-off-by: Michel Hidalgo <[email protected]>
osrf · Jun 22, 2022 · 0b29923 · 0b29923
1 parent 3a87941
commit 0b29923
Show file tree

Hide file tree

Showing 2 changed files with 57 additions and 55 deletions.
diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf
@@ -93,17 +93,17 @@
           <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.
+            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>
-            <!--
+          <imu>
+            <orientation_reference_frame>
+              <localization>NED</localization>
+            </orientation_reference_frame>
+            <enable_orientation>1</enable_orientation>
+            <linear_acceleration>
+              <!--
                 Use zero-mean gaussian distributions for each
                 linear acceleration component and its (turn on)
                 bias.
@@ -116,28 +116,28 @@
                 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>
-            <!--
+              -->
+              <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 type="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.
@@ -150,26 +150,28 @@
                 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>
+              -->
+              <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>
+          </imu>
+          <update_rate>10</update_rate>
           <topic>/tethys/ahrs/imu</topic>
         </sensor>
         <!--Magnetometer in a Sparton AHRS-M2 -->
@@ -182,8 +184,8 @@
           <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.
+            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>

diff --git a/lrauv_system_tests/test/vehicle/test_ahrs.cc b/lrauv_system_tests/test/vehicle/test_ahrs.cc
@@ -114,8 +114,8 @@ static constexpr double Be{5.5645e-6};    // T
 static constexpr double Bn{22.8758e-6};   // T
 static constexpr double Bu{-42.3884e-6};  // T
 
-static constexpr double angularVelocityTolerance{1e-4};
-static constexpr double linearAccelerationTolerance{1e-4};
+static constexpr double angularVelocityTolerance{3e-2};
+static constexpr double linearAccelerationTolerance{3e-2};
 static constexpr double magneticFieldTolerance{1e-8};
 static constexpr double orientationTolerance{1e-4};