diff --git a/Code/Sensors/Sensy/MYOSA_VibrationLogger_ESP32/firmware/MYOSA_VibrationLogger/MYOSA_VibrationLogger.ino b/Code/Sensors/Sensy/MYOSA_VibrationLogger_ESP32/firmware/MYOSA_VibrationLogger/MYOSA_VibrationLogger.ino index 60d2028..16b6eef 100644 --- a/Code/Sensors/Sensy/MYOSA_VibrationLogger_ESP32/firmware/MYOSA_VibrationLogger/MYOSA_VibrationLogger.ino +++ b/Code/Sensors/Sensy/MYOSA_VibrationLogger_ESP32/firmware/MYOSA_VibrationLogger/MYOSA_VibrationLogger.ino @@ -3,7 +3,8 @@ ------------------------------------------------ - No external sensor libraries required - Uses I2C MPU6050 (Accel/Gyro board in MYOSA kit) - - Optionally uses I2C Si7021 (Temp+Humidity board in MYOSA kit) for ambient temperature + - Optionally uses I2C Si7021 (Temp+Humidity board in MYOSA kit) for ambient + temperature - Streams data every 0.5s over: * Serial (human-readable by default, CSV optional) * BLE notifications (binary packet for Web Bluetooth) @@ -14,10 +15,10 @@ - Si7021 at I2C address 0x40 (optional) Notes: - - "Vibration" here is computed as RMS and peak of *linear* acceleration magnitude - over a 0.5 second window. - - Linear acceleration is approximated by subtracting a low-pass gravity estimate - (good for vibration/shaking; not a full IMU fusion). + - "Vibration" here is computed as RMS and peak of *linear* acceleration + magnitude over a 0.5 second window. + - Linear acceleration is approximated by subtracting a low-pass gravity + estimate (good for vibration/shaking; not a full IMU fusion). */ #include @@ -39,51 +40,40 @@ // ------------------------- User-tunable settings ------------------------- -// Serial output format: -// 0 = CSV (good for spreadsheets) -// 1 = Human-readable key/value line (easy to read in Serial Monitor) +// Choose serial output format: 0 for CSV (spreadsheets), 1 for Human-readable static constexpr uint8_t SERIAL_FORMAT = 1; -// Tracking output period (ms). Requirement: 0.5 seconds. +// Tracking output period (ms). This determines how often a complete "packet" of summarized vibration data is sent out. static constexpr uint32_t WINDOW_MS = 500; -// Sensor sampling rate (Hz). Higher gives better vibration accuracy. +// Sensor sampling rate in Hertz (Hz). How many raw readings the ESP32 asks the motion sensor for every single second. static constexpr uint16_t SAMPLE_HZ = 500; -// Accelerometer range: more headroom = less clipping on strong shakes. +// Accelerometer physical range. More headroom (higher Gs) means it can measure violent shaking without clipping. static constexpr MPU6050Minimal::AccelRange ACCEL_RANGE = MPU6050Minimal::RANGE_4G; -// MPU6050 DLPF config: -// 1=184Hz, 2=94Hz, 3=44Hz... +// MPU6050 DLPF config: 1 = 184Hz filter. static constexpr uint8_t MPU_DLPF_CFG = 1; -// I2C pins (ESP32 defaults: SDA=21, SCL=22). Leave as -1 to use defaults. +// I2C pins. -1 means use the default pins for the board (usually SDA=21, SCL=22). static constexpr int8_t I2C_SDA_PIN = -1; static constexpr int8_t I2C_SCL_PIN = -1; -// I2C clock (Hz). 400k helps higher sample rates. +// Speed of the I2C wires. 400,000 Hz (Fast Mode). static constexpr uint32_t I2C_CLOCK_HZ = 400000; -// BLE device name (what users will see when scanning) +// The text name that will show up when scanning for Bluetooth devices static const char *BLE_DEVICE_NAME = "MYOSA-VibeLogger"; // ------------------------------------------------------------------------- -// Shake score + severity thresholds (easy to understand) +// Shake score + severity thresholds // ------------------------------------------------------------------------- -// We compute a SHAKE SCORE in the range 0..1000 from RMS vibration. -// - 0..5 => Still (deadband to avoid hypersensitivity) -// - 6..(CAUTION-1) => Light -// - CAUTION..(PARTIAL-1) => Caution -// - PARTIAL..(SEVERE-1) => Partial -// - SEVERE..1000 => Severe -// -// Set these three thresholds to tune when warnings kick in. -// Tip: Start low, then raise until "Light" covers normal background vibration. +// Below, we define the scores (0-1000) that push the reading into different severity buckets. static constexpr uint16_t THRESH_CAUTION_SCORE = 25; static constexpr uint16_t THRESH_PARTIAL_SCORE = 80; static constexpr uint16_t THRESH_SEVERE_SCORE = 150; -// Still is always 0..5. +// Any score 0..5 is categorized as "Still" to prevent tiny natural background vibrations from triggering. static constexpr uint16_t STILL_SCORE_MAX = 5; static_assert(THRESH_CAUTION_SCORE > STILL_SCORE_MAX, @@ -95,14 +85,12 @@ static_assert(THRESH_SEVERE_SCORE > THRESH_PARTIAL_SCORE, static_assert(THRESH_SEVERE_SCORE <= 1000, "THRESH_SEVERE_SCORE must be <= 1000"); -// ------------------------- BLE UUIDs (must match web client) ------------------------- - +// ------------------------- BLE UUIDs (ID numbers used by Bluetooth) ------------------------- static const char *SERVICE_UUID = "7b4e3a0d-6f1b-4d1a-9c62-4c6b8a2e7a10"; static const char *DATA_CHAR_UUID = "7b4e3a0d-6f1b-4d1a-9c62-4c6b8a2e7a11"; static const char *TIME_CHAR_UUID = "7b4e3a0d-6f1b-4d1a-9c62-4c6b8a2e7a12"; // ------------------------- Globals ------------------------- - MPU6050Minimal mpu; SI7021Minimal si7021; bool hasMpu = false; @@ -112,20 +100,27 @@ BLEServer *g_server = nullptr; BLECharacteristic *g_dataChar = nullptr; BLECharacteristic *g_timeChar = nullptr; +// 'volatile' is used here as these values are modified in background callbacks volatile bool g_deviceConnected = false; +volatile bool g_shouldAdvertise = false; -// Time sync from browser +// ------------------------- Time sync mechanics ------------------------- static portMUX_TYPE g_timeMux = portMUX_INITIALIZER_UNLOCKED; -static uint64_t g_epochBaseMs = 0; // epoch ms at the moment millisBase was captured -static uint32_t g_millisBase = 0; // millis() when epochBaseMs was set + +// Tracking what the real world time (Epoch) was at a specific uptime (Millis) +static uint64_t g_epochBaseMs = 0; // The real world time received from browser (in ms since 1970) +static uint32_t g_millisBase = 0; // The internal ESP32 uptime when we received the real-world time static bool g_timeSynced = false; +// Calculates current real world time using the baseline sync and current uptime static uint64_t getEpochMsOrZero() { if (!g_timeSynced) { return 0; } uint64_t baseMs; uint32_t baseMillis; + + // Protect the read operation since time may update from another core portENTER_CRITICAL(&g_timeMux); baseMs = g_epochBaseMs; baseMillis = g_millisBase; @@ -136,6 +131,7 @@ static uint64_t getEpochMsOrZero() { return baseMs + (uint64_t)delta; } +// Saves the real world time when we receive it over BLE static void setEpochMs(uint64_t epochMs) { portENTER_CRITICAL(&g_timeMux); g_epochBaseMs = epochMs; @@ -144,21 +140,22 @@ static void setEpochMs(uint64_t epochMs) { portEXIT_CRITICAL(&g_timeMux); } -// Binary packet sent over BLE notifications (little-endian) -#pragma pack(push, 1) +// ------------------------- Bluetooth Packet Structure ------------------------- +// #pragma pack(push, 1) instructs the compiler to pack these variables without empty padding bytes +#pragma pack(push, 1) struct VibePacket { - uint32_t t_s; // Unix time seconds (0 if not synced) - uint16_t t_ms; // milliseconds part (0-999) - uint16_t rms_mg; // RMS linear acceleration magnitude (milli-g) - uint16_t peak_mg; // Peak linear acceleration magnitude (milli-g) - uint16_t shake_score; // 0..1000 (derived from RMS) - int16_t temp_c_x100; // temperature (°C * 100) - uint8_t level; // 0..4 intensity bucket + uint32_t t_s; // Real time in seconds + uint16_t t_ms; // Milliseconds fraction of a second (0-999) + uint16_t rms_mg; // RMS average shaking energy in milli-G forces + uint16_t peak_mg; // Single highest spike shake force in this 0.5s window + uint16_t shake_score; // 0..1000 score + int16_t temp_c_x100; // Temp in celsius multiplied by 100 (e.g., 23.50C sent as 2350) + uint8_t level; // 0 to 4 meaning Still, Light, Caution, Partial, Severe }; #pragma pack(pop) -// Convert configured accelerometer range to its full-scale g value. -// Used to map RMS vibration into a 0..1000 "shake score". +// ------------------------- Data Math Utilities ------------------------- +// Convert the hardware setting (like RANGE_4G) to actual math decimal (4.0) static constexpr float accelRangeFullScaleG(MPU6050Minimal::AccelRange r) { return (r == MPU6050Minimal::RANGE_2G) ? 2.0f : (r == MPU6050Minimal::RANGE_4G) ? 4.0f : @@ -169,17 +166,18 @@ static constexpr float accelRangeFullScaleG(MPU6050Minimal::AccelRange r) { static constexpr float SCORE_FULL_SCALE_G = accelRangeFullScaleG(ACCEL_RANGE); +// Formula to translate raw physics math into a 0-1000 score static uint16_t shakeScoreFromRmsG(float rms_g) { if (!isfinite(rms_g) || rms_g <= 0.0f) { return 0; } - // Scale RMS (g) relative to the sensor full-scale (g) into 0..1000. - // Example: With RANGE_4G, rms_g = 0.10g => score = 25. + // Convert based on percentage of max force. const float s = (rms_g / SCORE_FULL_SCALE_G) * 1000.0f; const int score = (int)lroundf(s); return (uint16_t)constrain(score, 0, 1000); } +// Convert a numerical score into a 0 to 4 intensity category static uint8_t levelFromScore(uint16_t score) { if (score <= STILL_SCORE_MAX) { return 0; // Still @@ -211,6 +209,8 @@ static const char *levelLabel(uint8_t level) { } } +// ------------------------- Bluetooth Handlers ------------------------- + class MyServerCallbacks : public BLEServerCallbacks { void onConnect(BLEServer *pServer) override { g_deviceConnected = true; @@ -218,19 +218,18 @@ class MyServerCallbacks : public BLEServerCallbacks { void onDisconnect(BLEServer *pServer) override { g_deviceConnected = false; - // Make it connectable again - BLEDevice::startAdvertising(); + // Tell the main loop to resume advertising so devices can reconnect + g_shouldAdvertise = true; } }; +// Handlers for receiving the time payload from the client class TimeCharCallbacks : public BLECharacteristicCallbacks { void onWrite(BLECharacteristic *pCharacteristic) override { const size_t len = pCharacteristic->getLength(); const uint8_t *data = pCharacteristic->getData(); - if (!data) { - Serial.println("[Time] No data (null pointer)"); - return; - } + + if (!data) return; if (len == 8) { uint64_t epochMs = 0; @@ -242,8 +241,6 @@ class TimeCharCallbacks : public BLECharacteristicCallbacks { memcpy(&epochS, data, 4); setEpochMs((uint64_t)epochS * 1000ULL); Serial.println("[Time] Synced via 4-byte epoch seconds"); - } else { - Serial.printf("[Time] Unexpected payload length: %u\n", (unsigned)len); } } }; @@ -277,13 +274,14 @@ static void setupBle() { BLEDevice::startAdvertising(); } -// Vibration accumulator (for each 0.5 second window) +// ------------------------- Sensor Math Engine ------------------------- +// Custom structure to hold all math being compiled over our 0.5s window struct VibeAccum { - float sumSq = 0.0f; - float peak = 0.0f; - uint32_t n = 0; + float sumSq = 0.0f; // Running tally of shake power for RMS + float peak = 0.0f; // Highest spike felt in the current window + uint32_t n = 0; // Number of readings taken (usually ~250 per 0.5s) - // Gravity estimate (low-pass filtered accel) + // Running low-pass average of G-force to estimate gravity direction float gx = 0.0f; float gy = 0.0f; float gz = 0.0f; @@ -299,13 +297,12 @@ static void resetWindow() { g_acc.n = 0; } +// Feeds one single reading from the sensor into our math engine static void addSample(float ax_g, float ay_g, float az_g) { - // Gravity estimation via simple IIR low-pass filter. - // Choose a time constant that tracks orientation changes but ignores vibration. - // alpha = dt / (tau + dt) + // Use a low-pass filter to separate Gravity from linear Shaking. constexpr float tau = 0.5f; // seconds - constexpr float dt = 1.0f / (float)SAMPLE_HZ; // seconds - constexpr float alpha = dt / (tau + dt); + constexpr float dt = 1.0f / (float)SAMPLE_HZ; // time between readings + constexpr float alpha = dt / (tau + dt); // low-pass multiplier if (!g_acc.gravityInit) { g_acc.gx = ax_g; @@ -313,17 +310,22 @@ static void addSample(float ax_g, float ay_g, float az_g) { g_acc.gz = az_g; g_acc.gravityInit = true; } else { - g_acc.gx += alpha * (ax_g - g_acc.gx); + // Slowly pull the gravity variable toward the current reading + g_acc.gx += alpha * (ax_g - g_acc.gx); g_acc.gy += alpha * (ay_g - g_acc.gy); g_acc.gz += alpha * (az_g - g_acc.gz); } + // Find pure shaking by subtracting the gravity offset const float lx = ax_g - g_acc.gx; - const float ly = ay_g - g_acc.gy; + const float ly = ay_g - g_acc.gy; const float lz = az_g - g_acc.gz; + + // Total combined power of shaking in any direction const float linMag = sqrtf(lx * lx + ly * ly + lz * lz); g_acc.sumSq += linMag * linMag; + if (linMag > g_acc.peak) { g_acc.peak = linMag; } @@ -343,14 +345,11 @@ static bool readTemperatureC(float &tempC) { } static void publishWindow() { - // Even if the sensor isn't ready, still publish a packet every WINDOW_MS so - // the BLE dashboard can connect and show a clear "sensor missing" state. - - bool validVibe = (hasMpu && g_acc.n > 0); + bool validVibe = (hasMpu && g_acc.n > 0); float rms_g = NAN; float peak_g = NAN; - uint16_t shake_score = 0; + uint16_t shake_score = 0; uint8_t level = 0; if (validVibe) { @@ -363,22 +362,22 @@ static void publishWindow() { float tempC = NAN; bool validTemp = readTemperatureC(tempC); - // Timestamp + // Handle Timestamp const uint64_t epochMs = getEpochMsOrZero(); uint32_t t_s = 0; uint16_t t_ms = 0; + if (epochMs != 0) { t_s = (uint32_t)(epochMs / 1000ULL); t_ms = (uint16_t)(epochMs % 1000ULL); } - // Serial output + // Handle serial printing out the physical USB connection const uint64_t serialTimeMs = (epochMs != 0) ? epochMs : (uint64_t)millis(); if (validVibe) { if (SERIAL_FORMAT == 0) { - // CSV - Serial.printf("%llu,%.4f,%.4f,%.2f,%u,%u,%s\n", + Serial.printf("%llu,%.4f,%.4f,%.2f,%u,%u,%s\n", (unsigned long long)serialTimeMs, (double)rms_g, (double)peak_g, @@ -387,13 +386,12 @@ static void publishWindow() { (unsigned)level, levelLabel(level)); } else { - // Human-readable const char *timeKey = (epochMs != 0) ? "epoch_ms" : "uptime_ms"; Serial.printf("%s=%llu | rms_g=%.4f | peak_g=%.4f | temp_c=%.2f | shake_score=%u/1000 | level=%u (%s)\n", timeKey, (unsigned long long)serialTimeMs, (double)rms_g, - (double)peak_g, + (double)peak_g, (double)(validTemp ? tempC : NAN), (unsigned)shake_score, (unsigned)level, @@ -401,7 +399,6 @@ static void publishWindow() { } } else { if (SERIAL_FORMAT == 0) { - // CSV: Use NaN to make it obvious that the sensor isn't available. Serial.printf("%llu,nan,nan,%.2f,0,0,NoSensor\n", (unsigned long long)serialTimeMs, (double)(validTemp ? tempC : NAN)); @@ -414,22 +411,21 @@ static void publishWindow() { } } - // BLE notification + // Handle Bluetooth Transmission if (g_deviceConnected && g_dataChar) { VibePacket pkt; pkt.t_s = t_s; pkt.t_ms = t_ms; if (validVibe) { - // Clamp mg values to uint16 const int rms_mg = (int)lroundf(rms_g * 1000.0f); const int peak_mg = (int)lroundf(peak_g * 1000.0f); - pkt.rms_mg = (uint16_t)constrain(rms_mg, 0, 65534); + pkt.rms_mg = (uint16_t)constrain(rms_mg, 0, 65534); pkt.peak_mg = (uint16_t)constrain(peak_mg, 0, 65534); pkt.shake_score = shake_score; pkt.level = level; } else { - // Sentinel "invalid" values so the web UI can show "sensor missing". + // Send max invalid sentinel values to indicate sensor disconnect pkt.rms_mg = 65535; pkt.peak_mg = 65535; pkt.shake_score = 65535; @@ -440,7 +436,7 @@ static void publishWindow() { const int temp_x100 = (int)lroundf(tempC * 100.0f); pkt.temp_c_x100 = (int16_t)constrain(temp_x100, -32767, 32767); } else { - pkt.temp_c_x100 = (int16_t)-32768; + pkt.temp_c_x100 = (int16_t)-32768; // Error code baseline } g_dataChar->setValue((uint8_t *)&pkt, sizeof(pkt)); @@ -454,7 +450,6 @@ void setup() { Serial.begin(115200); delay(200); - // I2C init if (I2C_SDA_PIN >= 0 && I2C_SCL_PIN >= 0) { Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN); } else { @@ -462,11 +457,10 @@ void setup() { } Wire.setClock(I2C_CLOCK_HZ); - // Sensor init Serial.println("[Init] Searching for MPU6050..."); hasMpu = mpu.begin(Wire, -1, SAMPLE_HZ, ACCEL_RANGE, MPU_DLPF_CFG); if (hasMpu) { - Serial.printf("[Init] MPU6050 found at 0x%02X\n", mpu.address()); + Serial.printf("[Init] MPU6050 found at 0x%02X\n", mpu.address()); } else { Serial.println("[Init] MPU6050 not found. BLE will still start so you can connect."); Serial.println(" Fix wiring (SDA=21, SCL=22, 3.3V, GND) and reset to retry."); @@ -481,12 +475,10 @@ void setup() { Serial.println("[Init] Si7021 not found (using MPU6050 internal temperature)"); } - // BLE init Serial.println("[Init] Starting BLE..."); setupBle(); Serial.println("[Init] BLE advertising started"); - // Serial output legend Serial.println("# Output every 0.5s"); Serial.println("# epoch_ms / uptime_ms : epoch milliseconds if time-synced, otherwise uptime milliseconds"); Serial.println("# rms_g : RMS linear acceleration magnitude over last 0.5s (g)"); @@ -509,12 +501,24 @@ void loop() { static uint32_t lastWindowMs = millis(); static uint32_t lastProbeMs = 0; - const uint32_t samplePeriodUs = (uint32_t)(1000000UL / SAMPLE_HZ); + const uint32_t samplePeriodUs = (uint32_t)(1000000UL / SAMPLE_HZ); + + if (g_shouldAdvertise) { + g_shouldAdvertise = false; + BLEDevice::startAdvertising(); + } - // Take as many samples as needed to catch up (prevents drift if loop jitters). uint32_t nowUs = micros(); - while ((int32_t)(nowUs - lastSampleUs) >= (int32_t)samplePeriodUs) { - lastSampleUs += samplePeriodUs; + + // Pacemaker to enforce accurate sample taking intervals + if ((int32_t)(nowUs - lastSampleUs) >= (int32_t)samplePeriodUs) { + + // Manage timing lag to prevent processing stale data + if ((nowUs - lastSampleUs) > (samplePeriodUs * 2)) { + lastSampleUs = nowUs; + } else { + lastSampleUs += samplePeriodUs; + } if (hasMpu) { float ax, ay, az; @@ -522,21 +526,23 @@ void loop() { addSample(ax, ay, az); } } - - nowUs = micros(); } - // Publish every WINDOW_MS const uint32_t nowMs = millis(); - // If MPU is missing, probe occasionally so users can fix wiring without re-flashing. - if (!hasMpu && (uint32_t)(nowMs - lastProbeMs) >= 2000U) { - lastProbeMs = nowMs; - Serial.println("[Init] Re-trying MPU6050..."); - hasMpu = mpu.begin(Wire, -1, SAMPLE_HZ, ACCEL_RANGE, MPU_DLPF_CFG); - if (hasMpu) { - Serial.printf("[Init] MPU6050 found at 0x%02X\n", mpu.address()); - // If we didn't have Si7021, try it again too. + // Occasional retry checks to handle hot-plugging missing sensors + if ((uint32_t)(nowMs - lastProbeMs) >= 2000U) { + if (!hasMpu || !hasSi7021) { + lastProbeMs = nowMs; + + if (!hasMpu) { + Serial.println("[Init] Re-trying MPU6050..."); + hasMpu = mpu.begin(Wire, -1, SAMPLE_HZ, ACCEL_RANGE, MPU_DLPF_CFG); + if (hasMpu) { + Serial.printf("[Init] MPU6050 found at 0x%02X\n", mpu.address()); + } + } + if (!hasSi7021) { if (si7021.begin(Wire, 0x40)) { hasSi7021 = true; @@ -546,6 +552,7 @@ void loop() { } } + // Pacemaker for the regular 0.5s broadcast window if ((int32_t)(nowMs - lastWindowMs) >= (int32_t)WINDOW_MS) { lastWindowMs += WINDOW_MS; publishWindow();