From 8bd044e80eefa23d1b2911d75d788fffb781e147 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 7 Jul 2017 16:26:19 -1000
Subject: [PATCH] mpu9250:mag rework the setup to veify HW first

   If the setup is unsucessful fo not register the devices or
   allocate resources.
---
 src/drivers/mpu9250/mag.cpp | 44 +++++++++++++++++++------------------
 1 file changed, 23 insertions(+), 21 deletions(-)

diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp
index eacf37d99e..fe43891f95 100644
--- a/src/drivers/mpu9250/mag.cpp
+++ b/src/drivers/mpu9250/mag.cpp
@@ -123,36 +123,38 @@ MPU9250_mag::~MPU9250_mag()
 int
 MPU9250_mag::init()
 {
-	int ret;
+	int ret = ak8963_setup();
 
-	ret = CDev::init();
+	if (ret == OK) {
 
-	/* if setup failed, bail now */
-	if (ret != OK) {
-		DEVICE_DEBUG("MPU9250 mag init failed");
-		return ret;
-	}
+		ret = CDev::init();
 
-	_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
+		/* if setup failed, bail now */
+		if (ret != OK) {
+			DEVICE_DEBUG("MPU9250 mag init failed");
+			return ret;
+		}
 
-	if (_mag_reports == nullptr) {
-		goto out;
-	}
+		_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
 
-	_mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
+		if (_mag_reports == nullptr) {
+			goto out;
+		}
+
+		_mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
 
-	ak8963_setup();
 
-	/* advertise sensor topic, measure manually to initialize valid report */
-	struct mag_report mrp;
-	_mag_reports->get(&mrp);
+		/* advertise sensor topic, measure manually to initialize valid report */
+		struct mag_report mrp;
+		_mag_reports->get(&mrp);
 
-	_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
-					 &_mag_orb_class_instance, ORB_PRIO_LOW);
-//			   &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
+		_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
+						 &_mag_orb_class_instance, ORB_PRIO_LOW);
+		//			   &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
 
-	if (_mag_topic == nullptr) {
-		warnx("ADVERT FAIL");
+		if (_mag_topic == nullptr) {
+			warnx("ADVERT FAIL");
+		}
 	}
 
 out: