add simple imu and mag publishers

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

Src/modules/imu/CMakeLists.txt

@@ -0,0 +1,11 @@
+# Copyright (C) 2023 Dmitry Ponomarev <[email protected]>
+# 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)

Src/modules/imu/imu.cpp

@@ -0,0 +1,49 @@
+/**
+ * This program is free software under the GNU General Public License v3.
+ * See <https://www.gnu.org/licenses/> for details.
+ * Author: Dmitry Ponomarev <[email protected]>
+ */
+
+#include "imu.hpp"
+#include "periphery/spi/spi.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();
+    }
+}

Src/modules/imu/imu.hpp

@@ -0,0 +1,44 @@
+/**
+ * This program is free software under the GNU General Public License v3.
+ * See <https://www.gnu.org/licenses/> for details.
+ * Author: Dmitry Ponomarev <[email protected]>
+ */
+
+#ifndef SRC_MODULES_CANOPEN_HPP_
+#define SRC_MODULES_CANOPEN_HPP_
+
+#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 inline float raw_gyro_to_rad_per_second(int16_t raw_gyro) {
+        return raw_gyro * 3.14159265358979323846f / 131.0f / 180.0f;
+    }
+
+    static constexpr inline 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_