From 996739df12d324436b3e4c6eb59edc13eb764355 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Wed, 18 Mar 2015 02:05:48 -0300
Subject: [PATCH] AP_Compass: Do not panic if compass is not found

Copter uses 2 compasses for linux configuration, but one of them may not
be available. Do not panic if a AK8963 isn't found.
---
 libraries/AP_Compass/AP_Compass_AK8963.cpp | 25 ++++++++++++++++------
 libraries/AP_Compass/AP_Compass_AK8963.h   |  2 ++
 2 files changed, 20 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp
index 33a2d3be31..3b6f3554cf 100644
--- a/libraries/AP_Compass/AP_Compass_AK8963.cpp
+++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp
@@ -105,13 +105,6 @@ extern const AP_HAL::HAL& hal;
 
 AK8963_MPU9250_SPI_Backend::AK8963_MPU9250_SPI_Backend()
 {
-    _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
-
-    if (_spi == NULL) {
-        hal.scheduler->panic(PSTR("Cannot get SPIDevice_MPU9250"));
-    }
-
-    _spi_sem = _spi->get_semaphore();
 }
 
 bool AK8963_MPU9250_SPI_Backend::sem_take_blocking()
@@ -148,6 +141,19 @@ bool AK8963_MPU9250_SPI_Backend::sem_take_nonblocking()
     return got;
 }
 
+bool AK8963_MPU9250_SPI_Backend::init()
+{
+    _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
+
+    if (_spi == NULL) {
+        hal.console->println_P(PSTR("Cannot get SPIDevice_MPU9250"));
+        return false;
+    }
+
+    _spi_sem = _spi->get_semaphore();
+    return true;
+}
+
 void AK8963_MPU9250_SPI_Backend::read(uint8_t address, uint8_t *buf, uint32_t count)
 {
     ASSERT(count < 10);
@@ -226,6 +232,11 @@ bool AP_Compass_AK8963_MPU9250::init()
     if (_backend == NULL) {
         hal.scheduler->panic(PSTR("_backend coudln't be allocated"));
     }
+    if (!_backend->init()) {
+        delete _backend;
+        _backend = NULL;
+        return false;
+    }
     return AP_Compass_AK8963::init();
 #else
 #error Wrong backend for AK8963 is selected
diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h
index 49b47c1f03..d185cd8aa8 100644
--- a/libraries/AP_Compass/AP_Compass_AK8963.h
+++ b/libraries/AP_Compass/AP_Compass_AK8963.h
@@ -18,6 +18,7 @@ class AK8963_Backend
         virtual bool sem_take_nonblocking() = 0;
         virtual bool sem_take_blocking() = 0;
         virtual bool sem_give() = 0;
+        virtual bool init() = 0;
         virtual uint8_t read(uint8_t address) 
         {
             uint8_t value;
@@ -93,6 +94,7 @@ class AK8963_MPU9250_SPI_Backend: public AK8963_Backend
         bool sem_take_nonblocking();
         bool sem_take_blocking();
         bool sem_give();
+        bool init() ;
         ~AK8963_MPU9250_SPI_Backend() {}
 
     private: