upd params, add vibration estimation (#73)

Src/modules/imu/imu.cpp

@@ -4,26 +4,31 @@
  * Author: Dmitry Ponomarev <[email protected]>
  */
 
+#include <math.h>
 #include "imu.hpp"
 #include "params.hpp"
 
 REGISTER_MODULE(ImuModule)
 
 void ImuModule::init() {
-    initialized = imu.initialize();
+    bool imu_initialized = imu.initialize();
     mode = Module::Mode::STANDBY;
+    initialized = imu_initialized;
 }
 
 void ImuModule::update_params() {
-    enabled = static_cast<bool>(paramsGetIntegerValue(PARAM_IMU_ENABLE));
-    health = (!enabled || initialized) ? Module::Status::OK : Module::Status::MAJOR_FAILURE;
-    if (enabled) {
+    auto pub_frequency = static_cast<uint16_t>(
+                                paramsGetIntegerValue(IntParamsIndexes::PARAM_IMU_PUB_FREQUENCY));
+    pub_timeout_ms = pub_frequency == 0 ? 0 : 1000 / pub_frequency;
+    bitmask = static_cast<uint8_t>(paramsGetIntegerValue(IntParamsIndexes::PARAM_IMU_MODE_BITMASK));
+    health = (!bitmask || initialized) ? Module::Status::OK : Module::Status::MAJOR_FAILURE;
+    if (bitmask) {
         mode = initialized ? Mode::STANDBY : Mode::INITIALIZATION;
     }
 }
 
 void ImuModule::spin_once() {
-    if (!enabled || !initialized) {
+    if (!bitmask || !initialized) {
         return;
     }
 
@@ -32,26 +37,51 @@ void ImuModule::spin_once() {
         mag.publish();
     }
 
-    bool updated{false};
+    bool updated[2]{false, false};
+
+    std::array<int16_t, 3>  accel_raw = {0, 0, 0};
+    std::array<int16_t, 3>  gyro_raw  = {0, 0, 0};
+    std::array<float, 3>    gyro      = {0.0f, 0.0f, 0.0f};
+    std::array<float, 3>    accel     = {0.0f, 0.0f, 0.0f};
+    if (imu.read_gyroscope(&gyro_raw) >= 0) {
+        gyro = {
+                raw_gyro_to_rad_per_second(gyro_raw[0]),
+                raw_gyro_to_rad_per_second(gyro_raw[1]),
+                raw_gyro_to_rad_per_second(gyro_raw[2])};
+        pub.msg.rate_gyro_latest[0] = gyro[0];
+        pub.msg.rate_gyro_latest[1] = gyro[1];
+        pub.msg.rate_gyro_latest[2] = gyro[2];
+        updated[0] = true;
+    }
 
-    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;
+        accel = {
+                raw_accel_to_meter_per_square_second(accel_raw[0]),
+                raw_accel_to_meter_per_square_second(accel_raw[1]),
+                raw_accel_to_meter_per_square_second(accel_raw[2])};
+        get_vibration(accel);
+        pub.msg.accelerometer_latest[0] = accel[0];
+        pub.msg.accelerometer_latest[1] = accel[1];
+        pub.msg.accelerometer_latest[2] = accel[2];
+        updated[1] = 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 (pub_timeout_ms != 0 && HAL_GetTick() - pub.msg.timestamp / 1000 > pub_timeout_ms) {
+        if (updated[0] && updated[1]) {
+            pub.msg.timestamp = HAL_GetTick() * 1000;
+            pub.publish();
+        }
     }
+}
 
-    if (updated) {
-        pub.msg.timestamp = HAL_GetTick() * 1000;
-        pub.publish();
+void ImuModule::get_vibration(std::array<float, 3> data) {
+    if (bitmask & static_cast<std::underlying_type_t<Bitmask>>(Bitmask::ENABLE_VIB_ESTIM)) {
+        float diff_magnitude = 0.0f;
+        for (uint8_t i = 0; i < 3; i++) {
+            diff_magnitude += std::pow(data[i] - pub.msg.accelerometer_latest[i], 2);
+        }
+        vibration = 0.99f * vibration + 0.01f * std::sqrt(diff_magnitude);
+        pub.msg.integration_interval = vibration;
+        return;
     }
 }

Src/modules/imu/imu.hpp

@@ -4,8 +4,8 @@
  * Author: Dmitry Ponomarev <[email protected]>
  */
 
-#ifndef SRC_MODULES_CANOPEN_HPP_
-#define SRC_MODULES_CANOPEN_HPP_
+#ifndef SRC_MODULES_IMU_HPP_
+#define SRC_MODULES_IMU_HPP_
 
 #include <numbers>
 #include "module.hpp"
@@ -18,20 +18,31 @@ extern "C" {
 
 class ImuModule : public Module {
 public:
-    ImuModule() : Module(10.0) {}
+    enum class Bitmask : uint8_t {
+        DISABLED                    = 0,
+        ENABLE_VIB_ESTIM            = 2,
+        ENABLE_FFT_ACC              = 4,
+        ENABLE_FFT_GYR              = 8,
+        ENABLE_ALL_BY_DEFAULT       = 15,
+    };
+    ImuModule() : Module(400.0, Protocol::DRONECAN) {}
     void init() override;
 
 protected:
     void spin_once() override;
     void update_params() override;
+    void get_vibration(std::array<float, 3> data);
+    void fft();
 
 private:
     DronecanPublisher<AhrsRawImu> pub;
     DronecanPublisher<MagneticFieldStrength2> mag;
     Mpu9250 imu;
+    float vibration = 0.0f;
     bool initialized{false};
     bool enabled{false};
-
+    uint8_t bitmask{0};
+    uint16_t pub_timeout_ms{0};
     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;
     }
@@ -45,4 +56,4 @@ class ImuModule : public Module {
 }
 #endif
 
-#endif  // SRC_MODULES_CANOPEN_HPP_
+#endif  // SRC_MODULES_IMU_HPP_

Src/modules/imu/params.yaml

@@ -1,8 +1,23 @@
-imu.enable:
+imu.mode:
   type: Integer
-  note: Enable or disable IMU publisher.
-  enum: PARAM_IMU_ENABLE
+  note: 
+      "Bit mask to enable IMU features:
+    </br> Bit 1 - enable RawImu publisher,
+    </br> Bit 2 - enable vibration metric publishing to RawImu.integration_interval,
+    </br> Bit 3 - enable FFT acceleration publishing to RawImu.accelerometer_integral,
+    </br> Bit 4 - enable FFT acceleration publishing to RawImu.rate_gyro_integral,
+    </br> By default 15 that mean enable all publishers"
+  enum: PARAM_IMU_MODE_BITMASK
   flags: mutable
   default: 0
   min: 0
-  max: 1
+  max: 16
+
+imu.pub_frequency:
+  type: Integer
+  note: Frequency of IMU publisher [Hz].
+  enum: PARAM_IMU_PUB_FREQUENCY
+  flags: mutable
+  default: 1
+  min: 0
+  max: 400

docs/dronecan/README.md

@@ -30,7 +30,8 @@ The node has the following registers:
 | pwm4.max                | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
 | pwm4.def                | PWM duration when setpoint is negative or there is no setpoint at all. |
 | pwm.cmd_type            | 0 means RawCommand, 1 means ArrayCommand, 2 is reserved for hardpoint.Command. |
-| imu.enable              | Enable or disable IMU publisher. |
+| imu.mode                | Bit mask to enable IMU features: </br> Bit 1 - enable RawImu publisher, </br> Bit 2 - enable vibration metric publishing to RawImu.integration_interval, </br> Bit 3 - enable FFT acceleration publishing to RawImu.accelerometer_integral, </br> Bit 4 - enable FFT acceleration publishing to RawImu.rate_gyro_integral, </br> By default 15 that mean enable all publishers |
+| imu.pub_frequency       | Frequency of IMU publisher [Hz]. |
 
 > This docs was automatically generated. Do not edit it manually.