MPU9250 setup

Pera-Swarm · Mar 20, 2024 · 33b0943 · 33b0943
1 parent eb32cb4
commit 33b0943
Show file tree

Hide file tree

Showing 7 changed files with 142 additions and 75 deletions.
diff --git a/src/config/pins.h b/src/config/pins.h
@@ -15,3 +15,4 @@
 // ------------------------------------------------------------------ Other Pins
 
 // --------------------------------------------------------- I2C  Port addresses
+#define I2C_ADDRESS_MPU9250 0x68
diff --git a/src/define.h b/src/define.h
@@ -18,3 +18,6 @@
 #include "utilities/memory/memory.h"
 
 // *** Modules **************************************************
+
+// Motion Sensor
+#include "sensors/motion/motion.h"
diff --git a/src/sensors/motion/motion.cpp b/src/sensors/motion/motion.cpp
@@ -0,0 +1,3 @@
+#include "motion.h"
+
+SW_MPU9250 motion(false);
diff --git a/src/sensors/motion/motion.h b/src/sensors/motion/motion.h
@@ -0,0 +1,5 @@
+#pragma once
+
+#include "./src/robot_mpu9250.h"
+
+extern SW_MPU9250 motion;
diff --git a/src/sensors/motion/src/robot_mpu9250.cpp b/src/sensors/motion/src/robot_mpu9250.cpp
@@ -0,0 +1,101 @@
+#include <Wire.h>
+
+#include "robot_mpu9250.h"
+#include "config/pins.h"
+#include "MPU9250.h"
+
+MPU9250 mpu;
+
+SW_MPU9250::SW_MPU9250(bool useCalibration)
+{
+    // TODO: useCalibration config
+}
+
+void SW_MPU9250::begin()
+{
+    Wire.begin();
+
+    if (!mpu.setup(I2C_ADDRESS_MPU9250))
+    { // change to your own address
+        while (1)
+        {
+            Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
+            delay(5000);
+        }
+    }
+
+    Serial.println(">> Motion\t:enabled");
+}
+
+void SW_MPU9250::calibrate()
+{
+    // calibrate anytime you want to
+    Serial.println("Accel Gyro calibration will start in 5sec.");
+    Serial.println("Please leave the device still on the flat plane.");
+    mpu.verbose(true);
+    delay(5000);
+    mpu.calibrateAccelGyro();
+
+    Serial.println("Mag calibration will start in 5sec.");
+    Serial.println("Please Wave device in a figure eight until done.");
+    delay(5000);
+    mpu.calibrateMag();
+
+    print_calibration();
+    mpu.verbose(false);
+}
+
+void SW_MPU9250::read()
+{
+}
+
+void SW_MPU9250::test()
+{
+    if (mpu.update())
+    {
+        static uint32_t prev_ms = millis();
+        if (millis() > prev_ms + 25)
+        {
+            print_roll_pitch_yaw();
+            prev_ms = millis();
+        }
+    }
+}
+
+void SW_MPU9250::print_calibration()
+{
+    Serial.println("< calibration parameters >");
+    Serial.println("accel bias [g]: ");
+    Serial.print(mpu.getAccBiasX() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
+    Serial.print(", ");
+    Serial.print(mpu.getAccBiasY() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
+    Serial.print(", ");
+    Serial.print(mpu.getAccBiasZ() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
+    Serial.println();
+    Serial.println("gyro bias [deg/s]: ");
+    Serial.print(mpu.getGyroBiasX() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
+    Serial.print(", ");
+    Serial.print(mpu.getGyroBiasY() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
+    Serial.print(", ");
+    Serial.print(mpu.getGyroBiasZ() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
+    Serial.println();
+    Serial.println("mag bias [mG]: ");
+    Serial.print(mpu.getMagBiasX());
+    Serial.print(", ");
+    Serial.print(mpu.getMagBiasY());
+    Serial.print(", ");
+    Serial.print(mpu.getMagBiasZ());
+    Serial.println();
+    Serial.println("mag scale []: ");
+    Serial.print(mpu.getMagScaleX());
+    Serial.print(", ");
+    Serial.print(mpu.getMagScaleY());
+    Serial.print(", ");
+    Serial.print(mpu.getMagScaleZ());
+    Serial.println();
+}
+
+void SW_MPU9250::print_roll_pitch_yaw()
+{
+    Serial.printf("%8.2f %8.2f %8.2f\n", mpu.getYaw(), mpu.getPitch(), mpu.getRoll());
+}
diff --git a/src/sensors/motion/src/robot_mpu9250.h b/src/sensors/motion/src/robot_mpu9250.h
@@ -0,0 +1,24 @@
+#ifndef MPU9250_h
+#define MPU9250_h
+
+#include <Arduino.h>
+
+class SW_MPU9250
+{
+public:
+    SW_MPU9250(bool useCalibration);
+
+    void begin();
+
+    // TODO
+    void read();
+
+    void calibrate();
+    void test();
+
+private:
+    void print_calibration();
+    void print_roll_pitch_yaw();
+};
+
+#endif
diff --git a/src/src.cpp b/src/src.cpp
@@ -2,98 +2,28 @@
 #include <Arduino.h>
 #include "define.h"
 
-// https://github.com/hideakitai/MPU9250
-#include "MPU9250.h"
-MPU9250 mpu; // You can also use MPU9255 as is
-
 int ROBOT_ID;
 
 // This is to hide non-test related source code.
 // https://docs.platformio.org/en/latest/plus/unit-testing.html
 #ifndef UNIT_TEST
 
-void print_roll_pitch_yaw()
-{
-    Serial.printf("%8.2f %8.2f %8.2f\n", mpu.getYaw(), mpu.getPitch(), mpu.getRoll());
-}
-
-void print_calibration()
-{
-    Serial.println("< calibration parameters >");
-    Serial.println("accel bias [g]: ");
-    Serial.print(mpu.getAccBiasX() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
-    Serial.print(", ");
-    Serial.print(mpu.getAccBiasY() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
-    Serial.print(", ");
-    Serial.print(mpu.getAccBiasZ() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
-    Serial.println();
-    Serial.println("gyro bias [deg/s]: ");
-    Serial.print(mpu.getGyroBiasX() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
-    Serial.print(", ");
-    Serial.print(mpu.getGyroBiasY() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
-    Serial.print(", ");
-    Serial.print(mpu.getGyroBiasZ() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
-    Serial.println();
-    Serial.println("mag bias [mG]: ");
-    Serial.print(mpu.getMagBiasX());
-    Serial.print(", ");
-    Serial.print(mpu.getMagBiasY());
-    Serial.print(", ");
-    Serial.print(mpu.getMagBiasZ());
-    Serial.println();
-    Serial.println("mag scale []: ");
-    Serial.print(mpu.getMagScaleX());
-    Serial.print(", ");
-    Serial.print(mpu.getMagScaleY());
-    Serial.print(", ");
-    Serial.print(mpu.getMagScaleZ());
-    Serial.println();
-}
-
 void setup()
 {
     Serial.begin(115200);
     Wire.begin();
     delay(2000);
 
-    if (!mpu.setup(0x68))
-    { // change to your own address
-        while (1)
-        {
-            Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
-            delay(5000);
-        }
-    }
-}
-
-void calibration()
-{
-    // calibrate anytime you want to
-    Serial.println("Accel Gyro calibration will start in 5sec.");
-    Serial.println("Please leave the device still on the flat plane.");
-    mpu.verbose(true);
-    delay(5000);
-    mpu.calibrateAccelGyro();
-
-    Serial.println("Mag calibration will start in 5sec.");
-    Serial.println("Please Wave device in a figure eight until done.");
-    delay(5000);
-    mpu.calibrateMag();
-
-    print_calibration();
-    mpu.verbose(false);
+    motion.begin();
 }
 
 void loop()
 {
-    if (mpu.update())
+    static uint32_t prev_ms = millis();
+    if (millis() > prev_ms + 25)
     {
-        static uint32_t prev_ms = millis();
-        if (millis() > prev_ms + 25)
-        {
-            print_roll_pitch_yaw();
-            prev_ms = millis();
-        }
+        motion.test();
+        prev_ms = millis();
     }
 }