From 2b8ce340676d4812eb272842ba9dd3177dc93a62 Mon Sep 17 00:00:00 2001
From: PonomarevDA <ponomarevda96@gmail.com>
Date: Sun, 11 Aug 2024 15:26:53 +0300
Subject: [PATCH] add simple imu and mag publishers

---
 Src/applications/dronecan/CMakeLists.txt |  4 ++
 Src/modules/imu/CMakeLists.txt           | 11 ++++++
 Src/modules/imu/imu.cpp                  | 48 ++++++++++++++++++++++++
 Src/modules/imu/imu.hpp                  | 45 ++++++++++++++++++++++
 4 files changed, 108 insertions(+)
 create mode 100644 Src/modules/imu/CMakeLists.txt
 create mode 100644 Src/modules/imu/imu.cpp
 create mode 100644 Src/modules/imu/imu.hpp

diff --git a/Src/applications/dronecan/CMakeLists.txt b/Src/applications/dronecan/CMakeLists.txt
index 214667b..b55f1dc 100644
--- a/Src/applications/dronecan/CMakeLists.txt
+++ b/Src/applications/dronecan/CMakeLists.txt
@@ -8,3 +8,7 @@ cmake_path(GET APPLICATIONS_DIR PARENT_PATH SRC_DIR)
 include(${SRC_DIR}/modules/dronecan/core/CMakeLists.txt)
 include(${SRC_DIR}/modules/dronecan/circuit_status/CMakeLists.txt)
 include(${SRC_DIR}/modules/dronecan/pwm/CMakeLists.txt)
+
+if(USE_PLATFORM_NODE_V3)
+    include(${SRC_DIR}/modules/imu/CMakeLists.txt)
+endif()
diff --git a/Src/modules/imu/CMakeLists.txt b/Src/modules/imu/CMakeLists.txt
new file mode 100644
index 0000000..62c21ac
--- /dev/null
+++ b/Src/modules/imu/CMakeLists.txt
@@ -0,0 +1,11 @@
+# Copyright (C) 2023 Dmitry Ponomarev <ponomarevda96@gmail.com>
+# Distributed under the terms of the GPL v3 license, available in the file LICENSE.
+
+cmake_path(GET CMAKE_CURRENT_LIST_DIR PARENT_PATH MODULES_DIR)
+cmake_path(GET MODULES_DIR PARENT_PATH SRC_DIR)
+
+list(APPEND APPLICATION_SOURCES
+    ${CMAKE_CURRENT_LIST_DIR}/imu.cpp
+)
+
+include(${SRC_DIR}/drivers/mpu9250/CMakeLists.txt)
diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp
new file mode 100644
index 0000000..54f54f2
--- /dev/null
+++ b/Src/modules/imu/imu.cpp
@@ -0,0 +1,48 @@
+/**
+ * This program is free software under the GNU General Public License v3.
+ * See <https://www.gnu.org/licenses/> for details.
+ * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
+ */
+
+#include "imu.hpp"
+
+REGISTER_MODULE(ImuModule)
+
+void ImuModule::init() {
+    auto res = imu.initialize();
+    health = res ? Module::Status::OK : Module::Status::FATAL_MALFANCTION;
+    mode = Module::Mode::OPERATIONAL;
+}
+
+void ImuModule::spin_once() {
+    if (health == Module::Status::FATAL_MALFANCTION) {
+        return;
+    }
+
+    std::array<int16_t, 3> mag_raw;
+    imu.read_magnetometer(&mag_raw);
+    mag.publish();  // publish anyway
+
+    bool updated{false};
+
+    std::array<int16_t, 3> accel_raw;
+    if (imu.read_accelerometer(&accel_raw) >= 0) {
+        pub.msg.accelerometer_latest[0] = raw_accel_to_meter_per_square_second(accel_raw[0]);
+        pub.msg.accelerometer_latest[1] = raw_accel_to_meter_per_square_second(accel_raw[1]);
+        pub.msg.accelerometer_latest[2] = raw_accel_to_meter_per_square_second(accel_raw[2]);
+        updated = true;
+    }
+
+    std::array<int16_t, 3> gyro_raw;
+    if (imu.read_gyroscope(&gyro_raw) >= 0) {
+        pub.msg.rate_gyro_latest[0] = raw_gyro_to_rad_per_second(gyro_raw[0]);
+        pub.msg.rate_gyro_latest[1] = raw_gyro_to_rad_per_second(gyro_raw[1]);
+        pub.msg.rate_gyro_latest[2] = raw_gyro_to_rad_per_second(gyro_raw[2]);
+        updated = true;
+    }
+
+    if (updated) {
+        pub.msg.timestamp = HAL_GetTick() * 1000;
+        pub.publish();
+    }
+}
diff --git a/Src/modules/imu/imu.hpp b/Src/modules/imu/imu.hpp
new file mode 100644
index 0000000..aa7453d
--- /dev/null
+++ b/Src/modules/imu/imu.hpp
@@ -0,0 +1,45 @@
+/**
+ * This program is free software under the GNU General Public License v3.
+ * See <https://www.gnu.org/licenses/> for details.
+ * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
+ */
+
+#ifndef SRC_MODULES_CANOPEN_HPP_
+#define SRC_MODULES_CANOPEN_HPP_
+
+#include <numbers>
+#include "module.hpp"
+#include "publisher.hpp"
+#include "drivers/mpu9250/mpu9250.hpp"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+class ImuModule : public Module {
+public:
+    ImuModule() : Module(10.0) {}
+    void init() override;
+
+protected:
+    void spin_once() override;
+
+private:
+    DronecanPublisher<AhrsRawImu> pub;
+    DronecanPublisher<MagneticFieldStrength2> mag;
+    Mpu9250 imu;
+
+    static constexpr float raw_gyro_to_rad_per_second(int16_t raw_gyro) {
+        return raw_gyro * std::numbers::pi_v<float> / 131.0f / 180.0f;
+    }
+
+    static constexpr float raw_accel_to_meter_per_square_second(int16_t raw_accel) {
+        return raw_accel * 9.80665f / 16384.0f;
+    }
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // SRC_MODULES_CANOPEN_HPP_