From 651d47514372b2d0ef1c4bfd6cb8a2367df13b98 Mon Sep 17 00:00:00 2001 From: Anastasiia Stepanova Date: Sun, 5 May 2024 23:08:44 +0300 Subject: [PATCH] Pr new pmw module (#24) --- .github/workflows/dronecan.yml | 2 +- .github/workflows/sonarcloud.yml | 2 +- Src/dronecan_application/CMakeLists.txt | 2 + Src/dronecan_application/README.md | 23 ++ Src/dronecan_application/algorithms.cpp | 74 ++++++ Src/dronecan_application/algorithms.hpp | 59 ++++ Src/dronecan_application/application.cpp | 5 + .../modules/PWMModule.cpp | 251 ++++++++++++++++++ .../modules/PWMModule.hpp | 87 ++++++ Src/dronecan_application/params.yaml | 207 +++++++++++++++ Src/periphery/pwm/pwm.hpp | 10 + Src/platform/stm32f103/pwm.cpp | 22 ++ Src/platform/ubuntu/pwm.cpp | 17 +- 13 files changed, 755 insertions(+), 6 deletions(-) create mode 100644 Src/dronecan_application/algorithms.cpp create mode 100644 Src/dronecan_application/algorithms.hpp create mode 100644 Src/dronecan_application/modules/PWMModule.cpp create mode 100644 Src/dronecan_application/modules/PWMModule.hpp diff --git a/.github/workflows/dronecan.yml b/.github/workflows/dronecan.yml index e52769f..3fde83a 100644 --- a/.github/workflows/dronecan.yml +++ b/.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 \ No newline at end of file diff --git a/.github/workflows/sonarcloud.yml b/.github/workflows/sonarcloud.yml index 975b5cc..a51f6b5 100644 --- a/.github/workflows/sonarcloud.yml +++ b/.github/workflows/sonarcloud.yml @@ -31,4 +31,4 @@ jobs: - name: Run sonar-scanner env: SONAR_TOKEN: ${{ secrets.SONAR_TOKEN }} - run: sonar-scanner + run: sonar-scanner \ No newline at end of file diff --git a/Src/dronecan_application/CMakeLists.txt b/Src/dronecan_application/CMakeLists.txt index 9bd64a1..d9a7fa2 100644 --- a/Src/dronecan_application/CMakeLists.txt +++ b/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 diff --git a/Src/dronecan_application/README.md b/Src/dronecan_application/README.md index 5bfee0d..f605f3b 100644 --- a/Src/dronecan_application/README.md +++ b/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. diff --git a/Src/dronecan_application/algorithms.cpp b/Src/dronecan_application/algorithms.cpp new file mode 100644 index 0000000..8f0ab9e --- /dev/null +++ b/Src/dronecan_application/algorithms.cpp @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2022-2023 Dmitry Ponomarev + * 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; +} diff --git a/Src/dronecan_application/algorithms.hpp b/Src/dronecan_application/algorithms.hpp new file mode 100644 index 0000000..a86df04 --- /dev/null +++ b/Src/dronecan_application/algorithms.hpp @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2022-2023 Dmitry Ponomarev + * 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 +#include + +#include + +#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_ diff --git a/Src/dronecan_application/application.cpp b/Src/dronecan_application/application.cpp index 1416a39..78938e9 100644 --- a/Src/dronecan_application/application.cpp +++ b/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(); diff --git a/Src/dronecan_application/modules/PWMModule.cpp b/Src/dronecan_application/modules/PWMModule.cpp new file mode 100644 index 0000000..851de07 --- /dev/null +++ b/Src/dronecan_application/modules/PWMModule.cpp @@ -0,0 +1,251 @@ +#include "PWMModule.hpp" + +#define CHANNEL(channel) IntParamsIndexes::PARAM_PWM_##channel##_CH +#define MIN(channel) IntParamsIndexes::PARAM_PWM_##channel##_MIN +#define MAX(channel) IntParamsIndexes::PARAM_PWM_##channel##_MAX +#define DEF(channel) IntParamsIndexes::PARAM_PWM_##channel##_DEF +#define FB(channel) IntParamsIndexes::PARAM_PWM_##channel##_FB + +Logger PWMModule::logger = Logger("PWMModule"); + +uint16_t PWMModule::ttl_cmd = 0; +uint16_t PWMModule::pwm_freq = 1000; +uint8_t PWMModule::pwm_cmd_type = 0; + +ModuleStatus PWMModule::module_status = ModuleStatus::MODULE_OK; + +PWMModule PWMModule::instance = PWMModule(); + +PWMModule::PWMModule() { + update_params(); + init(); +} + +PwmChannelInfo PWMModule::params[static_cast(PwmPin::PWM_AMOUNT)] = { + {.pin = PwmPin::PWM_1}, // PWM1 + {.pin = PwmPin::PWM_2}, // PWM2 + {.pin = PwmPin::PWM_3}, // PWM3 + {.pin = PwmPin::PWM_4}, // PWM4 +}; + +PwmChannelsParamsNames + PWMModule::params_names[static_cast(PwmPin::PWM_AMOUNT)] = { + {.min = MIN(1), .max = MAX(1), .def = DEF(1), .ch = CHANNEL(1), .fb = FB(1)}, // PWM1 + {.min = MIN(2), .max = MAX(2), .def = DEF(2), .ch = CHANNEL(2), .fb = FB(2)}, // PWM2 + {.min = MIN(3), .max = MAX(3), .def = DEF(3), .ch = CHANNEL(3), .fb = FB(3)}, // PWM3 + {.min = MIN(4), .max = MAX(4), .def = DEF(4), .ch = CHANNEL(4), .fb = FB(4)}, // PWM4 +}; + +PWMModule& PWMModule::get_instance() { + static bool instance_initialized = false; + if (!instance_initialized) { + instance_initialized = true; + instance.init(); + } + return instance; +} + +void PWMModule::init() { + logger.init("PWMModule"); + for (int i = 0; i < static_cast(PwmPin::PWM_AMOUNT); i++) { + PwmPeriphery::init(params[i].pin); + } + uavcanSubscribe(UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID, + raw_command_callback); + uavcanSubscribe(UAVCAN_EQUIPMENT_ACTUATOR_ARRAY_COMMAND_SIGNATURE, + UAVCAN_EQUIPMENT_ACTUATOR_ARRAY_COMMAND_ID, + array_command_callback); +} + +void PWMModule::spin_once() { + uint32_t crnt_time_ms = HAL_GetTick(); + + static uint32_t next_update_ms = 0; + if (crnt_time_ms > next_update_ms) { + next_update_ms = crnt_time_ms + 1000; + instance.update_params(); + instance.apply_params(); + } + + for (int i = 0; i < static_cast(PwmPin::PWM_AMOUNT); i++) { + auto pwm = params[i]; + if (crnt_time_ms > pwm.cmd_end_time_ms) { + pwm.command_val = pwm.def; + } + PwmPeriphery::set_duration(pwm.pin, pwm.command_val); + } + + status_pub_timeout_ms = 1; + static uint32_t next_pub_ms = 5000; + + if (module_status == ModuleStatus::MODULE_OK && + crnt_time_ms > next_pub_ms) { + publish_state(); + next_pub_ms = crnt_time_ms + status_pub_timeout_ms; + } +} + +void PWMModule::update_params() { + module_status = ModuleStatus::MODULE_OK; + + pwm_freq = paramsGetIntegerValue(IntParamsIndexes::PARAM_PWM_FREQUENCY); + pwm_cmd_type = paramsGetIntegerValue(IntParamsIndexes::PARAM_PWM_CMD_TYPE); + + ttl_cmd = paramsGetIntegerValue(IntParamsIndexes::PARAM_PWM_CMD_TTL_MS); + status_pub_timeout_ms = 100; + uint8_t max_channel = 0; + + bool params_error = false; + switch (pwm_cmd_type) { + case 0: + max_channel = NUMBER_OF_RAW_CMD_CHANNELS - 1; + break; + case 1: + max_channel = 255; + break; + default: + params_error = true; + break; + } + + static uint32_t last_warn_pub_time_ms = 0; + for (int i = 0; i < static_cast(PwmPin::PWM_AMOUNT); i++) { + params[i].fb = paramsGetIntegerValue(params_names[i].fb); + auto channel = paramsGetIntegerValue(params_names[i].ch); + if (channel < max_channel) { + params[i].channel = channel; + } else { + params[i].channel = max_channel; + params_error = true; + } + + auto min = paramsGetIntegerValue(params_names[i].min); + auto max = paramsGetIntegerValue(params_names[i].max); + auto def = paramsGetIntegerValue(params_names[i].def); + params[i].def = def; + params[i].min = min; + params[i].max = max; + } + + if (params_error) { + module_status = ModuleStatus::MODULE_WARN; + if (last_warn_pub_time_ms < HAL_GetTick()) { + last_warn_pub_time_ms = HAL_GetTick() + 10000; + logger.log_debug("check parameters"); + } + } +} + +void PWMModule::apply_params() { + for (int i = 0; i < static_cast(PwmPin::PWM_AMOUNT); i++) { + if (PwmPeriphery::get_frequency(params[i].pin) != pwm_freq) { + PwmPeriphery::set_frequency(params[i].pin, pwm_freq); + } + switch (pwm_cmd_type) { + case 0: + publish_state = publish_esc_status; + break; + + case 1: + publish_state = publish_actuator_status; + break; + + default: + return; + } + } +} + +void PWMModule::publish_esc_status() { + static uint8_t transfer_id = 0; + EscStatus_t msg{}; + auto crnt_time_ms = HAL_GetTick(); + static uint32_t + next_status_pub_ms[static_cast(PwmPin::PWM_AMOUNT)]; + for (int i = 0; i < static_cast(PwmPin::PWM_AMOUNT); i++) { + auto pwm = params[i]; + if (pwm.channel < 0 || pwm.fb == 0 || + next_status_pub_ms[i] > crnt_time_ms) { + continue; + } + msg = {}; + msg.esc_index = pwm.channel; + auto pwm_val = PwmPeriphery::get_duration(pwm.pin); + auto scaled_value = mapPwmToPct(pwm_val, pwm.min, pwm.max); + msg.power_rating_pct = (uint8_t)(scaled_value); + if (dronecan_equipment_esc_status_publish(&msg, &transfer_id) == 0) { + transfer_id++; + next_status_pub_ms[i] = crnt_time_ms + ((pwm.fb > 1) ? 100 : 1000); + } + } +} + +void PWMModule::publish_actuator_status() { + static uint8_t transfer_id = 0; + ActuatorStatus_t msg {}; + auto crnt_time_ms = HAL_GetTick(); + static uint32_t + next_status_pub_ms[static_cast(PwmPin::PWM_AMOUNT)]; + for (int i =0; i < static_cast(PwmPin::PWM_AMOUNT); i++) { + auto pwm = params[i]; + if (pwm.channel < 0 || pwm.fb == 0 || + next_status_pub_ms[i] > crnt_time_ms) { + continue; + } + msg.actuator_id = pwm.channel; + auto pwm_val = PwmPeriphery::get_duration(pwm.pin); + auto scaled_value = mapPwmToPct(pwm_val, pwm.min, pwm.max); + msg.power_rating_pct = (uint8_t) scaled_value; + if (dronecan_equipment_actuator_status_publish(&msg, &transfer_id) == 0) { + transfer_id++; + } + next_status_pub_ms[i] = crnt_time_ms + ((pwm.fb > 1) ? 100 : 1000); + } +} + +void PWMModule::raw_command_callback(CanardRxTransfer* transfer) { + if (module_status != ModuleStatus::MODULE_OK || pwm_cmd_type != 0) return; + RawCommand_t command; + int8_t ch_num = + dronecan_equipment_esc_raw_command_deserialize(transfer, &command); + if (ch_num <= 0) { + return; + } + for (int i = 0; i < static_cast(PwmPin::PWM_AMOUNT); i++) { + auto pwm = ¶ms[i]; + if (pwm->channel < 0) { + continue; + } + if (command.raw_cmd[pwm->channel] >= 0) { + pwm->cmd_end_time_ms = HAL_GetTick() + ttl_cmd; + pwm->command_val = mapRawCommandToPwm(command.raw_cmd[pwm->channel], + pwm->min, pwm->max, pwm->def); + } else { + pwm->command_val = pwm->def; + } + } +} + +void PWMModule::array_command_callback(CanardRxTransfer* transfer) { + if (module_status != ModuleStatus::MODULE_OK || pwm_cmd_type != 1) return; + ArrayCommand_t command; + int8_t ch_num = dronecan_equipment_actuator_arraycommand_deserialize( + transfer, &command); + if (ch_num <= 0) { + return; + } + for (int i = 0; i < static_cast(PwmPin::PWM_AMOUNT); i++) { + auto pwm = ¶ms[i]; + if (pwm->channel < 0) { + continue; + } + for (uint8_t j = 0; j < ch_num; j++) { + if (command.commads[j].actuator_id != pwm->channel) { + continue; + } + pwm->cmd_end_time_ms = HAL_GetTick() + ttl_cmd; + pwm->command_val = mapActuatorCommandToPwm(command.commads[j].command_value, + pwm->min, pwm->max, pwm->def); + } + } +} diff --git a/Src/dronecan_application/modules/PWMModule.hpp b/Src/dronecan_application/modules/PWMModule.hpp new file mode 100644 index 0000000..2b448c1 --- /dev/null +++ b/Src/dronecan_application/modules/PWMModule.hpp @@ -0,0 +1,87 @@ +#ifndef SRC_MODULE_PWMMODULE_HPP_ +#define SRC_MODULE_PWMMODULE_HPP_ + +#include +#include "../algorithms.hpp" +#include "dronecan.h" +#include "params.hpp" + +#include "../logger.hpp" +#include "periphery/pwm/pwm.hpp" +#include "uavcan/equipment/indication/LightsCommand.h" +#include "uavcan/equipment/esc/RawCommand.h" +#include "uavcan/equipment/esc/Status.h" +#include "uavcan/equipment/actuator/ArrayCommand.h" +#include "uavcan/equipment/actuator/Status.h" + +enum class ModuleStatus: uint8_t { + MODULE_OK = 0, // The module is functioning properly + MODULE_WARN = 1, // The module encountered a minor failure + MODULE_ERROR = 2, // The module encountered a major failure + MODULE_CRITICAL = 3, // The module suffered a fatal malfunction +}; + +struct PwmChannelInfo { + PwmPin pin; + uint16_t min; + uint16_t max; + uint16_t def; + int16_t channel; + uint16_t command_val; + uint32_t cmd_end_time_ms; + uint32_t next_status_pub_ms; + uint8_t fb; +}; + +struct PwmChannelsParamsNames { + IntegerParamValue_t min; + IntegerParamValue_t max; + IntegerParamValue_t def; + IntegerParamValue_t ch; + IntegerParamValue_t fb; +}; + +class PWMModule { +public: + void spin_once(); + static PWMModule &get_instance(); + static PwmChannelInfo params[static_cast(PwmPin::PWM_AMOUNT)]; + static PwmChannelsParamsNames params_names[static_cast(PwmPin::PWM_AMOUNT)]; + static ModuleStatus module_status; + +protected: + PWMModule(); + +private: + static PWMModule instance; + void (*callback)(CanardRxTransfer*); + void (*publish_state)(); + + void init(); + void update_params(); + void update_pwm(); + void apply_params(); + + static void raw_command_callback(CanardRxTransfer* transfer); + static void array_command_callback(CanardRxTransfer* transfer); + + static void publish_esc_status(); + static void publish_actuator_status(); + static void publish_raw_command(); + static void publish_array_command(); + + static uint16_t pwm_freq; + static uint8_t pwm_cmd_type; + + static uint16_t ttl_cmd; + uint16_t status_pub_timeout_ms; + bool verbose; + + static bool publish_error; + static Logger logger; + + PWMModule& operator = (const PWMModule&) = delete; + explicit PWMModule(PWMModule *other) = delete; +}; + +#endif // SRC_MODULE_PWMMODULE_HPP_ diff --git a/Src/dronecan_application/params.yaml b/Src/dronecan_application/params.yaml index 7d8bc0f..09e51c3 100644 --- a/Src/dronecan_application/params.yaml +++ b/Src/dronecan_application/params.yaml @@ -13,3 +13,210 @@ system.name: enum: PARAM_SYSTEM_NAME flags: mutable default: "co.raccoonlab.mini" + +pwm.cmd_ttl_ms: + type: Integer + note: TTL of specified by pwm.cmd_type commands [ms]. + enum: PARAM_PWM_CMD_TTL_MS + flags: mutable + default: 500 + min: 0 + max: 10000 + +pwm.frequency: + type: Integer + note: PWM frequency [Hz]. + enum: PARAM_PWM_FREQUENCY + flags: mutable + default: 50 + min: 50 + max: 400 + +pwm.cmd_type: + type: Integer + note: 0 means RawCommand, 1 means ArrayCommand, 2 is reserved for hardpoint.Command. + enum: PARAM_PWM_CMD_TYPE + flags: mutable + default: 0 + min: 0 + max: 2 + +pwm.1_ch: + type: Integer + note: Index of setpoint channel. [-1; 255]. -1 means disabled, # -2 means GPIO SET. + enum: PARAM_PWM_1_CH + flags: mutable + default: -1 + min: -1 + max: 255 + +pwm.1_min: + type: Integer + note: PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) + enum: PARAM_PWM_1_MIN + flags: mutable + default: 1000 + min: 1000 + max: 2000 + +pwm.1_max: + type: Integer + note: PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) + enum: PARAM_PWM_1_MAX + flags: mutable + default: 2000 + min: 1000 + max: 2000 + +pwm.1_def: + type: Integer + note: PWM duration when setpoint is negative or there is no setpoint at all. + enum: PARAM_PWM_1_DEF + flags: mutable + default: 1000 + min: 1000 + max: 2000 + +pwm.1_feedback: + type: Integer + note: 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. + enum: PARAM_PWM_1_FB + flags: mutable + default: 0 + min: 0 + max: 2 + +pwm.2_ch: + type: Integer + note: Index of setpoint channel. [-1; 255]. -1 means disabled, # -2 means GPIO SET. + enum: PARAM_PWM_2_CH + flags: mutable + default: -1 + min: -1 + max: 255 + +pwm.2_min: + type: Integer + note: PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) + enum: PARAM_PWM_2_MIN + flags: mutable + default: 1000 + min: 1000 + max: 2000 + +pwm.2_max: + type: Integer + note: PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) + enum: PARAM_PWM_2_MAX + flags: mutable + default: 2000 + min: 1000 + max: 2000 + +pwm.2_def: + type: Integer + note: PWM duration when setpoint is negative or there is no setpoint at all. + enum: PARAM_PWM_2_DEF + flags: mutable + default: 1000 + min: 1000 + max: 2000 + +pwm.2_feedback: + type: Integer + note: 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. + enum: PARAM_PWM_2_FB + flags: mutable + default: 0 + min: 0 + max: 2 + +pwm.3_ch: + type: Integer + note: Index of setpoint channel. [-1; 255]. -1 means disabled, # -2 means GPIO SET. + enum: PARAM_PWM_3_CH + flags: mutable + default: -1 + min: -1 + max: 255 + +pwm.3_min: + type: Integer + note: PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) + enum: PARAM_PWM_3_MIN + flags: mutable + default: 1000 + min: 1000 + max: 2000 + +pwm.3_max: + type: Integer + note: PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) + enum: PARAM_PWM_3_MAX + flags: mutable + default: 2000 + min: 1000 + max: 2000 + +pwm.3_def: + type: Integer + note: PWM duration when setpoint is negative or there is no setpoint at all. + enum: PARAM_PWM_3_DEF + flags: mutable + default: 1000 + min: 1000 + max: 2000 + +pwm.3_feedback: + type: Integer + note: 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. + enum: PARAM_PWM_3_FB + flags: mutable + default: 0 + min: 0 + max: 2 + +pwm.4_ch: + type: Integer + note: Index of setpoint channel. [-1; 255]. -1 means disabled, # -2 means GPIO SET. + enum: PARAM_PWM_4_CH + flags: mutable + default: -1 + min: -1 + max: 255 + +pwm.4_min: + type: Integer + note: PWM duration when setpoint is min (RawCommand is 0 or Command is -1.0) + enum: PARAM_PWM_4_MIN + flags: mutable + default: 1000 + min: 1000 + max: 2000 + +pwm.4_max: + type: Integer + note: PWM duration when setpoint is max (RawCommand is 8191 or Command is 1.0) + enum: PARAM_PWM_4_MAX + flags: mutable + default: 2000 + min: 1000 + max: 2000 + +pwm.4_def: + type: Integer + note: PWM duration when setpoint is negative or there is no setpoint at all. + enum: PARAM_PWM_4_DEF + flags: mutable + default: 1000 + min: 1000 + max: 2000 + +pwm.4_feedback: + type: Integer + note: 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. + enum: PARAM_PWM_4_FB + flags: mutable + default: 0 + min: 0 + max: 2 diff --git a/Src/periphery/pwm/pwm.hpp b/Src/periphery/pwm/pwm.hpp index bc6660b..88b257a 100644 --- a/Src/periphery/pwm/pwm.hpp +++ b/Src/periphery/pwm/pwm.hpp @@ -41,6 +41,16 @@ class PwmPeriphery { * @return the duration of the PWM signal for a specific PWM pin in microseconds */ static uint32_t get_duration(PwmPin pin); + + /** + * @return the frequency of the PWM signal for a specific PWM pin in Hz + */ + static uint32_t get_frequency(PwmPin pwm_pin); + + /** + * @brief Set the frequency of the PWM signal for a specific PWM pin in Hz + */ + static void set_frequency(PwmPin pwm_pin, uint32_t frequency_hz); }; #ifdef __cplusplus diff --git a/Src/platform/stm32f103/pwm.cpp b/Src/platform/stm32f103/pwm.cpp index e4da21f..0cc6aea 100644 --- a/Src/platform/stm32f103/pwm.cpp +++ b/Src/platform/stm32f103/pwm.cpp @@ -49,3 +49,25 @@ uint32_t PwmPeriphery::get_duration(PwmPin pwm_pin) { auto& pwm_pin_info = info[static_cast(pwm_pin)]; return pwm_pin_info.ccr; } + +void PwmPeriphery::set_frequency(PwmPin pwm_pin, uint32_t frequency_hz) { + if (pwm_pin > PwmPin::PWM_AMOUNT) { + return; + } + + auto& pwm_pin_info = info[static_cast(pwm_pin)]; + volatile uint32_t* arr_reg = &(pwm_pin_info.htim.Instance->ARR); + uint16_t period_us = 1000000 / frequency_hz; + *arr_reg = period_us; +} + +uint32_t PwmPeriphery::get_frequency(PwmPin pwm_pin) { + if (pwm_pin > PwmPin::PWM_AMOUNT) { + return -1; + } + + auto& pwm_pin_info = info[static_cast(pwm_pin)]; + volatile uint32_t* arr_reg = &(pwm_pin_info.htim.Instance->ARR); + uint32_t frequency = 1000000 / *arr_reg; + return frequency; +} diff --git a/Src/platform/ubuntu/pwm.cpp b/Src/platform/ubuntu/pwm.cpp index 4cc27cf..bf3c0bb 100644 --- a/Src/platform/ubuntu/pwm.cpp +++ b/Src/platform/ubuntu/pwm.cpp @@ -6,17 +6,26 @@ #include "periphery/pwm/pwm.hpp" +uint32_t pwm[(int)PwmPin::PWM_AMOUNT]; +uint32_t pwm_freq[(int)PwmPin::PWM_AMOUNT]; + int8_t PwmPeriphery::init(PwmPin pwm_pin) { (void)pwm_pin; return 0; } void PwmPeriphery::set_duration(const PwmPin pwm_pin, uint32_t duration_us) { - (void)pwm_pin; - (void)duration_us; + pwm[(int)pwm_pin] = duration_us; } uint32_t PwmPeriphery::get_duration(PwmPin pwm_pin) { - (void)pwm_pin; - return 0; + return pwm[(int)pwm_pin]; +} + +void PwmPeriphery::set_frequency(const PwmPin pwm_pin, uint32_t frequency_hz) { + pwm_freq[(int) pwm_pin] = frequency_hz; +} + +uint32_t PwmPeriphery::get_frequency(PwmPin pwm_pin) { + return pwm_freq[(int)pwm_pin]; }