From 2f37837c77053c8eacec952b0bedd8f8e1514b79 Mon Sep 17 00:00:00 2001 From: Finlay Davidson Date: Sun, 5 Mar 2023 20:51:34 +0100 Subject: [PATCH] motioncontroller: Fix changes in InfiniTime#1659 --- InfiniTime | 2 +- sim/components/motion/MotionController.cpp | 55 +++++++++------------ sim/components/motion/MotionController.h | 57 ++++++++++++++-------- 3 files changed, 61 insertions(+), 53 deletions(-) diff --git a/InfiniTime b/InfiniTime index 9bbd2dd..e55a76f 160000 --- a/InfiniTime +++ b/InfiniTime @@ -1 +1 @@ -Subproject commit 9bbd2ddbc4e44742913e35e25b21b80d80efe36b +Subproject commit e55a76f740561378479eda5ff8c32e09db9f1693 diff --git a/sim/components/motion/MotionController.cpp b/sim/components/motion/MotionController.cpp index 331b5df..da90eae 100644 --- a/sim/components/motion/MotionController.cpp +++ b/sim/components/motion/MotionController.cpp @@ -1,5 +1,7 @@ #include "components/motion/MotionController.h" -//#include "os/os_cputime.h" + +//#include + using namespace Pinetime::Controllers; void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) { @@ -10,66 +12,56 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) // if (service != nullptr && (this->x != x || this->y != y || this->z != z)) { // service->OnNewMotionValues(x, y, z); // } +// +// lastTime = time; +// time = xTaskGetTickCount(); this->x = x; + lastY = this->y; this->y = y; + lastZ = this->z; this->z = z; + int32_t deltaSteps = nbSteps - this->nbSteps; - this->nbSteps = nbSteps; if (deltaSteps > 0) { currentTripSteps += deltaSteps; } + this->nbSteps = nbSteps; } -bool MotionController::Should_RaiseWake(bool isSleeping) { +bool MotionController::ShouldRaiseWake(bool isSleeping) { if ((x + 335) <= 670 && z < 0) { - if (not isSleeping) { + if (!isSleeping) { if (y <= 0) { return false; - } else { - lastYForWakeUp = 0; - return false; } + lastYForRaiseWake = 0; + return false; } if (y >= 0) { - lastYForWakeUp = 0; + lastYForRaiseWake = 0; return false; } - if (y + 230 < lastYForWakeUp) { - lastYForWakeUp = y; + if (y + 230 < lastYForRaiseWake) { + lastYForRaiseWake = y; return true; } } return false; } -bool MotionController::Should_ShakeWake(uint16_t thresh) { +bool MotionController::ShouldShakeWake(uint16_t thresh) { return false; -// bool wake = false; -// auto diff = xTaskGetTickCount() - lastShakeTime; -// lastShakeTime = xTaskGetTickCount(); // /* 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) - lastYForShake - lastZForShake) / diff * 100; -// //(.2 * speed) + ((1 - .2) * accumulatedspeed); +// int32_t speed = std::abs(z + (y / 2) + (x / 4) - lastY / 2 - lastZ) / (time - lastTime) * 100; +// //(.2 * speed) + ((1 - .2) * accumulatedSpeed); // // implemented without floats as .25Alpha -// accumulatedspeed = (speed / 5) + ((accumulatedspeed / 5) * 4); +// accumulatedSpeed = (speed / 5) + ((accumulatedSpeed / 5) * 4); // -// if (accumulatedspeed > thresh) { -// wake = true; -// } -// lastXForShake = x / 4; -// lastYForShake = y / 2; -// lastZForShake = z; -// return wake; -} -int32_t MotionController::currentShakeSpeed() { - return accumulatedspeed; +// return accumulatedSpeed > thresh; } -void MotionController::IsSensorOk(bool isOk) { - isSensorOk = isOk; -} void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) { switch (types) { case Drivers::Bma421::DeviceTypes::BMA421: @@ -83,6 +75,3 @@ void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) { break; } } -//void MotionController::SetService(Pinetime::Controllers::MotionService* service) { -// this->service = service; -//} diff --git a/sim/components/motion/MotionController.h b/sim/components/motion/MotionController.h index a16560b..fd8610a 100644 --- a/sim/components/motion/MotionController.h +++ b/sim/components/motion/MotionController.h @@ -1,8 +1,11 @@ #pragma once #include -#include -//#include + +//#include + +#include "drivers/Bma421.h" +//#include "components/ble/MotionService.h" namespace Pinetime { namespace Controllers { @@ -19,12 +22,15 @@ namespace Pinetime { int16_t X() const { return x; } + int16_t Y() const { return y; } + int16_t Z() const { return z; } + uint32_t NbSteps() const { return nbSteps; } @@ -32,14 +38,22 @@ namespace Pinetime { void ResetTrip() { currentTripSteps = 0; } + uint32_t GetTripSteps() const { return currentTripSteps; } - bool Should_ShakeWake(uint16_t thresh); - bool Should_RaiseWake(bool isSleeping); - int32_t currentShakeSpeed(); - void IsSensorOk(bool isOk); + bool ShouldShakeWake(uint16_t thresh); + bool ShouldRaiseWake(bool isSleeping); + + int32_t CurrentShakeSpeed() const { + return accumulatedSpeed; + } + + void IsSensorOk(bool isOk) { + isSensorOk = isOk; + } + bool IsSensorOk() const { return isSensorOk; } @@ -49,24 +63,29 @@ namespace Pinetime { } void Init(Pinetime::Drivers::Bma421::DeviceTypes types); -// void SetService(Pinetime::Controllers::MotionService* service); + +// void SetService(Pinetime::Controllers::MotionService* service) { +// this->service = service; +// } private: - uint32_t nbSteps; + uint32_t nbSteps = 0; uint32_t currentTripSteps = 0; - int16_t x; - int16_t y; - int16_t z; - int16_t lastYForWakeUp = 0; + +// TickType_t lastTime = 0; +// TickType_t time = 0; + + int16_t x = 0; + int16_t lastYForRaiseWake = 0; + int16_t lastY = 0; + int16_t y = 0; + int16_t lastZ = 0; + int16_t z = 0; + int32_t accumulatedSpeed = 0; + bool isSensorOk = false; DeviceTypes deviceType = DeviceTypes::Unknown; // Pinetime::Controllers::MotionService* service = nullptr; - - int16_t lastXForShake = 0; - int16_t lastYForShake = 0; - int16_t lastZForShake = 0; - int32_t accumulatedspeed = 0; - uint32_t lastShakeTime = 0; }; } -} \ No newline at end of file +}