From 33b094307dc74d2c29bfb06382686987f45674e3 Mon Sep 17 00:00:00 2001 From: NuwanJ Date: Wed, 20 Mar 2024 13:44:18 +0530 Subject: [PATCH] MPU9250 setup --- src/config/pins.h | 1 + src/define.h | 3 + src/sensors/motion/motion.cpp | 3 + src/sensors/motion/motion.h | 5 ++ src/sensors/motion/src/robot_mpu9250.cpp | 101 +++++++++++++++++++++++ src/sensors/motion/src/robot_mpu9250.h | 24 ++++++ src/src.cpp | 80 ++---------------- 7 files changed, 142 insertions(+), 75 deletions(-) create mode 100644 src/sensors/motion/motion.cpp create mode 100644 src/sensors/motion/motion.h create mode 100644 src/sensors/motion/src/robot_mpu9250.cpp create mode 100644 src/sensors/motion/src/robot_mpu9250.h diff --git a/src/config/pins.h b/src/config/pins.h index c6e89e9..fa424bc 100644 --- a/src/config/pins.h +++ b/src/config/pins.h @@ -15,3 +15,4 @@ // ------------------------------------------------------------------ Other Pins // --------------------------------------------------------- I2C Port addresses +#define I2C_ADDRESS_MPU9250 0x68 \ No newline at end of file diff --git a/src/define.h b/src/define.h index 90897d9..47b45fa 100644 --- 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" \ No newline at end of file diff --git a/src/sensors/motion/motion.cpp b/src/sensors/motion/motion.cpp new file mode 100644 index 0000000..d55f3a6 --- /dev/null +++ b/src/sensors/motion/motion.cpp @@ -0,0 +1,3 @@ +#include "motion.h" + +SW_MPU9250 motion(false); \ No newline at end of file diff --git a/src/sensors/motion/motion.h b/src/sensors/motion/motion.h new file mode 100644 index 0000000..2e01650 --- /dev/null +++ b/src/sensors/motion/motion.h @@ -0,0 +1,5 @@ +#pragma once + +#include "./src/robot_mpu9250.h" + +extern SW_MPU9250 motion; \ No newline at end of file diff --git a/src/sensors/motion/src/robot_mpu9250.cpp b/src/sensors/motion/src/robot_mpu9250.cpp new file mode 100644 index 0000000..5170bc3 --- /dev/null +++ b/src/sensors/motion/src/robot_mpu9250.cpp @@ -0,0 +1,101 @@ +#include + +#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()); +} \ No newline at end of file diff --git a/src/sensors/motion/src/robot_mpu9250.h b/src/sensors/motion/src/robot_mpu9250.h new file mode 100644 index 0000000..9bf4dcc --- /dev/null +++ b/src/sensors/motion/src/robot_mpu9250.h @@ -0,0 +1,24 @@ +#ifndef MPU9250_h +#define MPU9250_h + +#include + +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 \ No newline at end of file diff --git a/src/src.cpp b/src/src.cpp index f5bc6fb..35c08ad 100644 --- a/src/src.cpp +++ b/src/src.cpp @@ -2,98 +2,28 @@ #include #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(); } }