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:
Finlay Davidson 2023-06-25 15:59:34 +02:00 committed by JF
parent 3085bb3990
commit cfe21103ea
5 changed files with 137 additions and 0 deletions

View file

@ -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(

View file

@ -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) {

View file

@ -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
View 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
View 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);
}
}