Fixup MPU calibration and Z angle calculation

Author: Tasssadar

library.json

@@ -35,7 +35,7 @@
       "version": "https://github.com/RoboticsBrno/Esp32-lx16a/archive/refs/tags/v1.2.1.zip"
     }
   ],
-  "version": "1.4.1",
+  "version": "1.5.0",
   "frameworks": ["espidf", "arduino"],
   "platforms": "espressif32",
   "build": {

src/RBCXMpu.cpp

@@ -25,7 +25,9 @@ static constexpr int CALIB_OFFSET_NB_MES = 1;
 
 namespace rb {
 
-Mpu::Mpu() { m_preInterval = xTaskGetTickCount() * portTICK_RATE_MS; }
+Mpu::Mpu() {
+    m_lastTicks = xTaskGetTickCount();
+}
 
 Mpu::~Mpu() {}
 
@@ -57,15 +59,14 @@ void Mpu::setState(const CoprocStat_MpuStat& msg) {
     m_compressCoef = msg.compressCoef;
     calculateAcc(msg.accel);
     calculateGyro(msg.gyro);
-
-    if (m_firstRead) {
-        m_firstRead = false;
-        calcOffsets();
-    }
     calculateAngle();
 }
 
-void Mpu::calcOffsets() {
+void Mpu::clearCalibrationData() {
+    memset(&m_mpuMotionOffset, 0, sizeof(m_mpuMotionOffset));
+}
+
+void Mpu::setCalibrationData() {
     m_mpuMotionOffset.accel.x = m_mpuMotion.accel.x / CALIB_OFFSET_NB_MES;
     m_mpuMotionOffset.accel.y = m_mpuMotion.accel.y / CALIB_OFFSET_NB_MES;
     m_mpuMotionOffset.accel.z
@@ -101,13 +102,10 @@ void Mpu::calculateGyro(const CoprocStat_MpuVector& gyro) {
 }
 
 void Mpu::calculateAngle() {
-    m_mpuMotion.angle.x = 0;
-    m_mpuMotion.angle.y = 0;
-    m_mpuMotion.angle.z = 0;
-
-    float sgZ = (m_mpuMotion.accel.z >= 0)
+    const float sgZ = (m_mpuMotion.accel.z >= 0)
         - (m_mpuMotion.accel.z
             < 0); // allow one angle to go from -180° to +180°
+
     m_mpuMotion.angleAcc.x
         = atan2(m_mpuMotion.accel.y,
               sgZ
@@ -120,31 +118,34 @@ void Mpu::calculateAngle() {
               sqrt(m_mpuMotion.accel.z * m_mpuMotion.accel.z
                   + m_mpuMotion.accel.y * m_mpuMotion.accel.y))
         * RAD_2_DEG; // [- 90°,+ 90°]
-
-    unsigned long Tnew = xTaskGetTickCount() * portTICK_RATE_MS;
-    m_interval = (Tnew - m_preInterval) * 1e-3;
-    m_preInterval = Tnew;
+    
+    const TickType_t curTicks = xTaskGetTickCount();
+    const float elapsedSeconds = float((curTicks  - m_lastTicks) * portTICK_RATE_MS) / 1000.f;
+    m_lastTicks = curTicks;
 
     // Correctly wrap X and Y angles (special thanks to Edgar Bonet!)
     // https://github.com/gabriel-milan/TinyMPU6050/issues/6
     m_mpuMotion.angle.x
         = wrap((m_mpuMotion.angleAcc.x
-                   + wrap(m_mpuMotion.angle.x + m_mpuMotion.gyro.x * m_interval
+                   + wrap( m_mpuMotion.gyro.x * elapsedSeconds
                            - m_mpuMotion.angleAcc.x,
                        180))
                 + m_mpuMotion.angleAcc.x,
             180);
     m_mpuMotion.angle.y = wrap(
         (m_mpuMotion.angleAcc.y
-            + wrap(m_mpuMotion.angle.y + sgZ * m_mpuMotion.gyro.y * m_interval
+            + wrap(sgZ * m_mpuMotion.gyro.y * elapsedSeconds
                     - m_mpuMotion.angleAcc.y,
                 90))
             + m_mpuMotion.angleAcc.y,
         90);
 
-    // angleZ += gyroZ*m_interval; // not wrapped (to do???)
     m_mpuMotion.angle.z
-        += m_mpuMotion.gyro.z * m_interval; // not wrapped (to do???)
+        += m_mpuMotion.gyro.z * elapsedSeconds; // not wrapped (to do???)
+}
+
+void Mpu::resetAngleZ() {
+    m_mpuMotion.angle.z = 0;
 }
 
 /* Wrap an angle in the range [-limit,+limit] (special thanks to Edgar Bonet!) */
@@ -171,8 +172,6 @@ float Mpu::getAngleX() { return m_mpuMotion.angle.x; }
 float Mpu::getAngleY() { return m_mpuMotion.angle.y; }
 float Mpu::getAngleZ() { return m_mpuMotion.angle.z; }
 
-long Mpu::getInterval() { return m_interval; }
-
 void Mpu::setCompressCoef(uint8_t coef) {
     sendMpuReq(
         CoprocReq_MpuReq {

src/RBCXMpu.h

@@ -1,4 +1,7 @@
 #pragma once
+
+#include <freertos/FreeRTOS.h>
+
 #include "rbcx.pb.h"
 namespace rb {
 
@@ -57,83 +60,95 @@ class Mpu {
      */
     void sendStop();
 
+    /**
+     * @brief Records current Gyro and Accelerometer data as "start" position
+     * and subtracts it from future readings
+     */
+    void setCalibrationData();
+
+    /**
+     * @brief Clears the data recorded by setCalibrationData
+     */
+    void clearCalibrationData();
+
     /**
      * @brief Get the Mpu acceleration data.
-     * @return The Mpu acceleration data.
+     * @return The Mpu acceleration data in Gs.
      */
     MpuVector getAcc();
 
     /**
      * @brief Get the Mpu acceleration data on the X axis.
-     * @return The Mpu acceleration data on the X axis.
+     * @return The Mpu acceleration data on the X axis in Gs.
      */
     float getAccX();
 
     /**
      * @brief Get the Mpu acceleration data on the Y axis.
-     * @return The Mpu acceleration data on the Y axis.
+     * @return The Mpu acceleration data on the Y axis in Gs.
      */
     float getAccY();
 
     /**
      * @brief Get the Mpu acceleration data on the Z axis.
-     * @return The Mpu acceleration data on the Z axis.
+     * @return The Mpu acceleration data on the Z axis in Gs.
      */
     float getAccZ();
 
     /**
      * @brief Get the Mpu gyroscope data.
-     * @return The Mpu gyroscope data.
+     * @return The Mpu gyroscope data in degrees per second.
      */
     MpuVector getGyro();
 
     /**
      * @brief Get the Mpu gyroscope data on the X axis.
-     * @return The Mpu gyroscope data on the X axis.
+     * @return The Mpu gyroscope data on the X axis in degrees per second.
      */
     float getGyroX();
 
     /**
      * @brief Get the Mpu gyroscope data on the Y axis.
-     * @return The Mpu gyroscope data on the Y axis.
+     * @return The Mpu gyroscope data on the Y axis in degrees per second.
      */
     float getGyroY();
 
     /**
      * @brief Get the Mpu gyroscope data on the Z axis.
-     * @return The Mpu gyroscope data on the Z axis.
+     * @return The Mpu gyroscope data on the Z axis in degrees per second.
      */
     float getGyroZ();
 
     /**
      * @brief Get the Mpu angle data.
-     * @return The Mpu angle data.
+     * @return The Mpu angle data in degrees.
      */
     MpuVector getAngle();
 
     /**
      * @brief Get the Mpu angle data on the X axis.
-     * @return The Mpu angle data on the X axis.
+     * @return The Mpu angle data on the X axis in degrees.
      */
     float getAngleX();
 
     /**
      * @brief Get the Mpu angle data on the Y axis.
-     * @return The Mpu angle data on the Y axis.
+     * @return The Mpu angle data on the Y axis in degrees.
      */
     float getAngleY();
 
     /**
      * @brief Get the Mpu angle data on the Z axis.
-     * @return The Mpu angle data on the Z axis.
+     * 
+     * The Z angle is relative to the original heading of the robot at the start of the program,
+     * or after latest resetAngleZ() call, NOT to geographic north.
+     * 
+     * @return The Mpu angle data on the Z axis in degrees.
      */
     float getAngleZ();
 
-    /**
-     * @brief Get the Mpu sending interval.
-     * @return The Mpu sending interval
-     */
-    long getInterval();
+    // Reset heading back to 0
+    void resetAngleZ();
 
     /**
      * @brief Set the Mpu compression coefficient (10 - 20).
@@ -152,10 +167,9 @@ class Mpu {
     MpuMotion9 m_mpuMotion;
     MpuMotion6 m_mpuMotionOffset;
 
-    long m_preInterval;
-    int32_t m_interval;
-    bool m_firstRead = true;
-    uint8_t m_compressCoef = 4;
+    TickType_t m_lastTicks;
+
+    uint8_t m_compressCoef;
 
     Mpu();
     Mpu(const Mpu&) = delete;
@@ -166,7 +180,6 @@ class Mpu {
     void calculateGyro(const CoprocStat_MpuVector& gyro);
     void calculateAngle();
 
-    void calcOffsets();
     float wrap(float angle, float limit);
 
     void sendMpuReq(CoprocReq_MpuReq mpuReq);

src/RBCXVersion.h

@@ -5,5 +5,5 @@
 #define RBCX
 
 #define RB3204_MAJOR 1
-#define RB3204_MINOR 4
-#define RB3204_PATCH 1
+#define RB3204_MINOR 5
+#define RB3204_PATCH 0