From 2fe2c8fbda1e72294b2bf7c95c7ac37ea2cdbfa3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 27 Jun 2018 13:43:27 +1000
Subject: [PATCH] AP_Compass: initialise compass IDs saved in SITL

this ensures we don't get uncalibrates compasses in SITL
---
 libraries/AP_Compass/AP_Compass_Backend.cpp | 8 ++++++++
 libraries/AP_Compass/AP_Compass_Backend.h   | 3 +++
 libraries/AP_Compass/AP_Compass_SITL.cpp    | 3 +++
 3 files changed, 14 insertions(+)

diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp
index d35630d344e..6fd3c86f039 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.cpp
+++ b/libraries/AP_Compass/AP_Compass_Backend.cpp
@@ -132,6 +132,14 @@ void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id)
     _compass._state[instance].dev_id.set_and_notify(dev_id);
 }
 
+/*
+  save dev_id, used by SITL
+*/
+void AP_Compass_Backend::save_dev_id(uint8_t instance)
+{
+    _compass._state[instance].dev_id.save();
+}
+
 /*
   set external for an instance
 */
diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h
index 5193a83f7cf..8d5f2f6789a 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.h
+++ b/libraries/AP_Compass/AP_Compass_Backend.h
@@ -94,6 +94,9 @@ protected:
     // set dev_id for an instance
     void set_dev_id(uint8_t instance, uint32_t dev_id);
 
+    // save dev_id, used by SITL
+    void save_dev_id(uint8_t instance);
+
     // set external state for an instance
     void set_external(uint8_t instance, bool external);
 
diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp
index a2dfe7c0891..98cf43157f8 100644
--- a/libraries/AP_Compass/AP_Compass_SITL.cpp
+++ b/libraries/AP_Compass/AP_Compass_SITL.cpp
@@ -15,6 +15,9 @@ AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
         for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
             _compass_instance[i] = register_compass();
             set_dev_id(_compass_instance[i], AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 0, DEVTYPE_SITL));
+
+            // save so the compass always comes up configured in SITL
+            save_dev_id(_compass_instance[i]);
         }
         hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
     }