Merge pull request #117 from FintasticMan/fix_raise_wake
raisewake: Fix for new raise to wake impl
This commit is contained in:
commit
0dd9c5e84d
|
@ -211,6 +211,8 @@ target_sources(infinisim PUBLIC
|
||||||
${InfiniTime_DIR}/src/systemtask/SystemTask.cpp
|
${InfiniTime_DIR}/src/systemtask/SystemTask.cpp
|
||||||
${InfiniTime_DIR}/src/systemtask/SystemMonitor.h
|
${InfiniTime_DIR}/src/systemtask/SystemMonitor.h
|
||||||
${InfiniTime_DIR}/src/systemtask/SystemMonitor.cpp
|
${InfiniTime_DIR}/src/systemtask/SystemMonitor.cpp
|
||||||
|
${InfiniTime_DIR}/src/utility/Math.h
|
||||||
|
${InfiniTime_DIR}/src/utility/Math.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
target_sources(infinisim PUBLIC
|
target_sources(infinisim PUBLIC
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
Subproject commit 6d0d8c7d6373062cd084c170d2e50e1535c9e95d
|
Subproject commit 2b1eae7f597ea6c210b4c15a73ab5ba116fc5d2a
|
|
@ -2,25 +2,59 @@
|
||||||
|
|
||||||
//#include <task.h>
|
//#include <task.h>
|
||||||
|
|
||||||
|
#include "utility/Math.h"
|
||||||
|
|
||||||
using namespace Pinetime::Controllers;
|
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) {
|
void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) {
|
||||||
// if (this->nbSteps != nbSteps && service != nullptr) {
|
// if (this->nbSteps != nbSteps && service != nullptr) {
|
||||||
// service->OnNewStepCountValue(nbSteps);
|
// service->OnNewStepCountValue(nbSteps);
|
||||||
// }
|
// }
|
||||||
//
|
//
|
||||||
// if (service != nullptr && (this->x != x || this->y != y || this->z != z)) {
|
// if (service != nullptr && (this->x != 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;
|
||||||
this->x = x;
|
this->x = x;
|
||||||
lastY = this->y;
|
yHistory++;
|
||||||
this->y = y;
|
yHistory[0] = y;
|
||||||
lastZ = this->z;
|
zHistory++;
|
||||||
this->z = z;
|
zHistory[0] = z;
|
||||||
|
|
||||||
|
stats = GetAccelStats();
|
||||||
|
|
||||||
int32_t deltaSteps = nbSteps - this->nbSteps;
|
int32_t deltaSteps = nbSteps - this->nbSteps;
|
||||||
if (deltaSteps > 0) {
|
if (deltaSteps > 0) {
|
||||||
|
@ -29,35 +63,56 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
|
||||||
this->nbSteps = nbSteps;
|
this->nbSteps = nbSteps;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MotionController::ShouldRaiseWake(bool isSleeping) {
|
MotionController::AccelStats MotionController::GetAccelStats() const {
|
||||||
if ((x + 335) <= 670 && z < 0) {
|
AccelStats stats;
|
||||||
if (!isSleeping) {
|
|
||||||
if (y <= 0) {
|
for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
|
||||||
return false;
|
stats.yMean += yHistory[histSize - i];
|
||||||
|
stats.zMean += zHistory[histSize - i];
|
||||||
|
stats.prevYMean += yHistory[1 + i];
|
||||||
|
stats.prevZMean += zHistory[1 + i];
|
||||||
}
|
}
|
||||||
lastYForRaiseWake = 0;
|
stats.yMean /= AccelStats::numHistory;
|
||||||
return false;
|
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (y >= 0) {
|
bool MotionController::ShouldRaiseWake() const {
|
||||||
lastYForRaiseWake = 0;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (y + 230 < lastYForRaiseWake) {
|
|
||||||
lastYForRaiseWake = y;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return false;
|
return false;
|
||||||
|
// 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) {
|
bool MotionController::ShouldShakeWake(uint16_t thresh) {
|
||||||
return false;
|
return false;
|
||||||
// /* 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 = std::abs(z + (y / 2) + (x / 4) - lastY / 2 - lastZ) / (time - lastTime) * 100;
|
// 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);
|
// // (.2 * speed) + ((1 - .2) * accumulatedSpeed);
|
||||||
// // implemented without floats as .25Alpha
|
// accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5;
|
||||||
// accumulatedSpeed = (speed / 5) + ((accumulatedSpeed / 5) * 4);
|
|
||||||
//
|
//
|
||||||
// return accumulatedSpeed > thresh;
|
// return accumulatedSpeed > thresh;
|
||||||
}
|
}
|
||||||
|
|
|
@ -6,6 +6,7 @@
|
||||||
|
|
||||||
#include "drivers/Bma421.h"
|
#include "drivers/Bma421.h"
|
||||||
//#include "components/ble/MotionService.h"
|
//#include "components/ble/MotionService.h"
|
||||||
|
#include "utility/CircularBuffer.h"
|
||||||
|
|
||||||
namespace Pinetime {
|
namespace Pinetime {
|
||||||
namespace Controllers {
|
namespace Controllers {
|
||||||
|
@ -24,11 +25,11 @@ namespace Pinetime {
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t Y() const {
|
int16_t Y() const {
|
||||||
return y;
|
return yHistory[0];
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t Z() const {
|
int16_t Z() const {
|
||||||
return z;
|
return zHistory[0];
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t NbSteps() const {
|
uint32_t NbSteps() const {
|
||||||
|
@ -44,20 +45,12 @@ namespace Pinetime {
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ShouldShakeWake(uint16_t thresh);
|
bool ShouldShakeWake(uint16_t thresh);
|
||||||
bool ShouldRaiseWake(bool isSleeping);
|
bool ShouldRaiseWake() const;
|
||||||
|
|
||||||
int32_t CurrentShakeSpeed() const {
|
int32_t CurrentShakeSpeed() const {
|
||||||
return accumulatedSpeed;
|
return accumulatedSpeed;
|
||||||
}
|
}
|
||||||
|
|
||||||
void IsSensorOk(bool isOk) {
|
|
||||||
isSensorOk = isOk;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool IsSensorOk() const {
|
|
||||||
return isSensorOk;
|
|
||||||
}
|
|
||||||
|
|
||||||
DeviceTypes DeviceType() const {
|
DeviceTypes DeviceType() const {
|
||||||
return deviceType;
|
return deviceType;
|
||||||
}
|
}
|
||||||
|
@ -75,15 +68,29 @@ namespace Pinetime {
|
||||||
// TickType_t lastTime = 0;
|
// TickType_t lastTime = 0;
|
||||||
// TickType_t time = 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 x = 0;
|
||||||
int16_t lastYForRaiseWake = 0;
|
static constexpr uint8_t histSize = 8;
|
||||||
int16_t lastY = 0;
|
Utility::CircularBuffer<int16_t, histSize> yHistory = {};
|
||||||
int16_t y = 0;
|
Utility::CircularBuffer<int16_t, histSize> zHistory = {};
|
||||||
int16_t lastZ = 0;
|
|
||||||
int16_t z = 0;
|
|
||||||
int32_t accumulatedSpeed = 0;
|
int32_t accumulatedSpeed = 0;
|
||||||
|
|
||||||
bool isSensorOk = false;
|
|
||||||
DeviceTypes deviceType = DeviceTypes::Unknown;
|
DeviceTypes deviceType = DeviceTypes::Unknown;
|
||||||
// Pinetime::Controllers::MotionService* service = nullptr;
|
// Pinetime::Controllers::MotionService* service = nullptr;
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue
Block a user