add dronecan pmw module

.github/workflows/dronecan.yml

@@ -50,4 +50,4 @@ jobs:
         run: make sitl_dronecan
 
       - name: Run SITL for 5 seconds
-        run: timeout 5s make run || res=$?; if [[ $res -ne 124 && $res -ne 0 ]]; then exit $res; fi
+        run: timeout 5s make run || res=$?; if [[ $res -ne 124 && $res -ne 0 ]]; then exit $res; fi

.github/workflows/sonarcloud.yml

@@ -34,4 +34,4 @@ jobs:
       - name: Run sonar-scanner
         env:
           SONAR_TOKEN: ${{ secrets.SONAR_TOKEN }}
-        run: sonar-scanner
+        run: sonar-scanner

Src/dronecan_application/CMakeLists.txt

@@ -17,7 +17,9 @@ set(applicationSourceCode
     ${DRONECAN_SOURCES}
     ${ROOT_DIR}/Src/dronecan_application/application.cpp
     ${ROOT_DIR}/Src/dronecan_application/modules/CircuitStatusModule.cpp
+    ${ROOT_DIR}/Src/dronecan_application/modules/PWMModule.cpp
     ${ROOT_DIR}/Src/dronecan_application/logger.cpp
+    ${ROOT_DIR}/Src/dronecan_application/algorithms.cpp
 
 )
 set(applicationHeaders

Src/dronecan_application/README.md

@@ -4,6 +4,29 @@ The node has the following registers:
 | -- | ----------------------- | ----------- |
 |  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 | pwm.cmd_ttl_ms          | TTL of specified by pwm.cmd_type commands [ms]. |
+|  4 | pwm.frequency           | PWM frequency [Hz]. |
+|  5 | pwm.cmd_type            | 0 means RawCommand, 1 means ArrayCommand, 2 is reserved for hardpoint.Command. |
+|  6 | pwm.1_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
+|  7 | pwm.1_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
+|  8 | pwm.1_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
+|  9 | pwm.1_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
+|  10 | 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. |
+|  11 | pwm.2_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
+|  12 | pwm.2_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
+|  13 | pwm.2_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
+|  14 | pwm.2_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
+|  15 | 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. |
+|  16 | pwm.3_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
+|  17 | pwm.3_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
+|  18 | pwm.3_max               | PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) |
+|  19 | pwm.3_def               | PWM duration when setpoint is negative or there is no setpoint at all. |
+|  20 | 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. |
+|  21 | pwm.4_ch                | Index of setpoint channel. [-1; 255]. -1 means disabled, |
+|  22 | pwm.4_min               | PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) |
+|  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. |
 
 > This docs was automatically generated. Do not edit it manually.
 

Src/dronecan_application/algorithms.cpp

@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2022-2023 Dmitry Ponomarev <[email protected]>
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+
+#include "algorithms.hpp"
+
+
+static const RawCommand RAW_COMMAND_MIN = 0;
+static const RawCommand RAW_COMMAND_MAX = 8191;
+
+
+static const ActuatorCommandValue ACT_COMMAND_MIN = -1.0f;
+static const ActuatorCommandValue ACT_COMMAND_MAX = 1.0f;
+
+PwmDurationUs mapRawCommandToPwm(RawCommand command,
+                                 PwmDurationUs min_pwm,
+                                 PwmDurationUs max_pwm,
+                                 PwmDurationUs def_pwm) {
+    PwmDurationUs pwm;
+    if (command < RAW_COMMAND_MIN || command > RAW_COMMAND_MAX) {
+        pwm = def_pwm;
+    } else {
+        pwm = (PwmDurationUs)mapFloat(command, RAW_COMMAND_MIN, RAW_COMMAND_MAX, min_pwm, max_pwm);
+    }
+    return pwm;
+}
+
+
+PwmDurationUs mapActuatorCommandToPwm(ActuatorCommandValue command,
+                                 PwmDurationUs min_pwm,
+                                 PwmDurationUs max_pwm,
+                                 PwmDurationUs def_pwm) {
+    PwmDurationUs pwm;
+    if (command < ACT_COMMAND_MIN || command > ACT_COMMAND_MAX) {
+        pwm = def_pwm;
+    } else {
+        pwm = (PwmDurationUs)mapFloat(command, ACT_COMMAND_MIN, ACT_COMMAND_MAX, min_pwm, max_pwm);
+    }
+    return pwm;
+}
+
+float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max) {
+    auto max = pwm_max;
+    auto min = pwm_min;
+    if (pwm_min > pwm_max) {
+        max = pwm_min;
+        min = pwm_max;
+    }
+    auto scaled_val = (pwm_val - min) * 100.0f / (max - min);
+    auto val = std::clamp(scaled_val, 0.f, 100.f);
+    return val;
+}
+
+float mapFloat(float value, float in_min, float in_max, float out_min, float out_max) {
+    float output;
+    if (value <= in_min && in_min <= in_max) {
+        output = out_min;
+    } else if (value >= in_max && in_min <= in_max) {
+        output = out_max;
+    } else if (out_min == out_max) {
+        output = out_min;
+    } else {
+        output = out_min + (value - in_min) / (in_max - in_min) * (out_max - out_min);
+        if (out_min <= out_max) {
+            output = std::clamp(output, out_min, out_max);
+        } else {
+            output = std::clamp(output, out_max, out_min);
+        }
+    }
+    return output;
+}

Src/dronecan_application/algorithms.hpp

@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2022-2023 Dmitry Ponomarev <[email protected]>
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+
+#ifndef DEVICES_SERVOS_COMMON_H_
+#define DEVICES_SERVOS_COMMON_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <algorithm>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef uint16_t PwmDurationUs;
+typedef int16_t RawCommand;
+typedef float ActuatorCommandValue;
+
+/**
+ * @brief  Map raw command value (in interval from 0 to 8191)
+ *         to PWM duration in us (in interval from min to max)
+ * @return pwm_duration if input is correct,
+ *         def_pwm if raw_command value is less than min or higher than max
+ */
+
+PwmDurationUs mapRawCommandToPwm(RawCommand rc_value, PwmDurationUs min_pwm,
+                                 PwmDurationUs max_pwm, PwmDurationUs def_pwm);
+
+/**
+ * @brief  Map array command value (in interval from -1.0 to 1.0)
+ *         to PWM duration in us (in interval from min to max)
+ * @return pwm_duration if input is correct,
+ *         def_pwm if raw_command value is less than min or higher than max
+ */
+
+PwmDurationUs mapActuatorCommandToPwm(ActuatorCommandValue ac_value,
+                                   PwmDurationUs min_pwm, PwmDurationUs max_pwm,
+                                   PwmDurationUs def_pwm);
+
+/**
+ * @brief  Map PWM duration in us (in interval from min to max) to pct
+ * @return pwm_duration in pct
+ */
+
+float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max);
+
+float mapFloat(float value, float in_min, float in_max, float out_min,
+               float out_max);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // DEVICES_SERVOS_COMMON_H_

Src/dronecan_application/application.cpp

@@ -10,6 +10,7 @@
 #include "params.hpp"
 #include "periphery/led/led.hpp"
 #include "periphery/iwdg/iwdg.hpp"
+#include "modules/PWMModule.hpp"
 #include "modules/CircuitStatusModule.hpp"
 
 
@@ -29,14 +30,18 @@ void application_entry_point() {
     uavcanInitApplication(node_id);
 
     CircuitStatusModule& status_module = CircuitStatusModule::get_instance();
+    PWMModule& pwm_module = PWMModule::get_instance();
     LedColor color = LedColor::BLUE_COLOR;
 
     if (!status_module.instance_initialized) {
         color = LedColor::RED_COLOR;
     }
+
     while(true) {
         LedPeriphery::toggle(color);
         status_module.spin_once();
+        pwm_module.spin_once();
+        uavcanSetNodeHealth((NodeStatusHealth_t) pwm_module.module_status);
         uavcanSpinOnce();
 
         WatchdogPeriphery::refresh();