From b078d8e4251e84f22e57d681d8a0fd927e633c2f Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Tue, 26 Oct 2021 17:43:33 +0200
Subject: [PATCH] Make BMX055 optional if MMC5603NJ present (#22698)

---
 selfdrive/sensord/sensors_qcom2.cc | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc
index 36526df0f..c3e5bbcdf 100644
--- a/selfdrive/sensord/sensors_qcom2.cc
+++ b/selfdrive/sensord/sensors_qcom2.cc
@@ -50,10 +50,10 @@ int sensor_loop() {
 
   // Sensor init
   std::vector<std::pair<Sensor *, bool>> sensors_init; // Sensor, required
-  sensors_init.push_back({&bmx055_accel, true});
-  sensors_init.push_back({&bmx055_gyro, true});
-  sensors_init.push_back({&bmx055_magn, true});
-  sensors_init.push_back({&bmx055_temp, true});
+  sensors_init.push_back({&bmx055_accel, false});
+  sensors_init.push_back({&bmx055_gyro, false});
+  sensors_init.push_back({&bmx055_magn, false});
+  sensors_init.push_back({&bmx055_temp, false});
 
   sensors_init.push_back({&lsm6ds3_accel, true});
   sensors_init.push_back({&lsm6ds3_gyro, true});
@@ -63,6 +63,7 @@ int sensor_loop() {
 
   sensors_init.push_back({&light, true});
 
+  bool has_magnetometer = false;
 
   // Initialize sensors
   std::vector<Sensor *> sensors;
@@ -75,10 +76,18 @@ int sensor_loop() {
         return -1;
       }
     } else {
+      if (sensor.first == &bmx055_magn || sensor.first == &mmc5603nj_magn) {
+        has_magnetometer = true;
+      }
       sensors.push_back(sensor.first);
     }
   }
 
+  if (!has_magnetometer) {
+    LOGE("No magnetometer present");
+    return -1;
+  }
+
   PubMaster pm({"sensorEvents"});
 
   while (!do_exit) {