will/InfiniTime
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

lowersleep: Improve algorithm by checking wrist angle

Browse Source 
Inspired by https://github.com/InfiniTimeOrg/InfiniTime/pull/827#issuecomment-1881580414.
This commit is contained in:
FintasticMan 2024-02-12 11:37:34 +01:00 committed by JF
parent 636af4d33d
commit f2df0c45ef
2 changed files with 23 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/components/motion/MotionController.cpp
View File

@ -40,15 +40,15 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
service->OnNewStepCountValue(nbSteps); service->OnNewStepCountValue(nbSteps);
} }
if (service != nullptr && (this->x != x || yHistory[0] != y || zHistory[0] != z)) { if (service != nullptr && (xHistory[0] != x || yHistory[0] != y || zHistory[0] != z)) {
service->OnNewMotionValues(x, y, z); service->OnNewMotionValues(x, y, z);
} }
lastTime = time; lastTime = time;
time = xTaskGetTickCount(); time = xTaskGetTickCount();
lastX = this->x; xHistory++;
this->x = x; xHistory[0] = x;
yHistory++; yHistory++;
yHistory[0] = y; yHistory[0] = y;
zHistory++; zHistory++;
@ -67,20 +67,26 @@ MotionController::AccelStats MotionController::GetAccelStats() const {
AccelStats stats; AccelStats stats;
for (uint8_t i = 0; i < AccelStats::numHistory; i++) { for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
stats.xMean += xHistory[histSize - i];
stats.yMean += yHistory[histSize - i]; stats.yMean += yHistory[histSize - i];
stats.zMean += zHistory[histSize - i]; stats.zMean += zHistory[histSize - i];
stats.prevXMean += xHistory[1 + i];
stats.prevYMean += yHistory[1 + i]; stats.prevYMean += yHistory[1 + i];
stats.prevZMean += zHistory[1 + i]; stats.prevZMean += zHistory[1 + i];
} }
stats.xMean /= AccelStats::numHistory;
stats.yMean /= AccelStats::numHistory; stats.yMean /= AccelStats::numHistory;
stats.zMean /= AccelStats::numHistory; stats.zMean /= AccelStats::numHistory;
stats.prevXMean /= AccelStats::numHistory;
stats.prevYMean /= AccelStats::numHistory; stats.prevYMean /= AccelStats::numHistory;
stats.prevZMean /= AccelStats::numHistory; stats.prevZMean /= AccelStats::numHistory;
for (uint8_t i = 0; i < AccelStats::numHistory; i++) { for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
stats.xVariance += (xHistory[histSize - i] - stats.xMean) * (xHistory[histSize - i] - stats.xMean);
stats.yVariance += (yHistory[histSize - i] - stats.yMean) * (yHistory[histSize - i] - stats.yMean); stats.yVariance += (yHistory[histSize - i] - stats.yMean) * (yHistory[histSize - i] - stats.yMean);
stats.zVariance += (zHistory[histSize - i] - stats.zMean) * (zHistory[histSize - i] - stats.zMean); stats.zVariance += (zHistory[histSize - i] - stats.zMean) * (zHistory[histSize - i] - stats.zMean);
} }
stats.xVariance /= AccelStats::numHistory;
stats.yVariance /= AccelStats::numHistory; stats.yVariance /= AccelStats::numHistory;
stats.zVariance /= AccelStats::numHistory; stats.zVariance /= AccelStats::numHistory;
@ -93,7 +99,7 @@ bool MotionController::ShouldRaiseWake() const {
constexpr int16_t yThresh = -64; constexpr int16_t yThresh = -64;
constexpr int16_t rollDegreesThresh = -45; constexpr int16_t rollDegreesThresh = -45;
if (x < -xThresh || x > xThresh) { if (std::abs(stats.xMean) > xThresh) {
return false; return false;
} }
@ -107,8 +113,9 @@ bool MotionController::ShouldRaiseWake() const {
bool MotionController::ShouldShakeWake(uint16_t thresh) { bool MotionController::ShouldShakeWake(uint16_t thresh) {
/* Currently Polling at 10hz, If this ever goes faster scalar and EMA might need adjusting */ /* Currently Polling at 10hz, If this ever goes faster scalar and EMA might need adjusting */
int32_t speed = int32_t speed = std::abs(zHistory[0] - zHistory[histSize - 1] + (yHistory[0] - yHistory[histSize - 1]) / 2 +
std::abs(zHistory[0] - zHistory[histSize - 1] + (yHistory[0] - yHistory[histSize - 1]) / 2 + (x - lastX) / 4) * 100 / (time - lastTime); (xHistory[0] - xHistory[histSize - 1]) / 4) *
100 / (time - lastTime);
// (.2 * speed) + ((1 - .2) * accumulatedSpeed); // (.2 * speed) + ((1 - .2) * accumulatedSpeed);
accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5; accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5;
@ -116,6 +123,11 @@ bool MotionController::ShouldShakeWake(uint16_t thresh) {
} }
bool MotionController::ShouldLowerSleep() const { bool MotionController::ShouldLowerSleep() const {
if ((stats.xMean > 887 && DegreesRolled(stats.xMean, stats.zMean, stats.prevXMean, stats.prevZMean) > 30) ||
(stats.xMean < -887 && DegreesRolled(stats.xMean, stats.zMean, stats.prevXMean, stats.prevZMean) < -30)) {
return true;
}
if (stats.yMean < 724 || DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < 30) { if (stats.yMean < 724 || DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < 30) {
return false; return false;
} }

8
src/components/motion/MotionController.h
View File

@ -21,7 +21,7 @@ namespace Pinetime {
void Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps); void Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps);
int16_t X() const { int16_t X() const {
return x; return xHistory[0];
} }
int16_t Y() const { int16_t Y() const {
@ -76,11 +76,14 @@ namespace Pinetime {
struct AccelStats { struct AccelStats {
static constexpr uint8_t numHistory = 2; static constexpr uint8_t numHistory = 2;
int16_t xMean = 0;
int16_t yMean = 0; int16_t yMean = 0;
int16_t zMean = 0; int16_t zMean = 0;
int16_t prevXMean = 0;
int16_t prevYMean = 0; int16_t prevYMean = 0;
int16_t prevZMean = 0; int16_t prevZMean = 0;
uint32_t xVariance = 0;
uint32_t yVariance = 0; uint32_t yVariance = 0;
uint32_t zVariance = 0; uint32_t zVariance = 0;
}; };
@ -89,9 +92,8 @@ namespace Pinetime {
AccelStats stats = {}; AccelStats stats = {};
int16_t lastX = 0;
int16_t x = 0;
static constexpr uint8_t histSize = 8; static constexpr uint8_t histSize = 8;
Utility::CircularBuffer<int16_t, histSize> xHistory = {};
Utility::CircularBuffer<int16_t, histSize> yHistory = {}; Utility::CircularBuffer<int16_t, histSize> yHistory = {};
Utility::CircularBuffer<int16_t, histSize> zHistory = {}; Utility::CircularBuffer<int16_t, histSize> zHistory = {};
int32_t accumulatedSpeed = 0; int32_t accumulatedSpeed = 0;