motioncontroller: Add functions for analysis
These are functions for converting acceleration due to gravity to angles in degrees, and some statistical analysis including the mean and variance.
This commit is contained in:
parent
3085bb3990
commit
cfe21103ea
|
@ -493,6 +493,8 @@ list(APPEND SOURCE_FILES
|
|||
|
||||
buttonhandler/ButtonHandler.cpp
|
||||
touchhandler/TouchHandler.cpp
|
||||
|
||||
utility/Math.cpp
|
||||
)
|
||||
|
||||
list(APPEND RECOVERY_SOURCE_FILES
|
||||
|
@ -558,6 +560,8 @@ list(APPEND RECOVERY_SOURCE_FILES
|
|||
components/fs/FS.cpp
|
||||
buttonhandler/ButtonHandler.cpp
|
||||
touchhandler/TouchHandler.cpp
|
||||
|
||||
utility/Math.cpp
|
||||
)
|
||||
|
||||
list(APPEND RECOVERYLOADER_SOURCE_FILES
|
||||
|
@ -677,6 +681,7 @@ set(INCLUDE_FILES
|
|||
components/motor/MotorController.h
|
||||
buttonhandler/ButtonHandler.h
|
||||
touchhandler/TouchHandler.h
|
||||
utility/Math.h
|
||||
)
|
||||
|
||||
include_directories(
|
||||
|
|
|
@ -2,8 +2,39 @@
|
|||
|
||||
#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);
|
||||
|
@ -23,6 +54,8 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
|
|||
zHistory++;
|
||||
zHistory[0] = z;
|
||||
|
||||
stats = GetAccelStats();
|
||||
|
||||
int32_t deltaSteps = nbSteps - this->nbSteps;
|
||||
if (deltaSteps > 0) {
|
||||
currentTripSteps += deltaSteps;
|
||||
|
@ -30,6 +63,30 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
|
|||
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(bool isSleeping) {
|
||||
if ((x + 335) <= 670 && zHistory[0] < 0) {
|
||||
if (!isSleeping) {
|
||||
|
|
|
@ -68,6 +68,22 @@ namespace Pinetime {
|
|||
TickType_t lastTime = 0;
|
||||
TickType_t time = 0;
|
||||
|
||||
struct AccelStats {
|
||||
static constexpr uint8_t numHistory = 2;
|
||||
|
||||
int16_t yMean = 0;
|
||||
int16_t zMean = 0;
|
||||
int16_t prevYMean = 0;
|
||||
int16_t prevZMean = 0;
|
||||
|
||||
uint32_t yVariance = 0;
|
||||
uint32_t zVariance = 0;
|
||||
};
|
||||
|
||||
AccelStats GetAccelStats() const;
|
||||
|
||||
AccelStats stats = {};
|
||||
|
||||
int16_t lastX = 0;
|
||||
int16_t x = 0;
|
||||
int16_t lastYForRaiseWake = 0;
|
||||
|
|
49
src/utility/Math.cpp
Normal file
49
src/utility/Math.cpp
Normal file
|
@ -0,0 +1,49 @@
|
|||
#include "utility/Math.h"
|
||||
|
||||
#include <lvgl/src/lv_misc/lv_math.h>
|
||||
|
||||
using namespace Pinetime::Utility;
|
||||
|
||||
#ifndef PINETIME_IS_RECOVERY
|
||||
|
||||
int16_t Pinetime::Utility::Asin(int16_t arg) {
|
||||
int16_t a = arg < 0 ? -arg : arg;
|
||||
|
||||
int16_t angle = 45;
|
||||
int16_t low = 0;
|
||||
int16_t high = 90;
|
||||
while (low <= high) {
|
||||
int16_t sinAngle = _lv_trigo_sin(angle);
|
||||
int16_t sinAngleSub = _lv_trigo_sin(angle - 1);
|
||||
int16_t sinAngleAdd = _lv_trigo_sin(angle + 1);
|
||||
|
||||
if (a >= sinAngleSub && a <= sinAngleAdd) {
|
||||
if (a <= (sinAngleSub + sinAngle) / 2) {
|
||||
angle--;
|
||||
} else if (a > (sinAngle + sinAngleAdd) / 2) {
|
||||
angle++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
if (a < sinAngle) {
|
||||
high = angle - 1;
|
||||
}
|
||||
|
||||
else {
|
||||
low = angle + 1;
|
||||
}
|
||||
|
||||
angle = (low + high) / 2;
|
||||
}
|
||||
|
||||
return arg < 0 ? -angle : angle;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
int16_t Pinetime::Utility::Asin(int16_t /*arg*/) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
10
src/utility/Math.h
Normal file
10
src/utility/Math.h
Normal file
|
@ -0,0 +1,10 @@
|
|||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace Pinetime {
|
||||
namespace Utility {
|
||||
// returns the arcsin of `arg`. asin(-32767) = -90, asin(32767) = 90
|
||||
int16_t Asin(int16_t arg);
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue
Block a user