usermod_mpu6050: Add options and outputs

Upgrade the MPU6050 usermod to provide configuration settings for
interrupt pin selection and calibration, a polled mode, and data
output for consumption by other usermods.

usermods/mpu6050_imu/readme.md

@@ -20,14 +20,11 @@ react to the globes orientation. See the blog post on building it <https://www.r
 
 I2Cdev and MPU6050 must be installed.
 
-To install them, add I2Cdevlib-MPU6050@fbde122cc5 to lib_deps in the platformio.ini file.
+To install them, add electroniccats/MPU6050@1.0.1 to lib_deps in the platformio.ini file.
-
-You also need to change lib_compat_mode from strict to soft in platformio.ini (This ignores that I2Cdevlib-MPU6050 doesn't list platform compatibility)
 
 For example:
 
 ```
-lib_compat_mode = soft
 lib_deps =
     FastLED@3.3.2
     NeoPixelBus@2.5.7
@@ -36,7 +33,7 @@ lib_deps =
     AsyncTCP@1.0.3
     Esp Async WebServer@1.2.0
     IRremoteESP8266@2.7.3
-    jrowberg/I2Cdevlib-MPU6050@^1.0.0
+    electroniccats/MPU6050@1.0.1
 ```
 
 ## Wiring

usermods/mpu6050_imu/usermod_mpu6050_imu.h

@@ -72,21 +72,31 @@ void IRAM_ATTR dmpDataReady() {
 }
 
 
+
 class MPU6050Driver : public Usermod {
   private:
     MPU6050 mpu;
-    bool enabled = true;
+
+    // configuration state
+    // default values are set in readFromConfig
+    // By making this a struct, we enable easy backup and comparison in the readFromConfig class
+    struct config_t {
+      bool enabled;
+      int8_t interruptPin;    
+      int16_t gyro_offset[3]; 
+      int16_t accel_offset[3];
+    };
+    config_t config;
 
     // MPU control/status vars
+    bool irqBound = false; // set true if we have bound the IRQ pin
     bool dmpReady = false;  // set true if DMP init was successful
-    uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
     uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
     uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
     uint16_t fifoCount;     // count of all bytes currently in FIFO
     uint8_t fifoBuffer[64]; // FIFO storage buffer
 
-    //NOTE: some of these can be removed to save memory, processing time
+    // TODO: some of these can be removed to save memory, processing time if the measurement isn't needed
-    //      if the measurement isn't needed
     Quaternion qat;         // [w, x, y, z]         quaternion container
     float euler[3];         // [psi, theta, phi]    Euler angle container
     float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container
@@ -95,17 +105,67 @@ class MPU6050Driver : public Usermod {
     VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
     VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
     VectorFloat gravity;    // [x, y, z]            gravity vector
+    uint32 sample_count;
 
-    static const int INTERRUPT_PIN = 15; // use pin 15 on ESP8266
+    // Usermod output
+    um_data_t um_data;
+
+    // config element names as progmem strs
+    static const char _name[];
+    static const char _enabled[];
+    static const char _interrupt_pin[];
+    static const char _x_acc_bias[];
+    static const char _y_acc_bias[];
+    static const char _z_acc_bias[];
+    static const char _x_gyro_bias[];
+    static const char _y_gyro_bias[];
+    static const char _z_gyro_bias[];
 
   public:
-    //Functions called by WLED
 
+    inline bool initDone() { return um_data.u_size != 0; };   // recycle this instead of storing an extra variable
+
+    //Functions called by WLED
     /*
      * setup() is called once at boot. WiFi is not yet connected at this point.
      */
     void setup() {
-      if (i2c_scl<0 || i2c_sda<0) { enabled = false; return; }
+      dmpReady = false;   // Start clean
+
+      // one time init
+      if (!initDone()) {
+        um_data.u_size = 9;
+        um_data.u_type = new um_types_t[um_data.u_size];
+        um_data.u_data = new void*[um_data.u_size];
+        um_data.u_data[0] = &qat;
+        um_data.u_type[0] = UMT_FLOAT_ARR;
+        um_data.u_data[1] = &euler;
+        um_data.u_type[1] = UMT_FLOAT_ARR;
+        um_data.u_data[2] = &ypr;
+        um_data.u_type[2] = UMT_FLOAT_ARR;
+        um_data.u_data[3] = &aa;
+        um_data.u_type[3] = UMT_INT16_ARR;
+        um_data.u_data[4] = &gy;
+        um_data.u_type[4] = UMT_INT16_ARR;
+        um_data.u_data[5] = &aaReal;
+        um_data.u_type[5] = UMT_INT16_ARR;
+        um_data.u_data[6] = &aaWorld;
+        um_data.u_type[6] = UMT_INT16_ARR;
+        um_data.u_data[7] = &gravity;
+        um_data.u_type[7] = UMT_FLOAT_ARR;
+        um_data.u_data[8] = &sample_count;
+        um_data.u_type[8] = UMT_UINT32;
+      }
+
+      if (!config.enabled) return;
+      if (i2c_scl<0 || i2c_sda<0) { DEBUG_PRINTLN(F("MPU6050: I2C is no good."));  return; }
+      // Check the interrupt pin
+      if (config.interruptPin >= 0) {
+        irqBound = pinManager.allocatePin(config.interruptPin, false, PinOwner::UM_IMU);
+        if (!irqBound) { DEBUG_PRINTLN(F("MPU6050: IRQ pin already in use.")); return; }
+        pinMode(config.interruptPin, INPUT);
+      };
+
       #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
         Wire.setClock(400000U); // 400kHz I2C clock. Comment this line if having compilation difficulties
       #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
@@ -115,7 +175,6 @@ class MPU6050Driver : public Usermod {
       // initialize device
       DEBUG_PRINTLN(F("Initializing I2C devices..."));
       mpu.initialize();
-      pinMode(INTERRUPT_PIN, INPUT);
 
       // verify connection
       DEBUG_PRINTLN(F("Testing device connections..."));
@@ -125,11 +184,16 @@ class MPU6050Driver : public Usermod {
       DEBUG_PRINTLN(F("Initializing DMP..."));
       devStatus = mpu.dmpInitialize();
 
-      // supply your own gyro offsets here, scaled for min sensitivity
+      // set offsets (from config)
-      mpu.setXGyroOffset(220);
+      mpu.setXGyroOffset(config.gyro_offset[0]);
-      mpu.setYGyroOffset(76);
+      mpu.setYGyroOffset(config.gyro_offset[1]);
-      mpu.setZGyroOffset(-85);
+      mpu.setZGyroOffset(config.gyro_offset[2]);
-      mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
+      mpu.setXAccelOffset(config.accel_offset[0]);
+      mpu.setYAccelOffset(config.accel_offset[1]);
+      mpu.setZAccelOffset(config.accel_offset[2]);
+
+      // set sample rate
+      mpu.setRate(16);  // ~100Hz
 
       // make sure it worked (returns 0 if so)
       if (devStatus == 0) {
@@ -137,17 +201,19 @@ class MPU6050Driver : public Usermod {
         DEBUG_PRINTLN(F("Enabling DMP..."));
         mpu.setDMPEnabled(true);
 
-        // enable Arduino interrupt detection
+        mpuInterrupt = true;
-        DEBUG_PRINTLN(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
+        if (irqBound) {
-        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
+          // enable Arduino interrupt detection
-        mpuIntStatus = mpu.getIntStatus();
+          DEBUG_PRINTLN(F("Enabling interrupt detection (Arduino external interrupt 0)..."));          
-
+          attachInterrupt(digitalPinToInterrupt(config.interruptPin), dmpDataReady, RISING);
-        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+        }
-        DEBUG_PRINTLN(F("DMP ready! Waiting for first interrupt..."));
-        dmpReady = true;
 
         // get expected DMP packet size for later comparison
         packetSize = mpu.dmpGetFIFOPacketSize();
+
+        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+        DEBUG_PRINTLN(F("DMP ready!"));
+        dmpReady = true;
       } else {
         // ERROR!
         // 1 = initial memory load failed
@@ -157,6 +223,9 @@ class MPU6050Driver : public Usermod {
         DEBUG_PRINT(devStatus);
         DEBUG_PRINTLN(")");
       }
+
+      fifoCount = 0;
+      sample_count = 0;
     }
 
     /*
@@ -173,28 +242,31 @@ class MPU6050Driver : public Usermod {
      */
     void loop() {
       // if programming failed, don't try to do anything
-      if (!enabled || !dmpReady || strip.isUpdating()) return;
+      if (!config.enabled || !dmpReady || strip.isUpdating()) return;
 
       // wait for MPU interrupt or extra packet(s) available
+      // mpuInterrupt is fixed on if interrupt pin is disabled
       if (!mpuInterrupt && fifoCount < packetSize) return;
 
       // reset interrupt flag and get INT_STATUS byte
-      mpuInterrupt = false;
+      auto mpuIntStatus = mpu.getIntStatus();
-      mpuIntStatus = mpu.getIntStatus();
+      // Update current FIFO count
-
-      // get current FIFO count
       fifoCount = mpu.getFIFOCount();
 
       // check for overflow (this should never happen unless our code is too inefficient)
       if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
         // reset so we can continue cleanly
         mpu.resetFIFO();
-        DEBUG_PRINTLN(F("FIFO overflow!"));
+        DEBUG_PRINTLN(F("MPU6050: FIFO overflow!"));
 
-        // otherwise, check for DMP data ready interrupt (this should happen frequently)
+        // otherwise, check for data ready
-      } else if (mpuIntStatus & 0x02) {
+      } else if (fifoCount >= packetSize) {
-        // wait for correct available data length, should be a VERY short wait
+        // clear local interrupt pending status, if not polling
-        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+        mpuInterrupt = !irqBound;
+
+        // DEBUG_PRINT(F("MPU6050: Processing packet: "));
+        // DEBUG_PRINT(fifoCount);
+        // DEBUG_PRINTLN(F(" bytes in FIFO"));
 
         // read a packet from FIFO
         mpu.getFIFOBytes(fifoBuffer, packetSize);
@@ -203,7 +275,6 @@ class MPU6050Driver : public Usermod {
         // (this lets us immediately read more without waiting for an interrupt)
         fifoCount -= packetSize;
 
-
         //NOTE: some of these can be removed to save memory, processing time
         //      if the measurement isn't needed
         mpu.dmpGetQuaternion(&qat, fifoBuffer);
@@ -214,87 +285,141 @@ class MPU6050Driver : public Usermod {
         mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
         mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &qat);
         mpu.dmpGetYawPitchRoll(ypr, &qat, &gravity);
+        ++sample_count;
       }
     }
 
-
-
     void addToJsonInfo(JsonObject& root)
     {
-      int reading = 20;
-      //this code adds "u":{"Light":[20," lux"]} to the info object
       JsonObject user = root["u"];
       if (user.isNull()) user = root.createNestedObject("u");
 
-      JsonObject imu_meas = user.createNestedObject("IMU");
+      // Unfortunately the web UI doesn't know how to print sub-objects: you just see '[object Object]'
-      JsonArray quat_json = imu_meas.createNestedArray("Quat");
+      // For now, we just put everything in the root userdata object.
+      //auto imu_meas = user.createNestedObject("IMU");
+      auto& imu_meas = user;
+
+      // If an element is an array, the UI expects two elements in the form [value, unit]
+      // Since our /value/ is an array, wrap it, eg. [[a, b, c]]
+      JsonArray quat_json = imu_meas.createNestedArray("Quat").createNestedArray();
       quat_json.add(qat.w);
       quat_json.add(qat.x);
       quat_json.add(qat.y);
       quat_json.add(qat.z);
-      JsonArray euler_json = imu_meas.createNestedArray("Euler");
+      JsonArray euler_json = imu_meas.createNestedArray("Euler").createNestedArray();
       euler_json.add(euler[0]);
       euler_json.add(euler[1]);
       euler_json.add(euler[2]);
-      JsonArray accel_json = imu_meas.createNestedArray("Accel");
+      JsonArray accel_json = imu_meas.createNestedArray("Accel").createNestedArray();
       accel_json.add(aa.x);
       accel_json.add(aa.y);
       accel_json.add(aa.z);
-      JsonArray gyro_json = imu_meas.createNestedArray("Gyro");
+      JsonArray gyro_json = imu_meas.createNestedArray("Gyro").createNestedArray();
       gyro_json.add(gy.x);
       gyro_json.add(gy.y);
       gyro_json.add(gy.z);
-      JsonArray world_json = imu_meas.createNestedArray("WorldAccel");
+      JsonArray world_json = imu_meas.createNestedArray("WorldAccel").createNestedArray();
       world_json.add(aaWorld.x);
       world_json.add(aaWorld.y);
       world_json.add(aaWorld.z);
-      JsonArray real_json = imu_meas.createNestedArray("RealAccel");
+      JsonArray real_json = imu_meas.createNestedArray("RealAccel").createNestedArray();
       real_json.add(aaReal.x);
       real_json.add(aaReal.y);
       real_json.add(aaReal.z);
-      JsonArray grav_json = imu_meas.createNestedArray("Gravity");
+      JsonArray grav_json = imu_meas.createNestedArray("Gravity").createNestedArray();
       grav_json.add(gravity.x);
       grav_json.add(gravity.y);
       grav_json.add(gravity.z);
-      JsonArray orient_json = imu_meas.createNestedArray("Orientation");
+      JsonArray orient_json = imu_meas.createNestedArray("Orientation").createNestedArray();
       orient_json.add(ypr[0]);
       orient_json.add(ypr[1]);
       orient_json.add(ypr[2]);
     }
 
 
-    /*
-     * addToJsonState() can be used to add custom entries to the /json/state part of the JSON API (state object).
-     * Values in the state object may be modified by connected clients
-     */
-    //void addToJsonState(JsonObject& root)
-    //{
-      //root["user0"] = userVar0;
-    //}
-
-
-    /*
-     * readFromJsonState() can be used to receive data clients send to the /json/state part of the JSON API (state object).
-     * Values in the state object may be modified by connected clients
-     */
-    //void readFromJsonState(JsonObject& root)
-    //{
-      //if (root["bri"] == 255) DEBUG_PRINTLN(F("Don't burn down your garage!"));
-    //}
-
-
     /*
      * addToConfig() can be used to add custom persistent settings to the cfg.json file in the "um" (usermod) object.
      * It will be called by WLED when settings are actually saved (for example, LED settings are saved)
      * I highly recommend checking out the basics of ArduinoJson serialization and deserialization in order to use custom settings!
      */
-//    void addToConfig(JsonObject& root)
+    void addToConfig(JsonObject& root)
-//    {
+    {
-//      JsonObject top = root.createNestedObject("MPU6050_IMU");
+      JsonObject top = root.createNestedObject(FPSTR(_name));
-//      JsonArray pins = top.createNestedArray("pin");
+
-//      pins.add(HW_PIN_SCL);
+      //save these vars persistently whenever settings are saved
-//      pins.add(HW_PIN_SDA);
+      top[FPSTR(_enabled)] = config.enabled;
-//    }
+      top[FPSTR(_interrupt_pin)] = config.interruptPin;
+      top[FPSTR(_x_acc_bias)] = config.accel_offset[0];
+      top[FPSTR(_y_acc_bias)] = config.accel_offset[1];
+      top[FPSTR(_z_acc_bias)] = config.accel_offset[2];
+      top[FPSTR(_x_gyro_bias)] = config.gyro_offset[0];
+      top[FPSTR(_y_gyro_bias)] = config.gyro_offset[1];
+      top[FPSTR(_z_gyro_bias)] = config.gyro_offset[2];
+    }
+
+    /*
+     * readFromConfig() can be used to read back the custom settings you added with addToConfig().
+     * This is called by WLED when settings are loaded (currently this only happens immediately after boot, or after saving on the Usermod Settings page)
+     *
+     * readFromConfig() is called BEFORE setup(). This means you can use your persistent values in setup() (e.g. pin assignments, buffer sizes),
+     * but also that if you want to write persistent values to a dynamic buffer, you'd need to allocate it here instead of in setup.
+     * If you don't know what that is, don't fret. It most likely doesn't affect your use case :)
+     *
+     * Return true in case the config values returned from Usermod Settings were complete, or false if you'd like WLED to save your defaults to disk (so any missing values are editable in Usermod Settings)
+     *
+     * getJsonValue() returns false if the value is missing, or copies the value into the variable provided and returns true if the value is present
+     * The configComplete variable is true only if the "exampleUsermod" object and all values are present.  If any values are missing, WLED will know to call addToConfig() to save them
+     *
+     * This function is guaranteed to be called on boot, but could also be called every time settings are updated
+     */
+    bool readFromConfig(JsonObject& root)
+    {
+      // default settings values could be set here (or below using the 3-argument getJsonValue()) instead of in the class definition or constructor
+      // setting them inside readFromConfig() is slightly more robust, handling the rare but plausible use case of single value being missing after boot (e.g. if the cfg.json was manually edited and a value was removed)
+      auto old_cfg = config;
+
+      JsonObject top = root[FPSTR(_name)];
+
+      bool configComplete = top.isNull();
+      // Ensure default configuration is loaded
+      configComplete &= getJsonValue(top[FPSTR(_enabled)], config.enabled, true);
+      configComplete &= getJsonValue(top[FPSTR(_interrupt_pin)], config.interruptPin, -1);
+      configComplete &= getJsonValue(top[FPSTR(_x_acc_bias)], config.accel_offset[0], 0);
+      configComplete &= getJsonValue(top[FPSTR(_y_acc_bias)], config.accel_offset[1], 0);
+      configComplete &= getJsonValue(top[FPSTR(_z_acc_bias)], config.accel_offset[2], 0);
+      configComplete &= getJsonValue(top[FPSTR(_x_gyro_bias)], config.gyro_offset[0], 0);
+      configComplete &= getJsonValue(top[FPSTR(_y_gyro_bias)], config.gyro_offset[1], 0);
+      configComplete &= getJsonValue(top[FPSTR(_z_gyro_bias)], config.gyro_offset[2], 0);
+
+      DEBUG_PRINT(FPSTR(_name));
+      if (top.isNull()) {
+        DEBUG_PRINTLN(F(": No config found. (Using defaults.)"));
+      } else if (!initDone()) {
+        DEBUG_PRINTLN(F(": config loaded."));
+      } else if (memcmp(&config, &old_cfg, sizeof(config)) == 0) {
+        DEBUG_PRINTLN(F(": config unchanged."));
+      } else {
+        DEBUG_PRINTLN(F(": config updated."));
+        // Previously loaded and config changed
+        if (irqBound && ((old_cfg.interruptPin != config.interruptPin) || !config.enabled)) {
+          detachInterrupt(old_cfg.interruptPin);
+          pinManager.deallocatePin(old_cfg.interruptPin, PinOwner::UM_IMU);            
+          irqBound = false;
+        }
+
+        // Just re-init
+        setup();
+      }
+
+      return configComplete;
+    }
+
+    bool getUMData(um_data_t **data)
+    {
+      if (!data || !config.enabled || !dmpReady) return false; // no pointer provided by caller or not enabled -> exit
+      *data = &um_data;
+      return true;
+    }
 
     /*
      * getId() allows you to optionally give your V2 usermod an unique ID (please define it in const.h!).
@@ -305,3 +430,14 @@ class MPU6050Driver : public Usermod {
     }
 
 };
+
+
+const char MPU6050Driver::_name[] PROGMEM = "MPU6050_IMU";
+const char MPU6050Driver::_enabled[] PROGMEM = "enabled";
+const char MPU6050Driver::_interrupt_pin[] PROGMEM = "interrupt_pin";
+const char MPU6050Driver::_x_acc_bias[] PROGMEM = "x_acc_bias";
+const char MPU6050Driver::_y_acc_bias[] PROGMEM = "y_acc_bias";
+const char MPU6050Driver::_z_acc_bias[] PROGMEM = "z_acc_bias";
+const char MPU6050Driver::_x_gyro_bias[] PROGMEM = "x_gyro_bias";
+const char MPU6050Driver::_y_gyro_bias[] PROGMEM = "y_gyro_bias";
+const char MPU6050Driver::_z_gyro_bias[] PROGMEM = "z_gyro_bias";

wled00/pin_manager.h

@@ -41,7 +41,7 @@ enum struct PinOwner : uint8_t {
   UM_Temperature       = USERMOD_ID_TEMPERATURE,        // 0x03 // Usermod "usermod_temperature.h"
   // #define USERMOD_ID_FIXNETSERVICES                  // 0x04 // Usermod "usermod_Fix_unreachable_netservices.h" -- Does not allocate pins
   UM_PIR               = USERMOD_ID_PIRSWITCH,          // 0x05 // Usermod "usermod_PIR_sensor_switch.h"
-  // #define USERMOD_ID_IMU                             // 0x06 // Usermod "usermod_mpu6050_imu.h" -- Uses "standard" HW_I2C pins
+  UM_IMU               = USERMOD_ID_IMU,                // 0x06 // Usermod "usermod_mpu6050_imu.h" -- Interrupt pin
   UM_FourLineDisplay   = USERMOD_ID_FOUR_LINE_DISP,     // 0x07 // Usermod "usermod_v2_four_line_display.h -- May use "standard" HW_I2C pins
   UM_RotaryEncoderUI   = USERMOD_ID_ROTARY_ENC_UI,      // 0x08 // Usermod "usermod_v2_rotary_encoder_ui.h"
   // #define USERMOD_ID_AUTO_SAVE                       // 0x09 // Usermod "usermod_v2_auto_save.h" -- Does not allocate pins