Add support for MPU-9250 IMU

.gitmodules

@@ -20,3 +20,6 @@
 [submodule "libraries/vl53l0x-arduino"]
 	path = libraries/vl53l0x-arduino
 	url = [email protected]:gkanwar/vl53l0x-arduino.git
+[submodule "libraries/SparkFun_MPU-9250_Breakout_Arduino_Library"]
+	path = libraries/SparkFun_MPU-9250_Breakout_Arduino_Library
+	url = https://github.com/sparkfun/SparkFun_MPU-9250_Breakout_Arduino_Library

Makefile_example

@@ -17,7 +17,7 @@ COREPATH = cores/teensy3
 # path location for Arduino libraries
 LIBRARYPATH = libraries
 
-LIBRARIES = $(LIBRARYPATH)/Wire $(LIBRARYPATH)/Encoder $(LIBRARYPATH)/SPI $(LIBRARYPATH)/Servo $(LIBRARYPATH)/Adafruit_TCS34725 $(LIBRARYPATH)/vl53l0x-arduino
+LIBRARIES = $(LIBRARYPATH)/Wire $(LIBRARYPATH)/Encoder $(LIBRARYPATH)/SPI $(LIBRARYPATH)/Servo $(LIBRARYPATH)/Adafruit_TCS34725 $(LIBRARYPATH)/vl53l0x-arduino $(LIBRARYPATH)/SparkFun_MPU-9250_Breakout_Arduino_Library
 
 #************************************************************************
 # Location of Teensyduino utilities, Toolchain, and Arduino Libraries.
@@ -107,7 +107,9 @@ SIZE = $(abspath $(COMPILERPATH))/arm-none-eabi-size
 
 # automatically create lists of the sources and objects
 LC_FILES := $(foreach library, $(LIBRARIES), $(wildcard $(library)/*.c))
+LC_FILES += $(foreach library, $(LIBRARIES), $(wildcard $(library)/src/*.c))
 LCPP_FILES := $(foreach library, $(LIBRARIES), $(wildcard $(library)/*.cpp))
+LCPP_FILES += $(foreach library, $(LIBRARIES), $(wildcard $(library)/src/*.cpp))
 TC_FILES := $(wildcard $(COREPATH)/*.c)
 TCPP_FILES := $(wildcard $(COREPATH)/*.cpp)
 TCPP_FILES := $(filter-out $(COREPATH)/main.cpp, $(TCPP_FILES))
@@ -117,6 +119,7 @@ INO_FILES := $(wildcard src/*.ino)
 
 # include paths for libraries
 L_INC := $(foreach lib,$(filter %/, $(wildcard $(LIBRARYPATH)/*/)), -I$(lib))
+L_INC += $(foreach lib,$(filter %/, $(wildcard $(LIBRARYPATH)/*/src/)), -I$(lib))
 
 SOURCES := $(C_FILES:.c=.o) $(CPP_FILES:.cpp=.o) $(INO_FILES:.ino=.o) $(TC_FILES:.c=.o) $(TCPP_FILES:.cpp=.o) $(LC_FILES:.c=.o) $(LCPP_FILES:.cpp=.o)
 OBJS := $(foreach src,$(SOURCES), $(BUILDDIR)/$(src))

README.md

@@ -53,6 +53,7 @@ Supported Devices
 - [x] Ultra-short range IR Distance Sensor (just a digital input)
 - [x] Color Sensor (I2C)
 - [x] Pololu VL53L0X time-of-flight distance sensor (I2C)
+- [x] Sparkfun MPU-9250 IMU breakout (I2C)
 
 Dependencies
 ------------

src/DeviceList.cpp

@@ -16,6 +16,7 @@
 #include "Color.h"
 #include "Servo.h"
 #include "TimeOfFlight.h"
+#include "Imu.h"
 
 namespace tamproxy {
 
@@ -125,6 +126,11 @@ std::vector<uint8_t> DeviceList::add(std::vector<uint8_t>& request) {
                 d = new TimeOfFlight(request[2], request[3]);
             } else { return {REQUEST_LENGTH_INVALID_CODE}; };
             break;
+        case IMU_CODE:
+            if (request.size() == 2) {
+                d = new Imu();
+            } else { return {REQUEST_LENGTH_INVALID_CODE}; };
+            break;
         default:
             return {DEVICE_INVALID_CODE};
     }

src/Imu.cpp

@@ -0,0 +1,75 @@
+#include "Imu.h"
+#include <cstdint>
+#include "MPU9250.h"
+#include "config.h"
+
+#define I2Cclock 400000
+#define I2Cport Wire
+#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0
+
+namespace tamproxy {
+
+Imu::Imu() {
+  sensor = new MPU9250(MPU9250_ADDRESS, I2Cport, I2Cclock);
+
+  sensor->calibrateMPU9250(sensor->gyroBias, sensor->accelBias);
+  sensor->initMPU9250();
+  sensor->initAK8963(sensor->factoryMagCalibration);
+}
+
+Imu::~Imu() {
+  delete sensor;
+}
+
+std::vector<uint8_t> Imu::handleRequest(std::vector<uint8_t> &request) {
+  if (request.size() != 1) {
+    return {REQUEST_LENGTH_INVALID_CODE};
+  } else if (request[0] != IMU_READ_CODE) {
+    return {REQUEST_BODY_INVALID_CODE};
+  } else {
+    uint16_t ax, ay, az;
+    uint16_t gx, gy, gz;
+    uint16_t mx, my, mz;
+
+    sensor->readAccelData(sensor->accelCount);  // Read the x/y/z adc values
+
+    ax = sensor->accelCount[0];
+    ay = sensor->accelCount[1];
+    az = sensor->accelCount[2];
+
+    sensor->readGyroData(sensor->gyroCount);  // Read the x/y/z adc values
+
+    gx = sensor->gyroCount[0];
+    gy = sensor->gyroCount[1];
+    gz = sensor->gyroCount[2];
+
+    sensor->readMagData(sensor->magCount);  // Read the x/y/z adc values
+
+    mx = sensor->magCount[0] * sensor->factoryMagCalibration[0];
+    my = sensor->magCount[1] * sensor->factoryMagCalibration[1];
+    mz = sensor->magCount[2] * sensor->factoryMagCalibration[2];
+
+    return {
+      static_cast<uint8_t>((ax >> 8) & 0xff),
+      static_cast<uint8_t>(ax & 0xff),
+      static_cast<uint8_t>((ay >> 8) & 0xff),
+      static_cast<uint8_t>(ay & 0xff),
+      static_cast<uint8_t>((az >> 8) & 0xff),
+      static_cast<uint8_t>(az & 0xff),
+      static_cast<uint8_t>((gx >> 8) & 0xff),
+      static_cast<uint8_t>(gx & 0xff),
+      static_cast<uint8_t>((gy >> 8) & 0xff),
+      static_cast<uint8_t>(gy & 0xff),
+      static_cast<uint8_t>((gz >> 8) & 0xff),
+      static_cast<uint8_t>(gz & 0xff),
+      static_cast<uint8_t>((mx >> 8) & 0xff),
+      static_cast<uint8_t>(mx & 0xff),
+      static_cast<uint8_t>((my >> 8) & 0xff),
+      static_cast<uint8_t>(my & 0xff),
+      static_cast<uint8_t>((mz >> 8) & 0xff),
+      static_cast<uint8_t>(mz & 0xff)
+    };
+  }
+}
+
+}

src/Imu.h

@@ -0,0 +1,22 @@
+#ifndef IMU_H
+#define IMU_H
+
+#include <cstdint>
+#include <vector>
+#include "Device.h"
+#include "MPU9250.h"
+
+namespace tamproxy {
+
+class Imu: public Device {
+private:
+  MPU9250 *sensor;
+public:
+  Imu();
+  ~Imu();
+  std::vector<uint8_t> handleRequest(std::vector<uint8_t> &request);
+};
+
+}
+
+#endif

src/config_example_teensy32.h

@@ -41,6 +41,8 @@
 #define GYRO_CODE 'G'
 #define GYRO_READ_CODE 'R'
 #define IR_CODE 'I'
+#define IMU_CODE 'i'
+#define IMU_READ_CODE 'R'
 #define MOTOR_CODE 'M'
 #define MOTOR_WRITE_CODE 'W'
 #define SERVO_CODE 'S'

src/config_example_teensy35.h

@@ -43,6 +43,8 @@
 #define GYRO_CODE 'G'
 #define GYRO_READ_CODE 'R'
 #define IR_CODE 'I'
+#define IMU_CODE 'i'
+#define IMU_READ_CODE 'R'
 #define MOTOR_CODE 'M'
 #define MOTOR_WRITE_CODE 'W'
 #define SERVO_CODE 'S'