motioncontroller: Fix changes in InfiniTime#1659
This commit is contained in:
parent
32fada34f4
commit
2f37837c77
3 changed files with 61 additions and 53 deletions
|
@ -1 +1 @@
|
|||
Subproject commit 9bbd2ddbc4e44742913e35e25b21b80d80efe36b
|
||||
Subproject commit e55a76f740561378479eda5ff8c32e09db9f1693
|
|
@ -1,5 +1,7 @@
|
|||
#include "components/motion/MotionController.h"
|
||||
//#include "os/os_cputime.h"
|
||||
|
||||
//#include <task.h>
|
||||
|
||||
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;
|
||||
//}
|
||||
|
|
|
@ -1,8 +1,11 @@
|
|||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <drivers/Bma421.h>
|
||||
//#include <components/ble/MotionService.h>
|
||||
|
||||
//#include <FreeRTOS.h>
|
||||
|
||||
#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;
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue