add parameter to enable/disable imu and improve led

Src/applications/dronecan/README.md

@@ -27,6 +27,7 @@ The node has the following registers:
 |  23 | pwm.4_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
 |  24 | pwm.4_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
 |  25 | pwm.4_feedback          | Indicates the operational mode of the node. 0 means disabled. When set to 1, the command of corresponding Status type for cmd_type will be transmitted (esc.RawCommand - esc.Status, actuator.ArrayCommand - actuator.Status) with frequency 1 Hz. When set to 2 - 10 Hz. |
+|  26 | imu.enable              | Enable or disable IMU publisher. |
 
 > This docs was automatically generated. Do not edit it manually.
 

Src/common/application.cpp

@@ -37,8 +37,9 @@ static int8_t init_board_periphery() {
  * 1. [Blinking Blue/Red]   Initialization
  * 2. [Blinking Red/Blue]   Maintenance
  * 3. [Blinking Blue]       Good
- * 4. [Blinking yellow]     Warning
- * 5. [Blinking red]        Error
+ * 4. [Blinking yellow]     Minor failure
+ * 5. [Blinking magenta]    Major failure
+ * 6. [Blinking red]        Fatal malfanction
  * @note https://docs.raccoonlab.co/guide/intro/leds.html
  */
 static void blink_board_led() {
@@ -50,8 +51,10 @@ static void blink_board_led() {
         colors = {Board::Led::Color::RED, Board::Led::Color::BLUE};
     } else if (mode == Module::Mode::MAINTENANCE) {
         colors = {Board::Led::Color::RED, Board::Led::Color::BLUE};
-    } else if (status >= Module::Status::MAJOR_FAILURE) {
+    } else if (status >= Module::Status::FATAL_MALFANCTION) {
         colors = {Board::Led::Color::RED, Board::Led::Color::BLACK};
+    } else if (status >= Module::Status::MAJOR_FAILURE) {
+        colors = {Board::Led::Color::MAGENTA, Board::Led::Color::BLACK};
     } else if (status >= Module::Status::MINOR_FAILURE) {
         colors = {Board::Led::Color::YELLOW, Board::Led::Color::BLACK};
     } else {

Src/modules/imu/CMakeLists.txt

@@ -8,4 +8,8 @@ list(APPEND APPLICATION_SOURCES
     ${CMAKE_CURRENT_LIST_DIR}/imu.cpp
 )
 
+list(APPEND LIBPARAMS_PARAMS
+    ${CMAKE_CURRENT_LIST_DIR}/params.yaml
+)
+
 include(${SRC_DIR}/drivers/mpu9250/CMakeLists.txt)

Src/modules/imu/imu.cpp

@@ -5,23 +5,29 @@
  */
 
 #include "imu.hpp"
+#include "params.hpp"
 
 REGISTER_MODULE(ImuModule)
 
 void ImuModule::init() {
-    auto res = imu.initialize();
-    health = res ? Module::Status::OK : Module::Status::FATAL_MALFANCTION;
+    initialized = imu.initialize();
     mode = Module::Mode::OPERATIONAL;
 }
 
+void ImuModule::update_params() {
+    enabled = static_cast<bool>(paramsGetIntegerValue(PARAM_IMU_ENABLE));
+    health = (!enabled || initialized) ? Module::Status::OK : Module::Status::MAJOR_FAILURE;
+}
+
 void ImuModule::spin_once() {
-    if (health == Module::Status::FATAL_MALFANCTION) {
+    if (!enabled || !initialized) {
         return;
     }
 
     std::array<int16_t, 3> mag_raw;
-    imu.read_magnetometer(&mag_raw);
-    mag.publish();  // publish anyway
+    if (imu.read_magnetometer(&mag_raw) >= 0) {
+        mag.publish();
+    }
 
     bool updated{false};
 

Src/modules/imu/imu.hpp

@@ -23,11 +23,14 @@ class ImuModule : public Module {
 
 protected:
     void spin_once() override;
+    void update_params() override;
 
 private:
     DronecanPublisher<AhrsRawImu> pub;
     DronecanPublisher<MagneticFieldStrength2> mag;
     Mpu9250 imu;
+    bool initialized{false};
+    bool enabled{false};
 
     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;

Src/modules/imu/params.yaml

@@ -0,0 +1,8 @@
+imu.enable:
+  type: Integer
+  note: Enable or disable IMU publisher.
+  enum: PARAM_IMU_ENABLE
+  flags: mutable
+  default: 0
+  min: 0
+  max: 1

Src/periphery/spi/spi.hpp

@@ -33,6 +33,8 @@ class SPI {
      */
     static int8_t read_registers(std::byte reg_address, std::byte* reg_values, uint8_t size);
 
+    static int8_t write_register(std::byte reg_address, std::byte reg_value);
+
     /**
      * @brief This function automatically controls the NSS pin
      * @return 0 on success, negative error otherwise

Src/platform/stm32g0b1/spi.cpp

@@ -5,6 +5,7 @@
  */
 
 #include "periphery/spi/spi.hpp"
+#include <array>
 #include <cstring>
 #include "main.h"
 
@@ -41,6 +42,13 @@ int8_t SPI::read_register(std::byte reg_address, std::byte* reg_value) {
     return HAL::SPI::transaction(&tx_byte, &reg_value[-1], 2);
 }
 
+int8_t SPI::write_register(std::byte reg_address, std::byte reg_value) {
+    HAL_Delay(10);
+    std::array<std::byte, 2> tx_buffer = {reg_address, reg_value};
+    std::array<std::byte, 2> rx_buffer = {std::byte{0}, std::byte{0}};
+    return HAL::SPI::transaction(tx_buffer.data(), rx_buffer.data(), tx_buffer.size());
+}
+
 int8_t SPI::transaction(std::byte* tx, std::byte* rx, uint8_t size) {
     if (tx == nullptr || rx == nullptr) {
         return -1;