diff --git a/Gui.h b/Gui.h index d06622f..d02c34c 100644 --- a/Gui.h +++ b/Gui.h @@ -181,8 +181,9 @@ extern uint32_t imu_log_samples; extern uint32_t imu_log_start_ms; // Forward declaration for touch logging (defined in IMULogger.h) void sensor_log_touch(int16_t x, int16_t y, bool pressed); -// Forward declarations for filtered accel (defined in .ino) +// Forward declarations for filtered accel and noise (defined in .ino) extern volatile float imu_ax_f, imu_ay_f, imu_az_f; +extern volatile float imu_noise; #ifndef PMU_TEMP_MIN #define PMU_TEMP_MIN -30 #endif @@ -817,10 +818,14 @@ static void gui_update_data() { float target_x = tilt_dir_x * mapped_r; float target_y = -tilt_dir_y * mapped_r; - // Spring-damper: overdamped for viscous fluid feel - // Sub-step at 20ms to keep integration stable at any frame rate - const float spring = 40.0f; - const float damping = 12.0f; + // Adaptive spring-damper: snappy when still, viscous when noisy + // noise < 0.001 (desk): spring=80 damp=16 → settles in ~0.15s + // noise > 0.01 (hand): spring=30 damp=14 → settles in ~0.4s + float noise_t = imu_noise < 0.001f ? 0.0f : + imu_noise > 0.01f ? 1.0f : + (imu_noise - 0.001f) / 0.009f; + float spring = 80.0f - 50.0f * noise_t; + float damping = 16.0f - 2.0f * noise_t; const float max_sub = 0.02f; int steps = (int)(dt / max_sub) + 1; float sdt = dt / steps; diff --git a/RNode_Firmware.ino b/RNode_Firmware.ino index f160257..8df27fd 100644 --- a/RNode_Firmware.ino +++ b/RNode_Firmware.ino @@ -33,17 +33,24 @@ volatile uint32_t imu_step_count = 0; volatile bool imu_wrist_tilt = false; - // Filtered accelerometer for bubble level (EMA, α=0.15) + // Filtered accelerometer for bubble level with adaptive noise tracking volatile float imu_ax_f = 0, imu_ay_f = 0, imu_az_f = 4096; - #define IMU_EMA_ALPHA 0.15f + volatile float imu_noise = 0; // running estimate of accel noise (0=still, 1+=noisy) void imu_accel_live_cb(uint8_t sensor_id, uint8_t *data, uint32_t size, uint64_t *timestamp, void *user_data) { if (size >= 6) { float ax = (int16_t)(data[0] | (data[1] << 8)); float ay = (int16_t)(data[2] | (data[3] << 8)); float az = (int16_t)(data[4] | (data[5] << 8)); - imu_ax_f += IMU_EMA_ALPHA * (ax - imu_ax_f); - imu_ay_f += IMU_EMA_ALPHA * (ay - imu_ay_f); - imu_az_f += IMU_EMA_ALPHA * (az - imu_az_f); + // Noise: EMA of squared deviation from filtered value + float dx = ax - imu_ax_f, dy = ay - imu_ay_f, dz = az - imu_az_f; + float dev = (dx*dx + dy*dy + dz*dz) / (4096.0f * 4096.0f); + imu_noise += 0.1f * (dev - imu_noise); + // Adaptive EMA: responsive when quiet, smooth when noisy + float alpha = (imu_noise < 0.001f) ? 0.4f : + (imu_noise < 0.01f) ? 0.2f : 0.08f; + imu_ax_f += alpha * (ax - imu_ax_f); + imu_ay_f += alpha * (ay - imu_ay_f); + imu_az_f += alpha * (az - imu_az_f); } }