diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 2102edd..591717e 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,142 +1,13 @@ -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-linux:main - options: --user 0 - - env: - XR_MODULE_NAME: ${{ github.event.repository.name }} - - steps: - - name: Checkout current module repo to ./Modules/${{ env.XR_MODULE_NAME }} - uses: actions/checkout@v4 - with: - path: Modules/${{ env.XR_MODULE_NAME }} - - - 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 20) - 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 CMD module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/CMD - path: Modules/CMD - - - name: Checkout SuperPower module - uses: actions/checkout@v4 - with: - repository: QDU-Robomaster/SuperPower - path: Modules/SuperPower - - - name: Setup Python & Install deps - run: | - python3 -m pip install --upgrade pip - pip3 install pyyaml requests - - - name: Add XRobot tools to PATH - run: | - echo "$HOME/.local/bin" >> $GITHUB_PATH - - - name: Install xrobot toolchain - run: | - pip3 install xrobot libxr - - - name: Run xrobot_setup - run: | - xrobot_setup || true - - - 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' }} - 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: Module CI +on: + push: + pull_request: + workflow_dispatch: +jobs: + build: + uses: xrobot-org/XRobot/.github/workflows/module-ci.yml@dev + with: + xrobot-ref: dev + libxr-ref: dev + dependency-ref: refs/heads/dev + template-args: '[]' diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/PowerControl.hpp b/PowerControl.hpp index de244d8..d6c10f8 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -1,530 +1,737 @@ -#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: '@nullptr' - - is_helm: false - - chassis_static_power_loss: 3.5 - - motor_count_3508: 4 - - motor_count_6020: 0 -template_args: [] -required_hardware: [] -depends: - - qdu-future/SuperPower -=== 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 -#define CHASSIS_POWER_LIMIT_MARGIN_W 4.0f /* 底盘限功率余量 */ - -/** - * @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 x3 = -b / (2.0f * a); - - float final_current = 0; - - if (delta < 1e-9f) { - final_current = x3; - } else { - float sqrt_delta = sqrtf(delta); - float x1 = (-b + sqrt_delta) / (2.0f * a); - float x2 = (-b - sqrt_delta) / (2.0f * a); - - 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; - /* 3508 组分配偏置: 先保底, 再分剩余功率 */ - struct AllocationBias3508 { - bool enabled = false; - float reserve_fraction = 0.0f; - float reserve_weight[MAX_MOTOR_COUNT] = {}; - float allocation_weight_scale[MAX_MOTOR_COUNT] = {}; - }; - - PowerControl(LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, - SuperPower* super_power, bool is_helm = false, - float chassis_static_power_loss = 0.0f, int motor_count_3508 = 4, - int motor_count_6020 = 4) - : superpower_(super_power), - 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); - ASSERT(superpower_ != nullptr); - 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) { - LibXR::Mutex::LockGuard lock(mutex_); - 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) { - LibXR::Mutex::LockGuard lock(mutex_); - 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 SetAllocationBias3508(const AllocationBias3508& bias) { - LibXR::Mutex::LockGuard lock(mutex_); - allocation_bias_3508_.enabled = bias.enabled; - allocation_bias_3508_.reserve_fraction = - std::clamp(bias.reserve_fraction, 0.0f, 1.0f); - for (int i = 0; i < MAX_MOTOR_COUNT; i++) { - allocation_bias_3508_.reserve_weight[i] = - std::max(0.0f, bias.reserve_weight[i]); - const float WEIGHT_SCALE = bias.allocation_weight_scale[i]; - /* 未配置时保持 1.0 */ - allocation_bias_3508_.allocation_weight_scale[i] = - WEIGHT_SCALE > 0.0f ? WEIGHT_SCALE : 1.0f; - } - } - - void CalculatePowerControlParam() { - LibXR::Mutex::LockGuard lock(mutex_); - /* 用实测功率修正 3508 功率模型参数 */ - 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) { - LibXR::Mutex::LockGuard lock(mutex_); - if (is_helm_) { - OutputLimitHelm(max_power); - } else { - OutputLimitOmni(max_power); - } - } - - PowerControlData GetPowerControlData() { - LibXR::Mutex::LockGuard lock(mutex_); - return powercontrol_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_ - CHASSIS_POWER_LIMIT_MARGIN_W; - - /* 正功率参与分配, 负功率等效增加可用功率 */ - 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; - } - - if (!allocation_bias_3508_.enabled) { - /* 默认按误差/需求混合权重分配 */ - 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 { - /* 偏置路径: 先保底, 再分剩余功率 */ - float reserve_power_3508[MAX_MOTOR_COUNT] = {}; - float residual_power_3508[MAX_MOTOR_COUNT] = {}; - float weighted_residual_3508[MAX_MOTOR_COUNT] = {}; - const float RESERVE_POWER_POOL = std::max(0.0f, available_power) * - allocation_bias_3508_.reserve_fraction; - float reserve_weight_sum = 0.0f; - float reserved_power_sum = 0.0f; - - for (int i = 0; i < motor_count_3508_; i++) { - if (motor_power_3508_[i] > 0.0f) { - reserve_weight_sum += allocation_bias_3508_.reserve_weight[i]; - } - } - - if (RESERVE_POWER_POOL > 1e-6f && reserve_weight_sum > 1e-6f) { - /* 保底池只分给正功率电机 */ - for (int i = 0; i < motor_count_3508_; i++) { - if (motor_power_3508_[i] <= 0.0f) { - continue; - } - const float RESERVE_QUOTA = - RESERVE_POWER_POOL * allocation_bias_3508_.reserve_weight[i] / - reserve_weight_sum; - reserve_power_3508[i] = - std::min(motor_power_3508_[i], RESERVE_QUOTA); - reserved_power_sum += reserve_power_3508[i]; - } - } - - /* 剩余功率再走普通分配 */ - const float REMAINING_AVAILABLE_POWER = - std::max(0.0f, available_power - reserved_power_sum); - float residual_required_power_sum = 0.0f; - for (int i = 0; i < motor_count_3508_; i++) { - residual_power_3508[i] = - std::max(0.0f, motor_power_3508_[i] - reserve_power_3508[i]); - residual_required_power_sum += residual_power_3508[i]; - } - - /* 剩余功率权重仍基于误差/需求, 再叠加偏置缩放 */ - float weighted_residual_sum = 0.0f; - if (residual_required_power_sum > 1e-6f) { - for (int i = 0; i < motor_count_3508_; i++) { - if (residual_power_3508[i] <= 0.0f) { - continue; - } - const float POWER_WEIGHT_ERROR = - (sum_error_ > 1e-6f) ? (speed_error_3508_[i] / sum_error_) - : 0.0f; - const float POWER_WEIGHT_PROP = - residual_power_3508[i] / residual_required_power_sum; - const float POWER_WEIGHT = - error_confidence_ * POWER_WEIGHT_ERROR + - (1.0f - error_confidence_) * POWER_WEIGHT_PROP; - weighted_residual_3508[i] = - POWER_WEIGHT * allocation_bias_3508_.allocation_weight_scale[i]; - weighted_residual_sum += weighted_residual_3508[i]; - } - } - - for (int i = 0; i < motor_count_3508_; i++) { - if (motor_power_3508_[i] > 0.0f && required_power_3508_sum > 1e-6f) { - float residual_quota = 0.0f; - if (residual_power_3508[i] > 0.0f && - REMAINING_AVAILABLE_POWER > 1e-6f) { - if (weighted_residual_sum > 1e-6f) { - residual_quota = REMAINING_AVAILABLE_POWER * - weighted_residual_3508[i] / - weighted_residual_sum; - } else if (residual_required_power_sum > 1e-6f) { - residual_quota = REMAINING_AVAILABLE_POWER * - residual_power_3508[i] / - residual_required_power_sum; - } - } - /* 单路电机的最终配额 = 保底配额 + 剩余池配额, 但不超过自身请求 */ - const float POWER_QUOTA = std::min( - motor_power_3508_[i], reserve_power_3508[i] + residual_quota); - 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 = - std::max(0.0f, max_power - k3_chassis_ - CHASSIS_POWER_LIMIT_MARGIN_W); - - 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]; - } - } - - 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]; - } - } - - 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_; /* 在线辨识 3508 二次损耗模型参数 */ - 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速度跟踪误差 */ - AllocationBias3508 allocation_bias_3508_{}; /* 3508 组动态分配偏置 */ - - 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_; /* RLS 输入: [i^2_sum, rpm^2_sum] */ - Eigen::Matrix params_3508_; /* RLS 输出: [k1, k2] */ - - 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_description: 底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮、麦轮与舵轮底盘 / Chassis power control Module that redistributes the motor output currents under a power limit, for omni-wheel, mecanum and steering-wheel chassis +depends: +- id: QDU-Robomaster/SuperPower + ref: same-or-dev +=== END MANIFEST === */ +// clang-format on + +#include +#include +#include +#include + +#include "RLS.hpp" +#include "SuperPower.hpp" +#include "message.hpp" +#include "thread.hpp" + +/// 速度误差总和高于该值时,功率全部按误差分配 +/// Above this speed error sum, power is allocated entirely by error +#define ERROR_POWERDISTRIBUTION_SET 20 +/// 速度误差总和低于该值时,功率全部按需求比例分配 +/// Below this speed error sum, power is allocated entirely by demand ratio +#define POP_POWERDISTRIBUTION 15 +/// 底盘限功率余量 (W) +/// Chassis power limit margin (W) +#define CHASSIS_POWER_LIMIT_MARGIN_W 4.0f + +/** + * @brief 计算单个电机的模型预测功率,不含静态损耗。 + * Compute the model-predicted power of one motor, excluding the static loss. + * + * @param current 电机电流。 + * Motor current. + * @param rpm 转子转速 (rpm)。 + * Rotor speed (rpm). + * @param kt 机械功率系数。 + * Mechanical power coefficient. + * @param k1 电流平方项系数。 + * Coefficient of the squared current term. + * @param k2 转速平方项系数。 + * Coefficient of the squared speed term. + * @return 预测功率 (W)。 + * Predicted power (W). + */ +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 由目标功率反解电流,结果限幅到 ±16384。 + * Solve the current from a target power, clamped to ±16384. + * + * @param target_power 目标功率 (W)。 + * Target power (W). + * @param rpm 转子转速 (rpm)。 + * Rotor speed (rpm). + * @param kt 机械功率系数。 + * Mechanical power coefficient. + * @param k1 电流平方项系数。 + * Coefficient of the squared current term. + * @param k2 转速平方项系数。 + * Coefficient of the squared speed term. + * @param original_current 原始期望电流,其符号决定取二次方程的哪个根。 + * Original desired current, whose sign selects the root of the + * quadratic equation. + * @return 反解得到的电流。 + * Solved current. + */ +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 x3 = -b / (2.0f * a); + + float final_current = 0; + + if (delta < 1e-9f) + { + final_current = x3; + } + else + { + float sqrt_delta = sqrtf(delta); + float x1 = (-b + sqrt_delta) / (2.0f * a); + float x2 = (-b - sqrt_delta) / (2.0f * a); + + if (original_current >= 0) + { + final_current = x1; + } + else + { + final_current = x2; + } + } + + return std::clamp(final_current, -16384.0f, 16384.0f); +} + +/// 最大电机数量 +/// Maximum number of motors +static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; + +/** + * @brief 功率限制后的输出。 + * Output after the power limit. + */ +struct PowerControlData +{ + /// 3508 功率限制后的电流 + /// Currents of the 3508 motors after the power limit + float new_output_current_3508[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + /// 6020 功率限制后的电流 + /// Currents of the 6020 motors after the power limit + float new_output_current_6020[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + /// 是否触发了功率限制 + /// Whether the power limit was applied + bool is_power_limited = false; +}; + +/** + * @brief 底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮、麦轮与舵轮 + * 底盘。 + * Chassis power control Module that redistributes the motor output currents + * under a power limit, for omni-wheel, mecanum and steering-wheel chassis. + */ +class PowerControl +{ + public: + /// 最大电机数量 + /// Maximum number of motors + static constexpr int MAX_MOTOR_COUNT = POWER_CONTROL_MAX_MOTOR_COUNT; + + /** + * @brief 3508 组的分配偏置:先按保底池分配,再分剩余功率。 + * Allocation bias of the 3508 group: distribute a reserve pool first, then + * the remaining power. + */ + struct AllocationBias3508 + { + /// 是否启用 + /// Whether enabled + bool enabled = false; + /// 可用功率中作为保底池的比例,范围 [0, 1] + /// Fraction of the available power used as the reserve pool, in [0, 1] + float reserve_fraction = 0.0f; + /// 保底池分配权重 + /// Reserve pool allocation weights + float reserve_weight[MAX_MOTOR_COUNT] = {}; + /// 剩余功率权重的缩放,未配置或不大于 0 时为 1 + /// Scale of the remaining-power weights, 1 when unset or not greater than 0 + float allocation_weight_scale[MAX_MOTOR_COUNT] = {}; + }; + + /** + * @brief 构造 PowerControl。 + * Construct PowerControl. + * + * @param super_power SuperPower 实例,提供实测底盘功率与超级电容在线状态。 + * SuperPower instance that provides the measured chassis power + * and the supercapacitor online state. + * @param is_helm true 为舵轮底盘,同时限制 6020;false 为全向轮或麦轮底盘。 + * True for a steering-wheel chassis, which also limits the 6020; false + * for an omni-wheel or mecanum chassis. + * @param chassis_static_power_loss 底盘静态功耗 (W)。 + * Chassis static power loss (W). + * @param motor_count_3508 3508 电机数量,最大 `MAX_MOTOR_COUNT`。 + * Number of 3508 motors, at most `MAX_MOTOR_COUNT`. + * @param motor_count_6020 6020 电机数量,最大 `MAX_MOTOR_COUNT`。 + * Number of 6020 motors, at most `MAX_MOTOR_COUNT`. + */ + PowerControl(SuperPower& super_power, bool is_helm = false, + float chassis_static_power_loss = 0.0f, int motor_count_3508 = 4, + int motor_count_6020 = 4) + : superpower_(&super_power), + 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) + { + 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); + } + + /** + * @brief 写入 3508 电机本周期的期望电流、转子转速和可选的速度跟踪误差。 + * Write the desired current, rotor speed and optional speed tracking error of + * the 3508 motors for this cycle. + * + * @param output_current 期望电流数组,长度为 3508 电机数量。 + * Desired current array with one entry per 3508 motor. + * @param rotorspeed_rpm 转子转速数组 (rpm),长度为 3508 电机数量。 + * Rotor speed array (rpm) with one entry per 3508 motor. + * @param speed_error 速度跟踪误差数组,取绝对值使用;为空时保持上一次的值。 + * Speed tracking error array, used as absolute values; the previous + * values are kept when null. + */ + void SetMotorData3508(float* output_current, float* rotorspeed_rpm, + float* speed_error = nullptr) + { + LibXR::Mutex::LockGuard lock(mutex_); + 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]); + } + } + } + + /** + * @brief 写入 6020 电机本周期的期望电流、转子转速和可选的速度跟踪误差。 + * Write the desired current, rotor speed and optional speed tracking error of + * the 6020 motors for this cycle. + * + * @param output_current 期望电流数组,长度为 6020 电机数量。 + * Desired current array with one entry per 6020 motor. + * @param rotorspeed_rpm 转子转速数组 (rpm),长度为 6020 电机数量。 + * Rotor speed array (rpm) with one entry per 6020 motor. + * @param speed_error 速度跟踪误差数组,取绝对值使用;为空时保持上一次的值。 + * Speed tracking error array, used as absolute values; the previous + * values are kept when null. + */ + void SetMotorData6020(float* output_current, float* rotorspeed_rpm, + float* speed_error = nullptr) + { + LibXR::Mutex::LockGuard lock(mutex_); + 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]); + } + } + } + + /** + * @brief 设置 3508 组的分配偏置,仅全向轮与麦轮路径使用。 + * Set the allocation bias of the 3508 group, used by the omni-wheel and + * mecanum path. + * + * @param bias 分配偏置。 + * Allocation bias. + */ + void SetAllocationBias3508(const AllocationBias3508& bias) + { + LibXR::Mutex::LockGuard lock(mutex_); + allocation_bias_3508_.enabled = bias.enabled; + allocation_bias_3508_.reserve_fraction = + std::clamp(bias.reserve_fraction, 0.0f, 1.0f); + for (int i = 0; i < MAX_MOTOR_COUNT; i++) + { + allocation_bias_3508_.reserve_weight[i] = std::max(0.0f, bias.reserve_weight[i]); + const float WEIGHT_SCALE = bias.allocation_weight_scale[i]; + /* 未配置时保持 1.0 */ + allocation_bias_3508_.allocation_weight_scale[i] = + WEIGHT_SCALE > 0.0f ? WEIGHT_SCALE : 1.0f; + } + } + + /** + * @brief 读取实测底盘功率,并用 RLS 在线辨识 3508 功率模型的 `k1`、`k2`。 + * Read the measured chassis power and identify `k1` and `k2` of the 3508 power + * model online with RLS. + */ + void CalculatePowerControlParam() + { + LibXR::Mutex::LockGuard lock(mutex_); + /* 用实测功率修正 3508 功率模型参数 */ + 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)); + } + } + + /** + * @brief 按功率上限计算限制后的输出电流,结果由 `GetPowerControlData()` 取回。 + * Compute the limited output currents under the power limit; the result is + * fetched with `GetPowerControlData()`. + * + * @param max_power 底盘功率上限 (W)。 + * Chassis power limit (W). + */ + void OutputLimit(float max_power) + { + LibXR::Mutex::LockGuard lock(mutex_); + if (is_helm_) + { + OutputLimitHelm(max_power); + } + else + { + OutputLimitOmni(max_power); + } + } + + /** + * @brief 取回功率限制后的输出。 + * Fetch the output after the power limit. + * + * @return 功率限制后的输出。 + * Output after the power limit. + */ + PowerControlData GetPowerControlData() + { + LibXR::Mutex::LockGuard lock(mutex_); + return powercontrol_data_; + } + + /** + * @brief 获取最近一次 `CalculatePowerControlParam()` 读到的实测功率。 + * Get the measured power read by the latest `CalculatePowerControlParam()`. + * + * @return 实测功率 (W)。 + * Measured power (W). + */ + float GetMeasuredPower() const { return measured_power_; } + + /** + * @brief 转发 `SuperPower::GetCapEnergy()`。 + * Forward `SuperPower::GetCapEnergy()`. + * + * @return 超级电容能量。 + * Supercapacitor energy. + */ + float GetCapEnergy() { return superpower_->GetCapEnergy(); } + + /** + * @brief 转发 `SuperPower::IsOnline()`。 + * Forward `SuperPower::IsOnline()`. + * + * @return 超级电容在线为 true。 + * True when the supercapacitor is online. + */ + bool IsOnline() { return superpower_->IsOnline(); } + + private: + void OutputLimitOmni(float max_power) + { + float required_power_3508_sum = 0.0f; + float available_power = max_power - k3_chassis_ - CHASSIS_POWER_LIMIT_MARGIN_W; + + /* 正功率参与分配, 负功率等效增加可用功率 */ + 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; + } + + if (!allocation_bias_3508_.enabled) + { + /* 默认按误差/需求混合权重分配 */ + 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 + { + /* 偏置路径: 先保底, 再分剩余功率 */ + float reserve_power_3508[MAX_MOTOR_COUNT] = {}; + float residual_power_3508[MAX_MOTOR_COUNT] = {}; + float weighted_residual_3508[MAX_MOTOR_COUNT] = {}; + const float RESERVE_POWER_POOL = + std::max(0.0f, available_power) * allocation_bias_3508_.reserve_fraction; + float reserve_weight_sum = 0.0f; + float reserved_power_sum = 0.0f; + + for (int i = 0; i < motor_count_3508_; i++) + { + if (motor_power_3508_[i] > 0.0f) + { + reserve_weight_sum += allocation_bias_3508_.reserve_weight[i]; + } + } + + if (RESERVE_POWER_POOL > 1e-6f && reserve_weight_sum > 1e-6f) + { + /* 保底池只分给正功率电机 */ + for (int i = 0; i < motor_count_3508_; i++) + { + if (motor_power_3508_[i] <= 0.0f) + { + continue; + } + const float RESERVE_QUOTA = RESERVE_POWER_POOL * + allocation_bias_3508_.reserve_weight[i] / + reserve_weight_sum; + reserve_power_3508[i] = std::min(motor_power_3508_[i], RESERVE_QUOTA); + reserved_power_sum += reserve_power_3508[i]; + } + } + + /* 剩余功率再走普通分配 */ + const float REMAINING_AVAILABLE_POWER = + std::max(0.0f, available_power - reserved_power_sum); + float residual_required_power_sum = 0.0f; + for (int i = 0; i < motor_count_3508_; i++) + { + residual_power_3508[i] = + std::max(0.0f, motor_power_3508_[i] - reserve_power_3508[i]); + residual_required_power_sum += residual_power_3508[i]; + } + + /* 剩余功率权重仍基于误差/需求, 再叠加偏置缩放 */ + float weighted_residual_sum = 0.0f; + if (residual_required_power_sum > 1e-6f) + { + for (int i = 0; i < motor_count_3508_; i++) + { + if (residual_power_3508[i] <= 0.0f) + { + continue; + } + const float POWER_WEIGHT_ERROR = + (sum_error_ > 1e-6f) ? (speed_error_3508_[i] / sum_error_) : 0.0f; + const float POWER_WEIGHT_PROP = + residual_power_3508[i] / residual_required_power_sum; + const float POWER_WEIGHT = error_confidence_ * POWER_WEIGHT_ERROR + + (1.0f - error_confidence_) * POWER_WEIGHT_PROP; + weighted_residual_3508[i] = + POWER_WEIGHT * allocation_bias_3508_.allocation_weight_scale[i]; + weighted_residual_sum += weighted_residual_3508[i]; + } + } + + for (int i = 0; i < motor_count_3508_; i++) + { + if (motor_power_3508_[i] > 0.0f && required_power_3508_sum > 1e-6f) + { + float residual_quota = 0.0f; + if (residual_power_3508[i] > 0.0f && REMAINING_AVAILABLE_POWER > 1e-6f) + { + if (weighted_residual_sum > 1e-6f) + { + residual_quota = REMAINING_AVAILABLE_POWER * weighted_residual_3508[i] / + weighted_residual_sum; + } + else if (residual_required_power_sum > 1e-6f) + { + residual_quota = REMAINING_AVAILABLE_POWER * residual_power_3508[i] / + residual_required_power_sum; + } + } + /* 单路电机的最终配额 = 保底配额 + 剩余池配额, 但不超过自身请求 */ + const float POWER_QUOTA = + std::min(motor_power_3508_[i], reserve_power_3508[i] + residual_quota); + 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 = + std::max(0.0f, max_power - k3_chassis_ - CHASSIS_POWER_LIMIT_MARGIN_W); + + 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]; + } + } + + 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]; + } + } + + 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_; /* 在线辨识 3508 二次损耗模型参数 */ + 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速度跟踪误差 */ + AllocationBias3508 allocation_bias_3508_{}; /* 3508 组动态分配偏置 */ + + 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_; /* RLS 输入: [i^2_sum, rpm^2_sum] */ + Eigen::Matrix params_3508_; /* RLS 输出: [k1, k2] */ + + 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 11332bc..842620a 100644 --- a/README.md +++ b/README.md @@ -1,42 +1,115 @@ -# 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 + +底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮、麦轮与舵轮底盘 / Chassis power control Module that redistributes the motor output currents under a power limit, for omni-wheel, mecanum and steering-wheel chassis + +## 1. 模块作用 / Purpose + +PowerControl 支持全向轮与麦轮底盘(仅 3508)和舵轮底盘(3508 驱动加 6020 转向)。它由底盘在自己的控制循环中调用,`SetMotorData3508()`、`SetMotorData6020()`、`SetAllocationBias3508()`、`CalculatePowerControlParam()`、`OutputLimit()` 与 `GetPowerControlData()` 内部由同一把互斥锁保护。 + +每个控制周期,底盘按下面顺序调用: + +1. `SetMotorData3508(output_current, rotorspeed_rpm, speed_error = nullptr)`,舵轮再调用 `SetMotorData6020(...)`:写入各电机本周期的期望电流、转子转速(rpm)和可选的速度跟踪误差(取绝对值)。数组长度为构造时的电机数量。 +2. `CalculatePowerControlParam()`:从 `SuperPower::GetChassisPower()` 读取实测底盘功率,扣除 3508 机械功率、底盘静态功耗(舵轮还扣除 6020 模型功率)后,用递归最小二乘(RLS,遗忘因子 0.99999)在线辨识 3508 功率模型 `P = kt·I·ω + k1·I² + k2·ω²` 中的 `k1`、`k2`。仅在超级电容在线、实测功率大于 5 W 且残差为正时更新,`k1`、`k2` 不小于 `1e-7`。 +3. `OutputLimit(max_power)`:可用功率为 `max_power - chassis_static_power_loss - 4 W` 余量。模型预测的总需求功率超出可用功率时,按权重给每个电机分配功率配额,再解二次方程反算电流,限幅 ±16384;未超出时原样输出。权重在“按速度误差分配”和“按功率需求比例分配”之间混合:误差总和大于 20 时全部按误差,15 至 20 之间线性过渡,小于 15 时按需求比例。负功率(回馈)电机退出分配,其功率计入可用功率(全向轮与麦轮)。 + - 全向轮与麦轮:限制 3508。 + - 舵轮:6020 组最多占可用功率的 80%,剩余给 3508 组;两组内部各自按上述权重分配。 +4. `GetPowerControlData()`:取回 `PowerControlData`,包含 `new_output_current_3508[]`、`new_output_current_6020[]` 和 `is_power_limited`。 + +其他接口: + +- `SetAllocationBias3508(const AllocationBias3508&)`:全向轮与麦轮路径使用。启用后先把可用功率的 `reserve_fraction` 作为保底池,按 `reserve_weight[]` 分给正功率电机(不超过其需求),剩余功率再按上述权重乘以 `allocation_weight_scale[]`(未配置或不大于 0 时为 1)分配。 +- `GetMeasuredPower()`:最近一次 `CalculatePowerControlParam()` 读到的实测功率。 +- `GetCapEnergy()`、`IsOnline()`:转发 `SuperPower` 的同名接口。 + +电机数量上限为 `PowerControl::MAX_MOTOR_COUNT`(6),超出部分被截断。3508 的 `kt` 与 6020 的 `kt`、`k1`、`k2` 是源码中的固定常数。 + +PowerControl supports omni-wheel and mecanum chassis (3508 only) and steering-wheel chassis (3508 drive plus 6020 steering). The chassis calls it in its own control loop, and `SetMotorData3508()`, `SetMotorData6020()`, `SetAllocationBias3508()`, `CalculatePowerControlParam()`, `OutputLimit()` and `GetPowerControlData()` are protected internally by the same mutex. + +Every control cycle, the chassis calls in the following order: + +1. `SetMotorData3508(output_current, rotorspeed_rpm, speed_error = nullptr)`, and for the steering-wheel chassis also `SetMotorData6020(...)`: write the desired current, rotor speed (rpm) and optional speed tracking error (absolute value) of each motor for this cycle. The array length is the motor count given at construction. +2. `CalculatePowerControlParam()`: read the measured chassis power from `SuperPower::GetChassisPower()`, subtract the 3508 mechanical power and the chassis static power loss (and for the steering-wheel chassis the 6020 model power), and identify `k1` and `k2` of the 3508 power model `P = kt·I·ω + k1·I² + k2·ω²` online with recursive least squares (RLS, forgetting factor 0.99999). The update runs only when the supercapacitor is online, the measured power exceeds 5 W and the residual is positive, and `k1` and `k2` are at least `1e-7`. +3. `OutputLimit(max_power)`: the available power is `max_power - chassis_static_power_loss` minus a 4 W margin. When the total demanded power predicted by the model exceeds the available power, a power quota is assigned to each motor by weight and the current is solved back from a quadratic equation, clamped to ±16384; otherwise the currents are output unchanged. The weight mixes "by speed error" and "by power demand ratio": entirely by error when the error sum is above 20, a linear transition between 15 and 20, and by demand ratio below 15. Motors with negative (regenerative) power leave the allocation and their power is added to the available power (omni-wheel and mecanum chassis). + - Omni-wheel and mecanum chassis: limits the 3508. + - Steering-wheel chassis: the 6020 group takes at most 80% of the available power and the rest goes to the 3508 group; inside each group the allocation uses the weights above. +4. `GetPowerControlData()`: fetch `PowerControlData`, which contains `new_output_current_3508[]`, `new_output_current_6020[]` and `is_power_limited`. + +Other interfaces: + +- `SetAllocationBias3508(const AllocationBias3508&)`: used by the omni-wheel and mecanum path. When enabled, `reserve_fraction` of the available power forms a reserve pool, which is distributed to the motors with positive power by `reserve_weight[]` (not exceeding their demand), and the remaining power is distributed with the weights above multiplied by `allocation_weight_scale[]` (1 when unset or not greater than 0). +- `GetMeasuredPower()`: the measured power read by the latest `CalculatePowerControlParam()`. +- `GetCapEnergy()`, `IsOnline()`: forward the same-named interfaces of `SuperPower`. + +The motor count is limited to `PowerControl::MAX_MOTOR_COUNT` (6), and larger values are truncated. The `kt` of the 3508 and the `kt`, `k1` and `k2` of the 6020 are fixed constants in the source. + +## 2. 构造接口 / Constructor + +```cpp +PowerControl(SuperPower& super_power, + bool is_helm = false, + float chassis_static_power_loss = 0.0f, + int motor_count_3508 = 4, + int motor_count_6020 = 4); +``` + +依赖: + +- `super_power`:`SuperPower` 实例,提供实测底盘功率和超级电容在线状态。 + +配置参数: + +- `is_helm`:`true` 为舵轮底盘,同时限制 6020;`false` 为全向轮或麦轮底盘,默认 `false`。 +- `chassis_static_power_loss`:底盘静态功耗,单位 W,从可用功率中扣除,默认 `0.0`。 +- `motor_count_3508`:3508 电机数量,默认 `4`,最大 6。 +- `motor_count_6020`:6020 电机数量,默认 `4`,最大 6。 + +Dependencies: + +- `super_power`: the `SuperPower` instance that provides the measured chassis power and the supercapacitor online state. + +Configuration parameters: + +- `is_helm`: `true` for a steering-wheel chassis, which also limits the 6020; `false` for an omni-wheel or mecanum chassis, default `false`. +- `chassis_static_power_loss`: chassis static power loss in W, deducted from the available power, default `0.0`. +- `motor_count_3508`: number of 3508 motors, default `4`, at most 6. +- `motor_count_6020`: number of 6020 motors, default `4`, at most 6. + +## 3. Topic + +无 / None + +## 4. 配置示例 / Configuration Example + +`xrobot instance add QDU-Robomaster/PowerControl` 写入的实例,`super_power` 填写为 `QDU-Robomaster/SuperPower` 实例的 id,该实例在 `modules:` 中排在本实例之前。PowerControl 实例通常再作为依赖传给底盘模块,例如 `QDU-Robomaster/Chassis`: + +An instance written by `xrobot instance add QDU-Robomaster/PowerControl`, with `super_power` set to the id of the `QDU-Robomaster/SuperPower` instance, which is listed before this instance in `modules:`. The PowerControl instance is usually passed on as a dependency to a chassis Module, for example `QDU-Robomaster/Chassis`: + +```yaml +modules: + - module: QDU-Robomaster/PowerControl + id: power_control + args: + - super_power: superpower + - is_helm: false + - chassis_static_power_loss: 4.5f + - motor_count_3508: 4 + - motor_count_6020: 0 +``` + +## 5. 依赖与硬件 / Dependencies and Hardware + +依赖: + +- `QDU-Robomaster/SuperPower`:提供实测底盘功率和超级电容在线状态。 +- Eigen(`Eigen/Core`,由 LibXR 自带):`RLS.hpp` 中的递归最小二乘估计器。 +- LibXR。 + +硬件:全向轮或麦轮底盘的 3508 电机,或舵轮底盘的 3508 驱动电机与 6020 转向电机;实测功率来自 `SuperPower`。 + +Dependencies: + +- `QDU-Robomaster/SuperPower`: provides the measured chassis power and the supercapacitor online state. +- Eigen (`Eigen/Core`, shipped with LibXR): the recursive least squares estimator in `RLS.hpp`. +- LibXR. + +Hardware: the 3508 motors of an omni-wheel or mecanum chassis, or the 3508 drive motors and 6020 steering motors of a steering-wheel chassis; the measured power comes from `SuperPower`. diff --git a/RLS.hpp b/RLS.hpp index 147cfe8..f887855 100644 --- a/RLS.hpp +++ b/RLS.hpp @@ -3,20 +3,29 @@ #include /** - * @brief 递归最小二乘(RLS)估计器 - * @tparam dim 参数维度 + * @brief 递归最小二乘(RLS)估计器。 + * Recursive least squares (RLS) estimator. + * + * @tparam dim 参数维度。 + * Parameter dimension. */ template class RLS { public: + /// 参数向量类型 + /// Parameter vector type using ParamVector = Eigen::Matrix; - RLS() = delete; // 必须只能带参数的构造函数 + RLS() = delete; /** - * @brief 构造 RLS 估计器 - * @param delta_ 初始协方差缩放系数 - * @param lambda_ 遗忘因子 + * @brief 构造 RLS 估计器。 + * Construct the RLS estimator. + * + * @param delta_ 初始协方差缩放系数。 + * Initial covariance scale factor. + * @param lambda_ 遗忘因子。 + * Forgetting factor. */ constexpr RLS(float delta_, float lambda_) : dimension_(dim), @@ -27,7 +36,8 @@ class RLS { } /** - * @brief 重置估计器状态 + * @brief 重置估计器状态。 + * Reset the estimator state. */ void Reset() { transmatrix_ = Eigen::Matrix::Identity() * delta_; @@ -36,10 +46,15 @@ class RLS { } /** - * @brief 执行一次 RLS 更新 - * @param sampleVector 输入样本向量 - * @param actualOutput 实际输出 - * @return const ParamVector& 当前参数估计 + * @brief 执行一次 RLS 更新。 + * Perform one RLS update. + * + * @param sample_vector 输入样本向量。 + * Input sample vector. + * @param actual_output 实际输出。 + * Actual output. + * @return 当前参数估计的引用。 + * Reference to the current parameter estimate. */ const ParamVector& Update(const ParamVector& sample_vector, float actual_output) { @@ -59,8 +74,11 @@ class RLS { } /** - * @brief 手动设置参数向量 - * @param updatedParams 参数向量 + * @brief 手动设置参数向量。 + * Set the parameter vector manually. + * + * @param updated_params 参数向量。 + * Parameter vector. */ void SetParamVector(const ParamVector& updated_params) { paramsvector_ = updated_params;