dezibot/src/motionDetection/MotionDetection.h

/**
 * @file MotionDetection.h
 * @author Hans Haupt
 * @brief This component controls the IMU (Accelerometer & Gyroscope) ICM-42670-P
 * @version 0.1
 * @date 2023-12-15
 * 
 * @copyright Copyright (c) 2023
 * 
 */
#ifndef MotionDetection_h
#define MotionDetection_h
#include "../power/PowerManager.h"
#include "IMU_CMDs.h"
#include <Arduino.h>
#include <SPI.h>

#define IMU_MAX_EXECUTION_DELAY_MS 1

struct IMUResult{
    int16_t x;
    int16_t y;
    int16_t z;
};
enum Axis{
    xAxis = 0x01,
    yAxis = 0x02,
    zAxis = 0x04
};

struct Orientation{
    int xRotation;
    int yRotation;
};

enum Direction{
    Front,
    Left,
    Right,
    Back,
    Neutral,
    Flipped,
    Error
};

struct FIFO_Package{
    int8_t header;
    IMUResult gyro;
    IMUResult accel;
    int16_t temperature;
    int16_t timestamp;
};


class MotionDetection{
protected:
    enum registerBank{MREG1,MREG2,MREG3};
    static const uint frequency = 24000000;
    static const uint16_t defaultShakeThreshold = 500;
    const uint bufferLength = 64*16;
    int8_t* buf = new int8_t[bufferLength];
    uint8_t readFromRegisterBank(registerBank bank,uint8_t reg);
    void writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value);
    void resetRegisterBankAccess();

    uint16_t cmdRead(uint8_t regHigh,uint8_t regLow);
    uint16_t cmdWrite(uint8_t regHigh,uint8_t regLow);
    uint8_t cmdRead(uint8_t reg);
    uint8_t cmdWrite(uint8_t reg);

    uint8_t readRegister(uint8_t reg);
    int16_t readDoubleRegister(uint8_t lowerReg);
    void writeRegister(uint8_t reg, uint8_t value);
    void initFIFO();

    SPIClass * handler = NULL;

    uint gForceCalib = 4050;
     
    
public:
    MotionDetection();

    /**
     * @brief initialized the  IMU Component. 
     * Wakes the IMU from Standby 
     * Set configuration 
     * 
     */
    void begin(void);

    /**
     * @brief stops the component
     * Sets the IMU to Low-Power-Mode
     * 
     */
    void end(void);

    /**
     * @brief Triggers a new Reading of the accelerationvalues and reads them from the IMU
     * 
     * @return IMUResult that contains the new read values
     */
    IMUResult getAcceleration(void);

    /**
     * @brief Triggers a new reading of the gyroscope and reads the values from the imu
     * 
     * @return IMUResult 
     */
    IMUResult getRotation(void);

    /**
     * @brief Reads the current On Chip temperature of the IMU 
     * 
     * @return normalized temperature in degree Centigrade
     */
    float getTemperature(void);

    /**
     * @brief Returns the value of reading the whoAmI register
     * When IMU working correctly, value should be 0x67
     * 
     * @return the value of the whoami register of the ICM-42670 
     */
    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 degrees, tilting the front leg up will increase negativ degrees
     * Tilting the robot to the right will increase the degrees until 180° (upside down), tilting it left will result in increasing negativ degrees (-1,-2,...,-180).
     * On the top there is a jump of the values from 180->-180 and vice versa.
     * 
     * Precision is rounded to 1 deg 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(int).
     * Please note that the imu is pretty sensitiv, even walking next to the table may influcene the result. 
     * 
    */
    Orientation getTilt();


    /**
     * @brief Checks in which direction (Front, Left, Right, Back) the robot is tilted. 
     * 
     * @attention Does only work if the robot is not moving
     *  
     * @param tolerance (optional, default = 10) how many degrees can the robot be tilted, and still will be considerd as neutral. 
     * 
     * 
     * @return Direction the direction in that the robot is tilted most. Front is onsiderd as the direction of driving.
     * If robot is not tilted more than the tolerance in any direction, return is Neutral.
     * If Robot is upside down, return is Flipped.
     * If Robot is moved, return is Error
     */
    Direction getTiltDirection(uint tolerance = 10);

    /**
     * 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);

    /**
     * @brief will read all availible packages from fifo, after 40ms Fifo is full
     * 
     * @param buffer pointer to FIFO_Package Struct that at least must have size 64 (this is the max package count with APEX Enabled)
     * 
     * @return the amount of acutally fetched packages 
    */
    uint getDataFromFIFO(FIFO_Package* buffer);

    /**
     * @brief Current consumtion of the sensor
     * 
     * @return 
     */
     float modelCurrentConsumption();


     /**
      * @brief Estimates charge consumption of the sensor for the given duration
      *
      * @param durationMs
      * @return float
      */
     float modelChargeConsumption(uint16_t durationMs);
};
#endif //MotionDetection