Add support for MPU from fw 1.3.0

.github/workflows/build-test.yml

@@ -8,7 +8,18 @@ jobs:
     strategy:
       matrix:
         example:
-          - examples/motors
+          - battery
+          - buzzer
+          - leds
+          - motorDriveToPosition
+          - motorPower
+          - motors
+          - mpu
+          - oled
+          - readEncoderPosition
+          - stupidServos
+          - ultrasoundBasic
+          - ultrasoundLeds
     steps:
       - uses: actions/checkout@v1
       - name: Set up Python
@@ -20,4 +31,4 @@ jobs:
       - name: Build examples
         run: platformio ci --lib="." --project-conf="./platformio.ini"
         env:
-          PLATFORMIO_CI_SRC: ${{ matrix.example }}
+          PLATFORMIO_CI_SRC: examples/${{ matrix.example }}

examples/mpu/main.cpp

@@ -26,13 +26,19 @@ void setup() {
     auto& mpu = rb::Manager::get().mpu();
 
     mpu.init();
+
+    // sets the 0 positions to current RBCX orientation
+    mpu.calibrateNow();
+    delay(1000);
+
     mpu.sendStart();
 
     while (true) {
-        printf("MPU - angle: X: %2.2f Y: %2.2f Z: %2.2f\n", mpu.getAngleX(), mpu.getAngleY(), mpu.getAngleZ());
+        auto angle = mpu.getAngle();
+        printf("MPU - angle: X: %2.2f Y: %2.2f Z: %2.2f\n", angle.x, angle.y, angle.z);
         delay(100);
     }
 
 }
 
-void loop() {}
+void loop() {}

examples/mpu_oled/main.cpp

@@ -0,0 +1,74 @@
+#include <Arduino.h>
+#include "RBCX.h"
+
+#define D_WIDTH 128
+#define D_HEIGHT 64
+
+/**
+ * @brief Show ball in the center of the screen and move it with y and z axis
+ * @param oled OLED display
+ * @param angle MPU angle
+*/
+void showCenterGyroBall(rb::Oled& oled, rb::Mpu &mpu) {
+    // show ball in the ceter of the screen - move with x an y axis
+    auto angle = mpu.getAngle();
+    oled.fill(rb::Oled::Black);
+    int x = D_WIDTH / 2 + angle.z * 2;
+    int y = D_HEIGHT / 2 + angle.y * 2;
+    oled.drawCircle(x, y, 5, rb::Oled::White);
+    oled.updateScreen();
+}
+
+/**
+ * @brief Show gyro info on the screen
+ * @param oled OLED display
+ * @param angle MPU angle
+*/
+void showGyroIInfo(rb::Oled& oled, rb::Mpu &mpu) {
+    auto angle = mpu.getAngle();
+    oled.fill(rb::Oled::Black);
+    oled.setCursor(0, 0);
+    oled.writeString(String(angle.x).c_str(), rb::Oled::Font_11x18, rb::Oled::White);
+    oled.setCursor(0, 20);
+    oled.writeString(String(angle.y).c_str(), rb::Oled::Font_11x18, rb::Oled::White);
+    oled.setCursor(0, 40);
+    oled.writeString(String(angle.z).c_str(), rb::Oled::Font_11x18, rb::Oled::White);
+    oled.updateScreen();
+}
+
+void setup() {
+    printf("RB3204-RBCX\n");
+
+    delay(500);
+
+    printf("Init manager\n");
+    auto& man = rb::Manager::get();
+    man.install();
+
+    man.leds().blue(true);
+
+    auto &oled = rb::Manager::get().oled();
+    auto &mpu = rb::Manager::get().mpu();
+    mpu.init();
+
+    mpu.calibrateNow();
+    mpu.sendStart();
+
+    oled.init(rb::Oled::Oled_128x64, true, false);
+
+    while (true) {
+        auto angle = mpu.getAngle();
+        printf("MPU - angle: X: %2.2f Y: %2.2f Z: %2.2f\n", angle.x, angle.y, angle.z);
+
+        // every 3 swithc between show ball and show info
+        if (millis() % 10000 < 5000) {
+            showCenterGyroBall(oled, mpu);
+        } else {
+            showGyroIInfo(oled, mpu);
+        }
+
+        delay(50);
+    }
+}
+
+void loop() {}

examples/oled/main.cpp

@@ -56,7 +56,7 @@ void setup() {
 
         oled.setCursor(5, oled.getHeight()/2);
         String text = "OLED w:" + String(oled.getWidth()) + " | h:" + String(oled.getHeight());
-        oled.writeString(text, rb::Oled::Font_7x10, rb::Oled::White);
+        oled.writeString(text.c_str(), rb::Oled::Font_7x10, rb::Oled::White);
         oled.updateScreen();        
         waitToNextTest();
 
@@ -95,4 +95,4 @@ void setup() {
     }
 }
 
-void loop() {}
+void loop() {}

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": {

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

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

src/RBCXManager.h

@@ -147,6 +147,7 @@ class Manager {
     }
 
     void coprocFwVersionAssert(uint32_t minVersion, const char* name);
+    bool coprocFwAtLeastVersion(uint32_t minVersion);
 
 private:
     Manager();