From f5e74146f04d482065e9021f55d86a65752f6fcc Mon Sep 17 00:00:00 2001 From: Lu-Xiaoyu <15627452+glowing-star@user.noreply.gitee.com> Date: Sat, 14 Mar 2026 15:44:43 +0800 Subject: [PATCH 1/5] =?UTF-8?q?=E5=8A=9F=E7=8E=87=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E5=8A=A0=E4=B8=8A=E9=80=9F=E5=BA=A6=E8=AF=AF=E5=B7=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PowerControl.hpp | 135 ++++++++++++++++++++++++++++++++++------------- 1 file changed, 98 insertions(+), 37 deletions(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index 321e48a..0ee4d44 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -1,4 +1,4 @@ -#pragma once +#pragma once // clang-format off /* === MODULE MANIFEST V2 === module_name: PowerControl @@ -15,6 +15,7 @@ depends: [] === END MANIFEST === */ // clang-format on +#include #include #include @@ -23,7 +24,10 @@ depends: [] #include "app_framework.hpp" #include "matrix.h" #include "message.hpp" -#include "thread.hpp" +#include "thread.hpp" + +#define ERROR_POWERDISTRIBUTION_SET 40 +#define POP_POWERDISTRIBUTION 20 /** * @brief 计算单个电机模型预测功率 (不含静态损耗) @@ -44,32 +48,20 @@ inline float solve_current_for_power(float target_power, float rpm, float kt, float c = k2 * rpm * rpm - target_power; float delta = b * b - 4.0f * a * c; - if (delta < 0.0f || a < 1e-9f) { - return std::clamp(original_current * 0.5f, -16384.0f, 16384.0f); - } - float sqrt_delta = sqrtf(delta); float x1 = (-b + sqrt_delta) / (2.0f * a); float x2 = (-b - sqrt_delta) / (2.0f * a); + float x3 = -b / (2.0f * a); - /*选择与原电流方向一致，且绝对值更小的解（即更靠近0的电流）*/ - /* TODO:优化选择逻辑 */ float final_current = 0; - if (original_current >= 0) { - if (x1 >= 0 && x1 <= original_current) { - final_current = x1; - } else if (x2 >= 0 && x2 <= original_current) { - final_current = x2; - } else { - final_current = original_current * 0.5f; - } + + if (delta < 1e-9f) { + original_current = x3; } else { - if (x1 <= 0 && x1 >= original_current) { + if (original_current >= 0) { final_current = x1; - } else if (x2 <= 0 && x2 >= original_current) { - final_current = x2; } else { - final_current = original_current * 0.5f; + final_current = x2; } } @@ -104,23 +96,31 @@ class PowerControl : public LibXR::Application { : motor_count_6020) { UNUSED(hw); UNUSED(app); - params_3508_[0][0] = 2.0e-07f; - params_3508_[1][0] = 3.0e-07f; + params_3508_[0][0] = 1.0e-07f; + params_3508_[1][0] = 1.0e-07f; k1_3508_ = params_3508_[0][0]; k2_3508_ = params_3508_[1][0]; } - void SetMotorData3508(float* output_current, float* rotorspeed_rpm) { + void SetMotorData3508(float* output_current, float* rotorspeed_rpm, + float* speed_error = nullptr) { for (int i = 0; i < motor_count_3508_; i++) { output_current_3508_[i] = output_current[i]; rotorspeed_rpm_3508_[i] = rotorspeed_rpm[i]; + if (speed_error) { + speed_error_3508_[i] = fabsf(speed_error[i]); + } } } - void SetMotorData6020(float* output_current, float* rotorspeed_rpm) { + void SetMotorData6020(float* output_current, float* rotorspeed_rpm, + float* speed_error = nullptr) { for (int i = 0; i < motor_count_6020_; i++) { output_current_6020_[i] = output_current[i]; rotorspeed_rpm_6020_[i] = rotorspeed_rpm[i]; + if (speed_error) { + speed_error_6020_[i] = fabsf(speed_error[i]); + } } } @@ -155,8 +155,8 @@ class PowerControl : public LibXR::Application { if (residual > 0 && online && measured_power_ > 5.0f) { params_3508_ = rls_.Update(samples_3508_, residual); - k1_3508_ = static_cast(fmax(params_3508_[0][0], 2.0e-07f)); - k2_3508_ = static_cast(fmax(params_3508_[1][0], 3.0e-07f)); + k1_3508_ = static_cast(fmax(params_3508_[0][0], 1.0e-07f)); + k2_3508_ = static_cast(fmax(params_3508_[1][0], 1.0e-07f)); } } @@ -183,9 +183,10 @@ class PowerControl : public LibXR::Application { private: void OutputLimitOmni(float max_power) { float required_power_3508_sum = 0.0f; - float available_power = max_power - k3_chassis_; + /* 计算每个电机功率, 并累计正功电机的误差之和 */ + sum_error_ = 0.0f; for (int i = 0; i < motor_count_3508_; i++) { motor_power_3508_[i] = calculate_motor_model_power( output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, @@ -193,6 +194,7 @@ class PowerControl : public LibXR::Application { if (motor_power_3508_[i] > 0) { required_power_3508_sum += motor_power_3508_[i]; + sum_error_ += speed_error_3508_[i]; } else { available_power -= motor_power_3508_[i]; } @@ -201,10 +203,31 @@ class PowerControl : public LibXR::Application { if (required_power_3508_sum > available_power) { powercontrol_data_.is_power_limited = true; + /* 计算误差置信度: sum_error 越大, 越倾向按误差分配功率 */ + if (sum_error_ > ERROR_POWERDISTRIBUTION_SET) { + error_confidence_ = 1.0f; + } else if (sum_error_ > POP_POWERDISTRIBUTION) { + error_confidence_ = std::clamp( + (sum_error_ - static_cast(POP_POWERDISTRIBUTION)) / + static_cast(ERROR_POWERDISTRIBUTION_SET - + POP_POWERDISTRIBUTION), + 0.0f, 1.0f); + } else { + error_confidence_ = 0.0f; + } + for (int i = 0; i < motor_count_3508_; i++) { if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { - float power_quota = available_power * - (motor_power_3508_[i] / required_power_3508_sum); + /* 误差权重: 按速度跟踪误差大小分配 */ + float power_weight_error = + (sum_error_ > 1e-6f) ? (speed_error_3508_[i] / sum_error_) : 0.0f; + /* 比例权重: 按功率需求比例分配 */ + float power_weight_prop = + motor_power_3508_[i] / required_power_3508_sum; + /* 混合权重 */ + float power_weight = error_confidence_ * power_weight_error + + (1.0f - error_confidence_) * power_weight_prop; + float power_quota = available_power * power_weight; powercontrol_data_.new_output_current_3508[i] = solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], @@ -226,6 +249,8 @@ class PowerControl : public LibXR::Application { void OutputLimitHelm(float max_power) { float required_power_3508_sum = 0.0f; float required_power_6020_sum = 0.0f; + float sum_error_3508 = 0.0f; + float sum_error_6020 = 0.0f; /*初始可用功率 = 最大功率 - 静态功耗*/ float available_power = max_power - k3_chassis_; @@ -237,6 +262,7 @@ class PowerControl : public LibXR::Application { if (motor_power_3508_[i] > 0) { required_power_3508_sum += motor_power_3508_[i]; + sum_error_3508 += speed_error_3508_[i]; } else { available_power -= motor_power_3508_[i]; } @@ -249,6 +275,7 @@ class PowerControl : public LibXR::Application { if (motor_power_6020_[i] > 0) { required_power_6020_sum += motor_power_6020_[i]; + sum_error_6020 += speed_error_6020_[i]; } else { available_power -= motor_power_6020_[i]; } @@ -268,35 +295,63 @@ class PowerControl : public LibXR::Application { float limit_power_3508_total = std::max(0.0f, available_power - limit_power_6020_total); + /* 6020 组: 误差置信度 + 混合权重分配 */ + float ec_6020 = 0.0f; + if (sum_error_6020 > ERROR_POWERDISTRIBUTION_SET) { + ec_6020 = 1.0f; + } else if (sum_error_6020 > POP_POWERDISTRIBUTION) { + ec_6020 = std::clamp( + (sum_error_6020 - static_cast(POP_POWERDISTRIBUTION)) / + static_cast(ERROR_POWERDISTRIBUTION_SET - + POP_POWERDISTRIBUTION), + 0.0f, 1.0f); + } + for (int i = 0; i < motor_count_6020_; i++) { if (motor_power_6020_[i] > 0 && required_power_6020_sum > 1e-6f) { - /*该电机的功率配额 = 总限额 * (该电机需求 / 总需求)*/ - float power_quota = limit_power_6020_total * - (motor_power_6020_[i] / required_power_6020_sum); + float pw_err = (sum_error_6020 > 1e-6f) + ? (speed_error_6020_[i] / sum_error_6020) + : 0.0f; + float pw_prop = motor_power_6020_[i] / required_power_6020_sum; + float pw = ec_6020 * pw_err + (1.0f - ec_6020) * pw_prop; + float power_quota = limit_power_6020_total * pw; powercontrol_data_.new_output_current_6020[i] = solve_current_for_power(power_quota, rotorspeed_rpm_6020_[i], kt_6020_, k1_6020_, k2_6020_, output_current_6020_[i]); } else { - /*负功或零功，不需要限制，直接通过*/ powercontrol_data_.new_output_current_6020[i] = output_current_6020_[i]; } } + /* 3508 组: 误差置信度 + 混合权重分配 */ + float ec_3508 = 0.0f; + if (sum_error_3508 > ERROR_POWERDISTRIBUTION_SET) { + ec_3508 = 1.0f; + } else if (sum_error_3508 > POP_POWERDISTRIBUTION) { + ec_3508 = std::clamp( + (sum_error_3508 - static_cast(POP_POWERDISTRIBUTION)) / + static_cast(ERROR_POWERDISTRIBUTION_SET - + POP_POWERDISTRIBUTION), + 0.0f, 1.0f); + } + for (int i = 0; i < motor_count_3508_; i++) { if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { - /*该电机的功率配额 = 总限额 * (该电机需求 / 总需求)*/ - float power_quota = limit_power_3508_total * - (motor_power_3508_[i] / required_power_3508_sum); + float pw_err = (sum_error_3508 > 1e-6f) + ? (speed_error_3508_[i] / sum_error_3508) + : 0.0f; + float pw_prop = motor_power_3508_[i] / required_power_3508_sum; + float pw = ec_3508 * pw_err + (1.0f - ec_3508) * pw_prop; + float power_quota = limit_power_3508_total * pw; powercontrol_data_.new_output_current_3508[i] = solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, k2_3508_, output_current_3508_[i]); } else { - // 负功或零功，不需要限制，直接通过 powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i]; } @@ -322,6 +377,12 @@ class PowerControl : public LibXR::Application { PowerControlData powercontrol_data_; float k3_chassis_; /* 底盘静态功耗 */ + float error_confidence_ = 0.0f; /* 误差置信度 */ + float sum_error_ = 0.0f; + + float speed_error_3508_[MAX_MOTOR_COUNT] = {}; /* 3508速度跟踪误差 */ + float speed_error_6020_[MAX_MOTOR_COUNT] = {}; /* 6020速度跟踪误差 */ + int motor_count_3508_; /* 3508电机数目 */ int motor_count_6020_; /* 6020电机数目 */ From d9346a1dec0e6775eb7aaa859be89683b89fe764 Mon Sep 17 00:00:00 2001 From: Firefly <728171597@qq.com> Date: Mon, 23 Mar 2026 22:49:25 +0800 Subject: [PATCH 2/5] =?UTF-8?q?RLS=E6=94=B9=E4=B8=BAEigen?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/build.yml | 186 ++++---- CMakeLists.txt | 72 ++-- PowerControl.hpp | 823 ++++++++++++++++++------------------ README.md | 84 ++-- RLS.hpp | 174 ++++---- 5 files changed, 672 insertions(+), 667 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 6b7484f..2846b94 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,93 +1,93 @@ -name: XRobot Module Build Test - -on: - push: - pull_request: - schedule: - - cron: '0 3 1 * *' # 每月1日凌晨3点（UTC）自动触发 - -jobs: - build: - runs-on: ubuntu-latest - container: - image: ghcr.io/xrobot-org/docker-image-stm32:main - options: --user 0 - - env: - XR_MODULE_NAME: ${{ github.event.repository.name }} - - steps: - - name: install pip packages - run: pip install libxr xrobot - - - name: Checkout bsp-dev-c - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/bsp-dev-c - path: bsp-dev-c - - - name: config libxr - run: cd bsp-dev-c && xr_cubemx_cfg -d ./ --xrobot - - - name: Checkout current module repo to bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} - uses: actions/checkout@v4 - with: - path: bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} - - - name: Checkout SuperPower module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/SuperPower - path: bsp-dev-c/Modules/SuperPower - - - name: Checkout Matrix module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/Matrix - path: bsp-dev-c/Modules/Matrix - - - name: Add modules - run: | - cd bsp-dev-c - rm -rf ./User/xrobot.yaml - xrobot_add_mod SuperPower --instance-id super_power - xrobot_add_mod PowerControl --instance-id power_control - cat ./User/xrobot.yaml - - - name: xrobot generate code - run: cd bsp-dev-c && xrobot_gen_main && cat ./User/xrobot_main.hpp - - - - name: config cmake - run: cd bsp-dev-c && export GCC_TOOLCHAIN_ROOT=/opt/arm-gnu-toolchain-14.2.rel1-x86_64-arm-none-eabi/bin && export CLANG_GCC_CMSIS_COMPILER=/opt/st-arm-clang && cmake . -DCMAKE_TOOLCHAIN_FILE:STRING=cmake/starm-clang.cmake -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=TRUE -Bbuild -G Ninja - - name: build - run: cd bsp-dev-c && cmake --build build - - - name: Create Tag via GitHub API - if: ${{ success() && github.event_name != 'pull_request' }} - uses: actions/github-script@v7 - with: - github-token: ${{ secrets.GITHUB_TOKEN }} - script: | - const moduleTag = process.env.XR_MODULE_NAME; - const date = new Date(); - const yyyy = date.getUTCFullYear(); - const mm = String(date.getUTCMonth() + 1).padStart(2, '0'); - const dd = String(date.getUTCDate()).padStart(2, '0'); - const HH = String(date.getUTCHours()).padStart(2, '0'); - const MM = String(date.getUTCMinutes()).padStart(2, '0'); - const SS = String(date.getUTCSeconds()).padStart(2, '0'); - const tagName = `${moduleTag}-${yyyy}${mm}${dd}-${HH}${MM}${SS}`; - - // 获取当前 commit sha - const sha = process.env.GITHUB_SHA; - - // 创建 tag reference - await github.rest.git.createRef({ - owner: context.repo.owner, - repo: context.repo.repo, - ref: `refs/tags/${tagName}`, - sha: sha - }); - console.log(`Created tag: ${tagName} on sha: ${sha}`); - +name: XRobot Module Build Test + +on: + push: + pull_request: + schedule: + - cron: '0 3 1 * *' # 每月1日凌晨3点（UTC）自动触发 + +jobs: + build: + runs-on: ubuntu-latest + container: + image: ghcr.io/xrobot-org/docker-image-stm32:main + options: --user 0 + + env: + XR_MODULE_NAME: ${{ github.event.repository.name }} + + steps: + - name: install pip packages + run: pip install libxr xrobot + + - name: Checkout bsp-dev-c + uses: actions/checkout@v4 + with: + repository: QDU-Robomaster/bsp-dev-c + path: bsp-dev-c + + - name: config libxr + run: cd bsp-dev-c && xr_cubemx_cfg -d ./ --xrobot + + - name: Checkout current module repo to bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} + uses: actions/checkout@v4 + with: + path: bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} + + - name: Checkout SuperPower module + uses: actions/checkout@v4 + with: + repository: QDU-Robomaster/SuperPower + path: bsp-dev-c/Modules/SuperPower + + - name: Checkout Matrix module + uses: actions/checkout@v4 + with: + repository: QDU-Robomaster/Matrix + path: bsp-dev-c/Modules/Matrix + + - name: Add modules + run: | + cd bsp-dev-c + rm -rf ./User/xrobot.yaml + xrobot_add_mod SuperPower --instance-id super_power + xrobot_add_mod PowerControl --instance-id power_control + cat ./User/xrobot.yaml + + - name: xrobot generate code + run: cd bsp-dev-c && xrobot_gen_main && cat ./User/xrobot_main.hpp + + + - name: config cmake + run: cd bsp-dev-c && export GCC_TOOLCHAIN_ROOT=/opt/arm-gnu-toolchain-14.2.rel1-x86_64-arm-none-eabi/bin && export CLANG_GCC_CMSIS_COMPILER=/opt/st-arm-clang && cmake . -DCMAKE_TOOLCHAIN_FILE:STRING=cmake/starm-clang.cmake -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=TRUE -Bbuild -G Ninja + - name: build + run: cd bsp-dev-c && cmake --build build + + - name: Create Tag via GitHub API + if: ${{ success() && github.event_name != 'pull_request' }} + uses: actions/github-script@v7 + with: + github-token: ${{ secrets.GITHUB_TOKEN }} + script: | + const moduleTag = process.env.XR_MODULE_NAME; + const date = new Date(); + const yyyy = date.getUTCFullYear(); + const mm = String(date.getUTCMonth() + 1).padStart(2, '0'); + const dd = String(date.getUTCDate()).padStart(2, '0'); + const HH = String(date.getUTCHours()).padStart(2, '0'); + const MM = String(date.getUTCMinutes()).padStart(2, '0'); + const SS = String(date.getUTCSeconds()).padStart(2, '0'); + const tagName = `${moduleTag}-${yyyy}${mm}${dd}-${HH}${MM}${SS}`; + + // 获取当前 commit sha + const sha = process.env.GITHUB_SHA; + + // 创建 tag reference + await github.rest.git.createRef({ + owner: context.repo.owner, + repo: context.repo.repo, + ref: `refs/tags/${tagName}`, + sha: sha + }); + console.log(`Created tag: ${tagName} on sha: ${sha}`); + diff --git a/CMakeLists.txt b/CMakeLists.txt index fbb25e7..506a3a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,36 +1,36 @@ -# CMakeLists.txt for PowerControl - -# Add module to include path -target_include_directories(xr PUBLIC ${CMAKE_CURRENT_LIST_DIR}) - -# Auto-include source files -file(GLOB MODULE_POWERCONTROL_SRC CONFIGURE_DEPENDS - "${CMAKE_CURRENT_LIST_DIR}/*.cpp" - "${CMAKE_CURRENT_LIST_DIR}/*.cxx" - "${CMAKE_CURRENT_LIST_DIR}/*.cc" - "${CMAKE_CURRENT_LIST_DIR}/*.c" -) - -target_sources(xr PRIVATE ${MODULE_POWERCONTROL_SRC}) - -# INTERFACE deps shell -get_filename_component(_MODULE_DIRNAME ${CMAKE_CURRENT_LIST_DIR} NAME) -set(_DEPS_TARGET "${_MODULE_DIRNAME}_deps") -add_library(${_DEPS_TARGET} INTERFACE) - -# find_package(YourPkg REQUIRED COMPONENTS a b c) - -# target_link_libraries(${_DEPS_TARGET} INTERFACE -# # YourLibA -# # YourLibB -# ) - -# target_compile_definitions(${_DEPS_TARGET} INTERFACE -# # FOO=1 -# ) -# target_compile_options(${_DEPS_TARGET} INTERFACE -# # -Wall -# ) - -# Register to global -set_property(GLOBAL APPEND PROPERTY XR_MODULE_DEPS ${_DEPS_TARGET}) +# CMakeLists.txt for PowerControl + +# Add module to include path +target_include_directories(xr PUBLIC ${CMAKE_CURRENT_LIST_DIR}) + +# Auto-include source files +file(GLOB MODULE_POWERCONTROL_SRC CONFIGURE_DEPENDS + "${CMAKE_CURRENT_LIST_DIR}/*.cpp" + "${CMAKE_CURRENT_LIST_DIR}/*.cxx" + "${CMAKE_CURRENT_LIST_DIR}/*.cc" + "${CMAKE_CURRENT_LIST_DIR}/*.c" +) + +target_sources(xr PRIVATE ${MODULE_POWERCONTROL_SRC}) + +# INTERFACE deps shell +get_filename_component(_MODULE_DIRNAME ${CMAKE_CURRENT_LIST_DIR} NAME) +set(_DEPS_TARGET "${_MODULE_DIRNAME}_deps") +add_library(${_DEPS_TARGET} INTERFACE) + +# find_package(YourPkg REQUIRED COMPONENTS a b c) + +# target_link_libraries(${_DEPS_TARGET} INTERFACE +# # YourLibA +# # YourLibB +# ) + +# target_compile_definitions(${_DEPS_TARGET} INTERFACE +# # FOO=1 +# ) +# target_compile_options(${_DEPS_TARGET} INTERFACE +# # -Wall +# ) + +# Register to global +set_property(GLOBAL APPEND PROPERTY XR_MODULE_DEPS ${_DEPS_TARGET}) diff --git a/PowerControl.hpp b/PowerControl.hpp index 0ee4d44..3240085 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -1,408 +1,415 @@ -#pragma once -// clang-format off -/* === MODULE MANIFEST V2 === -module_name: PowerControl -module_description: Power control for chassis (supports omni and helm wheel) -constructor_args: - - superpower: '@&super_power' - - is_helm: false - - chassis_static_power_loss: 3.5 - - motor_count_3508: 4 - - motor_count_6020: 0 -template_args: [] -required_hardware: [] -depends: [] -=== END MANIFEST === */ -// clang-format on - -#include -#include -#include - -#include "RLS.hpp" -#include "SuperPower.hpp" -#include "app_framework.hpp" -#include "matrix.h" -#include "message.hpp" -#include "thread.hpp" - -#define ERROR_POWERDISTRIBUTION_SET 40 -#define POP_POWERDISTRIBUTION 20 - -/** - * @brief 计算单个电机模型预测功率 (不含静态损耗) - */ -inline float calculate_motor_model_power(float current, float rpm, float kt, - float k1, float k2) { - return (kt * current * rpm) + (k1 * current * current) + (k2 * rpm * rpm); -} - -/** - * @brief 根据目标功率反解电流 - */ -inline float solve_current_for_power(float target_power, float rpm, float kt, - float k1, float k2, - float original_current) { - float a = k1; - float b = kt * rpm; - float c = k2 * rpm * rpm - target_power; - float delta = b * b - 4.0f * a * c; - - float sqrt_delta = sqrtf(delta); - float x1 = (-b + sqrt_delta) / (2.0f * a); - float x2 = (-b - sqrt_delta) / (2.0f * a); - float x3 = -b / (2.0f * a); - - float final_current = 0; - - if (delta < 1e-9f) { - original_current = x3; - } else { - if (original_current >= 0) { - final_current = x1; - } else { - final_current = x2; - } - } - - return std::clamp(final_current, -16384.0f, 16384.0f); -} - -static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; /* 最大电机数目 */ - -struct PowerControlData { - float new_output_current_3508[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; - float new_output_current_6020[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; - bool is_power_limited = false; -}; - -class PowerControl : public LibXR::Application { - public: - static constexpr int MAX_MOTOR_COUNT = POWER_CONTROL_MAX_MOTOR_COUNT; - - PowerControl(LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, - SuperPower* superpower, bool is_helm = false, - float chassis_static_power_loss = 0.0f, int motor_count_3508 = 4, - int motor_count_6020 = 4) - : superpower_(superpower), - is_helm_(is_helm), - rls_(1e-5f, 0.99999f), - k3_chassis_(chassis_static_power_loss), - motor_count_3508_(motor_count_3508 > MAX_MOTOR_COUNT - ? MAX_MOTOR_COUNT - : motor_count_3508), - motor_count_6020_(motor_count_6020 > MAX_MOTOR_COUNT - ? MAX_MOTOR_COUNT - : motor_count_6020) { - UNUSED(hw); - UNUSED(app); - params_3508_[0][0] = 1.0e-07f; - params_3508_[1][0] = 1.0e-07f; - k1_3508_ = params_3508_[0][0]; - k2_3508_ = params_3508_[1][0]; - } - - void SetMotorData3508(float* output_current, float* rotorspeed_rpm, - float* speed_error = nullptr) { - for (int i = 0; i < motor_count_3508_; i++) { - output_current_3508_[i] = output_current[i]; - rotorspeed_rpm_3508_[i] = rotorspeed_rpm[i]; - if (speed_error) { - speed_error_3508_[i] = fabsf(speed_error[i]); - } - } - } - - void SetMotorData6020(float* output_current, float* rotorspeed_rpm, - float* speed_error = nullptr) { - for (int i = 0; i < motor_count_6020_; i++) { - output_current_6020_[i] = output_current[i]; - rotorspeed_rpm_6020_[i] = rotorspeed_rpm[i]; - if (speed_error) { - speed_error_6020_[i] = fabsf(speed_error[i]); - } - } - } - - void CalculatePowerControlParam() { - /*从超电得到底盘的真实功率*/ - measured_power_ = superpower_->GetChassisPower(); - samples_3508_[0][0] = 0; - samples_3508_[1][0] = 0; - bool online = superpower_->IsOnline(); - - float mechanical_power = 0; - - for (int i = 0; i < motor_count_3508_; i++) { - samples_3508_[0][0] += output_current_3508_[i] * output_current_3508_[i]; - samples_3508_[1][0] += rotorspeed_rpm_3508_[i] * rotorspeed_rpm_3508_[i]; - mechanical_power += - kt_3508_ * output_current_3508_[i] * rotorspeed_rpm_3508_[i]; - } - - /*计算残差*/ - float residual = measured_power_ - mechanical_power - k3_chassis_; - - if (is_helm_) { - float power_6020 = 0; - for (int i = 0; i < motor_count_6020_; i++) { - power_6020 += calculate_motor_model_power(output_current_6020_[i], - rotorspeed_rpm_6020_[i], - kt_6020_, k1_6020_, k2_6020_); - } - residual -= power_6020; - } - - if (residual > 0 && online && measured_power_ > 5.0f) { - params_3508_ = rls_.Update(samples_3508_, residual); - k1_3508_ = static_cast(fmax(params_3508_[0][0], 1.0e-07f)); - k2_3508_ = static_cast(fmax(params_3508_[1][0], 1.0e-07f)); - } - } - - void OutputLimit(float max_power) { - mutex_.Lock(); - if (is_helm_) { - OutputLimitHelm(max_power); - } else { - OutputLimitOmni(max_power); - } - mutex_.Unlock(); - } - - PowerControlData GetPowerControlData() { - PowerControlData data; - mutex_.Lock(); - data = powercontrol_data_; - mutex_.Unlock(); - return data; - } - - void OnMonitor() override {} - - private: - void OutputLimitOmni(float max_power) { - float required_power_3508_sum = 0.0f; - float available_power = max_power - k3_chassis_; - - /* 计算每个电机功率, 并累计正功电机的误差之和 */ - sum_error_ = 0.0f; - for (int i = 0; i < motor_count_3508_; i++) { - motor_power_3508_[i] = calculate_motor_model_power( - output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, - k2_3508_); - - if (motor_power_3508_[i] > 0) { - required_power_3508_sum += motor_power_3508_[i]; - sum_error_ += speed_error_3508_[i]; - } else { - available_power -= motor_power_3508_[i]; - } - } - - if (required_power_3508_sum > available_power) { - powercontrol_data_.is_power_limited = true; - - /* 计算误差置信度: sum_error 越大, 越倾向按误差分配功率 */ - if (sum_error_ > ERROR_POWERDISTRIBUTION_SET) { - error_confidence_ = 1.0f; - } else if (sum_error_ > POP_POWERDISTRIBUTION) { - error_confidence_ = std::clamp( - (sum_error_ - static_cast(POP_POWERDISTRIBUTION)) / - static_cast(ERROR_POWERDISTRIBUTION_SET - - POP_POWERDISTRIBUTION), - 0.0f, 1.0f); - } else { - error_confidence_ = 0.0f; - } - - for (int i = 0; i < motor_count_3508_; i++) { - if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { - /* 误差权重: 按速度跟踪误差大小分配 */ - float power_weight_error = - (sum_error_ > 1e-6f) ? (speed_error_3508_[i] / sum_error_) : 0.0f; - /* 比例权重: 按功率需求比例分配 */ - float power_weight_prop = - motor_power_3508_[i] / required_power_3508_sum; - /* 混合权重 */ - float power_weight = error_confidence_ * power_weight_error + - (1.0f - error_confidence_) * power_weight_prop; - float power_quota = available_power * power_weight; - - powercontrol_data_.new_output_current_3508[i] = - solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], - kt_3508_, k1_3508_, k2_3508_, - output_current_3508_[i]); - } else { - powercontrol_data_.new_output_current_3508[i] = - output_current_3508_[i]; - } - } - } else { - powercontrol_data_.is_power_limited = false; - for (int i = 0; i < motor_count_3508_; i++) { - powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i]; - } - } - } - - void OutputLimitHelm(float max_power) { - float required_power_3508_sum = 0.0f; - float required_power_6020_sum = 0.0f; - float sum_error_3508 = 0.0f; - float sum_error_6020 = 0.0f; - - /*初始可用功率 = 最大功率 - 静态功耗*/ - float available_power = max_power - k3_chassis_; - - for (int i = 0; i < motor_count_3508_; i++) { - motor_power_3508_[i] = calculate_motor_model_power( - output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, - k2_3508_); - - if (motor_power_3508_[i] > 0) { - required_power_3508_sum += motor_power_3508_[i]; - sum_error_3508 += speed_error_3508_[i]; - } else { - available_power -= motor_power_3508_[i]; - } - } - - for (int i = 0; i < motor_count_6020_; i++) { - motor_power_6020_[i] = calculate_motor_model_power( - output_current_6020_[i], rotorspeed_rpm_6020_[i], kt_6020_, k1_6020_, - k2_6020_); - - if (motor_power_6020_[i] > 0) { - required_power_6020_sum += motor_power_6020_[i]; - sum_error_6020 += speed_error_6020_[i]; - } else { - available_power -= motor_power_6020_[i]; - } - } - - float total_required_power = - required_power_3508_sum + required_power_6020_sum; - - if (total_required_power > available_power) { - powercontrol_data_.is_power_limited = true; - - /*计算 6020 组的总功率限额*/ - float limit_power_6020_total = - std::min(required_power_6020_sum, available_power * 0.8f); - - /*计算 3508 组的总功率限额 (剩下的全部)*/ - float limit_power_3508_total = - std::max(0.0f, available_power - limit_power_6020_total); - - /* 6020 组: 误差置信度 + 混合权重分配 */ - float ec_6020 = 0.0f; - if (sum_error_6020 > ERROR_POWERDISTRIBUTION_SET) { - ec_6020 = 1.0f; - } else if (sum_error_6020 > POP_POWERDISTRIBUTION) { - ec_6020 = std::clamp( - (sum_error_6020 - static_cast(POP_POWERDISTRIBUTION)) / - static_cast(ERROR_POWERDISTRIBUTION_SET - - POP_POWERDISTRIBUTION), - 0.0f, 1.0f); - } - - for (int i = 0; i < motor_count_6020_; i++) { - if (motor_power_6020_[i] > 0 && required_power_6020_sum > 1e-6f) { - float pw_err = (sum_error_6020 > 1e-6f) - ? (speed_error_6020_[i] / sum_error_6020) - : 0.0f; - float pw_prop = motor_power_6020_[i] / required_power_6020_sum; - float pw = ec_6020 * pw_err + (1.0f - ec_6020) * pw_prop; - float power_quota = limit_power_6020_total * pw; - - powercontrol_data_.new_output_current_6020[i] = - solve_current_for_power(power_quota, rotorspeed_rpm_6020_[i], - kt_6020_, k1_6020_, k2_6020_, - output_current_6020_[i]); - } else { - powercontrol_data_.new_output_current_6020[i] = - output_current_6020_[i]; - } - } - - /* 3508 组: 误差置信度 + 混合权重分配 */ - float ec_3508 = 0.0f; - if (sum_error_3508 > ERROR_POWERDISTRIBUTION_SET) { - ec_3508 = 1.0f; - } else if (sum_error_3508 > POP_POWERDISTRIBUTION) { - ec_3508 = std::clamp( - (sum_error_3508 - static_cast(POP_POWERDISTRIBUTION)) / - static_cast(ERROR_POWERDISTRIBUTION_SET - - POP_POWERDISTRIBUTION), - 0.0f, 1.0f); - } - - for (int i = 0; i < motor_count_3508_; i++) { - if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { - float pw_err = (sum_error_3508 > 1e-6f) - ? (speed_error_3508_[i] / sum_error_3508) - : 0.0f; - float pw_prop = motor_power_3508_[i] / required_power_3508_sum; - float pw = ec_3508 * pw_err + (1.0f - ec_3508) * pw_prop; - float power_quota = limit_power_3508_total * pw; - - powercontrol_data_.new_output_current_3508[i] = - solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], - kt_3508_, k1_3508_, k2_3508_, - output_current_3508_[i]); - } else { - powercontrol_data_.new_output_current_3508[i] = - output_current_3508_[i]; - } - } - - } else { - /*功率充足，不限制*/ - powercontrol_data_.is_power_limited = false; - for (int i = 0; i < motor_count_3508_; i++) { - powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i]; - } - for (int i = 0; i < motor_count_6020_; i++) { - powercontrol_data_.new_output_current_6020[i] = output_current_6020_[i]; - } - } - } - - private: - LibXR::Mutex mutex_; - SuperPower* superpower_; - bool is_helm_; - RLS<2> rls_; - PowerControlData powercontrol_data_; - float k3_chassis_; /* 底盘静态功耗 */ - - float error_confidence_ = 0.0f; /* 误差置信度 */ - float sum_error_ = 0.0f; - - float speed_error_3508_[MAX_MOTOR_COUNT] = {}; /* 3508速度跟踪误差 */ - float speed_error_6020_[MAX_MOTOR_COUNT] = {}; /* 6020速度跟踪误差 */ - - int motor_count_3508_; /* 3508电机数目 */ - int motor_count_6020_; /* 6020电机数目 */ - - float kt_3508_ = 1.99688994e-6f; - float k1_3508_ = 0; - float k2_3508_ = 0; - Matrixf<2, 1> samples_3508_; - Matrixf<2, 1> params_3508_; - - float output_current_3508_[MAX_MOTOR_COUNT] = {}; - float rotorspeed_rpm_3508_[MAX_MOTOR_COUNT] = {}; - float motor_power_3508_[MAX_MOTOR_COUNT] = {}; - - float kt_6020_ = 1.42074505e-5f; - float k1_6020_ = 6.4276e-7f; - float k2_6020_ = 1.0e-10f; - - float output_current_6020_[MAX_MOTOR_COUNT] = {}; - float rotorspeed_rpm_6020_[MAX_MOTOR_COUNT] = {}; - float motor_power_6020_[MAX_MOTOR_COUNT] = {}; - - float measured_power_ = 0.0f; -}; +#pragma once +// clang-format off +/* === MODULE MANIFEST V2 === +module_name: PowerControl +module_description: Power control for chassis (supports omni and helm wheel) +constructor_args: + - superpower: '@&super_power' + - is_helm: false + - chassis_static_power_loss: 3.5 + - motor_count_3508: 4 + - motor_count_6020: 0 +template_args: [] +required_hardware: [] +depends: [] +=== END MANIFEST === */ +// clang-format on + +#include +#include +#include + +#include + +#include "RLS.hpp" +#include "SuperPower.hpp" +#include "app_framework.hpp" +#include "message.hpp" +#include "thread.hpp" + +#define ERROR_POWERDISTRIBUTION_SET 20 +#define POP_POWERDISTRIBUTION 15 + +/** + * @brief 计算单个电机模型预测功率 (不含静态损耗) + */ +inline float calculate_motor_model_power(float current, float rpm, float kt, + float k1, float k2) { + return (kt * current * rpm) + (k1 * current * current) + (k2 * rpm * rpm); +} + +/** + * @brief 根据目标功率反解电流 + */ +inline float solve_current_for_power(float target_power, float rpm, float kt, + float k1, float k2, + float original_current) { + float a = k1; + float b = kt * rpm; + float c = k2 * rpm * rpm - target_power; + float delta = b * b - 4.0f * a * c; + + float sqrt_delta = sqrtf(delta); + float x1 = (-b + sqrt_delta) / (2.0f * a); + float x2 = (-b - sqrt_delta) / (2.0f * a); + float x3 = -b / (2.0f * a); + + float final_current = 0; + + if (delta < 1e-9f) { + original_current = x3; + } else { + if (original_current >= 0) { + final_current = x1; + } else { + final_current = x2; + } + } + + return std::clamp(final_current, -16384.0f, 16384.0f); +} + +static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; /* 最大电机数目 */ + +struct PowerControlData { + float new_output_current_3508[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + float new_output_current_6020[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + bool is_power_limited = false; +}; + +class PowerControl : public LibXR::Application { + public: + static constexpr int MAX_MOTOR_COUNT = POWER_CONTROL_MAX_MOTOR_COUNT; + + PowerControl(LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, + SuperPower* superpower, bool is_helm = false, + float chassis_static_power_loss = 0.0f, int motor_count_3508 = 4, + int motor_count_6020 = 4) + : superpower_(superpower), + is_helm_(is_helm), + rls_(1e-5f, 0.99999f), + k3_chassis_(chassis_static_power_loss), + motor_count_3508_(motor_count_3508 > MAX_MOTOR_COUNT + ? MAX_MOTOR_COUNT + : motor_count_3508), + motor_count_6020_(motor_count_6020 > MAX_MOTOR_COUNT + ? MAX_MOTOR_COUNT + : motor_count_6020) { + UNUSED(hw); + UNUSED(app); + params_3508_(0, 0) = 2.0e-07f; + params_3508_(1, 0) = 3.0e-07f; + k1_3508_ = params_3508_(0, 0); + k2_3508_ = params_3508_(1, 0); + } + + void SetMotorData3508(float* output_current, float* rotorspeed_rpm, + float* speed_error = nullptr) { + for (int i = 0; i < motor_count_3508_; i++) { + output_current_3508_[i] = output_current[i]; + rotorspeed_rpm_3508_[i] = rotorspeed_rpm[i]; + if (speed_error) { + speed_error_3508_[i] = fabsf(speed_error[i]); + } + } + } + + void SetMotorData6020(float* output_current, float* rotorspeed_rpm, + float* speed_error = nullptr) { + for (int i = 0; i < motor_count_6020_; i++) { + output_current_6020_[i] = output_current[i]; + rotorspeed_rpm_6020_[i] = rotorspeed_rpm[i]; + if (speed_error) { + speed_error_6020_[i] = fabsf(speed_error[i]); + } + } + } + + void CalculatePowerControlParam() { + /*从超电得到底盘的真实功率*/ + measured_power_ = superpower_->GetChassisPower(); + samples_3508_(0, 0) = 0; + samples_3508_(1, 0) = 0; + bool online = superpower_->IsOnline(); + + float mechanical_power = 0; + + for (int i = 0; i < motor_count_3508_; i++) { + samples_3508_(0, 0) += output_current_3508_[i] * output_current_3508_[i]; + samples_3508_(1, 0) += rotorspeed_rpm_3508_[i] * rotorspeed_rpm_3508_[i]; + mechanical_power += + kt_3508_ * output_current_3508_[i] * rotorspeed_rpm_3508_[i]; + } + + /*计算残差*/ + float residual = measured_power_ - mechanical_power - k3_chassis_; + + if (is_helm_) { + float power_6020 = 0; + for (int i = 0; i < motor_count_6020_; i++) { + power_6020 += calculate_motor_model_power(output_current_6020_[i], + rotorspeed_rpm_6020_[i], + kt_6020_, k1_6020_, k2_6020_); + } + residual -= power_6020; + } + + if (residual > 0 && online && measured_power_ > 5.0f) { + params_3508_ = rls_.Update(samples_3508_, residual); + k1_3508_ = static_cast(fmax(params_3508_(0, 0), 1.0e-07f)); + k2_3508_ = static_cast(fmax(params_3508_(1, 0), 1.0e-07f)); + } + } + + void OutputLimit(float max_power) { + mutex_.Lock(); + if (is_helm_) { + OutputLimitHelm(max_power); + } else { + OutputLimitOmni(max_power); + } + mutex_.Unlock(); + } + + PowerControlData GetPowerControlData() { + PowerControlData data; + mutex_.Lock(); + data = powercontrol_data_; + mutex_.Unlock(); + return data; + } + + float GetMeasuredPower() const { return measured_power_; } + + float GetPercent() { return superpower_->GetPercentage(); } + + bool IsOnline() { return superpower_->IsOnline(); } + + void OnMonitor() override {} + + private: + void OutputLimitOmni(float max_power) { + float required_power_3508_sum = 0.0f; + float available_power = max_power - k3_chassis_ - 3.0f; + + /* 计算每个电机功率, 并累计正功电机的误差之和 */ + sum_error_ = 0.0f; + for (int i = 0; i < motor_count_3508_; i++) { + motor_power_3508_[i] = calculate_motor_model_power( + output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, + k2_3508_); + + if (motor_power_3508_[i] > 0) { + required_power_3508_sum += motor_power_3508_[i]; + sum_error_ += speed_error_3508_[i]; + } else { + available_power -= motor_power_3508_[i]; + } + } + + if (required_power_3508_sum > available_power) { + powercontrol_data_.is_power_limited = true; + + /* 计算误差置信度: sum_error 越大, 越倾向按误差分配功率 */ + if (sum_error_ > ERROR_POWERDISTRIBUTION_SET) { + error_confidence_ = 1.0f; + } else if (sum_error_ > POP_POWERDISTRIBUTION) { + error_confidence_ = std::clamp( + (sum_error_ - static_cast(POP_POWERDISTRIBUTION)) / + static_cast(ERROR_POWERDISTRIBUTION_SET - + POP_POWERDISTRIBUTION), + 0.0f, 1.0f); + } else { + error_confidence_ = 0.0f; + } + + for (int i = 0; i < motor_count_3508_; i++) { + if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { + /* 误差权重: 按速度跟踪误差大小分配 */ + float power_weight_error = + (sum_error_ > 1e-6f) ? (speed_error_3508_[i] / sum_error_) : 0.0f; + /* 比例权重: 按功率需求比例分配 */ + float power_weight_prop = + motor_power_3508_[i] / required_power_3508_sum; + /* 混合权重 */ + float power_weight = error_confidence_ * power_weight_error + + (1.0f - error_confidence_) * power_weight_prop; + float power_quota = available_power * power_weight; + + powercontrol_data_.new_output_current_3508[i] = + solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], + kt_3508_, k1_3508_, k2_3508_, + output_current_3508_[i]); + } else { + powercontrol_data_.new_output_current_3508[i] = + output_current_3508_[i]; + } + } + } else { + powercontrol_data_.is_power_limited = false; + for (int i = 0; i < motor_count_3508_; i++) { + powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i]; + } + } + } + + void OutputLimitHelm(float max_power) { + float required_power_3508_sum = 0.0f; + float required_power_6020_sum = 0.0f; + float sum_error_3508 = 0.0f; + float sum_error_6020 = 0.0f; + + /*初始可用功率 = 最大功率 - 静态功耗*/ + float available_power = max_power - k3_chassis_ - 3.0; + + for (int i = 0; i < motor_count_3508_; i++) { + motor_power_3508_[i] = calculate_motor_model_power( + output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, + k2_3508_); + + if (motor_power_3508_[i] > 0) { + required_power_3508_sum += motor_power_3508_[i]; + sum_error_3508 += speed_error_3508_[i]; + } else { + available_power -= motor_power_3508_[i]; + } + } + + for (int i = 0; i < motor_count_6020_; i++) { + motor_power_6020_[i] = calculate_motor_model_power( + output_current_6020_[i], rotorspeed_rpm_6020_[i], kt_6020_, k1_6020_, + k2_6020_); + + if (motor_power_6020_[i] > 0) { + required_power_6020_sum += motor_power_6020_[i]; + sum_error_6020 += speed_error_6020_[i]; + } else { + available_power -= motor_power_6020_[i]; + } + } + + float total_required_power = + required_power_3508_sum + required_power_6020_sum; + + if (total_required_power > available_power) { + powercontrol_data_.is_power_limited = true; + + /*计算 6020 组的总功率限额*/ + float limit_power_6020_total = + std::min(required_power_6020_sum, available_power * 0.8f); + + /*计算 3508 组的总功率限额 (剩下的全部)*/ + float limit_power_3508_total = + std::max(0.0f, available_power - limit_power_6020_total); + + /* 6020 组: 误差置信度 + 混合权重分配 */ + float ec_6020 = 0.0f; + if (sum_error_6020 > ERROR_POWERDISTRIBUTION_SET) { + ec_6020 = 1.0f; + } else if (sum_error_6020 > POP_POWERDISTRIBUTION) { + ec_6020 = std::clamp( + (sum_error_6020 - static_cast(POP_POWERDISTRIBUTION)) / + static_cast(ERROR_POWERDISTRIBUTION_SET - + POP_POWERDISTRIBUTION), + 0.0f, 1.0f); + } + + for (int i = 0; i < motor_count_6020_; i++) { + if (motor_power_6020_[i] > 0 && required_power_6020_sum > 1e-6f) { + float pw_err = (sum_error_6020 > 1e-6f) + ? (speed_error_6020_[i] / sum_error_6020) + : 0.0f; + float pw_prop = motor_power_6020_[i] / required_power_6020_sum; + float pw = ec_6020 * pw_err + (1.0f - ec_6020) * pw_prop; + float power_quota = limit_power_6020_total * pw; + + powercontrol_data_.new_output_current_6020[i] = + solve_current_for_power(power_quota, rotorspeed_rpm_6020_[i], + kt_6020_, k1_6020_, k2_6020_, + output_current_6020_[i]); + } else { + powercontrol_data_.new_output_current_6020[i] = + output_current_6020_[i]; + } + } + + /*误差置信度 + 混合权重分配 */ + float ec_3508 = 0.0f; + if (sum_error_3508 > ERROR_POWERDISTRIBUTION_SET) { + ec_3508 = 1.0f; + } else if (sum_error_3508 > POP_POWERDISTRIBUTION) { + ec_3508 = std::clamp( + (sum_error_3508 - static_cast(POP_POWERDISTRIBUTION)) / + static_cast(ERROR_POWERDISTRIBUTION_SET - + POP_POWERDISTRIBUTION), + 0.0f, 1.0f); + } + + for (int i = 0; i < motor_count_3508_; i++) { + if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { + float pw_err = (sum_error_3508 > 1e-6f) + ? (speed_error_3508_[i] / sum_error_3508) + : 0.0f; + float pw_prop = motor_power_3508_[i] / required_power_3508_sum; + float pw = ec_3508 * pw_err + (1.0f - ec_3508) * pw_prop; + float power_quota = limit_power_3508_total * pw; + + powercontrol_data_.new_output_current_3508[i] = + solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], + kt_3508_, k1_3508_, k2_3508_, + output_current_3508_[i]); + } else { + powercontrol_data_.new_output_current_3508[i] = + output_current_3508_[i]; + } + } + + } else { + /*功率充足，不限制*/ + powercontrol_data_.is_power_limited = false; + for (int i = 0; i < motor_count_3508_; i++) { + powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i]; + } + for (int i = 0; i < motor_count_6020_; i++) { + powercontrol_data_.new_output_current_6020[i] = output_current_6020_[i]; + } + } + } + + private: + LibXR::Mutex mutex_; + SuperPower* superpower_; + bool is_helm_; + RLS<2> rls_; + PowerControlData powercontrol_data_; + float k3_chassis_; /* 底盘静态功耗 */ + + float error_confidence_ = 0.0f; /* 误差置信度 */ + float sum_error_ = 0.0f; + + float speed_error_3508_[MAX_MOTOR_COUNT] = {}; /* 3508速度跟踪误差 */ + float speed_error_6020_[MAX_MOTOR_COUNT] = {}; /* 6020速度跟踪误差 */ + + int motor_count_3508_; /* 3508电机数目 */ + int motor_count_6020_; /* 6020电机数目 */ + + float kt_3508_ = 1.99688994e-6f; + float k1_3508_ = 0; + float k2_3508_ = 0; + Eigen::Matrix samples_3508_; + Eigen::Matrix params_3508_; + + float output_current_3508_[MAX_MOTOR_COUNT] = {}; + float rotorspeed_rpm_3508_[MAX_MOTOR_COUNT] = {}; + float motor_power_3508_[MAX_MOTOR_COUNT] = {}; + + float kt_6020_ = 1.42074505e-5f; + float k1_6020_ = 6.4276e-7f; + float k2_6020_ = 1.0e-10f; + + float output_current_6020_[MAX_MOTOR_COUNT] = {}; + float rotorspeed_rpm_6020_[MAX_MOTOR_COUNT] = {}; + float motor_power_6020_[MAX_MOTOR_COUNT] = {}; + + float measured_power_ = 0.0f; +}; diff --git a/README.md b/README.md index ac63bd5..11332bc 100644 --- a/README.md +++ b/README.md @@ -1,42 +1,42 @@ -# PowerControl - -## 1. 模块作用 -功率控制模块。根据功率预算限制底盘输出，保护供电系统。 -Manifest 描述：Power control for chassis (supports omni and helm wheel) - -## 2. 主要函数说明 -1. SetMotorData3508 / SetMotorData6020: 输入电机状态。 -2. CalculatePowerControlParam: 计算功率控制参数。 -3. OutputLimit / OutputLimitOmni / OutputLimitHelm: 按底盘类型限幅输出。 -4. GetPowerControlData: 提供限幅后的输出数据。 -5. RLS 辅助类: 参数重置与估计更新。 - -## 3. 接入步骤 -1. 添加模块并绑定 superpower。 -2. 在底盘循环中传入电机状态并读取限幅结果。 -3. 将返回结果用于底盘电流/力矩输出。 - -标准命令流程： - xrobot_add_mod PowerControl --instance-id powercontrol - xrobot_gen_main - cube-cmake --build /home/leo/Documents/bsp-dev-c/build/debug -- - -## 4. 配置示例（YAML） -module: PowerControl -entry_header: Modules/PowerControl/PowerControl.hpp -constructor_args: - - superpower: '@&super_power' - - is_helm: false - - chassis_static_power_loss: 0.5 -template_args: -[] - -## 5. 依赖与硬件 -Required Hardware: -[] - -Depends: -[] - -## 6. 代码入口 -Modules/PowerControl/PowerControl.hpp +# PowerControl + +## 1. 模块作用 +功率控制模块。根据功率预算限制底盘输出，保护供电系统。 +Manifest 描述：Power control for chassis (supports omni and helm wheel) + +## 2. 主要函数说明 +1. SetMotorData3508 / SetMotorData6020: 输入电机状态。 +2. CalculatePowerControlParam: 计算功率控制参数。 +3. OutputLimit / OutputLimitOmni / OutputLimitHelm: 按底盘类型限幅输出。 +4. GetPowerControlData: 提供限幅后的输出数据。 +5. RLS 辅助类: 参数重置与估计更新。 + +## 3. 接入步骤 +1. 添加模块并绑定 superpower。 +2. 在底盘循环中传入电机状态并读取限幅结果。 +3. 将返回结果用于底盘电流/力矩输出。 + +标准命令流程： + xrobot_add_mod PowerControl --instance-id powercontrol + xrobot_gen_main + cube-cmake --build /home/leo/Documents/bsp-dev-c/build/debug -- + +## 4. 配置示例（YAML） +module: PowerControl +entry_header: Modules/PowerControl/PowerControl.hpp +constructor_args: + - superpower: '@&super_power' + - is_helm: false + - chassis_static_power_loss: 0.5 +template_args: +[] + +## 5. 依赖与硬件 +Required Hardware: +[] + +Depends: +[] + +## 6. 代码入口 +Modules/PowerControl/PowerControl.hpp diff --git a/RLS.hpp b/RLS.hpp index a58351d..22243f0 100644 --- a/RLS.hpp +++ b/RLS.hpp @@ -1,88 +1,86 @@ -#include "FreeRTOS.h" -#include "matrix.h" -#include "task.h" - -#pragma once - -/** - * @brief 递归最小二乘（RLS）估计器 - * @tparam dim 参数维度 - */ -template -class RLS { - public: - RLS() = delete; // 必须只能带参数的构造函数 - - /** - * @brief 构造 RLS 估计器 - * @param delta_ 初始协方差缩放系数 - * @param lambda_ 遗忘因子 - */ - constexpr RLS(float delta_, float lambda_) - : dimension_(dim), - lambda_(lambda_), - delta_(delta_), - defaultparamsvector_(matrixf::zeros()) { - this->Reset(); // 初始化各个矩阵 - this->Validate(); // 验证参数合法性 - } - - /** - * @brief 重置估计器状态 - */ - void Reset() { - transmatrix_ = matrixf::eye() * delta_; - gainvector_ = matrixf::zeros(); - paramsvector_ = matrixf::zeros(); - } - - /** - * @brief 执行一次 RLS 更新 - * @param sampleVector 输入样本向量 - * @param actualOutput 实际输出 - * @return const Matrixf& 当前参数估计 - */ - const Matrixf& Update(Matrixf& sampleVector, - float actualOutput) { - gainvector_ = - (transmatrix_ * sampleVector) / - (1.0f + - (sampleVector.Trans() * transmatrix_ * sampleVector)[0][0] / lambda_) / - lambda_; - paramsvector_ += - gainvector_ * - (actualOutput - (sampleVector.Trans() * paramsvector_)[0][0]); - transmatrix_ = - (transmatrix_ - gainvector_ * sampleVector.Trans() * transmatrix_) / - lambda_; - - return paramsvector_; - } - - /** - * @brief 手动设置参数向量 - * @param updatedParams 参数向量 - */ - void SetParamVector(const Matrixf& updatedParams) { - paramsvector_ = updatedParams; - defaultparamsvector_ = updatedParams; - } - - private: - /** - * @brief 校验构造参数合法性 - */ - void Validate() const { - configASSERT(lambda_ >= 0.0f || lambda_ <= 1.0f); - configASSERT(delta_ > 0); - } - - uint32_t dimension_; - float lambda_; - float delta_; - - Matrixf transmatrix_; - Matrixf gainvector_; - Matrixf paramsvector_; - Matrixf defaultparamsvector_; -}; +#include + +#pragma once + +/** + * @brief 递归最小二乘（RLS）估计器 + * @tparam dim 参数维度 + */ +template +class RLS { + public: + using ParamVector = Eigen::Matrix; + + RLS() = delete; // 必须只能带参数的构造函数 + + /** + * @brief 构造 RLS 估计器 + * @param delta_ 初始协方差缩放系数 + * @param lambda_ 遗忘因子 + */ + constexpr RLS(float delta_, float lambda_) + : dimension_(dim), + lambda_(lambda_), + delta_(delta_), + defaultparamsvector_(ParamVector::Zero()) { + this->Reset(); // 初始化各个矩阵 + this->Validate(); // 验证参数合法性 + } + + /** + * @brief 重置估计器状态 + */ + void Reset() { + transmatrix_ = Eigen::Matrix::Identity() * delta_; + gainvector_ = ParamVector::Zero(); + paramsvector_ = ParamVector::Zero(); + } + + /** + * @brief 执行一次 RLS 更新 + * @param sampleVector 输入样本向量 + * @param actualOutput 实际输出 + * @return const ParamVector& 当前参数估计 + */ + const ParamVector& Update(const ParamVector& sample_vector, float actual_output) { + gainvector_ = + (transmatrix_ * sample_vector) / + (1.0f + (sample_vector.transpose() * transmatrix_ * sample_vector)(0, 0) / lambda_) / + lambda_; + paramsvector_ += + gainvector_ * + (actual_output - (sample_vector.transpose() * paramsvector_)(0, 0)); + transmatrix_ = + (transmatrix_ - gainvector_ * sample_vector.transpose() * transmatrix_) / + lambda_; + + return paramsvector_; + } + + /** + * @brief 手动设置参数向量 + * @param updatedParams 参数向量 + */ + void SetParamVector(const ParamVector& updated_params) { + paramsvector_ = updated_params; + defaultparamsvector_ = updated_params; + } + + private: + /** + * @brief 校验构造参数合法性 + */ + void Validate() const { + configASSERT(lambda_ >= 0.0f || lambda_ <= 1.0f); + configASSERT(delta_ > 0); + } + + uint32_t dimension_; + float lambda_; + float delta_; + + Eigen::Matrix transmatrix_; + ParamVector gainvector_; + ParamVector paramsvector_; + ParamVector defaultparamsvector_; +}; From d07469cffa633e8391c5910780225d5d4d97fefa Mon Sep 17 00:00:00 2001 From: Firefly <728171597@qq.com> Date: Mon, 23 Mar 2026 22:49:25 +0800 Subject: [PATCH 3/5] =?UTF-8?q?RLS=E6=94=B9=E4=B8=BAEigen?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/build.yml | 186 ++++---- CMakeLists.txt | 72 ++-- PowerControl.hpp | 822 ++++++++++++++++++------------------ README.md | 84 ++-- RLS.hpp | 176 ++++---- 5 files changed, 673 insertions(+), 667 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 6b7484f..2846b94 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,93 +1,93 @@ -name: XRobot Module Build Test - -on: - push: - pull_request: - schedule: - - cron: '0 3 1 * *' # 每月1日凌晨3点（UTC）自动触发 - -jobs: - build: - runs-on: ubuntu-latest - container: - image: ghcr.io/xrobot-org/docker-image-stm32:main - options: --user 0 - - env: - XR_MODULE_NAME: ${{ github.event.repository.name }} - - steps: - - name: install pip packages - run: pip install libxr xrobot - - - name: Checkout bsp-dev-c - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/bsp-dev-c - path: bsp-dev-c - - - name: config libxr - run: cd bsp-dev-c && xr_cubemx_cfg -d ./ --xrobot - - - name: Checkout current module repo to bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} - uses: actions/checkout@v4 - with: - path: bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} - - - name: Checkout SuperPower module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/SuperPower - path: bsp-dev-c/Modules/SuperPower - - - name: Checkout Matrix module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/Matrix - path: bsp-dev-c/Modules/Matrix - - - name: Add modules - run: | - cd bsp-dev-c - rm -rf ./User/xrobot.yaml - xrobot_add_mod SuperPower --instance-id super_power - xrobot_add_mod PowerControl --instance-id power_control - cat ./User/xrobot.yaml - - - name: xrobot generate code - run: cd bsp-dev-c && xrobot_gen_main && cat ./User/xrobot_main.hpp - - - - name: config cmake - run: cd bsp-dev-c && export GCC_TOOLCHAIN_ROOT=/opt/arm-gnu-toolchain-14.2.rel1-x86_64-arm-none-eabi/bin && export CLANG_GCC_CMSIS_COMPILER=/opt/st-arm-clang && cmake . -DCMAKE_TOOLCHAIN_FILE:STRING=cmake/starm-clang.cmake -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=TRUE -Bbuild -G Ninja - - name: build - run: cd bsp-dev-c && cmake --build build - - - name: Create Tag via GitHub API - if: ${{ success() && github.event_name != 'pull_request' }} - uses: actions/github-script@v7 - with: - github-token: ${{ secrets.GITHUB_TOKEN }} - script: | - const moduleTag = process.env.XR_MODULE_NAME; - const date = new Date(); - const yyyy = date.getUTCFullYear(); - const mm = String(date.getUTCMonth() + 1).padStart(2, '0'); - const dd = String(date.getUTCDate()).padStart(2, '0'); - const HH = String(date.getUTCHours()).padStart(2, '0'); - const MM = String(date.getUTCMinutes()).padStart(2, '0'); - const SS = String(date.getUTCSeconds()).padStart(2, '0'); - const tagName = `${moduleTag}-${yyyy}${mm}${dd}-${HH}${MM}${SS}`; - - // 获取当前 commit sha - const sha = process.env.GITHUB_SHA; - - // 创建 tag reference - await github.rest.git.createRef({ - owner: context.repo.owner, - repo: context.repo.repo, - ref: `refs/tags/${tagName}`, - sha: sha - }); - console.log(`Created tag: ${tagName} on sha: ${sha}`); - +name: XRobot Module Build Test + +on: + push: + pull_request: + schedule: + - cron: '0 3 1 * *' # 每月1日凌晨3点（UTC）自动触发 + +jobs: + build: + runs-on: ubuntu-latest + container: + image: ghcr.io/xrobot-org/docker-image-stm32:main + options: --user 0 + + env: + XR_MODULE_NAME: ${{ github.event.repository.name }} + + steps: + - name: install pip packages + run: pip install libxr xrobot + + - name: Checkout bsp-dev-c + uses: actions/checkout@v4 + with: + repository: QDU-Robomaster/bsp-dev-c + path: bsp-dev-c + + - name: config libxr + run: cd bsp-dev-c && xr_cubemx_cfg -d ./ --xrobot + + - name: Checkout current module repo to bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} + uses: actions/checkout@v4 + with: + path: bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} + + - name: Checkout SuperPower module + uses: actions/checkout@v4 + with: + repository: QDU-Robomaster/SuperPower + path: bsp-dev-c/Modules/SuperPower + + - name: Checkout Matrix module + uses: actions/checkout@v4 + with: + repository: QDU-Robomaster/Matrix + path: bsp-dev-c/Modules/Matrix + + - name: Add modules + run: | + cd bsp-dev-c + rm -rf ./User/xrobot.yaml + xrobot_add_mod SuperPower --instance-id super_power + xrobot_add_mod PowerControl --instance-id power_control + cat ./User/xrobot.yaml + + - name: xrobot generate code + run: cd bsp-dev-c && xrobot_gen_main && cat ./User/xrobot_main.hpp + + + - name: config cmake + run: cd bsp-dev-c && export GCC_TOOLCHAIN_ROOT=/opt/arm-gnu-toolchain-14.2.rel1-x86_64-arm-none-eabi/bin && export CLANG_GCC_CMSIS_COMPILER=/opt/st-arm-clang && cmake . -DCMAKE_TOOLCHAIN_FILE:STRING=cmake/starm-clang.cmake -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=TRUE -Bbuild -G Ninja + - name: build + run: cd bsp-dev-c && cmake --build build + + - name: Create Tag via GitHub API + if: ${{ success() && github.event_name != 'pull_request' }} + uses: actions/github-script@v7 + with: + github-token: ${{ secrets.GITHUB_TOKEN }} + script: | + const moduleTag = process.env.XR_MODULE_NAME; + const date = new Date(); + const yyyy = date.getUTCFullYear(); + const mm = String(date.getUTCMonth() + 1).padStart(2, '0'); + const dd = String(date.getUTCDate()).padStart(2, '0'); + const HH = String(date.getUTCHours()).padStart(2, '0'); + const MM = String(date.getUTCMinutes()).padStart(2, '0'); + const SS = String(date.getUTCSeconds()).padStart(2, '0'); + const tagName = `${moduleTag}-${yyyy}${mm}${dd}-${HH}${MM}${SS}`; + + // 获取当前 commit sha + const sha = process.env.GITHUB_SHA; + + // 创建 tag reference + await github.rest.git.createRef({ + owner: context.repo.owner, + repo: context.repo.repo, + ref: `refs/tags/${tagName}`, + sha: sha + }); + console.log(`Created tag: ${tagName} on sha: ${sha}`); + diff --git a/CMakeLists.txt b/CMakeLists.txt index fbb25e7..506a3a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,36 +1,36 @@ -# CMakeLists.txt for PowerControl - -# Add module to include path -target_include_directories(xr PUBLIC ${CMAKE_CURRENT_LIST_DIR}) - -# Auto-include source files -file(GLOB MODULE_POWERCONTROL_SRC CONFIGURE_DEPENDS - "${CMAKE_CURRENT_LIST_DIR}/*.cpp" - "${CMAKE_CURRENT_LIST_DIR}/*.cxx" - "${CMAKE_CURRENT_LIST_DIR}/*.cc" - "${CMAKE_CURRENT_LIST_DIR}/*.c" -) - -target_sources(xr PRIVATE ${MODULE_POWERCONTROL_SRC}) - -# INTERFACE deps shell -get_filename_component(_MODULE_DIRNAME ${CMAKE_CURRENT_LIST_DIR} NAME) -set(_DEPS_TARGET "${_MODULE_DIRNAME}_deps") -add_library(${_DEPS_TARGET} INTERFACE) - -# find_package(YourPkg REQUIRED COMPONENTS a b c) - -# target_link_libraries(${_DEPS_TARGET} INTERFACE -# # YourLibA -# # YourLibB -# ) - -# target_compile_definitions(${_DEPS_TARGET} INTERFACE -# # FOO=1 -# ) -# target_compile_options(${_DEPS_TARGET} INTERFACE -# # -Wall -# ) - -# Register to global -set_property(GLOBAL APPEND PROPERTY XR_MODULE_DEPS ${_DEPS_TARGET}) +# CMakeLists.txt for PowerControl + +# Add module to include path +target_include_directories(xr PUBLIC ${CMAKE_CURRENT_LIST_DIR}) + +# Auto-include source files +file(GLOB MODULE_POWERCONTROL_SRC CONFIGURE_DEPENDS + "${CMAKE_CURRENT_LIST_DIR}/*.cpp" + "${CMAKE_CURRENT_LIST_DIR}/*.cxx" + "${CMAKE_CURRENT_LIST_DIR}/*.cc" + "${CMAKE_CURRENT_LIST_DIR}/*.c" +) + +target_sources(xr PRIVATE ${MODULE_POWERCONTROL_SRC}) + +# INTERFACE deps shell +get_filename_component(_MODULE_DIRNAME ${CMAKE_CURRENT_LIST_DIR} NAME) +set(_DEPS_TARGET "${_MODULE_DIRNAME}_deps") +add_library(${_DEPS_TARGET} INTERFACE) + +# find_package(YourPkg REQUIRED COMPONENTS a b c) + +# target_link_libraries(${_DEPS_TARGET} INTERFACE +# # YourLibA +# # YourLibB +# ) + +# target_compile_definitions(${_DEPS_TARGET} INTERFACE +# # FOO=1 +# ) +# target_compile_options(${_DEPS_TARGET} INTERFACE +# # -Wall +# ) + +# Register to global +set_property(GLOBAL APPEND PROPERTY XR_MODULE_DEPS ${_DEPS_TARGET}) diff --git a/PowerControl.hpp b/PowerControl.hpp index 0ee4d44..df580dc 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -1,408 +1,414 @@ -#pragma once -// clang-format off -/* === MODULE MANIFEST V2 === -module_name: PowerControl -module_description: Power control for chassis (supports omni and helm wheel) -constructor_args: - - superpower: '@&super_power' - - is_helm: false - - chassis_static_power_loss: 3.5 - - motor_count_3508: 4 - - motor_count_6020: 0 -template_args: [] -required_hardware: [] -depends: [] -=== END MANIFEST === */ -// clang-format on - -#include -#include -#include - -#include "RLS.hpp" -#include "SuperPower.hpp" -#include "app_framework.hpp" -#include "matrix.h" -#include "message.hpp" -#include "thread.hpp" - -#define ERROR_POWERDISTRIBUTION_SET 40 -#define POP_POWERDISTRIBUTION 20 - -/** - * @brief 计算单个电机模型预测功率 (不含静态损耗) - */ -inline float calculate_motor_model_power(float current, float rpm, float kt, - float k1, float k2) { - return (kt * current * rpm) + (k1 * current * current) + (k2 * rpm * rpm); -} - -/** - * @brief 根据目标功率反解电流 - */ -inline float solve_current_for_power(float target_power, float rpm, float kt, - float k1, float k2, - float original_current) { - float a = k1; - float b = kt * rpm; - float c = k2 * rpm * rpm - target_power; - float delta = b * b - 4.0f * a * c; - - float sqrt_delta = sqrtf(delta); - float x1 = (-b + sqrt_delta) / (2.0f * a); - float x2 = (-b - sqrt_delta) / (2.0f * a); - float x3 = -b / (2.0f * a); - - float final_current = 0; - - if (delta < 1e-9f) { - original_current = x3; - } else { - if (original_current >= 0) { - final_current = x1; - } else { - final_current = x2; - } - } - - return std::clamp(final_current, -16384.0f, 16384.0f); -} - -static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; /* 最大电机数目 */ - -struct PowerControlData { - float new_output_current_3508[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; - float new_output_current_6020[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; - bool is_power_limited = false; -}; - -class PowerControl : public LibXR::Application { - public: - static constexpr int MAX_MOTOR_COUNT = POWER_CONTROL_MAX_MOTOR_COUNT; - - PowerControl(LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, - SuperPower* superpower, bool is_helm = false, - float chassis_static_power_loss = 0.0f, int motor_count_3508 = 4, - int motor_count_6020 = 4) - : superpower_(superpower), - is_helm_(is_helm), - rls_(1e-5f, 0.99999f), - k3_chassis_(chassis_static_power_loss), - motor_count_3508_(motor_count_3508 > MAX_MOTOR_COUNT - ? MAX_MOTOR_COUNT - : motor_count_3508), - motor_count_6020_(motor_count_6020 > MAX_MOTOR_COUNT - ? MAX_MOTOR_COUNT - : motor_count_6020) { - UNUSED(hw); - UNUSED(app); - params_3508_[0][0] = 1.0e-07f; - params_3508_[1][0] = 1.0e-07f; - k1_3508_ = params_3508_[0][0]; - k2_3508_ = params_3508_[1][0]; - } - - void SetMotorData3508(float* output_current, float* rotorspeed_rpm, - float* speed_error = nullptr) { - for (int i = 0; i < motor_count_3508_; i++) { - output_current_3508_[i] = output_current[i]; - rotorspeed_rpm_3508_[i] = rotorspeed_rpm[i]; - if (speed_error) { - speed_error_3508_[i] = fabsf(speed_error[i]); - } - } - } - - void SetMotorData6020(float* output_current, float* rotorspeed_rpm, - float* speed_error = nullptr) { - for (int i = 0; i < motor_count_6020_; i++) { - output_current_6020_[i] = output_current[i]; - rotorspeed_rpm_6020_[i] = rotorspeed_rpm[i]; - if (speed_error) { - speed_error_6020_[i] = fabsf(speed_error[i]); - } - } - } - - void CalculatePowerControlParam() { - /*从超电得到底盘的真实功率*/ - measured_power_ = superpower_->GetChassisPower(); - samples_3508_[0][0] = 0; - samples_3508_[1][0] = 0; - bool online = superpower_->IsOnline(); - - float mechanical_power = 0; - - for (int i = 0; i < motor_count_3508_; i++) { - samples_3508_[0][0] += output_current_3508_[i] * output_current_3508_[i]; - samples_3508_[1][0] += rotorspeed_rpm_3508_[i] * rotorspeed_rpm_3508_[i]; - mechanical_power += - kt_3508_ * output_current_3508_[i] * rotorspeed_rpm_3508_[i]; - } - - /*计算残差*/ - float residual = measured_power_ - mechanical_power - k3_chassis_; - - if (is_helm_) { - float power_6020 = 0; - for (int i = 0; i < motor_count_6020_; i++) { - power_6020 += calculate_motor_model_power(output_current_6020_[i], - rotorspeed_rpm_6020_[i], - kt_6020_, k1_6020_, k2_6020_); - } - residual -= power_6020; - } - - if (residual > 0 && online && measured_power_ > 5.0f) { - params_3508_ = rls_.Update(samples_3508_, residual); - k1_3508_ = static_cast(fmax(params_3508_[0][0], 1.0e-07f)); - k2_3508_ = static_cast(fmax(params_3508_[1][0], 1.0e-07f)); - } - } - - void OutputLimit(float max_power) { - mutex_.Lock(); - if (is_helm_) { - OutputLimitHelm(max_power); - } else { - OutputLimitOmni(max_power); - } - mutex_.Unlock(); - } - - PowerControlData GetPowerControlData() { - PowerControlData data; - mutex_.Lock(); - data = powercontrol_data_; - mutex_.Unlock(); - return data; - } - - void OnMonitor() override {} - - private: - void OutputLimitOmni(float max_power) { - float required_power_3508_sum = 0.0f; - float available_power = max_power - k3_chassis_; - - /* 计算每个电机功率, 并累计正功电机的误差之和 */ - sum_error_ = 0.0f; - for (int i = 0; i < motor_count_3508_; i++) { - motor_power_3508_[i] = calculate_motor_model_power( - output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, - k2_3508_); - - if (motor_power_3508_[i] > 0) { - required_power_3508_sum += motor_power_3508_[i]; - sum_error_ += speed_error_3508_[i]; - } else { - available_power -= motor_power_3508_[i]; - } - } - - if (required_power_3508_sum > available_power) { - powercontrol_data_.is_power_limited = true; - - /* 计算误差置信度: sum_error 越大, 越倾向按误差分配功率 */ - if (sum_error_ > ERROR_POWERDISTRIBUTION_SET) { - error_confidence_ = 1.0f; - } else if (sum_error_ > POP_POWERDISTRIBUTION) { - error_confidence_ = std::clamp( - (sum_error_ - static_cast(POP_POWERDISTRIBUTION)) / - static_cast(ERROR_POWERDISTRIBUTION_SET - - POP_POWERDISTRIBUTION), - 0.0f, 1.0f); - } else { - error_confidence_ = 0.0f; - } - - for (int i = 0; i < motor_count_3508_; i++) { - if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { - /* 误差权重: 按速度跟踪误差大小分配 */ - float power_weight_error = - (sum_error_ > 1e-6f) ? (speed_error_3508_[i] / sum_error_) : 0.0f; - /* 比例权重: 按功率需求比例分配 */ - float power_weight_prop = - motor_power_3508_[i] / required_power_3508_sum; - /* 混合权重 */ - float power_weight = error_confidence_ * power_weight_error + - (1.0f - error_confidence_) * power_weight_prop; - float power_quota = available_power * power_weight; - - powercontrol_data_.new_output_current_3508[i] = - solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], - kt_3508_, k1_3508_, k2_3508_, - output_current_3508_[i]); - } else { - powercontrol_data_.new_output_current_3508[i] = - output_current_3508_[i]; - } - } - } else { - powercontrol_data_.is_power_limited = false; - for (int i = 0; i < motor_count_3508_; i++) { - powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i]; - } - } - } - - void OutputLimitHelm(float max_power) { - float required_power_3508_sum = 0.0f; - float required_power_6020_sum = 0.0f; - float sum_error_3508 = 0.0f; - float sum_error_6020 = 0.0f; - - /*初始可用功率 = 最大功率 - 静态功耗*/ - float available_power = max_power - k3_chassis_; - - for (int i = 0; i < motor_count_3508_; i++) { - motor_power_3508_[i] = calculate_motor_model_power( - output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, - k2_3508_); - - if (motor_power_3508_[i] > 0) { - required_power_3508_sum += motor_power_3508_[i]; - sum_error_3508 += speed_error_3508_[i]; - } else { - available_power -= motor_power_3508_[i]; - } - } - - for (int i = 0; i < motor_count_6020_; i++) { - motor_power_6020_[i] = calculate_motor_model_power( - output_current_6020_[i], rotorspeed_rpm_6020_[i], kt_6020_, k1_6020_, - k2_6020_); - - if (motor_power_6020_[i] > 0) { - required_power_6020_sum += motor_power_6020_[i]; - sum_error_6020 += speed_error_6020_[i]; - } else { - available_power -= motor_power_6020_[i]; - } - } - - float total_required_power = - required_power_3508_sum + required_power_6020_sum; - - if (total_required_power > available_power) { - powercontrol_data_.is_power_limited = true; - - /*计算 6020 组的总功率限额*/ - float limit_power_6020_total = - std::min(required_power_6020_sum, available_power * 0.8f); - - /*计算 3508 组的总功率限额 (剩下的全部)*/ - float limit_power_3508_total = - std::max(0.0f, available_power - limit_power_6020_total); - - /* 6020 组: 误差置信度 + 混合权重分配 */ - float ec_6020 = 0.0f; - if (sum_error_6020 > ERROR_POWERDISTRIBUTION_SET) { - ec_6020 = 1.0f; - } else if (sum_error_6020 > POP_POWERDISTRIBUTION) { - ec_6020 = std::clamp( - (sum_error_6020 - static_cast(POP_POWERDISTRIBUTION)) / - static_cast(ERROR_POWERDISTRIBUTION_SET - - POP_POWERDISTRIBUTION), - 0.0f, 1.0f); - } - - for (int i = 0; i < motor_count_6020_; i++) { - if (motor_power_6020_[i] > 0 && required_power_6020_sum > 1e-6f) { - float pw_err = (sum_error_6020 > 1e-6f) - ? (speed_error_6020_[i] / sum_error_6020) - : 0.0f; - float pw_prop = motor_power_6020_[i] / required_power_6020_sum; - float pw = ec_6020 * pw_err + (1.0f - ec_6020) * pw_prop; - float power_quota = limit_power_6020_total * pw; - - powercontrol_data_.new_output_current_6020[i] = - solve_current_for_power(power_quota, rotorspeed_rpm_6020_[i], - kt_6020_, k1_6020_, k2_6020_, - output_current_6020_[i]); - } else { - powercontrol_data_.new_output_current_6020[i] = - output_current_6020_[i]; - } - } - - /* 3508 组: 误差置信度 + 混合权重分配 */ - float ec_3508 = 0.0f; - if (sum_error_3508 > ERROR_POWERDISTRIBUTION_SET) { - ec_3508 = 1.0f; - } else if (sum_error_3508 > POP_POWERDISTRIBUTION) { - ec_3508 = std::clamp( - (sum_error_3508 - static_cast(POP_POWERDISTRIBUTION)) / - static_cast(ERROR_POWERDISTRIBUTION_SET - - POP_POWERDISTRIBUTION), - 0.0f, 1.0f); - } - - for (int i = 0; i < motor_count_3508_; i++) { - if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { - float pw_err = (sum_error_3508 > 1e-6f) - ? (speed_error_3508_[i] / sum_error_3508) - : 0.0f; - float pw_prop = motor_power_3508_[i] / required_power_3508_sum; - float pw = ec_3508 * pw_err + (1.0f - ec_3508) * pw_prop; - float power_quota = limit_power_3508_total * pw; - - powercontrol_data_.new_output_current_3508[i] = - solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], - kt_3508_, k1_3508_, k2_3508_, - output_current_3508_[i]); - } else { - powercontrol_data_.new_output_current_3508[i] = - output_current_3508_[i]; - } - } - - } else { - /*功率充足，不限制*/ - powercontrol_data_.is_power_limited = false; - for (int i = 0; i < motor_count_3508_; i++) { - powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i]; - } - for (int i = 0; i < motor_count_6020_; i++) { - powercontrol_data_.new_output_current_6020[i] = output_current_6020_[i]; - } - } - } - - private: - LibXR::Mutex mutex_; - SuperPower* superpower_; - bool is_helm_; - RLS<2> rls_; - PowerControlData powercontrol_data_; - float k3_chassis_; /* 底盘静态功耗 */ - - float error_confidence_ = 0.0f; /* 误差置信度 */ - float sum_error_ = 0.0f; - - float speed_error_3508_[MAX_MOTOR_COUNT] = {}; /* 3508速度跟踪误差 */ - float speed_error_6020_[MAX_MOTOR_COUNT] = {}; /* 6020速度跟踪误差 */ - - int motor_count_3508_; /* 3508电机数目 */ - int motor_count_6020_; /* 6020电机数目 */ - - float kt_3508_ = 1.99688994e-6f; - float k1_3508_ = 0; - float k2_3508_ = 0; - Matrixf<2, 1> samples_3508_; - Matrixf<2, 1> params_3508_; - - float output_current_3508_[MAX_MOTOR_COUNT] = {}; - float rotorspeed_rpm_3508_[MAX_MOTOR_COUNT] = {}; - float motor_power_3508_[MAX_MOTOR_COUNT] = {}; - - float kt_6020_ = 1.42074505e-5f; - float k1_6020_ = 6.4276e-7f; - float k2_6020_ = 1.0e-10f; - - float output_current_6020_[MAX_MOTOR_COUNT] = {}; - float rotorspeed_rpm_6020_[MAX_MOTOR_COUNT] = {}; - float motor_power_6020_[MAX_MOTOR_COUNT] = {}; - - float measured_power_ = 0.0f; -}; +#pragma once +// clang-format off +/* === MODULE MANIFEST V2 === +module_name: PowerControl +module_description: Power control for chassis (supports omni and helm wheel) +constructor_args: + - superpower: '@&super_power' + - is_helm: false + - chassis_static_power_loss: 3.5 + - motor_count_3508: 4 + - motor_count_6020: 0 +template_args: [] +required_hardware: [] +depends: [] +=== END MANIFEST === */ +// clang-format on + +#include +#include +#include +#include + +#include "RLS.hpp" +#include "SuperPower.hpp" +#include "app_framework.hpp" +#include "message.hpp" +#include "thread.hpp" + +#define ERROR_POWERDISTRIBUTION_SET 20 +#define POP_POWERDISTRIBUTION 15 + +/** + * @brief 计算单个电机模型预测功率 (不含静态损耗) + */ +inline float calculate_motor_model_power(float current, float rpm, float kt, + float k1, float k2) { + return (kt * current * rpm) + (k1 * current * current) + (k2 * rpm * rpm); +} + +/** + * @brief 根据目标功率反解电流 + */ +inline float solve_current_for_power(float target_power, float rpm, float kt, + float k1, float k2, + float original_current) { + float a = k1; + float b = kt * rpm; + float c = k2 * rpm * rpm - target_power; + float delta = b * b - 4.0f * a * c; + + float sqrt_delta = sqrtf(delta); + float x1 = (-b + sqrt_delta) / (2.0f * a); + float x2 = (-b - sqrt_delta) / (2.0f * a); + float x3 = -b / (2.0f * a); + + float final_current = 0; + + if (delta < 1e-9f) { + original_current = x3; + } else { + if (original_current >= 0) { + final_current = x1; + } else { + final_current = x2; + } + } + + return std::clamp(final_current, -16384.0f, 16384.0f); +} + +static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; /* 最大电机数目 */ + +struct PowerControlData { + float new_output_current_3508[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + float new_output_current_6020[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + bool is_power_limited = false; +}; + +class PowerControl : public LibXR::Application { + public: + static constexpr int MAX_MOTOR_COUNT = POWER_CONTROL_MAX_MOTOR_COUNT; + + PowerControl(LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, + SuperPower* superpower, bool is_helm = false, + float chassis_static_power_loss = 0.0f, int motor_count_3508 = 4, + int motor_count_6020 = 4) + : superpower_(superpower), + is_helm_(is_helm), + rls_(1e-5f, 0.99999f), + k3_chassis_(chassis_static_power_loss), + motor_count_3508_(motor_count_3508 > MAX_MOTOR_COUNT + ? MAX_MOTOR_COUNT + : motor_count_3508), + motor_count_6020_(motor_count_6020 > MAX_MOTOR_COUNT + ? MAX_MOTOR_COUNT + : motor_count_6020) { + UNUSED(hw); + UNUSED(app); + params_3508_(0, 0) = 2.0e-07f; + params_3508_(1, 0) = 3.0e-07f; + k1_3508_ = params_3508_(0, 0); + k2_3508_ = params_3508_(1, 0); + } + + void SetMotorData3508(float* output_current, float* rotorspeed_rpm, + float* speed_error = nullptr) { + for (int i = 0; i < motor_count_3508_; i++) { + output_current_3508_[i] = output_current[i]; + rotorspeed_rpm_3508_[i] = rotorspeed_rpm[i]; + if (speed_error) { + speed_error_3508_[i] = fabsf(speed_error[i]); + } + } + } + + void SetMotorData6020(float* output_current, float* rotorspeed_rpm, + float* speed_error = nullptr) { + for (int i = 0; i < motor_count_6020_; i++) { + output_current_6020_[i] = output_current[i]; + rotorspeed_rpm_6020_[i] = rotorspeed_rpm[i]; + if (speed_error) { + speed_error_6020_[i] = fabsf(speed_error[i]); + } + } + } + + void CalculatePowerControlParam() { + /*从超电得到底盘的真实功率*/ + measured_power_ = superpower_->GetChassisPower(); + samples_3508_(0, 0) = 0; + samples_3508_(1, 0) = 0; + bool online = superpower_->IsOnline(); + + float mechanical_power = 0; + + for (int i = 0; i < motor_count_3508_; i++) { + samples_3508_(0, 0) += output_current_3508_[i] * output_current_3508_[i]; + samples_3508_(1, 0) += rotorspeed_rpm_3508_[i] * rotorspeed_rpm_3508_[i]; + mechanical_power += + kt_3508_ * output_current_3508_[i] * rotorspeed_rpm_3508_[i]; + } + + /*计算残差*/ + float residual = measured_power_ - mechanical_power - k3_chassis_; + + if (is_helm_) { + float power_6020 = 0; + for (int i = 0; i < motor_count_6020_; i++) { + power_6020 += calculate_motor_model_power(output_current_6020_[i], + rotorspeed_rpm_6020_[i], + kt_6020_, k1_6020_, k2_6020_); + } + residual -= power_6020; + } + + if (residual > 0 && online && measured_power_ > 5.0f) { + params_3508_ = rls_.Update(samples_3508_, residual); + k1_3508_ = static_cast(fmax(params_3508_(0, 0), 1.0e-07f)); + k2_3508_ = static_cast(fmax(params_3508_(1, 0), 1.0e-07f)); + } + } + + void OutputLimit(float max_power) { + mutex_.Lock(); + if (is_helm_) { + OutputLimitHelm(max_power); + } else { + OutputLimitOmni(max_power); + } + mutex_.Unlock(); + } + + PowerControlData GetPowerControlData() { + PowerControlData data; + mutex_.Lock(); + data = powercontrol_data_; + mutex_.Unlock(); + return data; + } + + float GetMeasuredPower() const { return measured_power_; } + + float GetCapEnergy() { return superpower_->GetCapEnergy(); } + + bool IsOnline() { return superpower_->IsOnline(); } + + void OnMonitor() override {} + + private: + void OutputLimitOmni(float max_power) { + float required_power_3508_sum = 0.0f; + float available_power = max_power - k3_chassis_ - 3.0f; + + /* 计算每个电机功率, 并累计正功电机的误差之和 */ + sum_error_ = 0.0f; + for (int i = 0; i < motor_count_3508_; i++) { + motor_power_3508_[i] = calculate_motor_model_power( + output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, + k2_3508_); + + if (motor_power_3508_[i] > 0) { + required_power_3508_sum += motor_power_3508_[i]; + sum_error_ += speed_error_3508_[i]; + } else { + available_power -= motor_power_3508_[i]; + } + } + + if (required_power_3508_sum > available_power) { + powercontrol_data_.is_power_limited = true; + + /* 计算误差置信度: sum_error 越大, 越倾向按误差分配功率 */ + if (sum_error_ > ERROR_POWERDISTRIBUTION_SET) { + error_confidence_ = 1.0f; + } else if (sum_error_ > POP_POWERDISTRIBUTION) { + error_confidence_ = std::clamp( + (sum_error_ - static_cast(POP_POWERDISTRIBUTION)) / + static_cast(ERROR_POWERDISTRIBUTION_SET - + POP_POWERDISTRIBUTION), + 0.0f, 1.0f); + } else { + error_confidence_ = 0.0f; + } + + for (int i = 0; i < motor_count_3508_; i++) { + if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { + /* 误差权重: 按速度跟踪误差大小分配 */ + float power_weight_error = + (sum_error_ > 1e-6f) ? (speed_error_3508_[i] / sum_error_) : 0.0f; + /* 比例权重: 按功率需求比例分配 */ + float power_weight_prop = + motor_power_3508_[i] / required_power_3508_sum; + /* 混合权重 */ + float power_weight = error_confidence_ * power_weight_error + + (1.0f - error_confidence_) * power_weight_prop; + float power_quota = available_power * power_weight; + + powercontrol_data_.new_output_current_3508[i] = + solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], + kt_3508_, k1_3508_, k2_3508_, + output_current_3508_[i]); + } else { + powercontrol_data_.new_output_current_3508[i] = + output_current_3508_[i]; + } + } + } else { + powercontrol_data_.is_power_limited = false; + for (int i = 0; i < motor_count_3508_; i++) { + powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i]; + } + } + } + + void OutputLimitHelm(float max_power) { + float required_power_3508_sum = 0.0f; + float required_power_6020_sum = 0.0f; + float sum_error_3508 = 0.0f; + float sum_error_6020 = 0.0f; + + /*初始可用功率 = 最大功率 - 静态功耗*/ + float available_power = max_power - k3_chassis_ - 3.0; + + for (int i = 0; i < motor_count_3508_; i++) { + motor_power_3508_[i] = calculate_motor_model_power( + output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, + k2_3508_); + + if (motor_power_3508_[i] > 0) { + required_power_3508_sum += motor_power_3508_[i]; + sum_error_3508 += speed_error_3508_[i]; + } else { + available_power -= motor_power_3508_[i]; + } + } + + for (int i = 0; i < motor_count_6020_; i++) { + motor_power_6020_[i] = calculate_motor_model_power( + output_current_6020_[i], rotorspeed_rpm_6020_[i], kt_6020_, k1_6020_, + k2_6020_); + + if (motor_power_6020_[i] > 0) { + required_power_6020_sum += motor_power_6020_[i]; + sum_error_6020 += speed_error_6020_[i]; + } else { + available_power -= motor_power_6020_[i]; + } + } + + float total_required_power = + required_power_3508_sum + required_power_6020_sum; + + if (total_required_power > available_power) { + powercontrol_data_.is_power_limited = true; + + /*计算 6020 组的总功率限额*/ + float limit_power_6020_total = + std::min(required_power_6020_sum, available_power * 0.8f); + + /*计算 3508 组的总功率限额 (剩下的全部)*/ + float limit_power_3508_total = + std::max(0.0f, available_power - limit_power_6020_total); + + /* 6020 组: 误差置信度 + 混合权重分配 */ + float ec_6020 = 0.0f; + if (sum_error_6020 > ERROR_POWERDISTRIBUTION_SET) { + ec_6020 = 1.0f; + } else if (sum_error_6020 > POP_POWERDISTRIBUTION) { + ec_6020 = std::clamp( + (sum_error_6020 - static_cast(POP_POWERDISTRIBUTION)) / + static_cast(ERROR_POWERDISTRIBUTION_SET - + POP_POWERDISTRIBUTION), + 0.0f, 1.0f); + } + + for (int i = 0; i < motor_count_6020_; i++) { + if (motor_power_6020_[i] > 0 && required_power_6020_sum > 1e-6f) { + float pw_err = (sum_error_6020 > 1e-6f) + ? (speed_error_6020_[i] / sum_error_6020) + : 0.0f; + float pw_prop = motor_power_6020_[i] / required_power_6020_sum; + float pw = ec_6020 * pw_err + (1.0f - ec_6020) * pw_prop; + float power_quota = limit_power_6020_total * pw; + + powercontrol_data_.new_output_current_6020[i] = + solve_current_for_power(power_quota, rotorspeed_rpm_6020_[i], + kt_6020_, k1_6020_, k2_6020_, + output_current_6020_[i]); + } else { + powercontrol_data_.new_output_current_6020[i] = + output_current_6020_[i]; + } + } + + /*误差置信度 + 混合权重分配 */ + float ec_3508 = 0.0f; + if (sum_error_3508 > ERROR_POWERDISTRIBUTION_SET) { + ec_3508 = 1.0f; + } else if (sum_error_3508 > POP_POWERDISTRIBUTION) { + ec_3508 = std::clamp( + (sum_error_3508 - static_cast(POP_POWERDISTRIBUTION)) / + static_cast(ERROR_POWERDISTRIBUTION_SET - + POP_POWERDISTRIBUTION), + 0.0f, 1.0f); + } + + for (int i = 0; i < motor_count_3508_; i++) { + if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { + float pw_err = (sum_error_3508 > 1e-6f) + ? (speed_error_3508_[i] / sum_error_3508) + : 0.0f; + float pw_prop = motor_power_3508_[i] / required_power_3508_sum; + float pw = ec_3508 * pw_err + (1.0f - ec_3508) * pw_prop; + float power_quota = limit_power_3508_total * pw; + + powercontrol_data_.new_output_current_3508[i] = + solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], + kt_3508_, k1_3508_, k2_3508_, + output_current_3508_[i]); + } else { + powercontrol_data_.new_output_current_3508[i] = + output_current_3508_[i]; + } + } + + } else { + /*功率充足，不限制*/ + powercontrol_data_.is_power_limited = false; + for (int i = 0; i < motor_count_3508_; i++) { + powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i]; + } + for (int i = 0; i < motor_count_6020_; i++) { + powercontrol_data_.new_output_current_6020[i] = output_current_6020_[i]; + } + } + } + + private: + LibXR::Mutex mutex_; + SuperPower* superpower_; + bool is_helm_; + RLS<2> rls_; + PowerControlData powercontrol_data_; + float k3_chassis_; /* 底盘静态功耗 */ + + float error_confidence_ = 0.0f; /* 误差置信度 */ + float sum_error_ = 0.0f; + + float speed_error_3508_[MAX_MOTOR_COUNT] = {}; /* 3508速度跟踪误差 */ + float speed_error_6020_[MAX_MOTOR_COUNT] = {}; /* 6020速度跟踪误差 */ + + int motor_count_3508_; /* 3508电机数目 */ + int motor_count_6020_; /* 6020电机数目 */ + + float kt_3508_ = 1.99688994e-6f; + float k1_3508_ = 0; + float k2_3508_ = 0; + Eigen::Matrix samples_3508_; + Eigen::Matrix params_3508_; + + float output_current_3508_[MAX_MOTOR_COUNT] = {}; + float rotorspeed_rpm_3508_[MAX_MOTOR_COUNT] = {}; + float motor_power_3508_[MAX_MOTOR_COUNT] = {}; + + float kt_6020_ = 1.42074505e-5f; + float k1_6020_ = 6.4276e-7f; + float k2_6020_ = 1.0e-10f; + + float output_current_6020_[MAX_MOTOR_COUNT] = {}; + float rotorspeed_rpm_6020_[MAX_MOTOR_COUNT] = {}; + float motor_power_6020_[MAX_MOTOR_COUNT] = {}; + + float measured_power_ = 0.0f; +}; diff --git a/README.md b/README.md index ac63bd5..11332bc 100644 --- a/README.md +++ b/README.md @@ -1,42 +1,42 @@ -# PowerControl - -## 1. 模块作用 -功率控制模块。根据功率预算限制底盘输出，保护供电系统。 -Manifest 描述：Power control for chassis (supports omni and helm wheel) - -## 2. 主要函数说明 -1. SetMotorData3508 / SetMotorData6020: 输入电机状态。 -2. CalculatePowerControlParam: 计算功率控制参数。 -3. OutputLimit / OutputLimitOmni / OutputLimitHelm: 按底盘类型限幅输出。 -4. GetPowerControlData: 提供限幅后的输出数据。 -5. RLS 辅助类: 参数重置与估计更新。 - -## 3. 接入步骤 -1. 添加模块并绑定 superpower。 -2. 在底盘循环中传入电机状态并读取限幅结果。 -3. 将返回结果用于底盘电流/力矩输出。 - -标准命令流程： - xrobot_add_mod PowerControl --instance-id powercontrol - xrobot_gen_main - cube-cmake --build /home/leo/Documents/bsp-dev-c/build/debug -- - -## 4. 配置示例（YAML） -module: PowerControl -entry_header: Modules/PowerControl/PowerControl.hpp -constructor_args: - - superpower: '@&super_power' - - is_helm: false - - chassis_static_power_loss: 0.5 -template_args: -[] - -## 5. 依赖与硬件 -Required Hardware: -[] - -Depends: -[] - -## 6. 代码入口 -Modules/PowerControl/PowerControl.hpp +# PowerControl + +## 1. 模块作用 +功率控制模块。根据功率预算限制底盘输出，保护供电系统。 +Manifest 描述：Power control for chassis (supports omni and helm wheel) + +## 2. 主要函数说明 +1. SetMotorData3508 / SetMotorData6020: 输入电机状态。 +2. CalculatePowerControlParam: 计算功率控制参数。 +3. OutputLimit / OutputLimitOmni / OutputLimitHelm: 按底盘类型限幅输出。 +4. GetPowerControlData: 提供限幅后的输出数据。 +5. RLS 辅助类: 参数重置与估计更新。 + +## 3. 接入步骤 +1. 添加模块并绑定 superpower。 +2. 在底盘循环中传入电机状态并读取限幅结果。 +3. 将返回结果用于底盘电流/力矩输出。 + +标准命令流程： + xrobot_add_mod PowerControl --instance-id powercontrol + xrobot_gen_main + cube-cmake --build /home/leo/Documents/bsp-dev-c/build/debug -- + +## 4. 配置示例（YAML） +module: PowerControl +entry_header: Modules/PowerControl/PowerControl.hpp +constructor_args: + - superpower: '@&super_power' + - is_helm: false + - chassis_static_power_loss: 0.5 +template_args: +[] + +## 5. 依赖与硬件 +Required Hardware: +[] + +Depends: +[] + +## 6. 代码入口 +Modules/PowerControl/PowerControl.hpp diff --git a/RLS.hpp b/RLS.hpp index a58351d..7b58dd0 100644 --- a/RLS.hpp +++ b/RLS.hpp @@ -1,88 +1,88 @@ -#include "FreeRTOS.h" -#include "matrix.h" -#include "task.h" - -#pragma once - -/** - * @brief 递归最小二乘（RLS）估计器 - * @tparam dim 参数维度 - */ -template -class RLS { - public: - RLS() = delete; // 必须只能带参数的构造函数 - - /** - * @brief 构造 RLS 估计器 - * @param delta_ 初始协方差缩放系数 - * @param lambda_ 遗忘因子 - */ - constexpr RLS(float delta_, float lambda_) - : dimension_(dim), - lambda_(lambda_), - delta_(delta_), - defaultparamsvector_(matrixf::zeros()) { - this->Reset(); // 初始化各个矩阵 - this->Validate(); // 验证参数合法性 - } - - /** - * @brief 重置估计器状态 - */ - void Reset() { - transmatrix_ = matrixf::eye() * delta_; - gainvector_ = matrixf::zeros(); - paramsvector_ = matrixf::zeros(); - } - - /** - * @brief 执行一次 RLS 更新 - * @param sampleVector 输入样本向量 - * @param actualOutput 实际输出 - * @return const Matrixf& 当前参数估计 - */ - const Matrixf& Update(Matrixf& sampleVector, - float actualOutput) { - gainvector_ = - (transmatrix_ * sampleVector) / - (1.0f + - (sampleVector.Trans() * transmatrix_ * sampleVector)[0][0] / lambda_) / - lambda_; - paramsvector_ += - gainvector_ * - (actualOutput - (sampleVector.Trans() * paramsvector_)[0][0]); - transmatrix_ = - (transmatrix_ - gainvector_ * sampleVector.Trans() * transmatrix_) / - lambda_; - - return paramsvector_; - } - - /** - * @brief 手动设置参数向量 - * @param updatedParams 参数向量 - */ - void SetParamVector(const Matrixf& updatedParams) { - paramsvector_ = updatedParams; - defaultparamsvector_ = updatedParams; - } - - private: - /** - * @brief 校验构造参数合法性 - */ - void Validate() const { - configASSERT(lambda_ >= 0.0f || lambda_ <= 1.0f); - configASSERT(delta_ > 0); - } - - uint32_t dimension_; - float lambda_; - float delta_; - - Matrixf transmatrix_; - Matrixf gainvector_; - Matrixf paramsvector_; - Matrixf defaultparamsvector_; -}; +#include + +#pragma once + +/** + * @brief 递归最小二乘（RLS）估计器 + * @tparam dim 参数维度 + */ +template +class RLS { + public: + using ParamVector = Eigen::Matrix; + + RLS() = delete; // 必须只能带参数的构造函数 + + /** + * @brief 构造 RLS 估计器 + * @param delta_ 初始协方差缩放系数 + * @param lambda_ 遗忘因子 + */ + constexpr RLS(float delta_, float lambda_) + : dimension_(dim), + lambda_(lambda_), + delta_(delta_), + defaultparamsvector_(ParamVector::Zero()) { + this->Reset(); // 初始化各个矩阵 + this->Validate(); // 验证参数合法性 + } + + /** + * @brief 重置估计器状态 + */ + void Reset() { + transmatrix_ = Eigen::Matrix::Identity() * delta_; + gainvector_ = ParamVector::Zero(); + paramsvector_ = ParamVector::Zero(); + } + + /** + * @brief 执行一次 RLS 更新 + * @param sampleVector 输入样本向量 + * @param actualOutput 实际输出 + * @return const ParamVector& 当前参数估计 + */ + const ParamVector& Update(const ParamVector& sample_vector, + float actual_output) { + gainvector_ = (transmatrix_ * sample_vector) / + (1.0f + (sample_vector.transpose() * transmatrix_ * + sample_vector)(0, 0) / + lambda_) / + lambda_; + paramsvector_ += + gainvector_ * + (actual_output - (sample_vector.transpose() * paramsvector_)(0, 0)); + transmatrix_ = (transmatrix_ - + gainvector_ * sample_vector.transpose() * transmatrix_) / + lambda_; + + return paramsvector_; + } + + /** + * @brief 手动设置参数向量 + * @param updatedParams 参数向量 + */ + void SetParamVector(const ParamVector& updated_params) { + paramsvector_ = updated_params; + defaultparamsvector_ = updated_params; + } + + private: + /** + * @brief 校验构造参数合法性 + */ + void Validate() const { + configASSERT(lambda_ >= 0.0f || lambda_ <= 1.0f); + configASSERT(delta_ > 0); + } + + uint32_t dimension_; + float lambda_; + float delta_; + + Eigen::Matrix transmatrix_; + ParamVector gainvector_; + ParamVector paramsvector_; + ParamVector defaultparamsvector_; +}; From 827f2c23551c3cf1fb18710ad52200e66244d185 Mon Sep 17 00:00:00 2001 From: llLeo306 Date: Thu, 26 Mar 2026 10:43:03 +0800 Subject: [PATCH 4/5] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E6=9E=84=E5=BB=BA?= =?UTF-8?q?=E5=B7=A5=E4=BD=9C=E6=B5=81=EF=BC=8C=E8=B0=83=E6=95=B4=E5=AE=B9?= =?UTF-8?q?=E5=99=A8=E9=95=9C=E5=83=8F=EF=BC=8C=E6=B7=BB=E5=8A=A0=E6=A8=A1?= =?UTF-8?q?=E5=9D=97=E7=94=9F=E6=88=90=E5=92=8C=E6=9E=84=E5=BB=BA=E6=AD=A5?= =?UTF-8?q?=E9=AA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/build.yml | 109 ++++++++++++++++++++++++++---------- 1 file changed, 79 insertions(+), 30 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 2846b94..aadf6eb 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -10,58 +10,108 @@ jobs: build: runs-on: ubuntu-latest container: - image: ghcr.io/xrobot-org/docker-image-stm32:main + image: ghcr.io/xrobot-org/docker-image-linux:main options: --user 0 env: XR_MODULE_NAME: ${{ github.event.repository.name }} steps: - - name: install pip packages - run: pip install libxr xrobot - - - name: Checkout bsp-dev-c + - name: Checkout current module repo to ./Modules/${{ env.XR_MODULE_NAME }} uses: actions/checkout@v4 with: - repository: QDU-Robomaster/bsp-dev-c - path: bsp-dev-c + path: Modules/${{ env.XR_MODULE_NAME }} - - name: config libxr - run: cd bsp-dev-c && xr_cubemx_cfg -d ./ --xrobot + - name: Create main.cpp + run: | + cat > main.cpp <<'EOF' + #include + #include "xrobot_main.hpp" + #include "libxr.hpp" + + int main() { + LibXR::PlatformInit(); + LibXR::STDIO::Printf("This is XRobot Module Template Test\n"); + LibXR::HardwareContainer hw; + XRobotMain(hw); + return 0; + } + EOF + + - name: Create minimal CMakeLists.txt + run: | + cat > CMakeLists.txt <<'EOF' + project(xrobot_mod_test CXX) + set(CMAKE_CXX_STANDARD 17) + add_executable(xr_test main.cpp) + set(XROBOT_MODULES_DIR ${CMAKE_CURRENT_SOURCE_DIR}/Modules/) + add_subdirectory(libxr) + target_include_directories(xr_test PUBLIC $ ${CMAKE_SOURCE_DIR}/User) + target_link_libraries(xr_test PUBLIC xr) + EOF + + - name: Pull libxr to ./libxr + run: git clone --depth=1 https://github.com/Jiu-xiao/libxr ./libxr + + - name: Checkout Referee module + uses: actions/checkout@v4 + with: + repository: QDU-Robomaster/Referee + path: Modules/Referee - - name: Checkout current module repo to bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} + - name: Checkout CMD module uses: actions/checkout@v4 with: - path: bsp-dev-c/Modules/${{ env.XR_MODULE_NAME }} + repository: QDU-Robomaster/CMD + path: Modules/CMD - name: Checkout SuperPower module uses: actions/checkout@v4 with: repository: QDU-Robomaster/SuperPower - path: bsp-dev-c/Modules/SuperPower + path: Modules/SuperPower - - name: Checkout Matrix module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/Matrix - path: bsp-dev-c/Modules/Matrix + - name: Setup Python & Install deps + run: | + python3 -m pip install --upgrade pip + pip3 install pyyaml requests - - name: Add modules + - name: Add XRobot tools to PATH run: | - cd bsp-dev-c - rm -rf ./User/xrobot.yaml - xrobot_add_mod SuperPower --instance-id super_power - xrobot_add_mod PowerControl --instance-id power_control - cat ./User/xrobot.yaml + echo "$HOME/.local/bin" >> $GITHUB_PATH - - name: xrobot generate code - run: cd bsp-dev-c && xrobot_gen_main && cat ./User/xrobot_main.hpp + - name: Install xrobot toolchain + run: | + pip3 install xrobot libxr + - name: Run xrobot_setup + run: | + xrobot_setup || true - - name: config cmake - run: cd bsp-dev-c && export GCC_TOOLCHAIN_ROOT=/opt/arm-gnu-toolchain-14.2.rel1-x86_64-arm-none-eabi/bin && export CLANG_GCC_CMSIS_COMPILER=/opt/st-arm-clang && cmake . -DCMAKE_TOOLCHAIN_FILE:STRING=cmake/starm-clang.cmake -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=TRUE -Bbuild -G Ninja - - name: build - run: cd bsp-dev-c && cmake --build build + - name: Run xrobot_init_mod + run: | + xrobot_init_mod + + - name: Add module + run: | + xrobot_add_mod BlinkLED --instance-id BlinkLED_0 + xrobot_add_mod Referee --instance-id referee + xrobot_add_mod CMD --instance-id cmd + xrobot_add_mod SuperPower --instance-id superpower + - name: Add this repo module + run: | + xrobot_add_mod ${{ env.XR_MODULE_NAME }} && cat User/xrobot.yaml + + - name: Generate main again + run: | + xrobot_setup + + - name: Build + run: | + mkdir -p build + cd build + cmake .. + make - name: Create Tag via GitHub API if: ${{ success() && github.event_name != 'pull_request' }} @@ -90,4 +140,3 @@ jobs: sha: sha }); console.log(`Created tag: ${tagName} on sha: ${sha}`); - From 664e8cab21a28a7537c3d77c1d2e88abbbfa7686 Mon Sep 17 00:00:00 2001 From: llLeo306 Date: Thu, 26 Mar 2026 10:45:21 +0800 Subject: [PATCH 5/5] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E6=9E=84=E9=80=A0?= =?UTF-8?q?=E5=87=BD=E6=95=B0=E5=8F=82=E6=95=B0=E4=B8=AD=E7=9A=84=E6=8B=BC?= =?UTF-8?q?=E5=86=99=E9=94=99=E8=AF=AF=EF=BC=8C=E5=B0=86'super=5Fpower'?= =?UTF-8?q?=E6=9B=B4=E6=94=B9=E4=B8=BA'superpower'?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PowerControl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index df580dc..b43040d 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -4,7 +4,7 @@ module_name: PowerControl module_description: Power control for chassis (supports omni and helm wheel) constructor_args: - - superpower: '@&super_power' + - superpower: '@&superpower' - is_helm: false - chassis_static_power_loss: 3.5 - motor_count_3508: 4