More testing of G sensor

1 files changed, 5 insertions, 4 deletions

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index a7c5ecc..6a3a84d 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -9,11 +9,12 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z) {
   this->z = z;
 }
 
-int16_t MotionController::G() {
+uint16_t MotionController::G() {
   float X = x; X *= X;
-  float Y = y; Y *= Y;
-  float Z = z; Z *= Z;
-  return (int16_t) std::sqrt(X + Y + Z);
+  float Y = y; X += Y * Y;
+  Y = z; X += Y * Y;
+  X = std::sqrt(X);
+  if (X > 0xffff) { return 0xffff; } else { return X; }
 }
 
 bool MotionController::Should_RaiseWake(bool isSleeping) {