johnny-five/lib/gyro.js

var Board = require("./board");
var events = require("events");
var util = require("util");
var Fn = require("./fn");

var sum = Fn.sum;
var toFixed = Fn.toFixed;

var priv = new Map();
var axes = ["x", "y", "z"];

var Controllers = {
  ANALOG: {
    initialize: {
      value: function(opts, dataHandler) {
        var pins = opts.pins || [],
          sensitivity, resolution,
          state = priv.get(this),
          dataPoints = {};

        if (opts.sensitivity === undefined) {
          throw new Error("Expected a Sensitivity");
        }

        // 4.88mV / (0.167mV/dps * 2)
        // 0.67 = 4X
        // 0.167 = 1X
        sensitivity = opts.sensitivity;
        resolution = opts.resolution || 4.88;
        state.K = resolution / sensitivity;

        pins.forEach(function(pin, index) {
          this.io.pinMode(pin, this.io.MODES.ANALOG);
          this.io.analogRead(pin, function(data) {
            var axis = axes[index];
            dataPoints[axis] = data;
            dataHandler(dataPoints);
          }.bind(this));
        }, this);
      }
    },
    toNormal: {
      value: function(raw) {
        return raw >> 2;
      }
    },
    toDegreesPerSecond: {
      value: function(raw, rawCenter) {
        var normal = this.toNormal(raw);
        var center = this.toNormal(rawCenter);
        var state = priv.get(this);

        return ((normal - center) * state.K) | 0;
      }
    }
  },
  // http://www.invensense.com/mems/gyro/mpu6050.html
  // Default to the +- 250 which has a 131 LSB/dps
  MPU6050: {
    initialize: {
      value: function(opts, dataHandler) {
        var IMU = require("./imu");
        var state = priv.get(this),
          driver = IMU.Drivers.get(this.board, "MPU6050", opts);

        state.sensitivity = opts.sensitivity || 131;

        driver.on("data", function(data) {
          dataHandler(data.gyro);
        });
      }
    },
    toNormal: {
      value: function(raw) {
        return (raw >> 11) + 127;
      }
    },
    toDegreesPerSecond: {
      // Datasheet available at https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf
      //
      // From paragraph 6.1 at page 12
      // Sensitivity scale factor
      // FS_SEL=0    131 LSB/dps -> 0,007633588 dps/LSB
      // FS_SEL=1    65.5 LSB/dps -> 0,015267176 dps/LSB
      // FS_SEL=2    32.8 LSB/dps -> 0,00304878 dps/LSB
      // FS_SEL=3    16.4 LSB/dps -> 0,06097561 dps/LSB
      // Using 4 digits resolution
      value: function(raw, rawCenter) {
        var state = priv.get(this);

        return toFixed((raw - rawCenter) / state.sensitivity, 4);
      }
    }
  },
  BNO055: {
    initialize: {
      value: function(opts, dataHandler) {
        var IMU = require("./imu");
        var state = priv.get(this),
          driver = IMU.Drivers.get(this.board, "BNO055", opts);

        // AF p.14, OUTPUT SIGNAL GYROSCOPE, set this to 16 as according to AF.51 the default for the unit register
        // is degrees. and there may be a bug in the Ada fruit code as it has the setting to radians disabled
        // but the sensitivity / scale set to 900 which is used for radian reps
        state.sensitivity = 16;

        driver.on("data", function(data) {
          dataHandler(data.gyro);
        });
      }
    },
    toNormal: {
      value: function(raw) {
        return raw;
      }
    },
    toDegreesPerSecond: {
      // Datasheet available at https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
      //
      // From Tble 3-22 at page 33
      // Gyroscope unit settings 1dps = 16 LSB -> resolution 0,0625 dps with +=2000 dps range
      value: function(raw) {
        var state = priv.get(this);
        return toFixed(raw / state.sensitivity, 4);
      }
    }
  },
};

function Gyro(opts) {
  if (!(this instanceof Gyro)) {
    return new Gyro(opts);
  }

  var controller = null;
  var isCalibrated = false;
  var sampleSize = 100;

  var state = {
    x: {
      angle: 0,
      value: 0,
      previous: 0,
      calibration: [],
      stash: [0, 0, 0, 0, 0],
      center: 0,
      hasValue: false
    },
    y: {
      angle: 0,
      value: 0,
      previous: 0,
      calibration: [],
      stash: [0, 0, 0, 0, 0],
      center: 0,
      hasValue: false
    },
    z: {
      angle: 0,
      value: 0,
      previous: 0,
      calibration: [],
      stash: [0, 0, 0, 0, 0],
      center: 0,
      hasValue: false
    }
  };

  Board.Component.call(
    this, opts = Board.Options(opts)
  );

  if (opts.controller && typeof opts.controller === "string") {
    controller = Controllers[opts.controller.toUpperCase()];
  } else {
    controller = opts.controller;
  }

  if (controller == null) {
    controller = Controllers.ANALOG;
  }

  Board.Controller.call(this, controller, opts);

  if (!this.toNormal) {
    this.toNormal = opts.toNormal || function(raw) {
      return raw;
    };
  }

  if (!this.toDegreesPerSecond) {
    this.toDegreesPerSecond = opts.toDegreesPerSecond || function(raw) {
      return raw;
    };
  }

  priv.set(this, state);

  if (typeof this.initialize === "function") {
    this.initialize(opts, function(data) {
      var isChange = false;

      Object.keys(data).forEach(function(axis) {
        var value = data[axis];
        var sensor = state[axis];

        sensor.previous = sensor.value;
        sensor.stash.shift();
        sensor.stash.push(value);
        sensor.hasValue = true;
        sensor.value = (sum(sensor.stash) / 5) | 0;

        if (!isCalibrated &&
          (state.x.calibration.length === sampleSize &&
            state.y.calibration.length === sampleSize &&
            (this.z === undefined || state.z.calibration.length === sampleSize))) {

          isCalibrated = true;
          state.x.center = (sum(state.x.calibration) / sampleSize) | 0;
          state.y.center = (sum(state.y.calibration) / sampleSize) | 0;
          state.z.center = (sum(state.z.calibration) / sampleSize) | 0;

          state.x.calibration.length = 0;
          state.y.calibration.length = 0;
          state.z.calibration.length = 0;
        } else {
          if (sensor.calibration.length < sampleSize) {
            sensor.calibration.push(value);
          }
        }

        if (sensor.previous !== sensor.value) {
          isChange = true;
        }
      }, this);

      if (isCalibrated) {
        state.x.angle += this.rate.x / 100;
        state.y.angle += this.rate.y / 100;
        state.z.angle += this.rate.z / 100;

        this.emit("data", {
          x: this.x,
          y: this.y,
          z: this.z
        });

        if (isChange) {
          this.emit("change", {
            x: this.x,
            y: this.y,
            z: this.z
          });
        }
      }
    }.bind(this));
  }

  Object.defineProperties(this, {
    isCalibrated: {
      get: function() {
        return isCalibrated;
      },
      set: function(value) {
        if (typeof value === "boolean") {
          isCalibrated = value;
        }
      }
    },
    pitch: {
      get: function() {
        return {
          rate: toFixed(this.rate.y, 2),
          angle: toFixed(state.y.angle, 2)
        };
      }
    },
    roll: {
      get: function() {
        return {
          rate: toFixed(this.rate.x, 2),
          angle: toFixed(state.x.angle, 2)
        };
      }
    },
    yaw: {
      get: function() {
        return {
          rate: this.z !== undefined ? toFixed(this.rate.z, 2) : 0,
          angle: this.z !== undefined ? toFixed(state.z.angle, 2) : 0
        };
      }
    },
    x: {
      get: function() {
        return toFixed(this.toNormal(state.x.value), 4);
      }
    },
    y: {
      get: function() {
        return toFixed(this.toNormal(state.y.value), 4);
      }
    },
    z: {
      get: function() {
        return state.z.hasValue ? toFixed(this.toNormal(state.z.value), 4) : undefined;
      }
    },
    rate: {
      get: function() {
        var x = this.toDegreesPerSecond(state.x.value, state.x.center);
        var y = this.toDegreesPerSecond(state.y.value, state.y.center);
        var z = state.z.hasValue ?
          this.toDegreesPerSecond(state.z.value, state.z.center) : 0;

        return {
          x: x,
          y: y,
          z: z
        };
      }
    }
  });
}

Object.defineProperties(Gyro, {
  TK_4X: {
    value: 0.67
  },
  TK_1X: {
    value: 0.167
  }
});


util.inherits(Gyro, events.EventEmitter);

Gyro.prototype.recalibrate = function() {
  this.isCalibrated = false;
};

/* istanbul ignore else */
if (!!process.env.IS_TEST_MODE) {
  Gyro.Controllers = Controllers;
  Gyro.purge = function() {
    priv.clear();
  };
}
module.exports = Gyro;