- update magnetometer calculations, work around for magnetometer issues, and make sure callback is always called on enableMPU9250/disableMPU9250

Author: sandeepmistry

lib/cc2650.js

@@ -31,7 +31,7 @@ var CC2650SensorTag = function(peripheral) {
   this.mpu9250mask = 0;
   this.mpu9250notifyCount = 0;
 
-  this.onMPU9250ChangeBinded = this.onMPU9250Change.bind(this);
+  this.onMPU9250ChangeBinded     = this.onMPU9250Change.bind(this);
   this.onLuxometerChangeBinded   = this.onLuxometerChange.bind(this);
 };
 
@@ -98,20 +98,27 @@ CC2650SensorTag.prototype.setMPU9250Period = function(period, callback) {
 CC2650SensorTag.prototype.enableMPU9250 = function(mask, callback) {
   this.mpu9250mask |= mask;
 
-  this.writeUInt16LECharacteristic(MPU9250_UUID, MPU9250_CONFIG_UUID, this.mpu9250mask, callback);
+  // for now, always write 0x007f, magnetometer does not seem to notify is specific mask is used
+  this.writeUInt16LECharacteristic(MPU9250_UUID, MPU9250_CONFIG_UUID, 0x007f, callback);
 };
 
 CC2650SensorTag.prototype.disableMPU9250 = function(mask, callback) {
   this.mpu9250mask &= ~mask;
 
-  this.writeUInt16LECharacteristic(MPU9250_UUID, MPU9250_CONFIG_UUID, this.mpu9250mask, callback);
+  if (this.mpu9250mask === 0) {
+    this.writeUInt16LECharacteristic(MPU9250_UUID, MPU9250_CONFIG_UUID, 0x0000, callback);
+  } else if (typeof(callback) === 'function') {
+    callback();
+  }
 };
 
 CC2650SensorTag.prototype.notifyMPU9250 = function(callback) {
   this.mpu9250notifyCount++;
 
   if (this.mpu9250notifyCount === 1) {
     this.notifyCharacteristic(MPU9250_UUID, MPU9250_DATA_UUID, true, this.onMPU9250ChangeBinded, callback);
+  } else if (typeof(callback) === 'function') {
+    callback();
   }
 };
 
@@ -120,6 +127,8 @@ CC2650SensorTag.prototype.unnotifyMPU9250 = function(callback) {
 
   if (this.mpu9250notifyCount === 0) {
     this.notifyCharacteristic(MPU9250_UUID, MPU9250_DATA_UUID, false, this.onMPU9250ChangeBinded, callback);
+  } else if (typeof(callback) === 'function') {
+    callback();
   }
 };
 
@@ -170,10 +179,10 @@ CC2650SensorTag.prototype.convertMPU9250Data = function(data, callback) {
   var y = data.readInt16LE(8) * 2.0 / 32768.0;
   var z = data.readInt16LE(10) * 2.0 / 32768.0;
 
-  // magnetometer
-  var xM = data.readInt16LE(12);
-  var yM = data.readInt16LE(14);
-  var zM = data.readInt16LE(16);
+  // magnetometer (page 50 of http://www.invensense.com/mems/gyro/documents/RM-MPU-9250A-00.pdf)
+  var xM = data.readInt16LE(12) * 4912.0 / 32760.0;
+  var yM = data.readInt16LE(14) * 4912.0 / 32760.0;
+  var zM = data.readInt16LE(16) * 4912.0 / 32760.0;
 
   callback(x, y, z, xG, yG, zG, xM, yM, zM);
 };