ref: fc5424cb72e477c5f1bbfaeddb5c50b851a965ae
src/components/motion/MotionController.cpp
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 |
#include "components/motion/MotionController.h" #include <task.h> #include "utility/Math.h" using namespace Pinetime::Controllers; namespace { constexpr inline int32_t Clamp(int32_t val, int32_t min, int32_t max) { return val < min ? min : (val > max ? max : val); } // only returns meaningful values if inputs are acceleration due to gravity int16_t DegreesRolled(int16_t y, int16_t z, int16_t prevY, int16_t prevZ) { int16_t prevYAngle = Pinetime::Utility::Asin(Clamp(prevY * 32, -32767, 32767)); int16_t yAngle = Pinetime::Utility::Asin(Clamp(y * 32, -32767, 32767)); if (z < 0 && prevZ < 0) { return yAngle - prevYAngle; } if (prevZ < 0) { if (y < 0) { return -prevYAngle - yAngle - 180; } return -prevYAngle - yAngle + 180; } if (z < 0) { if (y < 0) { return prevYAngle + yAngle + 180; } return prevYAngle + yAngle - 180; } return prevYAngle - yAngle; } } void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) { if (this->nbSteps != nbSteps && service != nullptr) { service->OnNewStepCountValue(nbSteps); } if (service != nullptr && (this->x != x || yHistory[0] != y || zHistory[0] != z)) { service->OnNewMotionValues(x, y, z); } lastTime = time; time = xTaskGetTickCount(); lastX = this->x; this->x = x; yHistory++; yHistory[0] = y; zHistory++; zHistory[0] = z; stats = GetAccelStats(); int32_t deltaSteps = nbSteps - this->nbSteps; if (deltaSteps > 0) { currentTripSteps += deltaSteps; } this->nbSteps = nbSteps; } MotionController::AccelStats MotionController::GetAccelStats() const { AccelStats stats; for (uint8_t i = 0; i < AccelStats::numHistory; i++) { stats.yMean += yHistory[histSize - i]; stats.zMean += zHistory[histSize - i]; stats.prevYMean += yHistory[1 + i]; stats.prevZMean += zHistory[1 + i]; } stats.yMean /= AccelStats::numHistory; stats.zMean /= AccelStats::numHistory; stats.prevYMean /= AccelStats::numHistory; stats.prevZMean /= AccelStats::numHistory; for (uint8_t i = 0; i < AccelStats::numHistory; i++) { 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.yVariance /= AccelStats::numHistory; stats.zVariance /= AccelStats::numHistory; return stats; } bool MotionController::ShouldRaiseWake() const { constexpr uint32_t varianceThresh = 56 * 56; constexpr int16_t xThresh = 384; constexpr int16_t yThresh = -64; constexpr int16_t rollDegreesThresh = -45; if (x < -xThresh || x > xThresh) { return false; } // if the variance is below the threshold, the accelerometer values can be considered to be from acceleration due to gravity if (stats.yVariance > varianceThresh || (stats.yMean < -724 && stats.zVariance > varianceThresh) || stats.yMean > yThresh) { return false; } return DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < rollDegreesThresh; } bool MotionController::ShouldShakeWake(uint16_t thresh) { /* Currently Polling at 10hz, If this ever goes faster scalar and EMA might need adjusting */ int32_t speed = std::abs(zHistory[0] - zHistory[histSize - 1] + (yHistory[0] - yHistory[histSize - 1]) / 2 + (x - lastX) / 4) * 100 / (time - lastTime); // (.2 * speed) + ((1 - .2) * accumulatedSpeed); accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5; return accumulatedSpeed > thresh; } bool MotionController::ShouldLowerSleep() const { if (stats.yMean < 724 || DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < 30) { return false; } for (uint8_t i = AccelStats::numHistory + 1; i < yHistory.Size(); i++) { if (yHistory[i] < 265) { return false; } } return true; } void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) { switch (types) { case Drivers::Bma421::DeviceTypes::BMA421: this->deviceType = DeviceTypes::BMA421; break; case Drivers::Bma421::DeviceTypes::BMA425: this->deviceType = DeviceTypes::BMA425; break; default: this->deviceType = DeviceTypes::Unknown; break; } } |