From 021b37f8e4d9862b59f2f03b2d8bc8d7d18028ab Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 11 May 2020 19:35:32 +1000
Subject: [PATCH] SITL: added new compass parameters

---
 libraries/SITL/SITL.cpp | 14 ++++++++++++++
 libraries/SITL/SITL.h   |  8 ++++++++
 2 files changed, 22 insertions(+)

diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 5e1520fe38..48755dea0f 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -95,6 +95,7 @@ const AP_Param::GroupInfo SITL::var_info[] = {
     AP_GROUPINFO("ENGINE_FAIL",   60, SITL,  engine_fail,  0),
     AP_GROUPINFO("GPS2_TYPE",     61, SITL,  gps2_type,  SITL::GPS_TYPE_UBLOX),
     AP_GROUPINFO("ODOM_ENABLE",   62, SITL,  odom_enable, 0),
+    AP_SUBGROUPEXTENSION("",      62, SITL,  var_info3),
     AP_SUBGROUPEXTENSION("",      63, SITL,  var_info2),
     AP_GROUPEND
 };
@@ -201,6 +202,19 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
 
 };
 
+// third table of user settable parameters for SITL. 
+const AP_Param::GroupInfo SITL::var_info3[] = {
+    AP_GROUPINFO("MAG1_DEVID",    3, SITL,  mag_devid[0], 97539),
+    AP_GROUPINFO("MAG2_DEVID",    4, SITL,  mag_devid[1], 131874),
+    AP_GROUPINFO("MAG3_DEVID",    5, SITL,  mag_devid[2], 263178),
+    AP_GROUPINFO("MAG4_DEVID",    6, SITL,  mag_devid[3], 97283),    
+    AP_GROUPINFO("MAG5_DEVID",    7, SITL,  mag_devid[4], 97795),
+    AP_GROUPINFO("MAG6_DEVID",    8, SITL,  mag_devid[5], 98051),
+    AP_GROUPINFO("MAG7_DEVID",    9, SITL,  mag_devid[6], 0),
+    AP_GROUPINFO("MAG8_DEVID",    10, SITL, mag_devid[7], 0),
+    AP_GROUPEND
+
+};
 
 /* report SITL state via MAVLink */
 void SITL::simstate_send(mavlink_channel_t chan)
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index d321818c47..7e2b26584b 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -3,6 +3,7 @@
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Common/Location.h>
+#include <AP_Compass/AP_Compass.h>
 
 #include "SIM_Buzzer.h"
 #include "SIM_Gripper_EPM.h"
@@ -66,6 +67,7 @@ public:
         mag_ofs.set(Vector3f(5, 13, -18));
         AP_Param::setup_object_defaults(this, var_info);
         AP_Param::setup_object_defaults(this, var_info2);
+        AP_Param::setup_object_defaults(this, var_info3);
         if (_singleton != nullptr) {
             AP_HAL::panic("Too many SITL instances");
         }
@@ -111,6 +113,11 @@ public:
     
     static const struct AP_Param::GroupInfo var_info[];
     static const struct AP_Param::GroupInfo var_info2[];
+    static const struct AP_Param::GroupInfo var_info3[];
+
+    // Board Orientation (and inverse)
+    Matrix3f ahrs_rotation;
+    Matrix3f ahrs_rotation_inv;
 
     // noise levels for simulated sensors
     AP_Float baro_noise;  // in metres
@@ -180,6 +187,7 @@ public:
     AP_Int8 gps_hdg_enabled; // enable the output of a NMEA heading HDT sentence
     AP_Int32 loop_delay; // extra delay to add to every loop
     AP_Float mag_scaling; // scaling factor on first compasses
+    AP_Int32 mag_devid[MAX_CONNECTED_MAGS]; // Mag devid
 
     // wind control
     enum WindType {