2 files changed, 25 insertions, 11 deletions
diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index 6a3a84d..0404e3f 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -1,4 +1,3 @@
-#include <cmath>
 #include "components/motion/MotionController.h"
 #include "os/os_cputime.h"
 using namespace Pinetime::Controllers;
@@ -9,14 +8,6 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z) {
   this->z = z;
 }
 
-uint16_t MotionController::G() {
-  float X = x; X *= X;
-  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) {
   if ((x + 335) <= 670 && z < 0) {
     if (not isSleeping) {
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index 21bfbdb..06429c8 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cmath>
 #include <cstdint>
 #include <drivers/Bma421.h>
 
@@ -25,8 +26,6 @@ namespace Pinetime {
         return z;
       }
 
-      int16_t G();
-
       bool Should_RaiseWake(bool isSleeping);
       void IsSensorOk(bool isOk);
       bool IsSensorOk() const {
@@ -39,6 +38,30 @@ namespace Pinetime {
 
       void Init(Pinetime::Drivers::Bma421::DeviceTypes types);
 
+      inline double GXYZ(double &X, double &Y, double &Z) const {
+        X = x; Y = y; Z = z;
+        return std::sqrt(X*X + Y*Y + Z*Z);
+      }
+
+      inline double G() const {
+        double X = x; X *= X;
+        double Y = y; X += Y * Y;
+        Y = z; X += Y * Y;
+        return std::sqrt(X) * 0.;
+      }
+
+      inline auto G_v0() const {
+        float X = x; X *= X;
+        float Y = y; X += Y * Y;
+        Y = z; X += Y * Y;
+        return std::sqrt(X);
+      }
+
+      inline uint16_t G_uint16() const {
+        auto X = G_v0();
+        if (X > 0xffff) { return 0xffff; } else { return X; }
+      }
+
     private:
       int16_t x;
       int16_t y;