WIP: tiltdetection

src/motionDetection/MotionDetection.cpp

@@ -1,5 +1,6 @@
 #include "MotionDetection.h"
-
+#include <cmath.h>
+#include <limits.h>
 MotionDetection::MotionDetection(){//:handler(FSPI){
     handler = new SPIClass(FSPI);
 };
@@ -58,6 +59,41 @@ int8_t MotionDetection::getWhoAmI(){
     return readRegister(WHO_AM_I);    
 };
 
+bool MotionDetection::isShaken(uint32_t threshold = defaultShakeThreshold,uint8_t axis = xAxis|yAxis|zAxis){
+
+};
+
+void MotionDetection::calibrateZAxis(uint gforceValue){
+    this->gForceCalib = gforceValue;
+};
+
+Orientation MotionDetection::getTilt(){
+    uint tolerance = 200;
+    IMUResult reading = this->getAcceleration();
+    bool flipped = reading.z < 0;
+    float accelrationStrenght = sqrt(reading.x*reading.x+reading.y*reading.y+reading.z*reading.z);
+    //check if reading is valid
+    if (abs(accelrationStrenght-this->gForceCalib) > tolerance ){
+        //total accelration is not gravitational force, error
+        return Orientation{xRotation = UINT_MAX,yRotation = UINT_MAX};
+    }
+
+    //calculates the angle between the two axis, therefore value between 0-90
+    uint yAngle;
+    uint xAngle;
+    if(result.z != 0){
+        yAngle = atan((result.x)/result.z)+0.5;
+        xAngle = atan(float(result.y)/result.z)+0.5;
+    } else {
+        yAngle = 90;
+        xAngle = 90;
+    }
+
+
+    //shift quadrants depending on signum
+
+};
+
 uint8_t MotionDetection::cmdRead(uint8_t reg){
     return (CMD_READ | (reg & ADDR_MASK));
 };

src/motionDetection/MotionDetection.h

@@ -17,6 +17,16 @@ struct IMUResult{
     int16_t y;
     int16_t z;
 };
+enum Axis{
+    xAxis = 0x01,
+    yAxis = 0x02,
+    zAxis = 0x04
+};
+
+struct Orientation{
+    uint xRotation;
+    uint yRotation;
+};
 
 
 class MotionDetection{
@@ -47,7 +57,8 @@ protected:
     static const uint8_t PWR_MGMT0 =         0x1F;
     static const uint8_t WHO_AM_I =          0x75;
 
-    static const uint frequency = 10000000;
+    static const uint frequency = 24000000;
+    static const uint16_t defaultShakeThreshold = 500;
 
     uint16_t cmdRead(uint8_t regHigh,uint8_t regLow);
     uint16_t cmdWrite(uint8_t regHigh,uint8_t regLow);
@@ -58,6 +69,8 @@ protected:
     int16_t readDoubleRegister(uint8_t lowerReg);
 
     SPIClass * handler = NULL;
+
+    gForceCalib = 2000;
      
     
 public:
@@ -83,21 +96,21 @@ public:
      * 
      * @return IMUResult that contains the new read values
      */
-    IMUResult getAcceleration();
+    IMUResult getAcceleration(void);
 
     /**
      * @brief Triggers a new reading of the gyroscope and reads the values from the imu
      * 
      * @return IMUResult 
      */
-    IMUResult getRotation();
+    IMUResult getRotation(void);
 
     /**
      * @brief Reads the current On Chip temperature of the IMU 
      * 
      * @return normalized temperature in degree Centigrade
      */
-    float getTemperature();
+    float getTemperature(void);
 
     /**
      * @brief Returns the value of reading the whoAmI register
@@ -105,6 +118,46 @@ public:
      * 
      * @return the value of the whoami register of the ICM-42670 
      */
-    int8_t getWhoAmI();
+    int8_t getWhoAmI(void);
+
+    /**
+     * @brief Detects if at the time of calling is shaken. Therefore the sum over all accelerationvalues is calculated 
+     * and checked against threshold. If sum > threshold a shake is detected, else not
+     * 
+     * @param threshold (optional) the level of acceleration that must be reached to detect a shake
+     * @param axis (optional) select which axis should be used for detection. Possible values ar xAxis,yAxis,zAxis
+     * It's possible to combine multiple axis with the bitwise or Operator | 
+     * For Example: to detect x and y axis: axis = xAxis|yAxis
+     * 
+     * @return true if a shake is detected, false else
+    */
+    bool isShaken(uint32_t threshold = defaultShakeThreshold,uint8_t axis = xAxis|yAxis|zAxis);
+
+    /**
+     * @brief calculates how the robot is tilted. It is set, that when the robot is placed normally on a flat table, the result will be (0,0)
+     * Tilting the robot, so that the front leg is deeper than the other to results in an increasing value, tilting the front leg up will let the value jump to 359 and then decreasing
+     * Tilting the robot to the right (..) will increase the value, tilting it left will jump to 359 and then decreased.
+     * 
+     * Precision is rounded to 1deg steps
+     * 
+     * @attention The results are only valid, if the robot is not moved in any way during the measurment, as the calculation is made by using the accelration values.
+     * If it's detected, that the robot is accelerated while measuring, the method will return max(uint).
+     * Please note that the imu is pretty sensitiv, even walking next to the table may influcene the result. 
+     * 
+    */
+    Orientation getTilt();
+
+
+    /**
+     * can be used to set a custom value for the gforceReading of the zaxis, which will improve the getTiltFunction.
+     * 
+     * @attention this method is not persisten, so the value is not stored when the programm is restarted / the robot is powerd off
+     * 
+     * @param gforceValue the value the IMU returns for the gravitationforce -> to get this value, place the robot on a leveled surface
+     * and read the value getAcceleration().z
+    */
+    void calibrateZAxis(uint gforceValue);
+
+
 };
 #endif //MotionDetection