From a6f36cdeb7b85effb66743b8ff359551364883c4 Mon Sep 17 00:00:00 2001
From: Michel Hidalgo <michel@ekumenlabs.com>
Date: Tue, 8 Feb 2022 13:51:34 -0300
Subject: [PATCH] Add AHRS to Tethys equipped model

Combining upstream IMU and magnetometer sensors

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
---
 .../models/tethys_equipped/model.sdf          | 28 +++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf
index 021f6a02..ce317df7 100644
--- a/lrauv_description/models/tethys_equipped/model.sdf
+++ b/lrauv_description/models/tethys_equipped/model.sdf
@@ -76,6 +76,34 @@
             </noise>
           </ignition:current>
         </sensor>
+        <sensor
+            element_id="base_link"
+            action="add"
+            name="ahrs_imu"
+            type="imu">
+          <always_on>1</always_on>
+          <update_rate>10</update_rate>
+          <pose degrees="true">0 0 0 180 0 0</pose>
+          <orientation_reference_frame>
+            <localization>NED</localization>
+          </orientation_reference_frame>
+          <enable_orientation>1</enable_orientation>
+          <topic>/model/tethys/ahrs/imu</topic>
+          <plugin filename="ignition-gazebo-imu-system"
+                  name="ignition::gazebo::systems::Imu"/>
+        </sensor>
+        <sensor
+            element_id="base_link"
+            action="add"
+            name="ahrs_magnetometer"
+            type="magnetometer">
+          <always_on>1</always_on>
+          <update_rate>10</update_rate>
+          <pose degrees="true">0 0 0 180 0 0</pose>
+          <topic>/model/tethys/ahrs/magnetometer</topic>
+          <plugin filename="ignition-gazebo-magnetometer-system"
+                  name="ignition::gazebo::systems::Magnetometer"/>
+        </sensor>
       </experimental:params>
       <!-- Joint controllers -->
       <plugin