Add support for MPU from fw 1.3.0

RoboticsBrno · Mar 2, 2024 · 0d872b9 · 0d872b9
1 parent 67dd9d9
commit 0d872b9
Show file tree

Hide file tree

Showing 7 changed files with 139 additions and 129 deletions.
diff --git a/library.json b/library.json
@@ -28,14 +28,14 @@
   "dependencies": [
     {
       "name": "RB3204-RBCX-coproc-comm",
-      "version": "https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/1ed2bbc8fda0b9b1c6592f5f39bd9bbb1213d569.zip"
+      "version": "https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/77f4ada13fb3b348cca408ac8f614597e61c370c.zip"
     },
     {
       "name": "Esp32-lx16a",
       "version": "https://github.com/RoboticsBrno/Esp32-lx16a/archive/refs/tags/v1.2.1.zip"
     }
   ],
-  "version": "1.5.0",
+  "version": "1.6.0",
   "frameworks": ["espidf", "arduino"],
   "platforms": "espressif32",
   "build": {

diff --git a/platformio.ini b/platformio.ini
@@ -23,6 +23,6 @@ build_flags =
     -fmax-errors=5
 
 lib_deps =
-    https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/1ed2bbc8fda0b9b1c6592f5f39bd9bbb1213d569.zip
+    https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/77f4ada13fb3b348cca408ac8f614597e61c370c.zip
     https://github.com/nanopb/nanopb/archive/nanopb-0.4.3.zip
     https://github.com/RoboticsBrno/Esp32-lx16a/archive/refs/tags/v1.2.1.zip
diff --git a/src/RBCXManager.cpp b/src/RBCXManager.cpp
@@ -158,6 +158,9 @@ void Manager::consumerRoutine() {
         case CoprocStat_mpuStat_tag:
             m_mpu.setState(msg.payload.mpuStat);
             break;
+        case CoprocStat_mpuCalibrationDone_tag:
+            m_mpu.onCalibrationDone(msg.payload.mpuCalibrationDone);
+            break;
         case CoprocStat_faultStat_tag:
             fault(msg.payload.faultStat);
             break;
@@ -194,13 +197,21 @@ void Manager::sendToCoproc(const CoprocReq& msg) {
     xTaskNotify(m_keepaliveTask, 0, eNoAction);
 }
 
-void Manager::coprocFwVersionAssert(uint32_t minVersion, const char* name) {
+bool Manager::coprocFwAtLeastVersion(uint32_t minVersion) {
     // Ignore zero version number and pass. Might be the cmd was not received yet,
     // but better than to crash in that case.
     if (m_coprocFwVersion.number == 0)
-        return;
+        return true;
+
+    // Ignore dirty versions for development
+    if (m_coprocFwVersion.dirty)
+        return true;
 
-    if (minVersion > m_coprocFwVersion.number) {
+    return minVersion <= m_coprocFwVersion.number;
+}
+
+void Manager::coprocFwVersionAssert(uint32_t minVersion, const char* name) {
+    if (!coprocFwAtLeastVersion(minVersion)) {
         printf("\n\nERROR: Please update your STM32 FW, '%s' requires version "
                "0x%06x and you have 0x%06x!\n\n",
             name, minVersion, m_coprocFwVersion.number);

diff --git a/src/RBCXManager.h b/src/RBCXManager.h
@@ -147,6 +147,7 @@ class Manager {
     }
 
     void coprocFwVersionAssert(uint32_t minVersion, const char* name);
+    bool coprocFwAtLeastVersion(uint32_t minVersion);
 
 private:
     Manager();

diff --git a/src/RBCXMpu.cpp b/src/RBCXMpu.cpp
@@ -2,15 +2,17 @@
 
 #include "RBCXManager.h"
 
+#define TAG "RBCXMpu"
+
 // Constant to convert raw temperature to Celsius degrees
 static constexpr float MPU6050_TEMP_LINEAR_COEF = 1.0 / 340.00;
 static constexpr float MPU6050_TEMP_OFFSET = 36.53;
 
 // Constant to convert raw gyroscope to degree/s
-static constexpr float MPU_GYRO_FACTOR_250 = 1.0 / 131.0;
+static constexpr float MPU6050_GYRO_FACTOR_250 = 1.0 / 131.0;
 // static constexpr float MPU6050_GYRO_FACTOR_500  = 1.0/65.5;
 // static constexpr float MPU6050_GYRO_FACTOR_1000 = 1.0/32.8;
-// static constexpr float MPU6050_GYRO_FACTOR_2000 = 1.0/16.4;
+static constexpr float MPU6050_GYRO_FACTOR_2000 = 1.0 / 16.4;
 
 // Constant to convert raw acceleration to m/s^2
 // static constexpr float GRAVITATIONAL_CONSTANT_G = 9.81;
@@ -21,13 +23,10 @@ static constexpr float MPU6050_ACCEL_FACTOR_2 = 1.0 / 16384.0;
 // static constexpr float MPU6050_ACCEL_FACTOR_16 = 1 / 2048.0;
 
 static constexpr float RAD_2_DEG = 57.29578; // [°/rad]
-static constexpr int CALIB_OFFSET_NB_MES = 1;
 
 namespace rb {
 
-Mpu::Mpu() {
-    m_lastTicks = xTaskGetTickCount();
-}
+Mpu::Mpu() { m_lastTicks = xTaskGetTickCount(); }
 
 Mpu::~Mpu() {}
 
@@ -59,22 +58,75 @@ void Mpu::setState(const CoprocStat_MpuStat& msg) {
     m_compressCoef = msg.compressCoef;
     calculateAcc(msg.accel);
     calculateGyro(msg.gyro);
-    calculateAngle();
+
+    if (msg.has_yawPitchRoll) {
+        m_mpuMotion.angle.x = msg.yawPitchRoll.x / 16384.f;
+        m_mpuMotion.angle.y = msg.yawPitchRoll.y / 16384.f;
+        m_mpuMotion.angle.z = msg.yawPitchRoll.z / 16384.f;
+    } else {
+        calculateAngle();
+    }
+}
+
+void Mpu::onCalibrationDone(const MpuCalibrationData& data) {
+    if (m_onCalibrationDoneCallback) {
+        std::vector<uint8_t> vec_data(data.data, data.data + sizeof(data.data));
+        m_onCalibrationDoneCallback(vec_data);
+
+        CalibrationDoneCb empty;
+        m_onCalibrationDoneCallback.swap(empty);
+    }
 }
 
 void Mpu::clearCalibrationData() {
-    memset(&m_mpuMotionOffset, 0, sizeof(m_mpuMotionOffset));
+    if (Manager::get().coprocFwAtLeastVersion(0x010300)) {
+        sendMpuReq(CoprocReq_MpuReq {
+            .which_mpuCmd = CoprocReq_MpuReq_restoreCalibrationData_tag,
+            .mpuCmd = {
+                .restoreCalibrationData = {
+                    .data = {},
+                },
+            },
+        });
+    } else {
+        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
-        = (m_mpuMotion.accel.z - 1.0) / CALIB_OFFSET_NB_MES;
+void Mpu::restoreCalibrationData(const uint8_t* data, size_t length) {
+    Manager::get().coprocFwVersionAssert(0x010300, "restoreCalibrationData");
+
+    auto req = CoprocReq_MpuReq {
+        .which_mpuCmd = CoprocReq_MpuReq_restoreCalibrationData_tag,
+    };
 
-    m_mpuMotionOffset.gyro.x = m_mpuMotion.gyro.x / CALIB_OFFSET_NB_MES;
-    m_mpuMotionOffset.gyro.y = m_mpuMotion.gyro.y / CALIB_OFFSET_NB_MES;
-    m_mpuMotionOffset.gyro.z = m_mpuMotion.gyro.z / CALIB_OFFSET_NB_MES;
+    if (length != sizeof(req.mpuCmd.restoreCalibrationData.data)) {
+        ESP_LOGE(TAG,
+            "Invalid calibration data length, %d != %d, perhaps from older FW "
+            "version?",
+            length, sizeof(req.mpuCmd.restoreCalibrationData.data));
+        return;
+    }
+
+    memcpy(req.mpuCmd.restoreCalibrationData.data, data, length);
+    sendMpuReq(req);
+}
+
+void Mpu::calibrateNow(CalibrationDoneCb callback) {
+    if (Manager::get().coprocFwAtLeastVersion(0x010300)) {
+        m_onCalibrationDoneCallback = callback;
+        sendMpuReq(CoprocReq_MpuReq {
+            .which_mpuCmd = CoprocReq_MpuReq_calibrateOffsets_tag,
+        });
+    } else {
+        m_mpuMotionOffset.accel.x = m_mpuMotion.accel.x;
+        m_mpuMotionOffset.accel.y = m_mpuMotion.accel.y;
+        m_mpuMotionOffset.accel.z = (m_mpuMotion.accel.z - 1.0);
+
+        m_mpuMotionOffset.gyro.x = m_mpuMotion.gyro.x;
+        m_mpuMotionOffset.gyro.y = m_mpuMotion.gyro.y;
+        m_mpuMotionOffset.gyro.z = m_mpuMotion.gyro.z;
+    }
 }
 
 void Mpu::calculateAcc(const CoprocStat_MpuVector& accel) {
@@ -90,14 +142,15 @@ void Mpu::calculateAcc(const CoprocStat_MpuVector& accel) {
 }
 
 void Mpu::calculateGyro(const CoprocStat_MpuVector& gyro) {
-    m_mpuMotion.gyro.x
-        = (((float)gyro.x / m_compressCoef) * MPU_GYRO_FACTOR_250)
+    const float factor = Manager::get().coprocFwAtLeastVersion(0x010300)
+        ? MPU6050_GYRO_FACTOR_2000
+        : MPU6050_GYRO_FACTOR_250;
+
+    m_mpuMotion.gyro.x = (((float)gyro.x / m_compressCoef) * factor)
         - m_mpuMotionOffset.gyro.x;
-    m_mpuMotion.gyro.y
-        = (((float)gyro.y / m_compressCoef) * MPU_GYRO_FACTOR_250)
+    m_mpuMotion.gyro.y = (((float)gyro.y / m_compressCoef) * factor)
         - m_mpuMotionOffset.gyro.y;
-    m_mpuMotion.gyro.z
-        = (((float)gyro.z / m_compressCoef) * MPU_GYRO_FACTOR_250)
+    m_mpuMotion.gyro.z = (((float)gyro.z / m_compressCoef) * factor)
         - m_mpuMotionOffset.gyro.z;
 }
 
@@ -118,36 +171,32 @@ 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°]
-    
+
     const TickType_t curTicks = xTaskGetTickCount();
-    const float elapsedSeconds = float((curTicks  - m_lastTicks) * portTICK_RATE_MS) / 1000.f;
+    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.gyro.x * elapsedSeconds
-                           - m_mpuMotion.angleAcc.x,
-                       180))
-                + m_mpuMotion.angleAcc.x,
-            180);
-    m_mpuMotion.angle.y = wrap(
-        (m_mpuMotion.angleAcc.y
-            + wrap(sgZ * m_mpuMotion.gyro.y * elapsedSeconds
-                    - m_mpuMotion.angleAcc.y,
-                90))
-            + m_mpuMotion.angleAcc.y,
-        90);
+    m_mpuMotion.angle.x = wrap(
+        (m_mpuMotion.angleAcc.x
+            + 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(sgZ * m_mpuMotion.gyro.y * elapsedSeconds
+                           - m_mpuMotion.angleAcc.y,
+                       90))
+                + m_mpuMotion.angleAcc.y,
+            90);
 
     m_mpuMotion.angle.z
         += 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!) */
 float Mpu::wrap(float angle, float limit) {
     while (angle > limit)
@@ -158,19 +207,8 @@ float Mpu::wrap(float angle, float limit) {
 }
 
 MpuVector Mpu::getAcc() { return m_mpuMotion.accel; }
-float Mpu::getAccX() { return m_mpuMotion.accel.x; }
-float Mpu::getAccY() { return m_mpuMotion.accel.y; }
-float Mpu::getAccZ() { return m_mpuMotion.accel.z; }
-
 MpuVector Mpu::getGyro() { return m_mpuMotion.gyro; }
-float Mpu::getGyroX() { return m_mpuMotion.gyro.x; }
-float Mpu::getGyroY() { return m_mpuMotion.gyro.y; }
-float Mpu::getGyroZ() { return m_mpuMotion.gyro.z; }
-
 MpuVector Mpu::getAngle() { return m_mpuMotion.angle; }
-float Mpu::getAngleX() { return m_mpuMotion.angle.x; }
-float Mpu::getAngleY() { return m_mpuMotion.angle.y; }
-float Mpu::getAngleZ() { return m_mpuMotion.angle.z; }
 
 void Mpu::setCompressCoef(uint8_t coef) {
     sendMpuReq(