Skip to content
This repository was archived by the owner on May 19, 2026. It is now read-only.

fix(running): update wrist tilt algorithm #63

Open
sdvsaienko wants to merge 1 commit into
base: main
Choose a base branch
from
Open

fix(running): update wrist tilt algorithm #63

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 74 additions & 33 deletions Apps/Running/Software/Libs/Header/WristTiltDetector.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,12 @@
* @file WristTiltDetector.hpp
* @date 22-04-2025
* @author Denys Saienko <denys.saienko@droid-technologies.com>
* @brief Wrist-tilt (watch-look) gesture detector for walking and running.
* @brief Wrist-tilt (watch-look) gesture detector for the Running app.
*
* The software detector only emits a gesture while the wearer is
* RUNNING. In the STATIONARY and WALKING states the physical
* WRIST_MOTION sensor handles the watch-look gesture, so this
* detector stays idle and fires nothing.
*
* Algorithm overview:
* 1. MOTION CLASSIFICATION (5 s sliding window on AY delta)
Expand All @@ -13,32 +18,38 @@
* crossed; the STATIONARY class has a dedicated lower bound so
* that noise near zero does not flip the state back and forth.
*
* 2. DYNAMIC GX THRESHOLD
* Each motion class has its own peak-GX detection threshold
* (raw int16 LSB from the sensor register). Higher thresholds
* for more vigorous activities prevent running arm-swing from
* flooding the gesture output.
* 2. PITCH ESTIMATE
* A low-pass filter (gravityAlpha) tracks the gravity vector on
* the AY/AZ axes; the wrist pitch is conceptually
* atan2(grav_y, grav_z). The filter runs on every sample so
* the estimate is settled when the RUNNING state is entered.
* The trigger only needs the sign of (pitch - threshold),
* which is evaluated algebraically with no trigonometry.
*
* 3. TILT STATE MACHINE
* IDLE -> if |GX| exceeds the dynamic threshold -> ACTIVE
* The machine runs in every motion class so a hold or cooldown
* already in progress always completes its timed transitions.
* Only the IDLE -> ACTIVE trigger is gated to RUNNING.
* IDLE -> while RUNNING, if |GX| exceeds gxThreshRunning OR
* pitch exceeds pitchThresholdDeg -> ACTIVE
* ACTIVE -> held for holdDurationS seconds, listener notified
* once on entry -> COOLDOWN
* COOLDOWN -> held for cooldownDurationS seconds -> IDLE
*
* Raw LSB units (not converted to physical units) are used throughout
* for efficiency and to avoid floating-point on resource-limited MCUs.
* Raw int16 LSB units (sensor register values) are used for the
* accelerometer and gyroscope inputs; only the pitch estimate uses
* floating point.
*
* Default tuning values are taken directly from wrist_tilt11.py and
* validated on three BMI270 datasets (running, walking, stationary):
* motionWindowS = 5.0 s
* avgSwingStationary= 50 LSB
* avgSwingWalking = 60 LSB
* avgSwingRunning = 400 LSB
* gxThreshStationary= 3000 LSB
* gxThreshWalking = 5100 LSB
* gxThreshRunning = 5300 LSB
* holdDurationS = 3.0 s
* cooldownDurationS = 0.5 s
* Default tuning values are taken from wrist_tilt17.py:
* motionWindowS = 5.0 s
* avgSwingStationary= 50 LSB
* avgSwingWalking = 60 LSB
* avgSwingRunning = 400 LSB
* gxThreshRunning = 10000 LSB
* gravityAlpha = 0.97
* pitchThresholdDeg = -10.0 deg
* holdDurationS = 3.0 s
* cooldownDurationS = 0.5 s
*
* Sample rate assumption: 100 Hz (10 ms / sample).
* Adjustable via Config::sampleRateHz.
Expand All @@ -57,7 +68,8 @@
* FIFO — no scaling or unit conversion applied.
******************************************************************************/
struct TiltImuSample {
int16_t ayLsb; /**< Accelerometer Y axis, raw LSB (motion classification) */
int16_t ayLsb; /**< Accelerometer Y axis, raw LSB (motion + pitch) */
int16_t azLsb; /**< Accelerometer Z axis, raw LSB (pitch) */
int16_t gxLsb; /**< Gyroscope X axis, raw LSB (gesture trigger) */
uint32_t timestampMs; /**< Absolute timestamp of this sample, ms */
};
Expand Down Expand Up @@ -121,16 +133,24 @@ class WristTiltDetector {
/** @} */

/**
* @defgroup GxThresholds Peak |GX| thresholds per motion class (raw LSB)
*
* A single sample must exceed this threshold to trigger the state
* machine transition IDLE -> ACTIVE.
* @{
* Peak |GX| threshold for the IDLE -> ACTIVE transition, raw LSB.
* Consulted only while RUNNING; one sample must exceed it.
*/
int32_t gxThreshStationary = 3000; /**< LSB — stationary wrist tilt */
int32_t gxThreshWalking = 5100; /**< LSB — walking wrist tilt */
int32_t gxThreshRunning = 5300; /**< LSB — running wrist tilt */
/** @} */
int32_t gxThreshRunning = 10000;

/**
* Gravity low-pass filter coefficient for the pitch estimate.
* Higher values track the gravity vector more slowly (more smoothing).
*/
float gravityAlpha = 0.97f;

/**
* Pitch threshold for the IDLE -> ACTIVE transition, degrees.
* Consulted only while RUNNING; pitch above this value triggers.
* Must lie in the range (-90, 0] so the trigger can be evaluated
* without trigonometry (see isPitchAboveThreshold()).
*/
float pitchThresholdDeg = -10.0f;

/**
* Duration to hold the ACTIVE state after a gesture is detected, s.
Expand Down Expand Up @@ -263,10 +283,24 @@ class WristTiltDetector {
void updateMotionClass(float avgSwing, uint32_t tsMs) noexcept;

/**
* @brief Return the GX detection threshold for the current motion class.
* @retval Threshold in raw LSB.
* @brief Update the gravity low-pass filter with one sample.
* @param ay AY value of the current sample.
* @param az AZ value of the current sample.
*/
int32_t dynamicGxThreshold() const noexcept;
void updateGravity(int16_t ay, int16_t az) noexcept;

/**
* @brief Test whether the current wrist pitch exceeds pitchThresholdDeg.
*
* Equivalent to atan2(grav_y, grav_z) > pitchThresholdDeg, but
* evaluated without trigonometry. The threshold is non-positive
* (range (-90, 0]), so the whole grav_y >= 0 half-plane is above
* it; for grav_y < 0 the pitch clears the threshold only inside
* a wedge of the grav_z > 0 quadrant.
*
* @retval true if pitch is above the threshold.
*/
bool isPitchAboveThreshold() const noexcept;

/**
* @brief Process one sample through the tilt state machine.
Expand All @@ -292,6 +326,13 @@ class WristTiltDetector {
int16_t mPrevAy = 0; /**< Previous AY for delta calc */
bool mHasPrevAy = false;

/*--------------------------------------------------------------------------
* Pitch estimate (gravity low-pass filter)
*------------------------------------------------------------------------*/
float mGravY = 0.0f; /**< Filtered gravity component on AY axis */
float mGravZ = 0.0f; /**< Filtered gravity component on AZ axis */
float mNegPitchTan = 0.0f; /**< tan(-pitchThresholdDeg), precomputed */

/*--------------------------------------------------------------------------
* Tilt state machine
*------------------------------------------------------------------------*/
Expand Down
4 changes: 4 additions & 0 deletions Apps/Running/Software/Libs/Sources/Service.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -347,6 +347,7 @@ void Service::handleSensorsData(uint16_t handle, SDK::Sensor::DataBatch& data)
SDK::SensorDataParser::FusionRaw::Data sample{};
parser.getData(sample);
batch[batchLen].ayLsb = sample.accel.y;
batch[batchLen].azLsb = sample.accel.z;
batch[batchLen].gxLsb = sample.gyro.x;
batch[batchLen].timestampMs = parser.getTimestamp();
//LOG_DEBUG("AY: %d, GX: %d\n", sample.accel.y, sample.gyro.x);
Expand Down Expand Up @@ -956,6 +957,9 @@ Service::LapDivSource Service::getLapDivSource()
void Service::onWristTilt(uint32_t timestampMs)
{
LOG_DEBUG("Wrist Tilt detected\n");
#if 1 // Debug only
playBuzzerPattern(200, 1, 0);
#endif
backlightOn();
}

Expand Down
58 changes: 45 additions & 13 deletions Apps/Running/Software/Libs/Sources/WristTiltDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,10 @@ WristTiltDetector::WristTiltDetector(const Config& cfg) noexcept
mHoldSamples = static_cast<uint32_t>(cfg.holdDurationS * sr + 0.5f);
mCooldownSamples = static_cast<uint32_t>(cfg.cooldownDurationS * sr + 0.5f);

/* Precompute tan(|pitchThresholdDeg|) for the trig-free pitch test. */
constexpr float kDegToRad = 0.0174532925199433f;
mNegPitchTan = std::tan(-cfg.pitchThresholdDeg * kDegToRad);

reset();
}

Expand All @@ -46,6 +50,9 @@ void WristTiltDetector::reset() noexcept
mPrevAy = 0;
mHasPrevAy = false;

mGravY = 0.0f;
mGravZ = 0.0f;

mTiltState = TiltState::IDLE;
mStateDeadline = 0u;
mSampleCount = 0u;
Expand Down Expand Up @@ -74,6 +81,10 @@ void WristTiltDetector::setConfig(const Config& cfg) noexcept
mHoldSamples = static_cast<uint32_t>(cfg.holdDurationS * sr + 0.5f);
mCooldownSamples = static_cast<uint32_t>(cfg.cooldownDurationS * sr + 0.5f);

/* Precompute tan(|pitchThresholdDeg|) for the trig-free pitch test. */
constexpr float kDegToRad = 0.0174532925199433f;
mNegPitchTan = std::tan(-cfg.pitchThresholdDeg * kDegToRad);

reset();
}

Expand Down Expand Up @@ -123,11 +134,11 @@ void WristTiltDetector::pushMotionSample(int16_t ay, int16_t prevAy) noexcept

float WristTiltDetector::computeAvgSwing() const noexcept
{
if (mMotionFilled < 2u) {
if (mMotionFilled < 1u) {
return 0.0f;
}
/* mMotionFilled - 1 because we store N deltas for N+1 samples. */
return mDeltaSum / static_cast<float>(mMotionFilled - 1u);
/* mMotionFilled is the count of stored deltas. */
return mDeltaSum / static_cast<float>(mMotionFilled);
}

void WristTiltDetector::updateMotionClass(float avgSwing,
Expand All @@ -152,14 +163,26 @@ void WristTiltDetector::updateMotionClass(float avgSwing,
}
}

int32_t WristTiltDetector::dynamicGxThreshold() const noexcept
void WristTiltDetector::updateGravity(int16_t ay, int16_t az) noexcept
{
switch (mMotionClass) {
case MotionClass::RUNNING: return mCfg.gxThreshRunning;
case MotionClass::WALKING: return mCfg.gxThreshWalking;
case MotionClass::STATIONARY: return mCfg.gxThreshStationary;
default: return mCfg.gxThreshStationary;
/* Per-axis low-pass filter isolating the gravity vector. */
const float alpha = mCfg.gravityAlpha;
mGravY = alpha * mGravY + (1.0f - alpha) * static_cast<float>(ay);
mGravZ = alpha * mGravZ + (1.0f - alpha) * static_cast<float>(az);
}

bool WristTiltDetector::isPitchAboveThreshold() const noexcept
{
/* pitch = atan2(grav_y, grav_z) > pitchThresholdDeg, without trig.
The threshold is non-positive, so the whole grav_y >= 0 half-plane
(pitch in [0, 180] deg) clears it. For grav_y < 0 the pitch is
negative and exceeds the threshold only in the grav_z > 0 quadrant
and within the wedge |pitch| < |threshold|, i.e.
-grav_y < grav_z * tan(|threshold|). */
if (mGravY >= 0.0f) {
return true;
}
return (mGravZ > 0.0f) && (mGravZ * mNegPitchTan > -mGravY);
}

bool WristTiltDetector::processSample(const TiltImuSample& sample) noexcept
Expand All @@ -179,18 +202,27 @@ bool WristTiltDetector::processSample(const TiltImuSample& sample) noexcept
mLastAvgSwing = avgSwing;
updateMotionClass(avgSwing, sample.timestampMs);

/* --- 2. Tilt state machine -------------------------------------------- */
/* --- 2. Gravity filter (updated every sample, all motion classes) ----- */
updateGravity(sample.ayLsb, sample.azLsb);

/* --- 3. Tilt state machine -------------------------------------------- */
/* The machine stays alive in every motion class so a hold or cooldown
in progress completes its timed transitions. Only the IDLE -> ACTIVE
trigger is gated to RUNNING: in STATIONARY / WALKING the physical
WRIST_MOTION sensor owns the gesture. */
const int32_t gxMag = (sample.gxLsb >= 0)
? static_cast<int32_t>(sample.gxLsb)
: -static_cast<int32_t>(sample.gxLsb);
const int32_t gxThresh = dynamicGxThreshold();
const bool gxAbove = (gxMag > gxThresh);
const bool gxAbove = (gxMag > mCfg.gxThreshRunning);
const bool pitchAbove = isPitchAboveThreshold();
const bool canTrigger = (mMotionClass == MotionClass::RUNNING)
&& (gxAbove || pitchAbove);

bool fired = false;

switch (mTiltState) {
case TiltState::IDLE:
if (gxAbove) {
if (canTrigger) {
mTiltState = TiltState::ACTIVE;
mStateDeadline = now + mHoldSamples;
++mTotalEvents;
Expand Down
Loading