diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c95612..2365a3e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -139,8 +139,6 @@ target_sources(infinisim PUBLIC sim/components/firmwarevalidator/FirmwareValidator.cpp sim/components/heartrate/HeartRateController.h sim/components/heartrate/HeartRateController.cpp - sim/components/motion/MotionController.h - sim/components/motion/MotionController.cpp sim/drivers/Bma421.h sim/drivers/Bma421.cpp sim/drivers/Cst816s.h @@ -209,6 +207,8 @@ target_sources(infinisim PUBLIC ${InfiniTime_DIR}/src/components/ble/SimpleWeatherService.cpp ${InfiniTime_DIR}/src/components/fs/FS.h ${InfiniTime_DIR}/src/components/fs/FS.cpp + ${InfiniTime_DIR}/src/components/motion/MotionController.h + ${InfiniTime_DIR}/src/components/motion/MotionController.cpp ${InfiniTime_DIR}/src/components/motor/MotorController.h ${InfiniTime_DIR}/src/components/motor/MotorController.cpp ${InfiniTime_DIR}/src/drivers/Hrs3300.h diff --git a/sim/components/motion/MotionController.cpp b/sim/components/motion/MotionController.cpp deleted file mode 100644 index 9404339..0000000 --- a/sim/components/motion/MotionController.cpp +++ /dev/null @@ -1,147 +0,0 @@ -#include "components/motion/MotionController.h" - -//#include - -#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 { - 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) { - return false; -// /* 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; -} - -bool MotionController::ShouldLowerSleep() const { - return false; -// if (stats.yMean < 724 || DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < 30) { -// return false; -// } -// -// for (uint8_t i = AccelStats::numHistory + 1; i < yHistory.Size(); i++) { -// if (yHistory[i] < 265) { -// return false; -// } -// } -// -// return true; -} - -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; - } -} diff --git a/sim/components/motion/MotionController.h b/sim/components/motion/MotionController.h deleted file mode 100644 index 664ec8f..0000000 --- a/sim/components/motion/MotionController.h +++ /dev/null @@ -1,103 +0,0 @@ -#pragma once - -#include - -//#include - -#include "drivers/Bma421.h" -#include "components/ble/MotionService.h" -#include "utility/CircularBuffer.h" - -namespace Pinetime { - namespace Controllers { - class MotionController { - public: - enum class DeviceTypes { - Unknown, - BMA421, - BMA425, - }; - - void Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps); - - int16_t X() const { - return x; - } - - int16_t Y() const { - return yHistory[0]; - } - - int16_t Z() const { - return zHistory[0]; - } - - uint32_t NbSteps() const { - return nbSteps; - } - - void ResetTrip() { - currentTripSteps = 0; - } - - uint32_t GetTripSteps() const { - return currentTripSteps; - } - - bool ShouldShakeWake(uint16_t thresh); - bool ShouldRaiseWake() const; - bool ShouldLowerSleep() const; - - int32_t CurrentShakeSpeed() const { - return accumulatedSpeed; - } - - DeviceTypes DeviceType() const { - return deviceType; - } - - void Init(Pinetime::Drivers::Bma421::DeviceTypes types); - - void SetService(Pinetime::Controllers::MotionService* service) { - this->service = service; - } - - Pinetime::Controllers::MotionService* GetService() const { - return service; - } - - private: - uint32_t nbSteps = 0; - uint32_t currentTripSteps = 0; - -// 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; - static constexpr uint8_t histSize = 8; - Utility::CircularBuffer yHistory = {}; - Utility::CircularBuffer zHistory = {}; - int32_t accumulatedSpeed = 0; - - DeviceTypes deviceType = DeviceTypes::Unknown; - Pinetime::Controllers::MotionService* service = nullptr; - }; - } -}