From 60a7c0bf79a62be9c0275b37644706533619d8d3 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Tue, 15 Sep 2026 04:38:12 +0100 Subject: [PATCH 01/15] feat: adopt static module construction --- PowerControl.hpp | 1106 ++++++++++++++++++++++++---------------------- 1 file changed, 578 insertions(+), 528 deletions(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index eaeda4a..edb295c 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -1,528 +1,578 @@ -#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: '@&superpower' - - 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 -#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); - 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_name: PowerControl +module_description: Power control for chassis (supports omni and helm wheel) +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" + +#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: + 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(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); + } + + 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() {} + + 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; +}; From 5848d6de9aaa3e0ae567726b74b9d2d4a4511937 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Fri, 18 Sep 2026 01:50:38 +0100 Subject: [PATCH 02/15] Migrate to static assembly --- .github/workflows/build.yml | 274 +++++++++++++++++------------------- PowerControl.hpp | 11 +- 2 files changed, 139 insertions(+), 146 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 2102edd..3e201c8 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,142 +1,132 @@ -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: XRobot Module Build Test +'on': + push: null + pull_request: null + schedule: + - cron: 0 3 1 * * +permissions: + contents: read +jobs: + build: + runs-on: ubuntu-latest + container: + image: ghcr.io/xrobot-org/docker-image-linux:main + options: --user 0 + env: + XR_MODULE_ID: ${{ github.repository }} + XR_MODULE_NAME: ${{ github.event.repository.name }} + XR_REF_NAME: ${{ github.head_ref || github.ref_name }} + XR_REF_KIND: ${{ github.ref_type }} + XR_TEMPLATE_ARGS: '[]' + XROBOT_REF: ${{ vars.XROBOT_REF || '' }} + LIBXR_REF: ${{ vars.LIBXR_REF || '' }} + PYTHONPATH: ${{ github.workspace }}/XRobot/src + steps: + - name: Checkout the exact Module revision + uses: actions/checkout@v4 + with: + path: Modules/${{ github.repository }} + ref: ${{ github.event.pull_request.head.sha || github.sha }} + submodules: recursive + - name: Checkout matching core tools + shell: bash + run: | + set -euo pipefail + checkout_tool() { + local repo="$1" directory="$2" selected="$3" available + if [[ -n "$selected" ]]; then + if [[ "$selected" != refs/heads/* && "$selected" != refs/tags/* && + ! "$selected" =~ ^[0-9a-fA-F]{40}$ ]]; then + echo "Use a full refs/heads/, refs/tags/ or commit SHA for $directory" >&2 + return 1 + fi + elif [[ "$XR_REF_KIND" == tag ]]; then + selected="refs/tags/$XR_REF_NAME" + else + available=$(git ls-remote --heads "$repo" "refs/heads/$XR_REF_NAME") + if [[ -n "$available" ]]; then + selected="refs/heads/$XR_REF_NAME" + else + selected=refs/heads/dev + fi + fi + git init "$directory" + git -C "$directory" remote add origin "$repo" + git -C "$directory" fetch --depth=1 origin "$selected" + git -C "$directory" checkout --detach FETCH_HEAD + git -C "$directory" submodule update --init --recursive --depth=1 + } + checkout_tool https://github.com/xrobot-org/XRobot.git XRobot "$XROBOT_REF" + checkout_tool https://github.com/xrobot-org/libxr.git libxr "$LIBXR_REF" + - name: Install build dependencies + run: | + apt-get update + apt-get install -y --no-install-recommends libgpiod-dev + python3 -m pip install ./XRobot + - name: Resolve sources and generate the compile-only constructor + run: | + python3 - <<'PYCODE' + import json + import os + from pathlib import Path + import subprocess + import yaml + from xrobot.InitModule import sync_modules_by_config + from xrobot.GenerateMain import generate_compile_check + from xrobot.ModuleParser import load_single_module + from xrobot.SourceManager import validate_id + + identity = validate_id(os.environ['XR_MODULE_ID']) + folder = Path('Modules') / identity + + def git(*args): + return subprocess.check_output(['git', '-C', str(folder), *args], text=True).strip() + + def write_yaml(path, data): + Path(path).write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8') + + # Pin the checked-out PR/branch commit. Only its logical dependency context moves. + context = ('refs/tags/' if os.environ['XR_REF_KIND'] == 'tag' else 'refs/heads/') + context += os.environ['XR_REF_NAME'] + write_yaml('ci-index.yaml', {'modules': [{'id': identity, 'repo': git('remote', 'get-url', 'origin')}]}) + write_yaml('Modules/sources.yaml', {'sources': [ + {'url': 'https://xrobot.work/xrobot-modules/index.yaml', 'priority': 0}, + {'url': 'https://qdu-robomaster.github.io/qdu-future-modules/index.yaml', 'priority': 0}, + {'url': '../ci-index.yaml', 'priority': -100}, + ]}) + write_yaml('Modules/modules.yaml', {'modules': [{ + 'id': identity, 'ref': git('rev-parse', 'HEAD'), 'context_ref': context, + }]}) + sync_modules_by_config('Modules/modules.yaml', 'Modules/sources.yaml', 'Modules') + manifest = load_single_module(folder) + if manifest.standalone: + generate_compile_check(identity, 'Modules', 'module_check.cpp', + json.loads(os.environ['XR_TEMPLATE_ARGS'])) + else: + # A library has no Module constructor. Its actual .cpp files still build in xr. + Path('module_check.cpp').write_text('#include "' + folder.name + '.hpp"\n', encoding='utf-8') + PYCODE + - name: Create native CMake project + run: | + cat > CMakeLists.txt <<'CMAKE' + cmake_minimum_required(VERSION 3.16) + project(xrobot_module_compile LANGUAGES C CXX ASM) + set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(LIBXR_SYSTEM linux CACHE STRING "LibXR execution backend") + set(LIBXR_DRIVER linux CACHE STRING "LibXR driver backend") + set(LIBXR_PRINT_INTEGER_ENABLE_64BIT ON CACHE BOOL "") + set(LIBXR_PRINT_FLOAT_ENABLE_DOUBLE ON CACHE BOOL "") + set(LIBXR_PRINT_ENABLE_POINTER ON CACHE BOOL "") + set(XROBOT_MODULES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Modules") + include(CTest) + add_subdirectory(libxr) + set_property(TARGET xr PROPERTY CXX_STANDARD 20) + set_property(TARGET xr PROPERTY CXX_STANDARD_REQUIRED ON) + add_library(module_check OBJECT module_check.cpp) + target_link_libraries(module_check PRIVATE xr) + CMAKE + - name: Compile Module sources and constructor + run: | + cmake -S . -B build -DBUILD_TESTING=OFF + cmake --build build --parallel 2 diff --git a/PowerControl.hpp b/PowerControl.hpp index edb295c..7a96ea7 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -92,10 +92,13 @@ class PowerControl float allocation_weight_scale[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), + 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), From 99eedba28fcf1d7a3310ba42f28e27fa05865038 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Mon, 28 Sep 2026 20:34:02 +0100 Subject: [PATCH 03/15] Remove unsupported module_name manifest key XRobot accepts only module_description, description, depends and standalone in the module manifest. --- PowerControl.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index 7a96ea7..1e7e637 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -1,7 +1,6 @@ #pragma once // clang-format off /* === MODULE MANIFEST V2 === -module_name: PowerControl module_description: Power control for chassis (supports omni and helm wheel) depends: - id: QDU-Robomaster/SuperPower From 2d0194e53d408e47d242ffd718fbadc6bd656767 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Mon, 28 Sep 2026 20:55:14 +0100 Subject: [PATCH 04/15] Remove the empty OnMonitor hook The static-assembly entry calls OnMonitor() only for modules that declare it; the empty body was left over from the removed application framework. --- PowerControl.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index 1e7e637..60f72f1 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -223,8 +223,6 @@ class PowerControl bool IsOnline() { return superpower_->IsOnline(); } - void OnMonitor() {} - private: void OutputLimitOmni(float max_power) { From 3bd6e16271e1b22c739e1c524b15af85f165330b Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Mon, 28 Sep 2026 21:51:28 +0100 Subject: [PATCH 05/15] Add the Apache-2.0 license The XRobot ecosystem Modules are published under the Apache License 2.0; the text is identical to xrobot-org/XRobot/LICENSE. --- LICENSE | 202 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 202 insertions(+) create mode 100644 LICENSE 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. From aa2a60378dc01fff8bf638d7e8c26dbd7b44df19 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Mon, 28 Sep 2026 21:53:36 +0100 Subject: [PATCH 06/15] Use the shared XRobot Module CI workflow Replace the generated inline workflow with a caller of xrobot-org/XRobot/.github/workflows/module-ci.yml, which resolves the dependencies, compiles the Module sources and a generated constructor probe against LibXR. During the coordinated 1.0 change the tools, LibXR and same-or-dev dependencies come from dev. --- .github/workflows/build.yml | 141 +++--------------------------------- 1 file changed, 11 insertions(+), 130 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 3e201c8..591717e 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,132 +1,13 @@ -name: XRobot Module Build Test -'on': - push: null - pull_request: null - schedule: - - cron: 0 3 1 * * -permissions: - contents: read +name: Module CI +on: + push: + pull_request: + workflow_dispatch: jobs: build: - runs-on: ubuntu-latest - container: - image: ghcr.io/xrobot-org/docker-image-linux:main - options: --user 0 - env: - XR_MODULE_ID: ${{ github.repository }} - XR_MODULE_NAME: ${{ github.event.repository.name }} - XR_REF_NAME: ${{ github.head_ref || github.ref_name }} - XR_REF_KIND: ${{ github.ref_type }} - XR_TEMPLATE_ARGS: '[]' - XROBOT_REF: ${{ vars.XROBOT_REF || '' }} - LIBXR_REF: ${{ vars.LIBXR_REF || '' }} - PYTHONPATH: ${{ github.workspace }}/XRobot/src - steps: - - name: Checkout the exact Module revision - uses: actions/checkout@v4 - with: - path: Modules/${{ github.repository }} - ref: ${{ github.event.pull_request.head.sha || github.sha }} - submodules: recursive - - name: Checkout matching core tools - shell: bash - run: | - set -euo pipefail - checkout_tool() { - local repo="$1" directory="$2" selected="$3" available - if [[ -n "$selected" ]]; then - if [[ "$selected" != refs/heads/* && "$selected" != refs/tags/* && - ! "$selected" =~ ^[0-9a-fA-F]{40}$ ]]; then - echo "Use a full refs/heads/, refs/tags/ or commit SHA for $directory" >&2 - return 1 - fi - elif [[ "$XR_REF_KIND" == tag ]]; then - selected="refs/tags/$XR_REF_NAME" - else - available=$(git ls-remote --heads "$repo" "refs/heads/$XR_REF_NAME") - if [[ -n "$available" ]]; then - selected="refs/heads/$XR_REF_NAME" - else - selected=refs/heads/dev - fi - fi - git init "$directory" - git -C "$directory" remote add origin "$repo" - git -C "$directory" fetch --depth=1 origin "$selected" - git -C "$directory" checkout --detach FETCH_HEAD - git -C "$directory" submodule update --init --recursive --depth=1 - } - checkout_tool https://github.com/xrobot-org/XRobot.git XRobot "$XROBOT_REF" - checkout_tool https://github.com/xrobot-org/libxr.git libxr "$LIBXR_REF" - - name: Install build dependencies - run: | - apt-get update - apt-get install -y --no-install-recommends libgpiod-dev - python3 -m pip install ./XRobot - - name: Resolve sources and generate the compile-only constructor - run: | - python3 - <<'PYCODE' - import json - import os - from pathlib import Path - import subprocess - import yaml - from xrobot.InitModule import sync_modules_by_config - from xrobot.GenerateMain import generate_compile_check - from xrobot.ModuleParser import load_single_module - from xrobot.SourceManager import validate_id - - identity = validate_id(os.environ['XR_MODULE_ID']) - folder = Path('Modules') / identity - - def git(*args): - return subprocess.check_output(['git', '-C', str(folder), *args], text=True).strip() - - def write_yaml(path, data): - Path(path).write_text(yaml.safe_dump(data, sort_keys=False), encoding='utf-8') - - # Pin the checked-out PR/branch commit. Only its logical dependency context moves. - context = ('refs/tags/' if os.environ['XR_REF_KIND'] == 'tag' else 'refs/heads/') - context += os.environ['XR_REF_NAME'] - write_yaml('ci-index.yaml', {'modules': [{'id': identity, 'repo': git('remote', 'get-url', 'origin')}]}) - write_yaml('Modules/sources.yaml', {'sources': [ - {'url': 'https://xrobot.work/xrobot-modules/index.yaml', 'priority': 0}, - {'url': 'https://qdu-robomaster.github.io/qdu-future-modules/index.yaml', 'priority': 0}, - {'url': '../ci-index.yaml', 'priority': -100}, - ]}) - write_yaml('Modules/modules.yaml', {'modules': [{ - 'id': identity, 'ref': git('rev-parse', 'HEAD'), 'context_ref': context, - }]}) - sync_modules_by_config('Modules/modules.yaml', 'Modules/sources.yaml', 'Modules') - manifest = load_single_module(folder) - if manifest.standalone: - generate_compile_check(identity, 'Modules', 'module_check.cpp', - json.loads(os.environ['XR_TEMPLATE_ARGS'])) - else: - # A library has no Module constructor. Its actual .cpp files still build in xr. - Path('module_check.cpp').write_text('#include "' + folder.name + '.hpp"\n', encoding='utf-8') - PYCODE - - name: Create native CMake project - run: | - cat > CMakeLists.txt <<'CMAKE' - cmake_minimum_required(VERSION 3.16) - project(xrobot_module_compile LANGUAGES C CXX ASM) - set(CMAKE_CXX_STANDARD 20) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(LIBXR_SYSTEM linux CACHE STRING "LibXR execution backend") - set(LIBXR_DRIVER linux CACHE STRING "LibXR driver backend") - set(LIBXR_PRINT_INTEGER_ENABLE_64BIT ON CACHE BOOL "") - set(LIBXR_PRINT_FLOAT_ENABLE_DOUBLE ON CACHE BOOL "") - set(LIBXR_PRINT_ENABLE_POINTER ON CACHE BOOL "") - set(XROBOT_MODULES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Modules") - include(CTest) - add_subdirectory(libxr) - set_property(TARGET xr PROPERTY CXX_STANDARD 20) - set_property(TARGET xr PROPERTY CXX_STANDARD_REQUIRED ON) - add_library(module_check OBJECT module_check.cpp) - target_link_libraries(module_check PRIVATE xr) - CMAKE - - name: Compile Module sources and constructor - run: | - cmake -S . -B build -DBUILD_TESTING=OFF - cmake --build build --parallel 2 + uses: xrobot-org/XRobot/.github/workflows/module-ci.yml@dev + with: + xrobot-ref: dev + libxr-ref: dev + dependency-ref: refs/heads/dev + template-args: '[]' From f2f27101b6cdf4addb90439d222936e8af5d1402 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Mon, 28 Sep 2026 22:06:37 +0100 Subject: [PATCH 07/15] Rewrite the README for XRobot 1.0 Describe the Module, its dependencies, its constructor (dependencies first, then the value configuration) and its use with xrobot module add / setup / instance add, with a configuration example seeded by the tool. Drop the dynamic-framework text and the dead MIGRATION.md link. --- README.md | 139 +++++++++++++++++++++++++++++++++++++----------------- 1 file changed, 97 insertions(+), 42 deletions(-) diff --git a/README.md b/README.md index 11332bc..7c17928 100644 --- a/README.md +++ b/README.md @@ -1,42 +1,97 @@ -# 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 + +底盘功率控制:按功率上限重新分配各电机的输出电流,支持全向轮(仅 3508)和舵轮 +(3508 驱动 + 6020 转向)两种底盘。 + +模块没有线程,由底盘在自己的控制循环中调用。所有公共接口内部用同一把互斥锁保护。 + +## 工作方式 + +每个控制周期,底盘按下面顺序调用: + +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` 是源码中的固定常数。 + +## 依赖 + +- `QDU-Robomaster/SuperPower`:提供实测底盘功率和超级电容在线状态。 + +外部依赖:Eigen(`Eigen/Core`,由 LibXR 自带),用于 `RLS.hpp` 中的递归最小二乘估计器。 + +## 构造接口 + +```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。 + +## 使用 + +```sh +xrobot module add QDU-Robomaster/PowerControl +xrobot setup +xrobot instance add QDU-Robomaster/PowerControl +``` + +`xrobot instance add` 在 `User/xrobot.yaml` 中写入一个实例,依赖项留空,默认值按源码写出; +把 `super_power` 填为 SuperPower 实例的 `id`: + +```yaml +modules: + - module: QDU-Robomaster/PowerControl + id: powercontrol_0 + args: + - super_power: superpower + - is_helm: 'false' + - chassis_static_power_loss: 0.0f + - motor_count_3508: '4' + - motor_count_6020: '4' +``` + +`superpower` 是 `QDU-Robomaster/SuperPower` 实例的 `id`,必须在 `modules:` 中排在本实例之前。 +本模块不直接使用 BSP 对象,不需要额外的 `XR_REGISTER`。PowerControl 实例通常再作为依赖传给 +底盘模块(如 `QDU-Robomaster/Chassis`)。 + +填好后再次运行 `xrobot setup`,生成 `User/xrobot_main.hpp`。 + +`xrobot module show .`(在本仓库中)或 `xrobot module show Modules/QDU-Robomaster/PowerControl` +(在 BSP 中)打印当前的构造函数。 From 57ffe12b707a0b036eb5a3cdd6fb089f474b8fbb Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Sat, 3 Oct 2026 18:36:57 +0100 Subject: [PATCH 08/15] Rewrite the README to the Module README structure --- README.md | 120 +++++++++++++++++++++++++++++++----------------------- 1 file changed, 69 insertions(+), 51 deletions(-) diff --git a/README.md b/README.md index 7c17928..6c94b70 100644 --- a/README.md +++ b/README.md @@ -1,49 +1,48 @@ # PowerControl -底盘功率控制:按功率上限重新分配各电机的输出电流,支持全向轮(仅 3508)和舵轮 -(3508 驱动 + 6020 转向)两种底盘。 +底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮与舵轮底盘 / Chassis power control Module that redistributes the motor output currents under a power limit, for omni-wheel and steering-wheel chassis -模块没有线程,由底盘在自己的控制循环中调用。所有公共接口内部用同一把互斥锁保护。 +## 1. 模块作用 / Purpose -## 工作方式 +PowerControl 支持全向轮底盘(仅 3508)和舵轮底盘(3508 驱动加 6020 转向)。它由底盘在自己的控制循环中调用,所有公共接口内部由同一把互斥锁保护。 每个控制周期,底盘按下面顺序调用: -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。 +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`。 +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)分配。 +- `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::MAX_MOTOR_COUNT`(6),超出部分被截断。3508 的 `kt` 与 6020 的 `kt`、`k1`、`k2` 是源码中的固定常数。 -## 依赖 +PowerControl supports omni-wheel chassis (3508 only) and steering-wheel chassis (3508 drive plus 6020 steering). The chassis calls it in its own control loop, and all public interfaces are protected internally by the same mutex. -- `QDU-Robomaster/SuperPower`:提供实测底盘功率和超级电容在线状态。 +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 chassis). + - Omni-wheel 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 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`. -外部依赖:Eigen(`Eigen/Core`,由 LibXR 自带),用于 `RLS.hpp` 中的递归最小二乘估计器。 +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, @@ -53,45 +52,64 @@ PowerControl(SuperPower& super_power, int motor_count_6020 = 4); ``` -依赖项: +依赖: -- `super_power`:`SuperPower` 实例。 +- `super_power`:`SuperPower` 实例,提供实测底盘功率和超级电容在线状态。 -配置: +配置参数: -- `is_helm`:`true` 为舵轮底盘(同时限制 6020),`false` 为全向轮底盘,默认 `false`。 +- `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: -```sh -xrobot module add QDU-Robomaster/PowerControl -xrobot setup -xrobot instance add QDU-Robomaster/PowerControl -``` +- `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 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 -`xrobot instance add` 在 `User/xrobot.yaml` 中写入一个实例,依赖项留空,默认值按源码写出; -把 `super_power` 填为 SuperPower 实例的 `id`: +无 / 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: powercontrol_0 + id: power_control args: - super_power: superpower - - is_helm: 'false' - - chassis_static_power_loss: 0.0f - - motor_count_3508: '4' - - motor_count_6020: '4' + - is_helm: false + - chassis_static_power_loss: 4.5 + - motor_count_3508: 4 + - motor_count_6020: 0 ``` -`superpower` 是 `QDU-Robomaster/SuperPower` 实例的 `id`,必须在 `modules:` 中排在本实例之前。 -本模块不直接使用 BSP 对象,不需要额外的 `XR_REGISTER`。PowerControl 实例通常再作为依赖传给 -底盘模块(如 `QDU-Robomaster/Chassis`)。 +## 5. 依赖与硬件 / Dependencies and Hardware + +依赖: + +- `QDU-Robomaster/SuperPower`:提供实测底盘功率和超级电容在线状态。 +- Eigen(`Eigen/Core`,由 LibXR 自带):`RLS.hpp` 中的递归最小二乘估计器。 +- LibXR。 + +硬件:全向轮底盘的 3508 电机,或舵轮底盘的 3508 驱动电机与 6020 转向电机;实测功率来自 `SuperPower`。 + +Dependencies: -填好后再次运行 `xrobot setup`,生成 `User/xrobot_main.hpp`。 +- `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. -`xrobot module show .`(在本仓库中)或 `xrobot module show Modules/QDU-Robomaster/PowerControl` -(在 BSP 中)打印当前的构造函数。 +Hardware: the 3508 motors of an omni-wheel chassis, or the 3508 drive motors and 6020 steering motors of a steering-wheel chassis; the measured power comes from `SuperPower`. From a4cc726306b6edf53fc9488dc103da711ac89a24 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Sat, 3 Oct 2026 18:38:05 +0100 Subject: [PATCH 09/15] Clean up comments --- PowerControl.hpp | 2 +- RLS.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index 60f72f1..ebe7d4e 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -1,7 +1,7 @@ #pragma once // clang-format off /* === MODULE MANIFEST V2 === -module_description: Power control for chassis (supports omni and helm wheel) +module_description: 底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮与舵轮底盘 / Chassis power control Module that redistributes the motor output currents under a power limit, for omni-wheel and steering-wheel chassis depends: - id: QDU-Robomaster/SuperPower ref: same-or-dev diff --git a/RLS.hpp b/RLS.hpp index 147cfe8..8c4d4d6 100644 --- a/RLS.hpp +++ b/RLS.hpp @@ -11,7 +11,7 @@ class RLS { public: using ParamVector = Eigen::Matrix; - RLS() = delete; // 必须只能带参数的构造函数 + RLS() = delete; /** * @brief 构造 RLS 估计器 From 1056f33656e6a4fab442c4e1fb39d0a327677dc6 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Sat, 3 Oct 2026 18:38:36 +0100 Subject: [PATCH 10/15] Document the public interface --- PowerControl.hpp | 170 +++++++++++++++++++++++++++++++++++++++++++++-- RLS.hpp | 42 ++++++++---- 2 files changed, 195 insertions(+), 17 deletions(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index ebe7d4e..4467ad9 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -18,12 +18,32 @@ module_description: 底盘功率控制模块:按功率上限重新分配各电 #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 -#define CHASSIS_POWER_LIMIT_MARGIN_W 4.0f /* 底盘限功率余量 */ +/// 底盘限功率余量 (W) +/// Chassis power limit margin (W) +#define CHASSIS_POWER_LIMIT_MARGIN_W 4.0f /** - * @brief 计算单个电机模型预测功率 (不含静态损耗) + * @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) @@ -32,7 +52,24 @@ inline float calculate_motor_model_power(float current, float rpm, float kt, flo } /** - * @brief 根据目标功率反解电流 + * @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) @@ -69,28 +106,77 @@ inline float solve_current_for_power(float target_power, float rpm, float kt, fl return std::clamp(final_current, -16384.0f, 16384.0f); } -static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; /* 最大电机数目 */ +/// 最大电机数量 +/// 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 and steering-wheel chassis. + */ class PowerControl { public: + /// 最大电机数量 + /// Maximum number of motors static constexpr int MAX_MOTOR_COUNT = POWER_CONTROL_MAX_MOTOR_COUNT; - /* 3508 组分配偏置: 先保底, 再分剩余功率 */ + + /** + * @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 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, @@ -112,6 +198,19 @@ class PowerControl 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) { @@ -127,6 +226,19 @@ class PowerControl } } + /** + * @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) { @@ -142,6 +254,13 @@ class PowerControl } } + /** + * @brief 设置 3508 组的分配偏置,仅全向轮路径使用。 + * Set the allocation bias of the 3508 group, used by the omni-wheel path. + * + * @param bias 分配偏置。 + * Allocation bias. + */ void SetAllocationBias3508(const AllocationBias3508& bias) { LibXR::Mutex::LockGuard lock(mutex_); @@ -158,6 +277,11 @@ class PowerControl } } + /** + * @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_); @@ -198,6 +322,14 @@ class PowerControl } } + /** + * @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_); @@ -211,16 +343,44 @@ class PowerControl } } + /** + * @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: diff --git a/RLS.hpp b/RLS.hpp index 8c4d4d6..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; /** - * @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; From 9b59d0289263c07f17ff301e8c372a13eb8770b2 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Sat, 3 Oct 2026 18:38:36 +0100 Subject: [PATCH 11/15] Write struct defaults with field names --- PowerControl.hpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index 4467ad9..57efa30 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -177,12 +177,9 @@ class PowerControl * @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) + 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), From e53ee68d5e2c9b16b3bc48071fd64b5251462db4 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Sat, 3 Oct 2026 21:25:41 +0100 Subject: [PATCH 12/15] Use the instance add output in the configuration example --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 6c94b70..e68786a 100644 --- a/README.md +++ b/README.md @@ -91,7 +91,7 @@ modules: args: - super_power: superpower - is_helm: false - - chassis_static_power_loss: 4.5 + - chassis_static_power_loss: 4.5f - motor_count_3508: 4 - motor_count_6020: 0 ``` From 80ab2f5acfdc1f23ee833dc87485e50448fdfe9f Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Sat, 3 Oct 2026 21:25:45 +0100 Subject: [PATCH 13/15] Match the documented defaults to the code --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index e68786a..b990a80 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ ## 1. 模块作用 / Purpose -PowerControl 支持全向轮底盘(仅 3508)和舵轮底盘(3508 驱动加 6020 转向)。它由底盘在自己的控制循环中调用,所有公共接口内部由同一把互斥锁保护。 +PowerControl 支持全向轮底盘(仅 3508)和舵轮底盘(3508 驱动加 6020 转向)。它由底盘在自己的控制循环中调用,`SetMotorData3508()`、`SetMotorData6020()`、`SetAllocationBias3508()`、`CalculatePowerControlParam()`、`OutputLimit()` 与 `GetPowerControlData()` 内部由同一把互斥锁保护。 每个控制周期,底盘按下面顺序调用: @@ -23,7 +23,7 @@ PowerControl 支持全向轮底盘(仅 3508)和舵轮底盘(3508 驱动加 电机数量上限为 `PowerControl::MAX_MOTOR_COUNT`(6),超出部分被截断。3508 的 `kt` 与 6020 的 `kt`、`k1`、`k2` 是源码中的固定常数。 -PowerControl supports omni-wheel chassis (3508 only) and steering-wheel chassis (3508 drive plus 6020 steering). The chassis calls it in its own control loop, and all public interfaces are protected internally by the same mutex. +PowerControl supports omni-wheel 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: From 0f25f5484eaf28509721715c6a58e79dccbfe9b9 Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Sun, 4 Oct 2026 09:38:58 +0100 Subject: [PATCH 14/15] README: match the Module documentation to the code --- README.md | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index b990a80..3b92db1 100644 --- a/README.md +++ b/README.md @@ -4,39 +4,39 @@ ## 1. 模块作用 / Purpose -PowerControl 支持全向轮底盘(仅 3508)和舵轮底盘(3508 驱动加 6020 转向)。它由底盘在自己的控制循环中调用,`SetMotorData3508()`、`SetMotorData6020()`、`SetAllocationBias3508()`、`CalculatePowerControlParam()`、`OutputLimit()` 与 `GetPowerControlData()` 内部由同一把互斥锁保护。 +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。 +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)分配。 +- `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 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. +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 chassis). - - Omni-wheel chassis: limits the 3508. +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 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). +- `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`. @@ -58,7 +58,7 @@ PowerControl(SuperPower& super_power, 配置参数: -- `is_helm`:`true` 为舵轮底盘,同时限制 6020;`false` 为全向轮底盘,默认 `false`。 +- `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。 @@ -69,7 +69,7 @@ Dependencies: Configuration parameters: -- `is_helm`: `true` for a steering-wheel chassis, which also limits the 6020; `false` for an omni-wheel chassis, default `false`. +- `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. @@ -104,7 +104,7 @@ modules: - Eigen(`Eigen/Core`,由 LibXR 自带):`RLS.hpp` 中的递归最小二乘估计器。 - LibXR。 -硬件:全向轮底盘的 3508 电机,或舵轮底盘的 3508 驱动电机与 6020 转向电机;实测功率来自 `SuperPower`。 +硬件:全向轮或麦轮底盘的 3508 电机,或舵轮底盘的 3508 驱动电机与 6020 转向电机;实测功率来自 `SuperPower`。 Dependencies: @@ -112,4 +112,4 @@ Dependencies: - Eigen (`Eigen/Core`, shipped with LibXR): the recursive least squares estimator in `RLS.hpp`. - LibXR. -Hardware: the 3508 motors of an omni-wheel chassis, or the 3508 drive motors and 6020 steering motors of a steering-wheel chassis; the measured power comes from `SuperPower`. +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`. From a8cf6cff393b3cab8df97de725f37eca9115cb7a Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Sun, 4 Oct 2026 15:02:22 +0100 Subject: [PATCH 15/15] Name the mecanum chassis in the Module description --- PowerControl.hpp | 16 +++++++++------- README.md | 2 +- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index 57efa30..d6c10f8 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -1,7 +1,7 @@ #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 and steering-wheel chassis +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 @@ -128,9 +128,10 @@ struct PowerControlData }; /** - * @brief 底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮与舵轮底盘。 + * @brief 底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮、麦轮与舵轮 + * 底盘。 * Chassis power control Module that redistributes the motor output currents - * under a power limit, for omni-wheel and steering-wheel chassis. + * under a power limit, for omni-wheel, mecanum and steering-wheel chassis. */ class PowerControl { @@ -167,9 +168,9 @@ class PowerControl * @param super_power SuperPower 实例,提供实测底盘功率与超级电容在线状态。 * SuperPower instance that provides the measured chassis power * and the supercapacitor online state. - * @param is_helm true 为舵轮底盘,同时限制 6020;false 为全向轮底盘。 + * @param is_helm true 为舵轮底盘,同时限制 6020;false 为全向轮或麦轮底盘。 * True for a steering-wheel chassis, which also limits the 6020; false - * for an omni-wheel chassis. + * 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`。 @@ -252,8 +253,9 @@ class PowerControl } /** - * @brief 设置 3508 组的分配偏置,仅全向轮路径使用。 - * Set the allocation bias of the 3508 group, used by the omni-wheel path. + * @brief 设置 3508 组的分配偏置,仅全向轮与麦轮路径使用。 + * Set the allocation bias of the 3508 group, used by the omni-wheel and + * mecanum path. * * @param bias 分配偏置。 * Allocation bias. diff --git a/README.md b/README.md index 3b92db1..842620a 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # PowerControl -底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮与舵轮底盘 / Chassis power control Module that redistributes the motor output currents under a power limit, for omni-wheel and steering-wheel chassis +底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮、麦轮与舵轮底盘 / Chassis power control Module that redistributes the motor output currents under a power limit, for omni-wheel, mecanum and steering-wheel chassis ## 1. 模块作用 / Purpose