Mirror/markqvist___RNode_Firmware
1
0
Fork 0
mirror of https://github.com/markqvist/RNode_Firmware.git synced 2026-04-27 14:30:33 +00:00
Code Issues Projects Releases Packages Wiki Activity

Adaptive bubble level: noise-driven filter and spring-damper tuning

Browse source 
Accel callback tracks noise as EMA of squared deviation from filtered
value, normalised to 1g². Three regimes:

  Quiet (desk, noise<0.001): α=0.4, spring=80, damp=16 → snappy
  Hand-held (0.001-0.01):    interpolated smoothly
  Noisy (walking, >0.01):    α=0.08, spring=30, damp=14 → viscous

Result: bubble is fast and responsive on a stable surface, but
smoothly damped when the watch is being worn or moved.
This commit is contained in:
GlassOnTin 2026-04-02 14:16:42 +01:00
commit 8b3f67c1ee
2 changed files with 22 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

15
Gui.h
View file

@ -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;

17
RNode_Firmware.ino
View file

@ -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);
}
}