2b1eae7f59
This new algorithm calculates the number of degrees that the wrist has rolled, and checks if that is above a threshold. First it checks if the wrist is still enough for the acceleration values to be considered mostly from gravity. It does this by calculating the mean over the past 2 SystemTask loops, and checking that the variance from that mean is below a threshold. Then it calculates the angle the wrist is being held at, and calculates the difference from the angle some time ago. If this difference is above the threshold, it wakes the watch.
131 lines
3.9 KiB
C++
131 lines
3.9 KiB
C++
#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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|