From b198c8c87da43e51d42278894b31b4455e240b73 Mon Sep 17 00:00:00 2001 From: kaMi-0932 <1580654962@qq.com> Date: Tue, 27 Oct 2026 18:26:56 +0800 Subject: [PATCH 1/2] Limit swerve power to drive motors --- PowerControl.hpp | 272 +++++++------------------------------ PowerLimit.hpp | 193 ++++++++++++++++++++++++++ tests/power_limit_test.cpp | 215 +++++++++++++++++++++++++++++ 3 files changed, 459 insertions(+), 221 deletions(-) create mode 100644 PowerLimit.hpp create mode 100644 tests/power_limit_test.cpp diff --git a/PowerControl.hpp b/PowerControl.hpp index d6c10f8..920b6b9 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -13,6 +13,7 @@ module_description: 底盘功率控制模块:按功率上限重新分配各电 #include #include +#include "PowerLimit.hpp" #include "RLS.hpp" #include "SuperPower.hpp" #include "message.hpp" @@ -28,88 +29,6 @@ module_description: 底盘功率控制模块:按功率上限重新分配各电 /// Chassis power limit margin (W) #define CHASSIS_POWER_LIMIT_MARGIN_W 4.0f -/** - * @brief 计算单个电机的模型预测功率,不含静态损耗。 - * Compute the model-predicted power of one motor, excluding the static loss. - * - * @param current 电机电流。 - * Motor current. - * @param rpm 转子转速 (rpm)。 - * Rotor speed (rpm). - * @param kt 机械功率系数。 - * Mechanical power coefficient. - * @param k1 电流平方项系数。 - * Coefficient of the squared current term. - * @param k2 转速平方项系数。 - * Coefficient of the squared speed term. - * @return 预测功率 (W)。 - * Predicted power (W). - */ -inline float calculate_motor_model_power(float current, float rpm, float kt, float k1, - float k2) -{ - return (kt * current * rpm) + (k1 * current * current) + (k2 * rpm * rpm); -} - -/** - * @brief 由目标功率反解电流,结果限幅到 ±16384。 - * Solve the current from a target power, clamped to ±16384. - * - * @param target_power 目标功率 (W)。 - * Target power (W). - * @param rpm 转子转速 (rpm)。 - * Rotor speed (rpm). - * @param kt 机械功率系数。 - * Mechanical power coefficient. - * @param k1 电流平方项系数。 - * Coefficient of the squared current term. - * @param k2 转速平方项系数。 - * Coefficient of the squared speed term. - * @param original_current 原始期望电流,其符号决定取二次方程的哪个根。 - * Original desired current, whose sign selects the root of the - * quadratic equation. - * @return 反解得到的电流。 - * Solved current. - */ -inline float solve_current_for_power(float target_power, float rpm, float kt, float k1, - float k2, float original_current) -{ - float a = k1; - float b = kt * rpm; - float c = k2 * rpm * rpm - target_power; - float delta = b * b - 4.0f * a * c; - - float x3 = -b / (2.0f * a); - - float final_current = 0; - - if (delta < 1e-9f) - { - final_current = x3; - } - else - { - float sqrt_delta = sqrtf(delta); - float x1 = (-b + sqrt_delta) / (2.0f * a); - float x2 = (-b - sqrt_delta) / (2.0f * a); - - if (original_current >= 0) - { - final_current = x1; - } - else - { - final_current = x2; - } - } - - return std::clamp(final_current, -16384.0f, 16384.0f); -} - -/// 最大电机数量 -/// Maximum number of motors -static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; - /** * @brief 功率限制后的输出。 * Output after the power limit. @@ -168,8 +87,8 @@ 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 为全向轮或麦轮底盘。 - * True for a steering-wheel chassis, which also limits the 6020; false + * @param is_helm true 为舵轮底盘,仅限制驱动轮,6020 透传;false 为全向轮或麦轮底盘。 + * True for drive-only power limiting on a steering-wheel chassis; false * for an omni-wheel or mecanum chassis. * @param chassis_static_power_loss 底盘静态功耗 (W)。 * Chassis static power loss (W). @@ -301,17 +220,7 @@ class PowerControl 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; - } + // The metered/modelled chassis power covers drive motors only. if (residual > 0 && online && measured_power_ > 5.0f) { @@ -382,6 +291,42 @@ class PowerControl */ bool IsOnline() { return superpower_->IsOnline(); } + struct PowerEstimate + { + float wheel_power_w = 0.0f; + float steer_power_w = 0.0f; // Steering is excluded from the drive-power budget. + float total_power_w = 0.0f; + bool valid = false; + }; + + /// Drive-motor model estimate (W), excluding steering and static board losses. + PowerEstimate EstimatePower(const float* current_3508, const float* rpm_3508) + { + LibXR::Mutex::LockGuard lock(mutex_); + PowerEstimate estimate{}; + if (motor_count_3508_ > 0 && (!current_3508 || !rpm_3508)) + { + return estimate; + } + for (int i = 0; i < motor_count_3508_; i++) + { + if (!std::isfinite(current_3508[i]) || !std::isfinite(rpm_3508[i])) + { + return {}; + } + const float POWER = calculate_motor_model_power(current_3508[i], rpm_3508[i], + kt_3508_, k1_3508_, k2_3508_); + if (!std::isfinite(POWER)) + { + return {}; + } + estimate.wheel_power_w += std::max(0.0f, POWER); + } + estimate.total_power_w = estimate.wheel_power_w; + estimate.valid = std::isfinite(estimate.total_power_w); + return estimate.valid ? estimate : PowerEstimate{}; + } + private: void OutputLimitOmni(float max_power) { @@ -567,133 +512,23 @@ class PowerControl 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]; - } - } + /* 最大功率只约束 3508 驱动轮,保留模型误差与控制延迟余量。 */ + const float AVAILABLE_POWER = max_power - CHASSIS_POWER_LIMIT_MARGIN_W; + /* 6020 舵向电机不参与限幅,始终保留原电流命令。 */ 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]; - } + powercontrol_data_.new_output_current_6020[i] = output_current_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 + const auto LIMITED = limit_power_proportionally( + output_current_3508_, rotorspeed_rpm_3508_, motor_count_3508_, AVAILABLE_POWER, + kt_3508_, k1_3508_, k2_3508_); + powercontrol_data_.is_power_limited = LIMITED.is_power_limited; + for (int i = 0; i < motor_count_3508_; i++) { - /*功率充足,不限制*/ - 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]; - } + motor_power_3508_[i] = LIMITED.requested_power[i]; + powercontrol_data_.new_output_current_3508[i] = LIMITED.current[i]; } } @@ -725,13 +560,8 @@ class PowerControl float rotorspeed_rpm_3508_[MAX_MOTOR_COUNT] = {}; float motor_power_3508_[MAX_MOTOR_COUNT] = {}; - float kt_6020_ = 1.42074505e-5f; - float k1_6020_ = 6.4276e-7f; - float k2_6020_ = 1.0e-10f; - float output_current_6020_[MAX_MOTOR_COUNT] = {}; float rotorspeed_rpm_6020_[MAX_MOTOR_COUNT] = {}; - float motor_power_6020_[MAX_MOTOR_COUNT] = {}; float measured_power_ = 0.0f; }; diff --git a/PowerLimit.hpp b/PowerLimit.hpp new file mode 100644 index 0000000..0285dda --- /dev/null +++ b/PowerLimit.hpp @@ -0,0 +1,193 @@ +#pragma once + +#include +#include + +static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; + +/* 电流单位为电机控制 LSB,转速为 rpm,模型功率为 W。 */ +inline float calculate_motor_model_power(float current, float rpm, float kt, float k1, + float k2) +{ + return kt * current * rpm + k1 * current * current + k2 * rpm * rpm; +} + +/* 只在零电流至原请求电流之间寻找模型功率最低点,保留请求方向。 */ +inline float minimum_model_power_current(float original_current, float rpm, float kt, + float k1) +{ + const float REQUEST = std::clamp(original_current, -16384.0f, 16384.0f); + if (k1 > 0.0f) + { + return std::clamp(-kt * rpm / (2.0f * k1), std::min(0.0f, REQUEST), + std::max(0.0f, REQUEST)); + } + return kt * rpm * REQUEST < 0.0f ? REQUEST : 0.0f; +} + +/* 取允许区间内最接近原请求的解;配额不可行时取区间内最低功率点。 + * 原请求制动时也保留其电流方向,不因选择二次方程的另一根而反向。 */ +inline float solve_current_for_power(float target_power, float rpm, float kt, float k1, + float k2, float original_current) +{ + if (!std::isfinite(target_power) || !std::isfinite(rpm) || + !std::isfinite(original_current) || !std::isfinite(kt) || !std::isfinite(k1) || + !std::isfinite(k2) || kt < 0.0f || k1 < 0.0f || k2 < 0.0f) + { + return 0.0f; + } + const float REQUEST = std::clamp(original_current, -16384.0f, 16384.0f); + const float TARGET = std::max(0.0f, target_power); + const float REQUESTED_POWER = calculate_motor_model_power(REQUEST, rpm, kt, k1, k2); + if (!std::isfinite(REQUESTED_POWER)) + { + return 0.0f; + } + if (REQUESTED_POWER <= TARGET) + { + return REQUEST; + } + + const float MINIMUM_CURRENT = minimum_model_power_current(REQUEST, rpm, kt, k1); + const float MINIMUM_POWER = + calculate_motor_model_power(MINIMUM_CURRENT, rpm, kt, k1, k2); + if (MINIMUM_POWER >= TARGET) + { + return MINIMUM_CURRENT; + } + + /* 在请求方向的电流幅值坐标上解方程,取较大的根。 */ + const float DIRECTION = REQUEST < 0.0f ? -1.0f : 1.0f; + const float B = kt * rpm * DIRECTION; + const float C = k2 * rpm * rpm - TARGET; + float magnitude = 0.0f; + if (k1 == 0.0f) + { + magnitude = -C / B; + } + else + { + const float SQRT_DELTA = std::sqrt(std::max(0.0f, B * B - 4.0f * k1 * C)); + /* B >= 0 时使用等价式,避免相近数相减损失精度。 */ + magnitude = + B >= 0.0f ? -2.0f * C / (B + SQRT_DELTA) : (-B + SQRT_DELTA) / (2.0f * k1); + } + return DIRECTION * + std::clamp(magnitude, std::fabs(MINIMUM_CURRENT), std::fabs(REQUEST)); +} + +struct ProportionalPowerLimitResult +{ + float current[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + float requested_power[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + float minimum_power_sum = 0.0f; + /* 非法输入默认零输出,并保持限幅状态,避免调用方透传请求。 */ + bool is_power_limited = true; + bool budget_feasible = false; +}; + +/* 按请求正功率比例分配。某轮配额达到最低可行功率后,固定该轮, + * 剩余预算继续按其余轮的请求比例分配,避免转速损耗造成配额不可行。 */ +inline ProportionalPowerLimitResult limit_power_proportionally( + const float* requested_current, const float* rpm, int motor_count, + float available_power, float kt, float k1, float k2) +{ + ProportionalPowerLimitResult result; + if (motor_count < 0 || motor_count > POWER_CONTROL_MAX_MOTOR_COUNT || + (motor_count > 0 && (!requested_current || !rpm)) || + !std::isfinite(available_power) || !std::isfinite(kt) || !std::isfinite(k1) || + !std::isfinite(k2) || kt < 0.0f || k1 < 0.0f || k2 < 0.0f) + { + return result; + } + + const float AVAILABLE_POWER = std::max(0.0f, available_power); + float minimum_current[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + float minimum_power[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + float required_power_sum = 0.0f; + bool request_clamped = false; + for (int i = 0; i < motor_count; ++i) + { + if (!std::isfinite(requested_current[i]) || !std::isfinite(rpm[i])) + { + return ProportionalPowerLimitResult{}; + } + result.current[i] = std::clamp(requested_current[i], -16384.0f, 16384.0f); + request_clamped = request_clamped || result.current[i] != requested_current[i]; + result.requested_power[i] = + calculate_motor_model_power(result.current[i], rpm[i], kt, k1, k2); + if (!std::isfinite(result.requested_power[i])) + { + return ProportionalPowerLimitResult{}; + } + minimum_current[i] = result.current[i]; + if (result.requested_power[i] > 0.0f) + { + required_power_sum += result.requested_power[i]; + minimum_current[i] = minimum_model_power_current(result.current[i], rpm[i], kt, k1); + minimum_power[i] = std::max( + 0.0f, calculate_motor_model_power(minimum_current[i], rpm[i], kt, k1, k2)); + result.minimum_power_sum += minimum_power[i]; + } + } + + if (!std::isfinite(required_power_sum) || !std::isfinite(result.minimum_power_sum)) + { + return ProportionalPowerLimitResult{}; + } + result.budget_feasible = result.minimum_power_sum <= AVAILABLE_POWER; + result.is_power_limited = request_clamped || required_power_sum > AVAILABLE_POWER; + if (required_power_sum <= AVAILABLE_POWER) + { + return result; + } + if (!result.budget_feasible) + { + for (int i = 0; i < motor_count; ++i) + { + result.current[i] = minimum_current[i]; + } + return result; + } + + bool fixed[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; + float fixed_power_sum = 0.0f; + float remaining_request_sum = required_power_sum; + float power_scale = 0.0f; + for (int iteration = 0; iteration < motor_count; ++iteration) + { + power_scale = + remaining_request_sum > 0.0f + ? std::clamp((AVAILABLE_POWER - fixed_power_sum) / remaining_request_sum, + 0.0f, 1.0f) + : 0.0f; + bool fixed_another = false; + for (int i = 0; i < motor_count; ++i) + { + if (!fixed[i] && result.requested_power[i] > 0.0f && + power_scale * result.requested_power[i] < minimum_power[i]) + { + fixed[i] = true; + fixed_power_sum += minimum_power[i]; + remaining_request_sum -= result.requested_power[i]; + fixed_another = true; + } + } + if (!fixed_another) + { + break; + } + } + + for (int i = 0; i < motor_count; ++i) + { + if (result.requested_power[i] > 0.0f) + { + const float POWER_QUOTA = + fixed[i] ? minimum_power[i] : power_scale * result.requested_power[i]; + result.current[i] = + solve_current_for_power(POWER_QUOTA, rpm[i], kt, k1, k2, result.current[i]); + } + } + return result; +} diff --git a/tests/power_limit_test.cpp b/tests/power_limit_test.cpp new file mode 100644 index 0000000..ba42b8c --- /dev/null +++ b/tests/power_limit_test.cpp @@ -0,0 +1,215 @@ +#include +#include +#include +#include + +#include "../PowerLimit.hpp" + +static constexpr float KT = 1.99688994e-6f; +static constexpr float K1 = 2.0e-7f; +static constexpr float K2 = 3.0e-7f; + +static void require(bool condition, const char* description) +{ + if (!condition) + { + std::fprintf(stderr, "FAILED: %s\n", description); + std::exit(EXIT_FAILURE); + } +} + +/* 用 double 独立检查输出模型正功率之和,避免仅比较实现自身的统计值。 */ +static double positive_power_sum(const float* current, const float* rpm, int count, + float kt = KT, float k1 = K1, float k2 = K2) +{ + double total = 0.0; + for (int i = 0; i < count; ++i) + { + const double CURRENT = current[i]; + const double RPM = rpm[i]; + const double POWER = kt * CURRENT * RPM + k1 * CURRENT * CURRENT + k2 * RPM * RPM; + total += std::max(0.0, POWER); + } + return total; +} + +static void check_current_bounds(const float* request, + const ProportionalPowerLimitResult& result, int count) +{ + for (int i = 0; i < count; ++i) + { + require(std::isfinite(result.current[i]), "finite current"); + require(std::fabs(result.current[i]) <= std::fabs(request[i]) + 0.001f, + "limiter cannot increase requested magnitude"); + require(result.current[i] * request[i] >= 0.0f, + "limiter cannot reverse requested current"); + } +} + +static void test_switching_regression() +{ + const float REQUEST[4] = {1834.0f, 262.0f, 262.0f, 262.0f}; + const float RPM[4] = {7000.0f, 7000.0f, 7000.0f, 7000.0f}; + const auto LIMITED = limit_power_proportionally(REQUEST, RPM, 4, 96.0f, KT, K1, K2); + require(LIMITED.is_power_limited && LIMITED.budget_feasible, + "original switching example must be limited and feasible"); + check_current_bounds(REQUEST, LIMITED, 4); + require(positive_power_sum(LIMITED.current, RPM, 4) <= 96.002, + "original switching example respects power budget"); + for (int i = 0; i < 4; ++i) + { + require(std::fabs(LIMITED.current[i] - REQUEST[i]) < 10.0f, + "slight overload only slightly changes each current"); + } + std::printf("96 W regression currents: %.3f %.3f %.3f %.3f LSB\n", LIMITED.current[0], + LIMITED.current[1], LIMITED.current[2], LIMITED.current[3]); + + const float TOTAL = static_cast(positive_power_sum(REQUEST, RPM, 4)); + const auto BELOW = + limit_power_proportionally(REQUEST, RPM, 4, TOTAL + 0.01f, KT, K1, K2); + const auto ABOVE = + limit_power_proportionally(REQUEST, RPM, 4, TOTAL - 0.01f, KT, K1, K2); + require(!BELOW.is_power_limited && ABOVE.is_power_limited, + "exercise both sides of switching threshold"); + for (int i = 0; i < 4; ++i) + { + require(BELOW.current[i] == REQUEST[i], "unlimited exact passthrough"); + require(std::fabs(ABOVE.current[i] - BELOW.current[i]) < 2.0f, + "continuous current near power threshold"); + } + + float negative_request[4]; + float negative_rpm[4]; + for (int i = 0; i < 4; ++i) + { + negative_request[i] = -REQUEST[i]; + negative_rpm[i] = -RPM[i]; + } + const auto REVERSED = + limit_power_proportionally(negative_request, negative_rpm, 4, 96.0f, KT, K1, K2); + for (int i = 0; i < 4; ++i) + { + require(std::fabs(REVERSED.current[i] + LIMITED.current[i]) < 0.001f, + "positive and negative rotation behave symmetrically"); + } +} + +static void test_floors_and_braking() +{ + const float REQUEST[4] = {6000.0f, 100.0f, 100.0f, 100.0f}; + const float RPM[4] = {7000.0f, 7000.0f, 7000.0f, 7000.0f}; + const auto FLOOR_LIMITED = + limit_power_proportionally(REQUEST, RPM, 4, 65.0f, KT, K1, K2); + require(FLOOR_LIMITED.budget_feasible, "floor constrained allocation feasible"); + check_current_bounds(REQUEST, FLOOR_LIMITED, 4); + require(positive_power_sum(FLOOR_LIMITED.current, RPM, 4) <= 65.002, + "reserve minimum powers before redistributing budget"); + require(positive_power_sum(FLOOR_LIMITED.current, RPM, 4) >= 64.998, + "use available budget when feasible"); + + const float HIGH_RPM[4] = {9000.0f, 9000.0f, 9000.0f, 9000.0f}; + const auto INFEASIBLE = + limit_power_proportionally(REQUEST, HIGH_RPM, 4, 96.0f, KT, K1, K2); + require(INFEASIBLE.is_power_limited && !INFEASIBLE.budget_feasible, + "speed loss above budget explicitly marked infeasible"); + for (int i = 0; i < 4; ++i) + { + require(INFEASIBLE.current[i] == 0.0f, + "infeasible motoring budget gives zero, never forced braking"); + } + + const float MIXED_REQUEST[4] = {6000.0f, -3000.0f, -16384.0f, 0.0f}; + const float MIXED_RPM[4] = {7000.0f, 7000.0f, 100.0f, 0.0f}; + const auto MIXED = + limit_power_proportionally(MIXED_REQUEST, MIXED_RPM, 4, 40.0f, KT, K1, K2); + check_current_bounds(MIXED_REQUEST, MIXED, 4); + require(MIXED.budget_feasible, "mixed drive and braking case feasible"); + require(MIXED.current[1] == MIXED_REQUEST[1], + "negative model power braking passes through unchanged"); + require(MIXED.current[2] < 0.0f, "positive power braking retains direction"); + require(positive_power_sum(MIXED.current, MIXED_RPM, 4) <= 40.002, + "mixed drive and braking respects positive power budget"); + + require(solve_current_for_power(0.0f, 7000.0f, KT, K1, K2, 262.0f) == 0.0f, + "low quota cannot synthesize braking from a drive request"); + require(solve_current_for_power(100.0f, 7000.0f, KT, K1, K2, 262.0f) == 262.0f, + "large quota cannot amplify requested current"); + require(solve_current_for_power(0.0f, 0.0f, KT, K1, K2, 1000.0f) == 0.0f, + "zero power at standstill gives zero current"); + require(std::fabs(solve_current_for_power(1.0f, 1000.0f, KT, 0.0f, 0.0f, 1000.0f) - + 1.0f / (KT * 1000.0f)) < 0.001f, + "linear model is handled without division by zero"); +} + +static void test_random_invariants() +{ + std::mt19937 random(20261015); + std::uniform_real_distribution current_distribution(-16384.0f, 16384.0f); + std::uniform_real_distribution rpm_distribution(-9500.0f, 9500.0f); + std::uniform_real_distribution budget_distribution(0.0f, 250.0f); + std::uniform_real_distribution k1_distribution(1.0e-7f, 1.0e-6f); + std::uniform_real_distribution k2_distribution(1.0e-7f, 5.0e-7f); + for (int iteration = 0; iteration < 4000; ++iteration) + { + const int COUNT = 1 + iteration % POWER_CONTROL_MAX_MOTOR_COUNT; + const float BUDGET = budget_distribution(random); + const float COPPER_LOSS = k1_distribution(random); + const float SPEED_LOSS = k2_distribution(random); + float request[POWER_CONTROL_MAX_MOTOR_COUNT]; + float rpm[POWER_CONTROL_MAX_MOTOR_COUNT]; + for (int i = 0; i < COUNT; ++i) + { + request[i] = current_distribution(random); + rpm[i] = rpm_distribution(random); + } + const auto RESULT = limit_power_proportionally(request, rpm, COUNT, BUDGET, KT, + COPPER_LOSS, SPEED_LOSS); + check_current_bounds(request, RESULT, COUNT); + const double OUTPUT_POWER = + positive_power_sum(RESULT.current, rpm, COUNT, KT, COPPER_LOSS, SPEED_LOSS); + if (RESULT.budget_feasible) + { + require(OUTPUT_POWER <= BUDGET + 0.002, + "feasible randomized budget must bound output power"); + } + else + { + require(RESULT.is_power_limited && RESULT.minimum_power_sum > BUDGET, + "infeasible randomized budget explicitly reported"); + require(std::fabs(OUTPUT_POWER - RESULT.minimum_power_sum) < 0.002, + "infeasible output minimizes achievable positive model power"); + } + for (int i = 0; i < COUNT; ++i) + { + const double ORIGINAL_POWER = + KT * static_cast(request[i]) * rpm[i] + + COPPER_LOSS * static_cast(request[i]) * request[i] + + SPEED_LOSS * static_cast(rpm[i]) * rpm[i]; + if (ORIGINAL_POWER < -0.001) + { + require(RESULT.current[i] == request[i], + "randomized negative power braking stays intact"); + } + } + } +} + +static void test_invalid_input() +{ + const float INVALID_RPM[1] = {std::numeric_limits::quiet_NaN()}; + const float REQUEST[1] = {1000.0f}; + const auto INVALID = + limit_power_proportionally(REQUEST, INVALID_RPM, 1, 96.0f, KT, K1, K2); + require( + INVALID.is_power_limited && !INVALID.budget_feasible && INVALID.current[0] == 0.0f, + "invalid feedback cannot bypass limiter"); +} + +int main() +{ + test_switching_regression(); + test_floors_and_braking(); + test_random_invariants(); + test_invalid_input(); + std::puts("Power limiter regression and 4000 invariant cases passed."); +} From 9d120c8cdb0702ca6ad0f13e5110531ca5bd6dd1 Mon Sep 17 00:00:00 2001 From: kaMi-0932 <1580654962@qq.com> Date: Tue, 3 Nov 2026 19:27:48 +0800 Subject: [PATCH 2/2] Restore upstream limiter and expose live drive power model --- PowerControl.hpp | 312 +++++++++++++++++++++++++++++++------ PowerLimit.hpp | 193 ----------------------- tests/power_limit_test.cpp | 215 ------------------------- 3 files changed, 261 insertions(+), 459 deletions(-) delete mode 100644 PowerLimit.hpp delete mode 100644 tests/power_limit_test.cpp diff --git a/PowerControl.hpp b/PowerControl.hpp index 920b6b9..bd543af 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -13,7 +13,6 @@ module_description: 底盘功率控制模块:按功率上限重新分配各电 #include #include -#include "PowerLimit.hpp" #include "RLS.hpp" #include "SuperPower.hpp" #include "message.hpp" @@ -29,6 +28,88 @@ module_description: 底盘功率控制模块:按功率上限重新分配各电 /// Chassis power limit margin (W) #define CHASSIS_POWER_LIMIT_MARGIN_W 4.0f +/** + * @brief 计算单个电机的模型预测功率,不含静态损耗。 + * Compute the model-predicted power of one motor, excluding the static loss. + * + * @param current 电机电流。 + * Motor current. + * @param rpm 转子转速 (rpm)。 + * Rotor speed (rpm). + * @param kt 机械功率系数。 + * Mechanical power coefficient. + * @param k1 电流平方项系数。 + * Coefficient of the squared current term. + * @param k2 转速平方项系数。 + * Coefficient of the squared speed term. + * @return 预测功率 (W)。 + * Predicted power (W). + */ +inline float calculate_motor_model_power(float current, float rpm, float kt, float k1, + float k2) +{ + return (kt * current * rpm) + (k1 * current * current) + (k2 * rpm * rpm); +} + +/** + * @brief 由目标功率反解电流,结果限幅到 ±16384。 + * Solve the current from a target power, clamped to ±16384. + * + * @param target_power 目标功率 (W)。 + * Target power (W). + * @param rpm 转子转速 (rpm)。 + * Rotor speed (rpm). + * @param kt 机械功率系数。 + * Mechanical power coefficient. + * @param k1 电流平方项系数。 + * Coefficient of the squared current term. + * @param k2 转速平方项系数。 + * Coefficient of the squared speed term. + * @param original_current 原始期望电流,其符号决定取二次方程的哪个根。 + * Original desired current, whose sign selects the root of the + * quadratic equation. + * @return 反解得到的电流。 + * Solved current. + */ +inline float solve_current_for_power(float target_power, float rpm, float kt, float k1, + float k2, float original_current) +{ + float a = k1; + float b = kt * rpm; + float c = k2 * rpm * rpm - target_power; + float delta = b * b - 4.0f * a * c; + + float x3 = -b / (2.0f * a); + + float final_current = 0; + + if (delta < 1e-9f) + { + final_current = x3; + } + else + { + float sqrt_delta = sqrtf(delta); + float x1 = (-b + sqrt_delta) / (2.0f * a); + float x2 = (-b - sqrt_delta) / (2.0f * a); + + if (original_current >= 0) + { + final_current = x1; + } + else + { + final_current = x2; + } + } + + return std::clamp(final_current, -16384.0f, 16384.0f); +} + +/// 最大电机数量 +/// Maximum number of motors +static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; + /** * @brief 功率限制后的输出。 * Output after the power limit. @@ -59,6 +140,25 @@ class PowerControl /// Maximum number of motors static constexpr int MAX_MOTOR_COUNT = POWER_CONTROL_MAX_MOTOR_COUNT; + /** + * @brief 3508 功率模型的同一时刻快照,供底盘约束求解使用。 + * A consistent snapshot of the 3508 power model for chassis constraints. + * + * Motor power is kt * current * rpm + k1 * current^2 + k2 * rpm^2, in W. + * Current is the signed motor command/feedback in LSB, not amperes; rpm is + * rotor speed before the gearbox. Static loss and margin are chassis watts. + * Coefficients are returned unchanged; callers must reject an invalid snapshot. + */ + struct DrivePowerModel + { + float kt = 0.0f; + float k1 = 0.0f; + float k2 = 0.0f; + float static_loss = 0.0f; + float margin = 0.0f; + bool valid = false; + }; + /** * @brief 3508 组的分配偏置:先按保底池分配,再分剩余功率。 * Allocation bias of the 3508 group: distribute a reserve pool first, then @@ -87,8 +187,8 @@ 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 为全向轮或麦轮底盘。 - * True for drive-only power limiting on a steering-wheel chassis; false + * @param is_helm true 为舵轮底盘,同时限制 6020;false 为全向轮或麦轮底盘。 + * True for a steering-wheel chassis, which also limits the 6020; false * for an omni-wheel or mecanum chassis. * @param chassis_static_power_loss 底盘静态功耗 (W)。 * Chassis static power loss (W). @@ -220,7 +320,17 @@ class PowerControl float residual = measured_power_ - mechanical_power - k3_chassis_; - // The metered/modelled chassis power covers drive motors only. + 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) { @@ -264,6 +374,27 @@ class PowerControl return powercontrol_data_; } + /** + * @brief 在同一把锁下读取在线辨识系数、静态损耗和限功率余量。 + * Read live coefficients, static loss and power margin under one lock. + * + * No model identification or motor output is performed by this accessor. + * The acceleration optimizer budgets the signed sum of drive motor powers + * against the selected chassis power limit minus static_loss and margin. + */ + DrivePowerModel GetDrivePowerModel() + { + LibXR::Mutex::LockGuard lock(mutex_); + DrivePowerModel model{ + kt_3508_, k1_3508_, k2_3508_, k3_chassis_, CHASSIS_POWER_LIMIT_MARGIN_W, false}; + model.valid = std::isfinite(model.kt) && model.kt >= 0.0f && + std::isfinite(model.k1) && model.k1 >= 0.0f && + std::isfinite(model.k2) && model.k2 >= 0.0f && + std::isfinite(model.static_loss) && model.static_loss >= 0.0f && + std::isfinite(model.margin) && model.margin >= 0.0f; + return model; + } + /** * @brief 获取最近一次 `CalculatePowerControlParam()` 读到的实测功率。 * Get the measured power read by the latest `CalculatePowerControlParam()`. @@ -291,42 +422,6 @@ class PowerControl */ bool IsOnline() { return superpower_->IsOnline(); } - struct PowerEstimate - { - float wheel_power_w = 0.0f; - float steer_power_w = 0.0f; // Steering is excluded from the drive-power budget. - float total_power_w = 0.0f; - bool valid = false; - }; - - /// Drive-motor model estimate (W), excluding steering and static board losses. - PowerEstimate EstimatePower(const float* current_3508, const float* rpm_3508) - { - LibXR::Mutex::LockGuard lock(mutex_); - PowerEstimate estimate{}; - if (motor_count_3508_ > 0 && (!current_3508 || !rpm_3508)) - { - return estimate; - } - for (int i = 0; i < motor_count_3508_; i++) - { - if (!std::isfinite(current_3508[i]) || !std::isfinite(rpm_3508[i])) - { - return {}; - } - const float POWER = calculate_motor_model_power(current_3508[i], rpm_3508[i], - kt_3508_, k1_3508_, k2_3508_); - if (!std::isfinite(POWER)) - { - return {}; - } - estimate.wheel_power_w += std::max(0.0f, POWER); - } - estimate.total_power_w = estimate.wheel_power_w; - estimate.valid = std::isfinite(estimate.total_power_w); - return estimate.valid ? estimate : PowerEstimate{}; - } - private: void OutputLimitOmni(float max_power) { @@ -512,23 +607,133 @@ class PowerControl void OutputLimitHelm(float max_power) { - /* 最大功率只约束 3508 驱动轮,保留模型误差与控制延迟余量。 */ - const float AVAILABLE_POWER = max_power - CHASSIS_POWER_LIMIT_MARGIN_W; + 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]; + } + } - /* 6020 舵向电机不参与限幅,始终保留原电流命令。 */ for (int i = 0; i < motor_count_6020_; i++) { - powercontrol_data_.new_output_current_6020[i] = output_current_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]; + } } - const auto LIMITED = limit_power_proportionally( - output_current_3508_, rotorspeed_rpm_3508_, motor_count_3508_, AVAILABLE_POWER, - kt_3508_, k1_3508_, k2_3508_); - powercontrol_data_.is_power_limited = LIMITED.is_power_limited; - for (int i = 0; i < motor_count_3508_; 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 { - motor_power_3508_[i] = LIMITED.requested_power[i]; - powercontrol_data_.new_output_current_3508[i] = LIMITED.current[i]; + /*功率充足,不限制*/ + 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]; + } } } @@ -560,8 +765,13 @@ class PowerControl float rotorspeed_rpm_3508_[MAX_MOTOR_COUNT] = {}; float motor_power_3508_[MAX_MOTOR_COUNT] = {}; + float kt_6020_ = 1.42074505e-5f; + float k1_6020_ = 6.4276e-7f; + float k2_6020_ = 1.0e-10f; + float output_current_6020_[MAX_MOTOR_COUNT] = {}; float rotorspeed_rpm_6020_[MAX_MOTOR_COUNT] = {}; + float motor_power_6020_[MAX_MOTOR_COUNT] = {}; float measured_power_ = 0.0f; }; diff --git a/PowerLimit.hpp b/PowerLimit.hpp deleted file mode 100644 index 0285dda..0000000 --- a/PowerLimit.hpp +++ /dev/null @@ -1,193 +0,0 @@ -#pragma once - -#include -#include - -static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6; - -/* 电流单位为电机控制 LSB,转速为 rpm,模型功率为 W。 */ -inline float calculate_motor_model_power(float current, float rpm, float kt, float k1, - float k2) -{ - return kt * current * rpm + k1 * current * current + k2 * rpm * rpm; -} - -/* 只在零电流至原请求电流之间寻找模型功率最低点,保留请求方向。 */ -inline float minimum_model_power_current(float original_current, float rpm, float kt, - float k1) -{ - const float REQUEST = std::clamp(original_current, -16384.0f, 16384.0f); - if (k1 > 0.0f) - { - return std::clamp(-kt * rpm / (2.0f * k1), std::min(0.0f, REQUEST), - std::max(0.0f, REQUEST)); - } - return kt * rpm * REQUEST < 0.0f ? REQUEST : 0.0f; -} - -/* 取允许区间内最接近原请求的解;配额不可行时取区间内最低功率点。 - * 原请求制动时也保留其电流方向,不因选择二次方程的另一根而反向。 */ -inline float solve_current_for_power(float target_power, float rpm, float kt, float k1, - float k2, float original_current) -{ - if (!std::isfinite(target_power) || !std::isfinite(rpm) || - !std::isfinite(original_current) || !std::isfinite(kt) || !std::isfinite(k1) || - !std::isfinite(k2) || kt < 0.0f || k1 < 0.0f || k2 < 0.0f) - { - return 0.0f; - } - const float REQUEST = std::clamp(original_current, -16384.0f, 16384.0f); - const float TARGET = std::max(0.0f, target_power); - const float REQUESTED_POWER = calculate_motor_model_power(REQUEST, rpm, kt, k1, k2); - if (!std::isfinite(REQUESTED_POWER)) - { - return 0.0f; - } - if (REQUESTED_POWER <= TARGET) - { - return REQUEST; - } - - const float MINIMUM_CURRENT = minimum_model_power_current(REQUEST, rpm, kt, k1); - const float MINIMUM_POWER = - calculate_motor_model_power(MINIMUM_CURRENT, rpm, kt, k1, k2); - if (MINIMUM_POWER >= TARGET) - { - return MINIMUM_CURRENT; - } - - /* 在请求方向的电流幅值坐标上解方程,取较大的根。 */ - const float DIRECTION = REQUEST < 0.0f ? -1.0f : 1.0f; - const float B = kt * rpm * DIRECTION; - const float C = k2 * rpm * rpm - TARGET; - float magnitude = 0.0f; - if (k1 == 0.0f) - { - magnitude = -C / B; - } - else - { - const float SQRT_DELTA = std::sqrt(std::max(0.0f, B * B - 4.0f * k1 * C)); - /* B >= 0 时使用等价式,避免相近数相减损失精度。 */ - magnitude = - B >= 0.0f ? -2.0f * C / (B + SQRT_DELTA) : (-B + SQRT_DELTA) / (2.0f * k1); - } - return DIRECTION * - std::clamp(magnitude, std::fabs(MINIMUM_CURRENT), std::fabs(REQUEST)); -} - -struct ProportionalPowerLimitResult -{ - float current[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; - float requested_power[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; - float minimum_power_sum = 0.0f; - /* 非法输入默认零输出,并保持限幅状态,避免调用方透传请求。 */ - bool is_power_limited = true; - bool budget_feasible = false; -}; - -/* 按请求正功率比例分配。某轮配额达到最低可行功率后,固定该轮, - * 剩余预算继续按其余轮的请求比例分配,避免转速损耗造成配额不可行。 */ -inline ProportionalPowerLimitResult limit_power_proportionally( - const float* requested_current, const float* rpm, int motor_count, - float available_power, float kt, float k1, float k2) -{ - ProportionalPowerLimitResult result; - if (motor_count < 0 || motor_count > POWER_CONTROL_MAX_MOTOR_COUNT || - (motor_count > 0 && (!requested_current || !rpm)) || - !std::isfinite(available_power) || !std::isfinite(kt) || !std::isfinite(k1) || - !std::isfinite(k2) || kt < 0.0f || k1 < 0.0f || k2 < 0.0f) - { - return result; - } - - const float AVAILABLE_POWER = std::max(0.0f, available_power); - float minimum_current[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; - float minimum_power[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; - float required_power_sum = 0.0f; - bool request_clamped = false; - for (int i = 0; i < motor_count; ++i) - { - if (!std::isfinite(requested_current[i]) || !std::isfinite(rpm[i])) - { - return ProportionalPowerLimitResult{}; - } - result.current[i] = std::clamp(requested_current[i], -16384.0f, 16384.0f); - request_clamped = request_clamped || result.current[i] != requested_current[i]; - result.requested_power[i] = - calculate_motor_model_power(result.current[i], rpm[i], kt, k1, k2); - if (!std::isfinite(result.requested_power[i])) - { - return ProportionalPowerLimitResult{}; - } - minimum_current[i] = result.current[i]; - if (result.requested_power[i] > 0.0f) - { - required_power_sum += result.requested_power[i]; - minimum_current[i] = minimum_model_power_current(result.current[i], rpm[i], kt, k1); - minimum_power[i] = std::max( - 0.0f, calculate_motor_model_power(minimum_current[i], rpm[i], kt, k1, k2)); - result.minimum_power_sum += minimum_power[i]; - } - } - - if (!std::isfinite(required_power_sum) || !std::isfinite(result.minimum_power_sum)) - { - return ProportionalPowerLimitResult{}; - } - result.budget_feasible = result.minimum_power_sum <= AVAILABLE_POWER; - result.is_power_limited = request_clamped || required_power_sum > AVAILABLE_POWER; - if (required_power_sum <= AVAILABLE_POWER) - { - return result; - } - if (!result.budget_feasible) - { - for (int i = 0; i < motor_count; ++i) - { - result.current[i] = minimum_current[i]; - } - return result; - } - - bool fixed[POWER_CONTROL_MAX_MOTOR_COUNT] = {}; - float fixed_power_sum = 0.0f; - float remaining_request_sum = required_power_sum; - float power_scale = 0.0f; - for (int iteration = 0; iteration < motor_count; ++iteration) - { - power_scale = - remaining_request_sum > 0.0f - ? std::clamp((AVAILABLE_POWER - fixed_power_sum) / remaining_request_sum, - 0.0f, 1.0f) - : 0.0f; - bool fixed_another = false; - for (int i = 0; i < motor_count; ++i) - { - if (!fixed[i] && result.requested_power[i] > 0.0f && - power_scale * result.requested_power[i] < minimum_power[i]) - { - fixed[i] = true; - fixed_power_sum += minimum_power[i]; - remaining_request_sum -= result.requested_power[i]; - fixed_another = true; - } - } - if (!fixed_another) - { - break; - } - } - - for (int i = 0; i < motor_count; ++i) - { - if (result.requested_power[i] > 0.0f) - { - const float POWER_QUOTA = - fixed[i] ? minimum_power[i] : power_scale * result.requested_power[i]; - result.current[i] = - solve_current_for_power(POWER_QUOTA, rpm[i], kt, k1, k2, result.current[i]); - } - } - return result; -} diff --git a/tests/power_limit_test.cpp b/tests/power_limit_test.cpp deleted file mode 100644 index ba42b8c..0000000 --- a/tests/power_limit_test.cpp +++ /dev/null @@ -1,215 +0,0 @@ -#include -#include -#include -#include - -#include "../PowerLimit.hpp" - -static constexpr float KT = 1.99688994e-6f; -static constexpr float K1 = 2.0e-7f; -static constexpr float K2 = 3.0e-7f; - -static void require(bool condition, const char* description) -{ - if (!condition) - { - std::fprintf(stderr, "FAILED: %s\n", description); - std::exit(EXIT_FAILURE); - } -} - -/* 用 double 独立检查输出模型正功率之和,避免仅比较实现自身的统计值。 */ -static double positive_power_sum(const float* current, const float* rpm, int count, - float kt = KT, float k1 = K1, float k2 = K2) -{ - double total = 0.0; - for (int i = 0; i < count; ++i) - { - const double CURRENT = current[i]; - const double RPM = rpm[i]; - const double POWER = kt * CURRENT * RPM + k1 * CURRENT * CURRENT + k2 * RPM * RPM; - total += std::max(0.0, POWER); - } - return total; -} - -static void check_current_bounds(const float* request, - const ProportionalPowerLimitResult& result, int count) -{ - for (int i = 0; i < count; ++i) - { - require(std::isfinite(result.current[i]), "finite current"); - require(std::fabs(result.current[i]) <= std::fabs(request[i]) + 0.001f, - "limiter cannot increase requested magnitude"); - require(result.current[i] * request[i] >= 0.0f, - "limiter cannot reverse requested current"); - } -} - -static void test_switching_regression() -{ - const float REQUEST[4] = {1834.0f, 262.0f, 262.0f, 262.0f}; - const float RPM[4] = {7000.0f, 7000.0f, 7000.0f, 7000.0f}; - const auto LIMITED = limit_power_proportionally(REQUEST, RPM, 4, 96.0f, KT, K1, K2); - require(LIMITED.is_power_limited && LIMITED.budget_feasible, - "original switching example must be limited and feasible"); - check_current_bounds(REQUEST, LIMITED, 4); - require(positive_power_sum(LIMITED.current, RPM, 4) <= 96.002, - "original switching example respects power budget"); - for (int i = 0; i < 4; ++i) - { - require(std::fabs(LIMITED.current[i] - REQUEST[i]) < 10.0f, - "slight overload only slightly changes each current"); - } - std::printf("96 W regression currents: %.3f %.3f %.3f %.3f LSB\n", LIMITED.current[0], - LIMITED.current[1], LIMITED.current[2], LIMITED.current[3]); - - const float TOTAL = static_cast(positive_power_sum(REQUEST, RPM, 4)); - const auto BELOW = - limit_power_proportionally(REQUEST, RPM, 4, TOTAL + 0.01f, KT, K1, K2); - const auto ABOVE = - limit_power_proportionally(REQUEST, RPM, 4, TOTAL - 0.01f, KT, K1, K2); - require(!BELOW.is_power_limited && ABOVE.is_power_limited, - "exercise both sides of switching threshold"); - for (int i = 0; i < 4; ++i) - { - require(BELOW.current[i] == REQUEST[i], "unlimited exact passthrough"); - require(std::fabs(ABOVE.current[i] - BELOW.current[i]) < 2.0f, - "continuous current near power threshold"); - } - - float negative_request[4]; - float negative_rpm[4]; - for (int i = 0; i < 4; ++i) - { - negative_request[i] = -REQUEST[i]; - negative_rpm[i] = -RPM[i]; - } - const auto REVERSED = - limit_power_proportionally(negative_request, negative_rpm, 4, 96.0f, KT, K1, K2); - for (int i = 0; i < 4; ++i) - { - require(std::fabs(REVERSED.current[i] + LIMITED.current[i]) < 0.001f, - "positive and negative rotation behave symmetrically"); - } -} - -static void test_floors_and_braking() -{ - const float REQUEST[4] = {6000.0f, 100.0f, 100.0f, 100.0f}; - const float RPM[4] = {7000.0f, 7000.0f, 7000.0f, 7000.0f}; - const auto FLOOR_LIMITED = - limit_power_proportionally(REQUEST, RPM, 4, 65.0f, KT, K1, K2); - require(FLOOR_LIMITED.budget_feasible, "floor constrained allocation feasible"); - check_current_bounds(REQUEST, FLOOR_LIMITED, 4); - require(positive_power_sum(FLOOR_LIMITED.current, RPM, 4) <= 65.002, - "reserve minimum powers before redistributing budget"); - require(positive_power_sum(FLOOR_LIMITED.current, RPM, 4) >= 64.998, - "use available budget when feasible"); - - const float HIGH_RPM[4] = {9000.0f, 9000.0f, 9000.0f, 9000.0f}; - const auto INFEASIBLE = - limit_power_proportionally(REQUEST, HIGH_RPM, 4, 96.0f, KT, K1, K2); - require(INFEASIBLE.is_power_limited && !INFEASIBLE.budget_feasible, - "speed loss above budget explicitly marked infeasible"); - for (int i = 0; i < 4; ++i) - { - require(INFEASIBLE.current[i] == 0.0f, - "infeasible motoring budget gives zero, never forced braking"); - } - - const float MIXED_REQUEST[4] = {6000.0f, -3000.0f, -16384.0f, 0.0f}; - const float MIXED_RPM[4] = {7000.0f, 7000.0f, 100.0f, 0.0f}; - const auto MIXED = - limit_power_proportionally(MIXED_REQUEST, MIXED_RPM, 4, 40.0f, KT, K1, K2); - check_current_bounds(MIXED_REQUEST, MIXED, 4); - require(MIXED.budget_feasible, "mixed drive and braking case feasible"); - require(MIXED.current[1] == MIXED_REQUEST[1], - "negative model power braking passes through unchanged"); - require(MIXED.current[2] < 0.0f, "positive power braking retains direction"); - require(positive_power_sum(MIXED.current, MIXED_RPM, 4) <= 40.002, - "mixed drive and braking respects positive power budget"); - - require(solve_current_for_power(0.0f, 7000.0f, KT, K1, K2, 262.0f) == 0.0f, - "low quota cannot synthesize braking from a drive request"); - require(solve_current_for_power(100.0f, 7000.0f, KT, K1, K2, 262.0f) == 262.0f, - "large quota cannot amplify requested current"); - require(solve_current_for_power(0.0f, 0.0f, KT, K1, K2, 1000.0f) == 0.0f, - "zero power at standstill gives zero current"); - require(std::fabs(solve_current_for_power(1.0f, 1000.0f, KT, 0.0f, 0.0f, 1000.0f) - - 1.0f / (KT * 1000.0f)) < 0.001f, - "linear model is handled without division by zero"); -} - -static void test_random_invariants() -{ - std::mt19937 random(20261015); - std::uniform_real_distribution current_distribution(-16384.0f, 16384.0f); - std::uniform_real_distribution rpm_distribution(-9500.0f, 9500.0f); - std::uniform_real_distribution budget_distribution(0.0f, 250.0f); - std::uniform_real_distribution k1_distribution(1.0e-7f, 1.0e-6f); - std::uniform_real_distribution k2_distribution(1.0e-7f, 5.0e-7f); - for (int iteration = 0; iteration < 4000; ++iteration) - { - const int COUNT = 1 + iteration % POWER_CONTROL_MAX_MOTOR_COUNT; - const float BUDGET = budget_distribution(random); - const float COPPER_LOSS = k1_distribution(random); - const float SPEED_LOSS = k2_distribution(random); - float request[POWER_CONTROL_MAX_MOTOR_COUNT]; - float rpm[POWER_CONTROL_MAX_MOTOR_COUNT]; - for (int i = 0; i < COUNT; ++i) - { - request[i] = current_distribution(random); - rpm[i] = rpm_distribution(random); - } - const auto RESULT = limit_power_proportionally(request, rpm, COUNT, BUDGET, KT, - COPPER_LOSS, SPEED_LOSS); - check_current_bounds(request, RESULT, COUNT); - const double OUTPUT_POWER = - positive_power_sum(RESULT.current, rpm, COUNT, KT, COPPER_LOSS, SPEED_LOSS); - if (RESULT.budget_feasible) - { - require(OUTPUT_POWER <= BUDGET + 0.002, - "feasible randomized budget must bound output power"); - } - else - { - require(RESULT.is_power_limited && RESULT.minimum_power_sum > BUDGET, - "infeasible randomized budget explicitly reported"); - require(std::fabs(OUTPUT_POWER - RESULT.minimum_power_sum) < 0.002, - "infeasible output minimizes achievable positive model power"); - } - for (int i = 0; i < COUNT; ++i) - { - const double ORIGINAL_POWER = - KT * static_cast(request[i]) * rpm[i] + - COPPER_LOSS * static_cast(request[i]) * request[i] + - SPEED_LOSS * static_cast(rpm[i]) * rpm[i]; - if (ORIGINAL_POWER < -0.001) - { - require(RESULT.current[i] == request[i], - "randomized negative power braking stays intact"); - } - } - } -} - -static void test_invalid_input() -{ - const float INVALID_RPM[1] = {std::numeric_limits::quiet_NaN()}; - const float REQUEST[1] = {1000.0f}; - const auto INVALID = - limit_power_proportionally(REQUEST, INVALID_RPM, 1, 96.0f, KT, K1, K2); - require( - INVALID.is_power_limited && !INVALID.budget_feasible && INVALID.current[0] == 0.0f, - "invalid feedback cannot bypass limiter"); -} - -int main() -{ - test_switching_regression(); - test_floors_and_braking(); - test_random_invariants(); - test_invalid_input(); - std::puts("Power limiter regression and 4000 invariant cases passed."); -}