Arming Module

CMakeLists.txt

@@ -19,6 +19,9 @@ if(USE_PLATFORM_UBUNTU)
     set(LIBPARAMS_PLATFORM ubuntu)
     set(CAN_PLATFORM socketcan)
     set(APP_PLATFORM ubuntu)
+    add_definitions(-DLIBPARAMS_PARAMS_DIR="${BUILD_SRC_DIR}")
+    add_definitions(-DLIBPARAMS_INIT_PARAMS_FILE_NAME="default_params")
+    add_definitions(-DLIBPARAMS_TEMP_PARAMS_FILE_NAME="temp_params")
 elseif(USE_PLATFORM_NODE_V2)
     set(LIBPARAMS_PLATFORM stm32f103)
     set(CAN_PLATFORM bxcan)
@@ -45,7 +48,7 @@ list(APPEND LIBPARAMS_PARAMS
 )
 
 include(${LIBPARAMS_PATH}/CMakeLists.txt)
-add_definitions(-DFLASH_DRIVER_STORAGE_FILE="${APPLICATION_DIR}/default_params.yml")
+
 list(APPEND APPLICATION_HEADERS ${ROOT_DIR}/Src ${APPLICATION_DIR})
 include(${APPLICATION_DIR}/CMakeLists.txt)
 

Src/applications/dronecan/CMakeLists.txt

@@ -9,6 +9,7 @@ add_definitions(-DCONFIG_USE_DRONECAN=1)
 
 include(${SRC_DIR}/modules/dronecan/core/CMakeLists.txt)
 include(${SRC_DIR}/modules/circuit_status/dronecan/CMakeLists.txt)
+include(${SRC_DIR}/modules/dronecan/arming/CMakeLists.txt)
 include(${SRC_DIR}/modules/dronecan/pwm/CMakeLists.txt)
 
 if(USE_PLATFORM_NODE_V3)

Src/applications/dronecan/README.md

@@ -1,49 +1,36 @@
-The node has the following interface:
-
-Cyphal Publishers:
-| №  | Data type and topic name  | Description |
-| -- | ------------------------- | ----------- |
-|  1 | [uavcan.si.sample.voltage.Scalar](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/si/sample/voltage/Scalar.1.0.dsdl) </br> crct.5v | Voltage 5V (after DC-DC).</br>The normal values are within [4.5, 5.5] Volts.</br>|
-|  2 | [uavcan.si.sample.voltage.Scalar](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/si/sample/voltage/Scalar.1.0.dsdl) </br> crct.vin | Voltage Vin (before DC-DC).</br>It make sense only when you power up the node from Molex 6-pin connector.</br>The normal values are within [5.5, 55] Volts.</br>|
-|  3 | [uavcan.si.sample.temperature.Scalar](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/si/sample/temperature/Scalar.1.0.dsdl) </br> crct.temperature | STM32 internal temperature.</br>It it expected that the temperature is a little bit higher then your room environment.</br>|
-|  4 | [reg.udral.service.actuator.common.Feedback](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/common/Feedback.0.1.dsdl) </br> feedback | |
-
-Cyphal Subscribers:
-| №  | Data type and topic name  | Description |
-| -- | ------------------------- | ----------- |
-|  1 | [reg.udral.service.actuator.common.sp.Vector31](https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/common/sp/Vector31.0.1.dsdl) </br> setpoint | |
-
 The node has the following registers:
 
 | №  | Register name           | Description |
 | -- | ----------------------- | ----------- |
 |  1 | uavcan.node.id          | Defines a node-ID. Allowed values [0,127]. |
 |  2 | system.name             | Defines custom node name. If empty, the node will use the default name. |
 |  3 | system.protocol         | Auto, Cyphal/CAN or DroneCAN |
-|  4 | pwm.cmd_ttl_ms          | TTL of specified by pwm.cmd_type commands [ms]. |
-|  5 | pwm.frequency           | PWM frequency [Hz]. |
-|  6 | pwm.cmd_type            | 0 means RawCommand, 1 means ArrayCommand, 2 is reserved for hardpoint.Command. |
-|  7 | pwm.1_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
-|  8 | pwm.1_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
-|  9 | pwm.1_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
-|  10 | pwm.1_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
-|  11 | pwm.1_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. |
-|  12 | pwm.2_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
-|  13 | pwm.2_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
-|  14 | pwm.2_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
-|  15 | pwm.2_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
-|  16 | pwm.2_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. |
-|  17 | pwm.3_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
-|  18 | pwm.3_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
-|  19 | pwm.3_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
-|  20 | pwm.3_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
-|  21 | pwm.3_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. |
-|  22 | pwm.4_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
-|  23 | pwm.4_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
-|  24 | pwm.4_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
-|  25 | pwm.4_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
-|  26 | 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. |
-|  27 | imu.enable              | Enable or disable IMU publisher. |
+|  4 | uavcan.node.description | User/integrator-defined, human-readable description of this specific node. |
+|  5 | stats.engaged_time      | Total engaged time of node in seconds. |
+|  6 | pwm.cmd_ttl_ms          | TTL of specified by pwm.cmd_type commands [ms]. |
+|  7 | pwm.frequency           | PWM frequency [Hz]. |
+|  8 | pwm.cmd_type            | 0 means RawCommand, 1 means ArrayCommand, 2 is reserved for hardpoint.Command. |
+|  9 | pwm.1_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
+|  10 | pwm.1_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
+|  11 | pwm.1_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
+|  12 | pwm.1_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
+|  13 | pwm.1_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. |
+|  14 | pwm.2_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
+|  15 | pwm.2_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
+|  16 | pwm.2_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
+|  17 | pwm.2_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
+|  18 | pwm.2_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. |
+|  19 | pwm.3_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
+|  20 | pwm.3_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
+|  21 | pwm.3_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
+|  22 | pwm.3_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
+|  23 | pwm.3_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. |
+|  24 | pwm.4_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
+|  25 | pwm.4_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
+|  26 | pwm.4_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
+|  27 | pwm.4_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
+|  28 | 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. |
+|  29 | imu.enable              | Enable or disable IMU publisher. |
 
 > This docs was automatically generated. Do not edit it manually.
 

Src/common/module.cpp

@@ -13,7 +13,7 @@ Module::Module(float frequency, Protocol proto) : protocol(proto),
 }
 
 void Module::init() {
-    mode = Mode::OPERATIONAL;
+    mode = Mode::STANDBY;
 }
 
 Module::Status Module::get_health() const {
@@ -49,7 +49,7 @@ void ModuleManager::register_module(Module* app_module) {
         modules_amount++;
         active_modules = std::span<Module*>(modules.data(), modules_amount);
     }
-};
+}
 
 void ModuleManager::init(Module::Protocol proto) {
     protocol = proto;
@@ -58,15 +58,15 @@ void ModuleManager::init(Module::Protocol proto) {
             app_module->init();
         }
     }
-};
+}
 
 void ModuleManager::process() {
     for (auto app_module : active_modules) {
         if (app_module->get_protocol() == protocol) {
             app_module->process();
         }
     }
-};
+}
 
 Module::Status ModuleManager::get_global_status() {
     auto global_status = Module::Status::OK;
@@ -81,7 +81,7 @@ Module::Status ModuleManager::get_global_status() {
 }
 
 Module::Mode ModuleManager::get_global_mode() {
-    auto global_mode = Module::Mode::OPERATIONAL;
+    auto global_mode = Module::Mode::STANDBY;
 
     for (auto app_module : active_modules) {
         if (app_module->get_protocol() == protocol && app_module->get_mode() > global_mode) {
@@ -102,7 +102,7 @@ uint8_t ModuleManager::get_vssc() {
         }
 
         auto is_health_bad = app_module->get_health() > Module::Status::OK;
-        auto is_mode_not_operational = app_module->get_mode() > Module::Mode::OPERATIONAL;
+        auto is_mode_not_operational = app_module->get_mode() > Module::Mode::ENGAGED;
         if (is_health_bad || is_mode_not_operational) {
             vssc += 1 << module_idx;
         }

Src/common/module.hpp

@@ -7,9 +7,9 @@
 #ifndef SRC_COMMON_MODULE_HPP_
 #define SRC_COMMON_MODULE_HPP_
 
+#include <span>
 #include <cstdint>
 #include <array>
-#include <span>
 #include "main.h"
 
 
@@ -23,9 +23,10 @@ class Module {
     };
 
     enum class Mode: uint8_t {
-        OPERATIONAL         = 0,    // After successful initialization
-        INITIALIZATION      = 1,    // After startup
-        MAINTENANCE         = 2,    // Calibration, self-test
+        STANDBY             = 0,    // Low-power state, waiting further commands or events
+        ENGAGED             = 1,    // Fully active and operational
+        INITIALIZATION      = 2,    // After startup
+        MAINTENANCE         = 3,    // Calibration, self-test
     };
 
     enum class Protocol: uint8_t {
@@ -70,7 +71,7 @@ class Module {
      * @brief The function manages the parameters from the persistent storage
      * The function is called in process() when the parameters are updated.
      */
-    virtual void update_params() {};
+    virtual void update_params() {}
 
     /**
      * @brief The function is not blocking. It should be finished ASAP.

Src/modules/circuit_status/cyphal/circuit_status.cpp

@@ -16,7 +16,7 @@
 REGISTER_MODULE(CyphalCircuitStatus)
 
 void CyphalCircuitStatus::init() {
-    mode = Mode::OPERATIONAL;
+    mode = Mode::STANDBY;
 }
 
 void CyphalCircuitStatus::update_params() {

Src/modules/circuit_status/dronecan/circuit_status.cpp

@@ -11,7 +11,7 @@
 REGISTER_MODULE(DronecanCircuitStatus)
 
 void DronecanCircuitStatus::init() {
-    mode = Module::Mode::OPERATIONAL;
+    mode = Module::Mode::STANDBY;
 }
 
 void DronecanCircuitStatus::spin_once() {

Src/modules/cyphal/core/cyphal_module.cpp

@@ -28,12 +28,16 @@ void CyphalModule::init() {
     cyphal::NodeGetInfoSubscriber::setHardwareVersion(2, 1);
 
     health = (res >= 0) ? Status::OK : Status::FATAL_MALFANCTION;
-    mode = Mode::OPERATIONAL;
+    mode = Mode::STANDBY;
 }
 
 void CyphalModule::spin_once() {
     cyphal.setNodeHealth(uavcan_node_Health_1_0{(uint8_t)(ModuleManager::get_global_status())});
-    cyphal.setNodeMode(uavcan_node_Mode_1_0{(uint8_t)(ModuleManager::get_global_mode())});
+    auto global_mode = (uint8_t)(ModuleManager::get_global_mode());
+    if (global_mode > 0 ) {
+        global_mode -= 1;
+    }
+    cyphal.setNodeMode(uavcan_node_Mode_1_0{global_mode});
     cyphal.setVSSC(ModuleManager::get_vssc());
     cyphal.process();
 }

Src/modules/cyphal/feedback/feedback.cpp

@@ -29,7 +29,6 @@ void FeedbackModule::update_params() {
 void FeedbackModule::spin_once() {
     pub.msg.heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
     pub.msg.heartbeat.readiness.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
-
     uint32_t pwm_ccr_reg_value = HAL::Pwm::get_duration(HAL::PwmPin::PWM_1);
     uint32_t pwm_duration = std::clamp(pwm_ccr_reg_value, (uint32_t)1000, (uint32_t)2000);
     pub.msg.demand_factor_pct = static_cast<int8_t>((pwm_duration - 1000) / 10);

Src/modules/cyphal/setpoint/setpoint.cpp

@@ -44,7 +44,7 @@ void SetpointModule::init() {
     HAL::Pwm::init(HAL::PwmPin::PWM_4);
 
     health = (sub.init() < 0) ? Status::FATAL_MALFANCTION : Status::OK;
-    mode = Mode::OPERATIONAL;
+    mode = Mode::STANDBY;
 }
 
 void SetpointModule::spin_once() {

Src/modules/dronecan/arming/CMakeLists.txt

@@ -0,0 +1,17 @@
+# Copyright (C) 2023 Dmitry Ponomarev <[email protected]>
+# Distributed under the terms of the GPL v3 license, available in the file LICENSE.
+
+
+list(APPEND APPLICATION_HEADERS
+    ${libparamsHeaders}
+    ${DRONECAN_HEADERS}
+    ${CMAKE_CURRENT_LIST_DIR}
+)
+
+list(APPEND APPLICATION_SOURCES
+    ${CMAKE_CURRENT_LIST_DIR}/arming.cpp
+)
+
+list(APPEND LIBPARAMS_PARAMS
+    ${CMAKE_CURRENT_LIST_DIR}/params.yaml
+)

Src/modules/dronecan/arming/arming.cpp

@@ -0,0 +1,40 @@
+/**
+ * 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 "arming.hpp"
+#include <storage.h>
+#include "can_driver.h"
+#include "common/algorithms.hpp"
+#include <params.hpp>
+#include <logger.hpp>
+
+Logger logger = Logger("ArmingModule");
+
+
+REGISTER_MODULE(ArmingModule)
+
+void ArmingModule::init() {
+    health = Status::OK;
+    prev_eng_time = paramsGetIntegerValue(IntParamsIndexes::PARAM_STATS_ENG_TIME);
+    mode = Module::Mode::STANDBY;
+}
+
+void ArmingModule::spin_once() {
+    global_mode = ModuleManager::get_global_mode();
+    auto crnt_time_ms = HAL_GetTick();
+    if (global_mode == Mode::ENGAGED && !is_armed) {
+        is_armed = true;
+        arm_start_time = crnt_time_ms;
+        return;
+    }
+    if (is_armed && global_mode != Mode::ENGAGED && arm_start_time + 1000 < crnt_time_ms) {
+        is_armed = false;
+        auto cur_eng_time = (crnt_time_ms - arm_start_time) / 1000;
+        prev_eng_time = cur_eng_time + prev_eng_time;
+        paramsSetIntegerValue(IntParamsIndexes::PARAM_STATS_ENG_TIME, int(prev_eng_time));
+        paramsSave();
+    }
+}

Src/modules/dronecan/arming/arming.hpp

@@ -0,0 +1,36 @@
+/**
+ * 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_ARMING_HPP_
+#define SRC_MODULES_ARMING_HPP_
+
+#include "module.hpp"
+#include "subscriber.hpp"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+class ArmingModule : public Module {
+public:
+    ArmingModule() : Module(2, Protocol::DRONECAN) {}
+    void init() override;
+
+protected:
+    void spin_once() override;
+
+private:
+    Mode global_mode;
+    bool is_armed = {false};
+    uint32_t arm_start_time = 0;
+    uint32_t prev_eng_time = 0;
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // SRC_MODULES_ARMING_HPP_

Src/modules/dronecan/arming/params.yaml

@@ -0,0 +1,8 @@
+stats.engaged_time:
+  type: Integer
+  note: Total engaged time of node in seconds.
+  enum: PARAM_STATS_ENG_TIME
+  flags: mutable
+  default: 0
+  min: 0
+  max: 2147483647

Src/modules/dronecan/canopen/canopen.cpp

@@ -21,7 +21,7 @@ void CanopenModule::init() {
         health = Status::FATAL_MALFANCTION;
     }
 
-    mode = Module::Mode::OPERATIONAL;
+    mode = Module::Mode::STANDBY;
 }
 
 void CanopenModule::spin_once() {

Src/modules/dronecan/core/dronecan_module.cpp

@@ -26,12 +26,16 @@ void DronecanModule::init() {
     int8_t res = uavcanInitApplication(node_id);
 
     health = (res >= 0) ? Status::OK : Status::FATAL_MALFANCTION;
-    mode = Mode::OPERATIONAL;
+    mode = Mode::STANDBY;
 }
 
 void DronecanModule::spin_once() {
     uavcanSetNodeHealth(static_cast<NodeStatusHealth_t>(ModuleManager::get_global_status()));
-    uavcanSetNodeStatusMode(static_cast<NodeStatusMode_t>(ModuleManager::get_global_mode()));
+    auto global_mode = (uint8_t)(ModuleManager::get_global_mode());
+    if (global_mode > 0 ) {
+        global_mode -= 1;
+    }
+    uavcanSetNodeStatusMode(static_cast<NodeStatusMode_t>(global_mode));
     uavcanSetVendorSpecificStatusCode(ModuleManager::get_vssc());
     uavcanSpinOnce();
 }

Src/modules/dronecan/core/logger.cpp

@@ -33,7 +33,7 @@ void Logger::publish() {
     _transfer_id++;
 }
 
-#if NDEBUG
+#ifdef NDEBUG
 void Logger::log_debug(const char* text) {
     (void)text;
     // Do nothing if _DEBUG is not defined