From d2dbaf52a11c2b0d734c9f86fa279c0206bc6728 Mon Sep 17 00:00:00 2001 From: Will Miles Date: Wed, 3 Jan 2024 13:08:07 -0500 Subject: [PATCH 1/5] usermod_mpu6050: Fix debug prints The MPU6050 library happens to choose the same defines as WLED, which collide and result in nothing printing. Un- and re-define the macros to work around this. --- usermods/mpu6050_imu/usermod_mpu6050_imu.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/usermods/mpu6050_imu/usermod_mpu6050_imu.h b/usermods/mpu6050_imu/usermod_mpu6050_imu.h index 748ddf1a6..0f8d5a319 100644 --- a/usermods/mpu6050_imu/usermod_mpu6050_imu.h +++ b/usermods/mpu6050_imu/usermod_mpu6050_imu.h @@ -33,6 +33,9 @@ #include "I2Cdev.h" +#undef DEBUG_PRINT +#undef DEBUG_PRINTLN +#undef DEBUG_PRINTF #include "MPU6050_6Axis_MotionApps20.h" //#include "MPU6050.h" // not necessary if using MotionApps include file @@ -42,6 +45,23 @@ #include "Wire.h" #endif +// Restore debug macros +// MPU6050 unfortunately uses the same macro names as WLED :( +#undef DEBUG_PRINT +#undef DEBUG_PRINTLN +#undef DEBUG_PRINTF +#ifdef WLED_DEBUG + #define DEBUG_PRINT(x) DEBUGOUT.print(x) + #define DEBUG_PRINTLN(x) DEBUGOUT.println(x) + #define DEBUG_PRINTF(x...) DEBUGOUT.printf(x) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTF(x...) +#endif + + + // ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================ From 9a006dc84a9d3afc4a6cffb7d03ba5f8943dbfbd Mon Sep 17 00:00:00 2001 From: Will Miles Date: Sun, 7 Jan 2024 14:32:22 -0500 Subject: [PATCH 2/5] 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 | 7 +- usermods/mpu6050_imu/usermod_mpu6050_imu.h | 284 +++++++++++++++------ wled00/pin_manager.h | 2 +- 3 files changed, 213 insertions(+), 80 deletions(-) diff --git a/usermods/mpu6050_imu/readme.md b/usermods/mpu6050_imu/readme.md index 412004151..b804ba602 100644 --- a/usermods/mpu6050_imu/readme.md +++ b/usermods/mpu6050_imu/readme.md @@ -20,14 +20,11 @@ react to the globes orientation. See the blog post on building it = 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 - mpu.setXGyroOffset(220); - mpu.setYGyroOffset(76); - mpu.setZGyroOffset(-85); - mpu.setZAccelOffset(1788); // 1688 factory default for my test chip + // set offsets (from config) + mpu.setXGyroOffset(config.gyro_offset[0]); + mpu.setYGyroOffset(config.gyro_offset[1]); + mpu.setZGyroOffset(config.gyro_offset[2]); + 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 - DEBUG_PRINTLN(F("Enabling interrupt detection (Arduino external interrupt 0)...")); - attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); - mpuIntStatus = mpu.getIntStatus(); - - // 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; + mpuInterrupt = true; + if (irqBound) { + // enable Arduino interrupt detection + DEBUG_PRINTLN(F("Enabling interrupt detection (Arduino external interrupt 0)...")); + attachInterrupt(digitalPinToInterrupt(config.interruptPin), dmpDataReady, RISING); + } // 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; - mpuIntStatus = mpu.getIntStatus(); - - // get current FIFO count + auto mpuIntStatus = mpu.getIntStatus(); + // Update 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) - } else if (mpuIntStatus & 0x02) { - // wait for correct available data length, should be a VERY short wait - while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + // otherwise, check for data ready + } else if (fifoCount >= packetSize) { + // clear local interrupt pending status, if not polling + 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"); - JsonArray quat_json = imu_meas.createNestedArray("Quat"); + // Unfortunately the web UI doesn't know how to print sub-objects: you just see '[object Object]' + // 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) -// { -// JsonObject top = root.createNestedObject("MPU6050_IMU"); -// JsonArray pins = top.createNestedArray("pin"); -// pins.add(HW_PIN_SCL); -// pins.add(HW_PIN_SDA); -// } + void addToConfig(JsonObject& root) + { + JsonObject top = root.createNestedObject(FPSTR(_name)); + + //save these vars persistently whenever settings are saved + 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"; diff --git a/wled00/pin_manager.h b/wled00/pin_manager.h index 6248a6066..1402de2bd 100644 --- a/wled00/pin_manager.h +++ b/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 From 6f8b3a6242a8080962d00ddf85feeaca6ccc6c3b Mon Sep 17 00:00:00 2001 From: Will Miles Date: Sun, 7 Jan 2024 14:25:47 -0500 Subject: [PATCH 3/5] usermod_list: Add IMU usermod --- wled00/usermods_list.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/wled00/usermods_list.cpp b/wled00/usermods_list.cpp index 6184571ac..ed4dfbb8d 100644 --- a/wled00/usermods_list.cpp +++ b/wled00/usermods_list.cpp @@ -197,6 +197,10 @@ #include "../usermods/pwm_outputs/usermod_pwm_outputs.h" #endif +#ifdef USERMOD_MPU6050_IMU +#include "../usermods/mpu6050_imu/usermod_mpu6050_imu.h" +#endif + void registerUsermods() { @@ -373,4 +377,9 @@ void registerUsermods() #ifdef USERMOD_INTERNAL_TEMPERATURE usermods.add(new InternalTemperatureUsermod()); #endif + + #ifdef USERMOD_MPU6050_IMU + static MPU6050Driver mpu6050; usermods.add(&mpu6050); + #endif + } From 0616d184e03ee7e439d428a05a55d3bfc1e572b8 Mon Sep 17 00:00:00 2001 From: Will Miles Date: Sun, 7 Jan 2024 14:26:11 -0500 Subject: [PATCH 4/5] Add GyroSurge usermod This usermod applies the IMU data to create a "surge" effect when the device is moved. --- usermods/mpu6050_imu/usermod_gyro_surge.h | 219 ++++++++++++++++++++++ wled00/usermods_list.cpp | 7 + 2 files changed, 226 insertions(+) create mode 100644 usermods/mpu6050_imu/usermod_gyro_surge.h diff --git a/usermods/mpu6050_imu/usermod_gyro_surge.h b/usermods/mpu6050_imu/usermod_gyro_surge.h new file mode 100644 index 000000000..c49d930ff --- /dev/null +++ b/usermods/mpu6050_imu/usermod_gyro_surge.h @@ -0,0 +1,219 @@ +#pragma once + +/* This usermod uses gyro data to provide a "surge" effect on movement + +Requires lib_deps = bolderflight/Bolder Flight Systems Eigen@^3.0.0 + +*/ + +#include "wled.h" + +// Eigen include block +#ifdef A0 +namespace { constexpr size_t A0_temp {A0}; } +#undef A0 +static constexpr size_t A0 {A0_temp}; +#endif + +#ifdef A1 +namespace { constexpr size_t A1_temp {A1}; } +#undef A1 +static constexpr size_t A1 {A1_temp}; +#endif + +#ifdef B0 +namespace { constexpr size_t B0_temp {B0}; } +#undef B0 +static constexpr size_t B0 {B0_temp}; +#endif + +#ifdef B1 +namespace { constexpr size_t B1_temp {B1}; } +#undef B1 +static constexpr size_t B1 {B1_temp}; +#endif + +#ifdef D0 +namespace { constexpr size_t D0_temp {D0}; } +#undef D0 +static constexpr size_t D0 {D0_temp}; +#endif + +#ifdef D1 +namespace { constexpr size_t D1_temp {D1}; } +#undef D1 +static constexpr size_t D1 {D1_temp}; +#endif + +#ifdef D2 +namespace { constexpr size_t D2_temp {D2}; } +#undef D2 +static constexpr size_t D2 {D2_temp}; +#endif + +#ifdef D3 +namespace { constexpr size_t D3_temp {D3}; } +#undef D3 +static constexpr size_t D3 {D3_temp}; +#endif + +#include "eigen.h" +#include + +constexpr auto ESTIMATED_G = 9.801; // m/s^2 +constexpr auto ESTIMATED_G_COUNTS = 8350.; +constexpr auto ESTIMATED_ANGULAR_RATE = (M_PI * 2000) / (INT16_MAX * 180); // radians per second + +// Horribly lame digital filter code +// Currently implements a static IIR filter. +template +class xir_filter { + typedef Eigen::Array array_t; + const array_t a_coeff, b_coeff; + const T gain; + array_t x, y; + + public: + xir_filter(T gain_, array_t a, array_t b) : a_coeff(std::move(a)), b_coeff(std::move(b)), gain(gain_), x(array_t::Zero()), y(array_t::Zero()) {}; + + T operator()(T input) { + x.head(C-1) = x.tail(C-1); // shift by one + x(C-1) = input / gain; + y.head(C-1) = y.tail(C-1); // shift by one + y(C-1) = (x * b_coeff).sum(); + y(C-1) -= (y.head(C-1) * a_coeff.head(C-1)).sum(); + return y(C-1); + } + + T last() { return y(C-1); }; +}; + + + +class GyroSurge : public Usermod { + private: + static const char _name[]; + bool enabled = true; + + // Params + uint8_t max = 0; + float sensitivity = 0; + + // State + uint32_t last_sample; + // 100hz input + // butterworth low pass filter at 20hz + xir_filter filter = { 1., { -0.36952738, 0.19581571, 1.}, {0.20657208, 0.41314417, 0.20657208} }; + // { 1., { 0., 0., 1.}, { 0., 0., 1. } }; // no filter + + + public: + + /* + * setup() is called once at boot. WiFi is not yet connected at this point. + */ + void setup() {}; + + + /* + * 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) + { + JsonObject top = root.createNestedObject(FPSTR(_name)); + + //save these vars persistently whenever settings are saved + top["max"] = max; + top["sensitivity"] = sensitivity; + } + + + /* + * 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) + + JsonObject top = root[FPSTR(_name)]; + + bool configComplete = !top.isNull(); + + configComplete &= getJsonValue(top["max"], max, 0); + configComplete &= getJsonValue(top["sensitivity"], sensitivity, 10); + + return configComplete; + } + + void loop() { + // get IMU data + um_data_t *um_data; + if (!usermods.getUMData(&um_data, USERMOD_ID_IMU)) { + // Apply max + strip.getSegment(0).fadeToBlackBy(max); + return; + } + uint32_t sample_count = *(uint32_t*)(um_data->u_data[8]); + + if (sample_count != last_sample) { + last_sample = sample_count; + // Calculate based on new data + // We use the raw gyro data (angular rate) + auto gyros = (int16_t*)um_data->u_data[4]; // 16384 == 2000 deg/s + + // Compute the overall rotation rate + // For my application (a plasma sword) we ignore X axis rotations (eg. around the long axis) + auto gyro_q = Eigen::AngleAxis { + //Eigen::AngleAxis(ESTIMATED_ANGULAR_RATE * gyros[0], Eigen::Vector3f::UnitX()) * + Eigen::AngleAxis(ESTIMATED_ANGULAR_RATE * gyros[1], Eigen::Vector3f::UnitY()) * + Eigen::AngleAxis(ESTIMATED_ANGULAR_RATE * gyros[2], Eigen::Vector3f::UnitZ()) }; + + // Filter the results + filter(std::min(sensitivity * gyro_q.angle(), 1.0f)); // radians per second +/* + Serial.printf("[%lu] Gy: %d, %d, %d -- ", millis(), (int)gyros[0], (int)gyros[1], (int)gyros[2]); + Serial.print(gyro_q.angle()); + Serial.print(", "); + Serial.print(sensitivity * gyro_q.angle()); + Serial.print(" --> "); + Serial.println(filter.last()); +*/ + } + }; // noop + + /* + * handleOverlayDraw() is called just before every show() (LED strip update frame) after effects have set the colors. + * Use this to blank out some LEDs or set them to a different color regardless of the set effect mode. + * Commonly used for custom clocks (Cronixie, 7 segment) + */ + void handleOverlayDraw() + { + + // TODO: some kind of timing analysis for filtering ... + + // Calculate brightness boost + auto r_float = std::max(std::min(filter.last(), 1.0f), 0.f); + auto result = (uint8_t) (r_float * max); + //Serial.printf("[%lu] %d -- ", millis(), result); + //Serial.println(r_float); + // TODO - multiple segment handling?? + strip.getSegment(0).fadeToBlackBy(max - result); + } +}; + +const char GyroSurge::_name[] PROGMEM = "GyroSurge"; \ No newline at end of file diff --git a/wled00/usermods_list.cpp b/wled00/usermods_list.cpp index ed4dfbb8d..b8247b9a5 100644 --- a/wled00/usermods_list.cpp +++ b/wled00/usermods_list.cpp @@ -201,6 +201,9 @@ #include "../usermods/mpu6050_imu/usermod_mpu6050_imu.h" #endif +#ifdef USERMOD_MPU6050_IMU +#include "../usermods/mpu6050_imu/usermod_gyro_surge.h" +#endif void registerUsermods() { @@ -210,6 +213,7 @@ void registerUsermods() * \/ \/ \/ */ //usermods.add(new MyExampleUsermod()); + #ifdef USERMOD_BATTERY usermods.add(new UsermodBattery()); #endif @@ -382,4 +386,7 @@ void registerUsermods() static MPU6050Driver mpu6050; usermods.add(&mpu6050); #endif + #ifdef USERMOD_GYRO_SURGE + static GyroSurge gyro_surge; usermods.add(&gyro_surge); + #endif } From 9c4d3cd6e2918123959d8dd84fc0831055dec7d5 Mon Sep 17 00:00:00 2001 From: Will Miles Date: Sun, 7 Jan 2024 15:50:15 -0500 Subject: [PATCH 5/5] platformio.ini: Add MPU6050 library dep --- platformio.ini | 2 ++ 1 file changed, 2 insertions(+) diff --git a/platformio.ini b/platformio.ini index fa6bf3bb8..50362de30 100644 --- a/platformio.ini +++ b/platformio.ini @@ -198,6 +198,8 @@ lib_deps = #For ADS1115 sensor uncomment following ; adafruit/Adafruit BusIO @ 1.13.2 ; adafruit/Adafruit ADS1X15 @ 2.4.0 + #For MPU6050 IMU uncomment follwoing + ; electroniccats/MPU6050 @1.0.1 extra_scripts = ${scripts_defaults.extra_scripts}