From 42cdbb8819bfc2e1efb80524ae7f6fb35e75aade Mon Sep 17 00:00:00 2001 From: shayana18 Date: Sat, 21 Mar 2026 00:24:56 -0700 Subject: [PATCH 01/35] base work for tire model done (pure slip).... will change for combined slip once coefficents are here.... testing TODO --- .../estimation/dynamics_estimation.hpp | 30 ++ .../estimation/tire_model.cpp | 310 ++++++++++++++++++ .../estimation/tire_model.hpp | 264 +++++++++++++++ .../constants.hpp} | 0 .../datatypes.hpp | 0 .../torque_distribution/torque_allocator.cpp | 0 .../torque_distribution/torque_allocator.hpp | 0 .../torque_distribution/torque_path.cpp | 0 .../torque_distribution/torque_path.hpp | 0 .../app/torque_vectoring/torque_vectoring.cpp | 36 ++ 10 files changed, 640 insertions(+) create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp rename firmware/hexray/VC/src/app/torque_vectoring/{datatypes/datatypes_vd_constants.hpp => shared_datatypes/constants.hpp} (100%) rename firmware/hexray/VC/src/app/torque_vectoring/{datatypes => shared_datatypes}/datatypes.hpp (100%) delete mode 100644 firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_allocator.cpp delete mode 100644 firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_allocator.hpp delete mode 100644 firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_path.cpp delete mode 100644 firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_path.hpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp new file mode 100644 index 0000000000..0448906c89 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp @@ -0,0 +1,30 @@ +class vehicleDynamics{ + public: + constexpr float estimateNormalForce_N(); + constexpr float estimateBodySlip(); + constexpr float estimateVehicleSpeed_MPS(); + struct { + float v_x; + float v_y; + float v_z; + } velocity_t; + + velocity_t vehicle_v + private: + constexpr float estimateLoadTransfer_N(); + constexpr float estimateDownForce_N(); + + + /* What does this class need to do? + - estimate load transfer + - estimate down force + - estimate normal force -- needs to be public + - estimate body slip -- needs to be public + - estimate vehicle speed -- needs to be public + */ +}; + +/* Leaving velocity estimate for EKF */ + + + diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp new file mode 100644 index 0000000000..c44b97dd5f --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -0,0 +1,310 @@ +#include "tire_model.hpp" + +#include + +using namespace app::tv::datatypes::vd_constants; + +namespace app::tv::estimation +{ +namespace +{ +[[nodiscard]] float safeMagnitude(const float value) +{ + return std::fmax(std::fabs(value), SMALL_EPSILON); +} +} // namespace + +[[nodiscard]] TireModel::Outputs TireModel::estimate(const StateInputs& inputs) +{ + estimateWheelVelocities(inputs.vehicle_velocity_x_mps, inputs.vehicle_velocity_y_mps, inputs.yaw_rate_radps); + estimateSlipAngle(inputs.steering_angle_rad); + estimateSlipRatio(inputs.wheel_angular_velocity_radps); + computePureLongitudinalForce(inputs.normal_load_N); + computePureLateralForce(inputs.normal_load_N); + return tire_outputs_; +} + +[[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity(const float slip_ratio, const float wheel_vel_x_mps) const +{ + const float wheel_surface_speed_mps = wheel_vel_x_mps * (1.0f + slip_ratio); + return wheel_surface_speed_mps / safeMagnitude(WHEEL_RADIUS_M); +} + +[[nodiscard]] float TireModel::wheelLongOffset_m() const +{ + return wheel_axle_ == WheelAxle::Front ? DIST_FRONT_AXLE_CG_m : -DIST_REAR_AXLE_CG_m; +} + +[[nodiscard]] float TireModel::wheelLatOffset_m() const +{ + return wheel_side_ == WheelSide::Left ? TRACK_WIDTH_m * 0.5f : -TRACK_WIDTH_m * 0.5f; +} + +[[nodiscard]] float TireModel::wheelSteeringAngle_rad(const float steering_angle_rad) const +{ + return wheel_axle_ == WheelAxle::Front ? steering_angle_rad : 0.0f; +} + +void TireModel::estimateWheelVelocities(float vehicle_velocity_x_mps, float vehicle_velocity_y_mps, float yaw_rate_radps) +{ + wheel_vel_x_mps_ = vehicle_velocity_x_mps - (yaw_rate_radps * wheelLatOffset_m()); + wheel_vel_y_mps_ = vehicle_velocity_y_mps + (yaw_rate_radps * wheelLongOffset_m()); +} + +void TireModel::estimateSlipAngle(float steering_angle_rad) +{ + tire_outputs_.slip_angle_rad = std::atan2(wheel_vel_y_mps_, wheel_vel_x_mps_) - wheelSteeringAngle_rad(steering_angle_rad); +} + +void TireModel::estimateSlipRatio(const float wheel_angular_velocity_radps) +{ + const float wheel_surface_speed_mps = wheel_angular_velocity_radps * WHEEL_RADIUS_M; + const float effective_wheel_speed_mps = std::cos(tire_outputs_.slip_angle_rad) * wheel_vel_x_mps_; + + tire_outputs_.slip_ratio = + (wheel_surface_speed_mps - effective_wheel_speed_mps) / safeMagnitude(effective_wheel_speed_mps); +} + +//-------------------------------------------------------------------- Pacejka 5.2 ----------------------------------------------------------------------// + + +void TireModel::pureFxMagicFormulaCoefficients(const float normal_load_N) +{ + // Assumes lambda terms are 1.0, gamma = 0, and pressure effects are captured by the discrete pressure row. + const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); + const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); + const float kappa_x = pureFx_Kappa(normalized_load_delta, tire_outputs_.slip_ratio); + const float c_x = pureFx_C(); + const float d_x = pureFx_D(clamped_normal_load_N, normalized_load_delta); + const float e_x = pureFx_E(normalized_load_delta, kappa_x); + const float k_x = pureFx_K(clamped_normal_load_N, normalized_load_delta); + const float b_x = pureFx_B(k_x, c_x, d_x); + const float s_vx = pureFx_Sv(clamped_normal_load_N, normalized_load_delta); + + pure_fx_mj_coefficients_ = { + .kappa_x = kappa_x, + .c_x = c_x, + .d_x = d_x, + .b_x = b_x, + .e_x = e_x, + .s_vx = s_vx, + }; +} + +constexpr float TireModel::pure_Fx() const +{ + // Pacejka Page 179 (4.E9): F_x0 + const float b_x_kappa_x = pure_fx_mj_coefficients_.b_x * pure_fx_mj_coefficients_.kappa_x; + + return pure_fx_mj_coefficients_.d_x * + std::sin( + pure_fx_mj_coefficients_.c_x * + std::atan( + b_x_kappa_x - + (pure_fx_mj_coefficients_.e_x * + (b_x_kappa_x - std::atan(b_x_kappa_x))))) + + pure_fx_mj_coefficients_.s_vx; +} + +void TireModel::pureFyMagicFormulaCoefficients(const float normal_load_N) +{ + // Assumes gamma = 0, lambda terms are 1.0, and pressure effects are captured by the discrete pressure row. + const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); + const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); + const float alpha_y = pureFy_Alpha(normalized_load_delta, tire_outputs_.slip_angle_rad); + const float c_y = pureFy_C(); + const float d_y = pureFy_D(clamped_normal_load_N, normalized_load_delta); + const float e_y = pureFy_E(normalized_load_delta, alpha_y); + const float k_y = pureFy_K(clamped_normal_load_N); + const float b_y = pureFy_B(k_y, c_y, d_y); + const float s_vy = pureFy_Sv(clamped_normal_load_N, normalized_load_delta); + + pure_fy_mj_coefficients_ = { + .alpha_y = alpha_y, + .c_y = c_y, + .d_y = d_y, + .b_y = b_y, + .e_y = e_y, + .s_vy = s_vy, + }; +} + +constexpr float TireModel::pure_Fy() const +{ + // Pacejka Page 180 (4.E19): F_y0 + const float b_y_alpha_y = pure_fy_mj_coefficients_.b_y * pure_fy_mj_coefficients_.alpha_y; + + return pure_fy_mj_coefficients_.d_y * + std::sin( + pure_fy_mj_coefficients_.c_y * + std::atan( + b_y_alpha_y - + (pure_fy_mj_coefficients_.e_y * + (b_y_alpha_y - std::atan(b_y_alpha_y))))) + + pure_fy_mj_coefficients_.s_vy; +} + +void TireModel::computePureLongitudinalForce(const float normal_load_N) +{ + pureFxMagicFormulaCoefficients(normal_load_N); + tire_outputs_.longitudinal_force_N = pure_Fx(); +} + +void TireModel::computePureLateralForce(const float normal_load_N) +{ + pureFyMagicFormulaCoefficients(normal_load_N); + tire_outputs_.lateral_force_N = pure_Fy(); +} + + +constexpr float TireModel::safeSignedDenominator(const float value) +{ + if (std::fabs(value) >= SMALL_EPSILON) + { + return value; + } + + return value < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; +} + +constexpr float TireModel::signum(const float value) +{ + if (value > 0.0f) + { + return 1.0f; + } + + if (value < 0.0f) + { + return -1.0f; + } + + return 0.0f; +} + +//-------------------------------------------------------------------- MJ Pure Coefficents 5.2 ----------------------------------------------------------------------// + + +constexpr float TireModel::normalizedLoadDelta(const float normal_load_N) +{ + return (normal_load_N - NOMINAL_FZ_N) / safeSignedDenominator(NOMINAL_FZ_N); +} + +constexpr float TireModel::pureFx_Sh(const float normalized_load_delta) const +{ + // Pacejka Page 179 (4.E17): S_Hx + return coeff_fx_.hx_1 + (coeff_fx_.hx_2 * normalized_load_delta); +} + +constexpr float TireModel::pureFx_Kappa(const float normalized_load_delta, const float slip_ratio) const +{ + // Pacejka Page 179 (4.E10): kappa_x = kappa + S_Hx + return slip_ratio + pureFx_Sh(normalized_load_delta); +} + +constexpr float TireModel::pureFx_C() const +{ + // Pacejka Page 179 (4.E11): C_x + return coeff_fx_.cx_1; +} + +constexpr float TireModel::pureFx_mu(const float normalized_load_delta) const +{ + // Pacejka Page 179 (4.E13): mu_x with pressure/camber terms reduced to the current assumptions. + return coeff_fx_.dx_1 + (coeff_fx_.dx_2 * normalized_load_delta); +} + +constexpr float TireModel::pureFx_D(const float normal_load_N, const float normalized_load_delta) const +{ + // Pacejka Page 179 (4.E12): D_x = mu_x * F_z + return pureFx_mu(normalized_load_delta) * normal_load_N; +} + +constexpr float TireModel::pureFx_E(const float normalized_load_delta, const float kappa_x) const +{ + // Pacejka Page 179 (4.E14): E_x + const float normalized_load_delta_squared = normalized_load_delta * normalized_load_delta; + return (coeff_fx_.ex_1 + (coeff_fx_.ex_2 * normalized_load_delta) + + (coeff_fx_.ex_3 * normalized_load_delta_squared)) * + (1.0f - (coeff_fx_.ex_4 * signum(kappa_x))); +} + +constexpr float TireModel::pureFx_K(const float normal_load_N, const float normalized_load_delta) const +{ + // Pacejka Page 179 (4.E15): K_xk + return normal_load_N * (coeff_fx_.kx_1 + (coeff_fx_.kx_2 * normalized_load_delta)) * + std::exp(coeff_fx_.kx_3 * normalized_load_delta); +} + +constexpr float TireModel::pureFx_B( + const float slip_stiffness, const float shape_factor, const float peak_factor) +{ + // Pacejka Page 179 (4.E16): B_x = K_xk / (C_x * D_x + epsilon_x) + return slip_stiffness / safeSignedDenominator(shape_factor * peak_factor); +} + +constexpr float TireModel::pureFx_Sv(const float normal_load_N, const float normalized_load_delta) const +{ + // Pacejka Page 179 (4.E18): S_Vx + return normal_load_N * (coeff_fx_.vx_1 + (coeff_fx_.vx_2 * normalized_load_delta)); +} + +constexpr float TireModel::pureFy_Sh(const float normalized_load_delta) const +{ + // Pacejka Page 180 (4.E27): S_Hy with gamma terms reduced to zero. + return coeff_fy_.hy_1 + (coeff_fy_.hy_2 * normalized_load_delta); +} + +constexpr float TireModel::pureFy_Alpha(const float normalized_load_delta, const float slip_angle_rad) const +{ + // Pacejka Page 180 (4.E20): alpha_y = alpha* + S_Hy + return slip_angle_rad + pureFy_Sh(normalized_load_delta); +} + +constexpr float TireModel::pureFy_C() const +{ + // Pacejka Page 180 (4.E21): C_y + return coeff_fy_.cy_1; +} + +constexpr float TireModel::pureFy_mu(const float normalized_load_delta) const +{ + // Pacejka Page 180 (4.E23): mu_y with pressure/camber terms reduced to the current assumptions. + return coeff_fy_.dy_1 + (coeff_fy_.dy_2 * normalized_load_delta); +} + +constexpr float TireModel::pureFy_D(const float normal_load_N, const float normalized_load_delta) const +{ + // Pacejka Page 180 (4.E22): D_y = mu_y * F_z + return pureFy_mu(normalized_load_delta) * normal_load_N; +} + +constexpr float TireModel::pureFy_E(const float normalized_load_delta, const float alpha_y) const +{ + // Pacejka Page 180 (4.E24): E_y with gamma terms reduced to zero. + return (coeff_fy_.ey_1 + (coeff_fy_.ey_2 * normalized_load_delta)) * + (1.0f - (coeff_fy_.ey_3 * signum(alpha_y))); +} + +constexpr float TireModel::pureFy_K(const float normal_load_N) const +{ + // Pacejka Page 180 (4.E25): K_yalpha with gamma/pressure terms reduced to zero. + const float denominator = safeSignedDenominator(coeff_fy_.ky_2 * NOMINAL_FZ_N); + return coeff_fy_.ky_1 * NOMINAL_FZ_N * + std::sin(2.0f * std::atan(normal_load_N / denominator)); +} + +constexpr float TireModel::pureFy_B( + const float cornering_stiffness, const float shape_factor, const float peak_factor) +{ + // Pacejka Page 180 (4.E26): B_y = K_yalpha / (C_y * D_y + epsilon_y) + return cornering_stiffness / safeSignedDenominator(shape_factor * peak_factor); +} + +constexpr float TireModel::pureFy_Sv(const float normal_load_N, const float normalized_load_delta) const +{ + // Pacejka Page 180 (4.E29): S_Vy with gamma terms reduced to zero. + return normal_load_N * (coeff_fy_.vy_1 + (coeff_fy_.vy_2 * normalized_load_delta)); +} + +} // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp new file mode 100644 index 0000000000..d520c61aaf --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -0,0 +1,264 @@ +#pragma once + +#include + +#include "torque_vectoring/shared_datatypes/constants.hpp" + +namespace app::tv::estimation +{ +class TireModel +{ +public: + enum class WheelSide + { + Left, + Right + }; + + enum class WheelAxle + { + Front, + Rear + }; + + enum class TirePressure + { + PSI_10, + PSI_12 + }; + + struct StateInputs + { + float wheel_angular_velocity_radps = 0.0f; + float vehicle_velocity_x_mps = 0.0f; + float vehicle_velocity_y_mps = 0.0f; + float yaw_rate_radps = 0.0f; + float steering_angle_rad = 0.0f; + float normal_load_N = 0.0f; + }; + + struct Outputs + { + float slip_ratio = 0.0f; + float slip_angle_rad = 0.0f; + float longitudinal_force_N = 0.0f; + float lateral_force_N = 0.0f; + }; + + constexpr TireModel(const TirePressure tire_pressure, const WheelSide wheel_side, const WheelAxle wheel_axle) + : coeff_fx_(selectFxCoefficients(tire_pressure)), + coeff_fy_(selectFyCoefficients(tire_pressure)), + wheel_side_(wheel_side), + wheel_axle_(wheel_axle) + { + } + + [[nodiscard]] Outputs estimate(const StateInputs& inputs); + [[nodiscard]] float slipRatioToWheelAngularVelocity(const float slip_ratio, const float wheel_vel_x_mps) const; + +private: + struct TireCoefficents_Fy + { + float cy_1; + float dy_1; + float dy_2; + float dy_3; + float ey_1; + float ey_2; + float ey_3; + float ey_4; + float ky_1; + float ky_2; + float ky_3; + float hy_1; + float hy_2; + float hy_3; + float vy_1; + float vy_2; + float vy_3; + float vy_4; + }; + + struct TireCoefficents_Fx + { + float cx_1; + float dx_1; + float dx_2; + float dx_3; + float ex_1; + float ex_2; + float ex_3; + float ex_4; + float kx_1; + float kx_2; + float kx_3; + float hx_1; + float hx_2; + float vx_1; + float vx_2; + }; + + struct PureFxMagicFormulaCoefficients + { + float kappa_x = 0.0f; + float c_x = 0.0f; + float d_x = 0.0f; + float b_x = 0.0f; + float e_x = 0.0f; + float s_vx = 0.0f; + }; + + struct PureFyMagicFormulaCoefficients + { + float alpha_y = 0.0f; + float c_y = 0.0f; + float d_y = 0.0f; + float b_y = 0.0f; + float e_y = 0.0f; + float s_vy = 0.0f; + }; + + /* + These are discrete pressure snapshots from the coefficient workbook. If pressure becomes a continuous input later, + replace this with interpolation or a refit against the raw tire data. + */ + static constexpr TireCoefficents_Fx Fx_10_PSI = { + 0.01284039486f, + 262.5114145f, + -41.08996459f, + 8.15087112f, + 1.175111664f, + 0.04701116929f, + -0.009561725595f, + -0.01302948897f, + 62.43954722f, + -0.05063343595f, + -0.3634867697f, + -0.0003767549902f, + 0.0008216827314f, + -0.002877423451f, + -0.01695834766f, + }; + + static constexpr TireCoefficents_Fx Fx_12_PSI = { + 0.008656294312f, + 366.1925838f, + -57.2826791f, + 8.208916842f, + 1.134744942f, + 0.04728092792f, + -0.02191230313f, + -0.01799018414f, + 62.46585932f, + 1.390484252f, + -0.3909753489f, + 0.0005429909716f, + -0.0007019708842f, + 0.01286770906f, + 0.004135955934f, + }; + + static constexpr TireCoefficents_Fy Fy_10_PSI = { + 1.652843597f, + -2.803116154f, + -0.2320399036f, + 17.97429428f, + 1.450330291f, + 0.5012143046f, + -0.02726754434f, + -0.7645474843f, + -52.17053158f, + 1.886718468f, + 1.530410198f, + -0.003839066017f, + -0.003970614639f, + -0.1017943816f, + -0.08574689113f, + -0.02575321928f, + 0.5309737209f, + 1.493197148f, + }; + + static constexpr TireCoefficents_Fy Fy_12_PSI = { + 0.90721342f, + -3.824114982f, + -0.4280518359f, + 15.56383821f, + 1.218066219f, + 0.3097318297f, + -0.02930740966f, + 0.2068388689f, + -54.60640233f, + 2.122730258f, + 1.671281037f, + -0.004171875264f, + -0.00360720284f, + -0.0787609381f, + -0.1057253032f, + -0.02043816084f, + 0.83972231f, + 1.494231303f, + }; + + static constexpr float NOMINAL_FZ_N = 750.0f; + static constexpr float WHEEL_RADIUS_M = (WHEEL_DIAMETER_IN * IN_TO_M) * 0.5f; + + [[nodiscard]] static constexpr TireCoefficents_Fx selectFxCoefficients(const TirePressure tire_pressure) + { + return tire_pressure == TirePressure::PSI_10 ? Fx_10_PSI : Fx_12_PSI; + } + + [[nodiscard]] static constexpr TireCoefficents_Fy selectFyCoefficients(const TirePressure tire_pressure) + { + return tire_pressure == TirePressure::PSI_10 ? Fy_10_PSI : Fy_12_PSI; + } + + [[nodiscard]] static constexpr float safeSignedDenominator(const float value); + [[nodiscard]] static constexpr float signum(const float value); + [[nodiscard]] static constexpr float normalizedLoadDelta(const float normal_load_N); + [[nodiscard]] constexpr float pureFx_Sh(const float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFx_Kappa(const float normalized_load_delta, const float slip_ratio) const; + [[nodiscard]] constexpr float pureFx_C() const; + [[nodiscard]] constexpr float pureFx_mu(const float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFx_D(const float normal_load_N, const float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFx_E(const float normalized_load_delta, const float kappa_x) const; + [[nodiscard]] constexpr float pureFx_K(const float normal_load_N, const float normalized_load_delta) const; + [[nodiscard]] static constexpr float pureFx_B( + const float slip_stiffness, const float shape_factor, const float peak_factor); + [[nodiscard]] constexpr float pureFx_Sv(const float normal_load_N, const float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_Sh(const float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_Alpha(const float normalized_load_delta, const float slip_angle_rad) const; + [[nodiscard]] constexpr float pureFy_C() const; + [[nodiscard]] constexpr float pureFy_mu(const float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_D(const float normal_load_N, const float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_E(const float normalized_load_delta, const float alpha_y) const; + [[nodiscard]] constexpr float pureFy_K(const float normal_load_N) const; + [[nodiscard]] static constexpr float pureFy_B( + const float cornering_stiffness, const float shape_factor, const float peak_factor); + [[nodiscard]] constexpr float pureFy_Sv(const float normal_load_N, const float normalized_load_delta) const; + void pureFxMagicFormulaCoefficients(const float normal_load_N); + [[nodiscard]] constexpr float pure_Fx() const; + void pureFyMagicFormulaCoefficients(const float normal_load_N); + [[nodiscard]] constexpr float pure_Fy() const; + + [[nodiscard]] float wheelLongOffset_m() const; + [[nodiscard]] float wheelLatOffset_m() const; + [[nodiscard]] float wheelSteeringAngle_rad(const float steering_angle_rad) const; + void estimateWheelVelocities(float vehicle_velocity_x_mps, float vehicle_velocity_y_mps, float yaw_rate_radps); + void estimateSlipAngle(float steering_angle_rad); + void estimateSlipRatio(const float wheel_angular_velocity_radps); + void computePureLongitudinalForce(const float normal_load_N); + void computePureLateralForce(const float normal_load_N); + void estimateForces(const float normal_load_N); + + const TireCoefficents_Fx coeff_fx_; + const TireCoefficents_Fy coeff_fy_; + const WheelSide wheel_side_; + const WheelAxle wheel_axle_; + PureFxMagicFormulaCoefficients pure_fx_mj_coefficients_{}; + PureFyMagicFormulaCoefficients pure_fy_mj_coefficients_{}; + float wheel_vel_x_mps_ = 0.0f; + float wheel_vel_y_mps_ = 0.0f; + Outputs tire_outputs_{}; +}; +} // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/datatypes/datatypes_vd_constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp similarity index 100% rename from firmware/hexray/VC/src/app/torque_vectoring/datatypes/datatypes_vd_constants.hpp rename to firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp similarity index 100% rename from firmware/hexray/VC/src/app/torque_vectoring/datatypes/datatypes.hpp rename to firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_allocator.cpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_allocator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_allocator.hpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_path.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_path.cpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_path.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_distribution/torque_path.hpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp new file mode 100644 index 0000000000..9260aea7db --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -0,0 +1,36 @@ +#include "torque_vectoring/estimation/tire_model.hpp" + +namespace app::tv +{ +namespace +{ +estimation::TireModel front_left_tire_model{ + estimation::TireModel::TirePressure::PSI_12, + estimation::TireModel::WheelSide::Left, + estimation::TireModel::WheelAxle::Front}; + +estimation::TireModel front_right_tire_model{ + estimation::TireModel::TirePressure::PSI_12, + estimation::TireModel::WheelSide::Right, + estimation::TireModel::WheelAxle::Front}; + +estimation::TireModel rear_left_tire_model{ + estimation::TireModel::TirePressure::PSI_12, + estimation::TireModel::WheelSide::Left, + estimation::TireModel::WheelAxle::Rear}; + +estimation::TireModel rear_right_tire_model{ + estimation::TireModel::TirePressure::PSI_12, + estimation::TireModel::WheelSide::Right, + estimation::TireModel::WheelAxle::Rear}; +} // namespace + +// optimzier +/* +// yaw rate control and pedal request + + + +*/ + +} // namespace app::tv From 26f9b135b8e06a71f6938c60877be6eb3638652d Mon Sep 17 00:00:00 2001 From: shayana18 Date: Fri, 27 Mar 2026 16:23:13 -0700 Subject: [PATCH 02/35] algo in progress changes --- .../estimation/dynamics_estimation.cpp | 78 +++++++++++++++++++ .../estimation/dynamics_estimation.hpp | 62 +++++++++------ .../shared_datatypes/constants.hpp | 1 + .../shared_datatypes/datatypes.hpp | 0 .../app/torque_vectoring/torque_vectoring.cpp | 3 + 5 files changed, 119 insertions(+), 25 deletions(-) create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp delete mode 100644 firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp new file mode 100644 index 0000000000..1e86b7a88f --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp @@ -0,0 +1,78 @@ +#include "dynamics_estimation.hpp" + +#include + +using namespace app::tv::datatypes::vd_constants; + +namespace app::tv::estimation +{ +namespace +{ +[[nodiscard]] float safeLongitudinalVelocity(const float v_x_mps) +{ + if (std::fabs(v_x_mps) >= SMALL_EPSILON) + { + return v_x_mps; + } + + return v_x_mps < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; +} +} // namespace + +float vehicleDynamics::estimateBodySlip(float v_x_mps, float v_y_mps) const +{ + return std::atan2(v_y_mps, safeLongitudinalVelocity(v_x_mps)); +} + +vehicleDynamics::normal_force_t vehicleDynamics::estimateNormalForce_N( + const float a_x_MPS2, const float a_y_MPS2, const float v_x_mps) const +{ + const float long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(a_x_MPS2); + const float lat_load_tf = LAT_ACCEL_TERM_VERTICAL_FORCE(a_y_MPS2); + const float down_force_n = estimateDownForce_N(v_x_mps); + const float front_cop = dynamicCopFront(a_x_MPS2); + const float right_cop = dynamicCopRight(a_y_MPS2); + + const float front_down_force_n = down_force_n * front_cop; + const float rear_down_force_n = down_force_n * (1.0f - front_cop); + + return { + .fz_fl_N = std::fmax( + 0.0f, + STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) - lat_load_tf + + (front_down_force_n * (1.0f - right_cop))), + .fz_fr_N = std::fmax( + 0.0f, + STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) + lat_load_tf + (front_down_force_n * right_cop)), + .fz_rl_N = std::fmax( + 0.0f, + STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) - lat_load_tf + + (rear_down_force_n * (1.0f - right_cop))), + .fz_rr_N = std::fmax( + 0.0f, + STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) + lat_load_tf + (rear_down_force_n * right_cop)), + }; +} + +float vehicleDynamics::dynamicCopFront(const float long_accel_mps2) +{ + (void)long_accel_mps2; + return 1.0f - COP_REAR; +} + +float vehicleDynamics::dynamicCopRight(const float lat_accel_mps2) +{ + (void)lat_accel_mps2; + return COP_RIGHT; +} + +float vehicleDynamics::estimateDrag_N(const float v_x_mps) const +{ + return 0.5f * AIR_DENSITY_KGPM3 * FRONTAL_AREA_M2 * DRAG_COEFF * v_x_mps * v_x_mps; +} + +float vehicleDynamics::estimateDownForce_N(const float v_x_mps) const +{ + return 0.5f * AIR_DENSITY_KGPM3 * FRONTAL_AREA_M2 * LIFT_COEFF * v_x_mps * v_x_mps; +} +} // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp index 0448906c89..435dbcb6e4 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp @@ -1,30 +1,42 @@ -class vehicleDynamics{ - public: - constexpr float estimateNormalForce_N(); - constexpr float estimateBodySlip(); - constexpr float estimateVehicleSpeed_MPS(); - struct { - float v_x; - float v_y; - float v_z; - } velocity_t; - - velocity_t vehicle_v - private: - constexpr float estimateLoadTransfer_N(); - constexpr float estimateDownForce_N(); +#pragma once +#include "torque_vectoring/shared_datatypes/constants.hpp" - /* What does this class need to do? - - estimate load transfer - - estimate down force - - estimate normal force -- needs to be public - - estimate body slip -- needs to be public - - estimate vehicle speed -- needs to be public - */ -}; +namespace app::tv::estimation +{ +class vehicleDynamics +{ +public: + struct normal_force_t + { + float fz_fl_N = 0.0f; + float fz_fr_N = 0.0f; + float fz_rl_N = 0.0f; + float fz_rr_N = 0.0f; + }; -/* Leaving velocity estimate for EKF */ + [[nodiscard]] normal_force_t estimateNormalForce_N(float a_x_MPS2, float a_y_MPS2, float v_x_mps) const; + [[nodiscard]] float estimateBodySlip(float v_x_mps, float v_y_mps) const; + [[nodiscard]] float estimateDrag_N(float v_x_mps) const; - +private: + static constexpr float FRONTAL_AREA_M2 = 0.94f; // m^2 from aero team + static constexpr float AIR_DENSITY_KGPM3 = 1.2205f; // kg/m^3 + static constexpr float LIFT_COEFF = 1.7f; // from aero team + static constexpr float DRAG_COEFF = 0.92f; + static constexpr float COP_REAR = 0.68f; // fraction of aero load acting behind the CG + static constexpr float COP_RIGHT = 0.5f; // fraction of aero load acting on the right side + static constexpr float STATIC_FRONT_AXLE_LOAD_N = datatypes::vd_constants::CAR_WEIGHT * + (datatypes::vd_constants::DIST_REAR_AXLE_CG_m / + datatypes::vd_constants::WHEELBASE_m); + static constexpr float STATIC_REAR_AXLE_LOAD_N = datatypes::vd_constants::CAR_WEIGHT * + (datatypes::vd_constants::DIST_FRONT_AXLE_CG_m / + datatypes::vd_constants::WHEELBASE_m); + static constexpr float STATIC_FRONT_WHEEL_LOAD_N = 0.5f * STATIC_FRONT_AXLE_LOAD_N; + static constexpr float STATIC_REAR_WHEEL_LOAD_N = 0.5f * STATIC_REAR_AXLE_LOAD_N; + [[nodiscard]] static float dynamicCopFront(float long_accel_mps2); + [[nodiscard]] static float dynamicCopRight(float lat_accel_mps2); + [[nodiscard]] float estimateDownForce_N(float v_x_mps) const; +}; +} // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index 507b85baf2..948649005c 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -34,6 +34,7 @@ inline constexpr float DIST_REAR_AXLE_CG_m = inline constexpr float DIST_HEIGHT_CG_m = 30.0f * CM_TO_M; // CG height (from suspension team) // Derived weight distribution properties +inline constexpr float CAR_WEIGHT = CAR_MASS_AT_CG_KG * GRAVITY; inline constexpr float WEIGHT_ACROSS_BODY = CAR_MASS_AT_CG_KG * GRAVITY / WHEELBASE_m; inline constexpr float REAR_WEIGHT_DISTRIBUTION = WEIGHT_ACROSS_BODY * DIST_REAR_AXLE_CG_m; inline constexpr float FRONT_WEIGHT_DISTRIBUTION = WEIGHT_ACROSS_BODY * DIST_FRONT_AXLE_CG_m; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index 9260aea7db..cbbd929b30 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -1,9 +1,12 @@ +#include "torque_vectoring/estimation/dynamics_estimation.hpp" #include "torque_vectoring/estimation/tire_model.hpp" namespace app::tv { namespace { +[[maybe_unused]] estimation::vehicleDynamics vehicle_dynamics_estimator{}; + estimation::TireModel front_left_tire_model{ estimation::TireModel::TirePressure::PSI_12, estimation::TireModel::WheelSide::Left, From 7cd32b8e8a2bac615acb085326fd6e364529347b Mon Sep 17 00:00:00 2001 From: shayana18 Date: Sun, 5 Apr 2026 13:06:45 -0700 Subject: [PATCH 03/35] work in progress, most peices are there --- firmware/hexray/VC/CMakeLists.txt | 4 +- .../torque_allocator/torque_allocator.cpp | 148 +++++++++ .../torque_allocator/torque_allocator.hpp | 59 ++++ .../yaw_rate_control/controllers_dyrc.cpp | 9 +- .../yaw_rate_control/controllers_dyrc.hpp | 7 +- .../estimation/dynamics_estimation.cpp | 37 ++- .../estimation/dynamics_estimation.hpp | 102 ++++++- .../estimation/tire_model.cpp | 63 ++-- .../estimation/tire_model.hpp | 15 +- .../estimation/vehicle_state_estimator.cpp | 288 ++++++++++++++++++ .../estimation/vehicle_state_estimator.hpp | 45 +++ .../shared_datatypes/constants.hpp | 78 +---- .../shared_datatypes/datatypes.hpp | 22 ++ .../app/torque_vectoring/torque_vectoring.cpp | 135 +++++++- 14 files changed, 876 insertions(+), 136 deletions(-) create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp diff --git a/firmware/hexray/VC/CMakeLists.txt b/firmware/hexray/VC/CMakeLists.txt index e56fd58401..3b94931d3b 100644 --- a/firmware/hexray/VC/CMakeLists.txt +++ b/firmware/hexray/VC/CMakeLists.txt @@ -128,7 +128,7 @@ if ("${TARGET}" STREQUAL "binary") "${CMAKE_CURRENT_BINARY_DIR}/app" ) - target_link_libraries("hexray_VC_app.elf" PRIVATE "hexray_VC_stm32" "hexray_VC_commit_info" "hexray_VC_jsoncan" "m" "sbg_ecom_${ARM_CORE}") + target_link_libraries("hexray_VC_app.elf" PRIVATE "hexray_VC_stm32" "hexray_VC_commit_info" "hexray_VC_jsoncan" "m" "autodiff_interface" "sbg_ecom_${ARM_CORE}") target_compile_definitions("hexray_VC_app.elf" PRIVATE VC) add_chimera_stm32h7("hexray_VC_chimera" "${CHIMERA_SRCS}" "${CHIMERA_INCLUDE_DIRS}" "hexray_chimera_v2_proto_cm7") @@ -163,6 +163,6 @@ elseif ("${TARGET}" STREQUAL "test") "${CMAKE_CURRENT_BINARY_DIR}/app" ) - target_link_libraries("hexray_VC_test" PRIVATE "hexray_VC_commit_info" "hexray_VC_jsoncan") + target_link_libraries("hexray_VC_test" PRIVATE "hexray_VC_commit_info" "hexray_VC_jsoncan" "autodiff_interface") target_compile_definitions("hexray_VC_test" PRIVATE STM32H733xx) endif () \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp new file mode 100644 index 0000000000..99a36ae2c6 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp @@ -0,0 +1,148 @@ +#include "torque_allocator.hpp" + +#include +#include + +using namespace app::tv::datatypes::vd_constants; +using WheelSide = app::tv::estimation::TireModel::WheelSide; +using WheelAxle = app::tv::estimation::TireModel::WheelAxle; + +namespace app::tv::controllers::allocator +{ +namespace +{ + +// ---- Optimizer tuning (module-private) ---- + +static constexpr float W_FX = 2.0f / 3.0f; // Weight on per-wheel force tracking error +static constexpr float W_MZ = 1.0f / 3.0f; // Weight on yaw moment tracking error +static constexpr int MAX_ITER = 8; // Gauss-Newton iterations per control cycle +static constexpr float SLIP_CLAMP = 0.3f; // Physical slip ratio bounds +static constexpr float MIN_HESSIAN = 1e-6f; // Prevents division by zero in Newton step + +// Wheel array index convention +static constexpr int FL = 0; +static constexpr int FR = 1; +static constexpr int RL = 2; +static constexpr int RR = 3; + +// Mz sign per wheel: right-side forces create positive yaw moment +static constexpr float MZ_SIGNS[4] = { -1.0f, 1.0f, -1.0f, 1.0f }; + +/** + * Build TireModel::StateInputs from shared vehicle state. + * The vehicle state and normal load are fixed during optimization; + * only the angular velocity changes across iterations (via trial slip ratio). + */ +[[nodiscard]] estimation::TireModel::StateInputs buildStateInputs( + const datatypes::datatypes::VehicleState& vs, + const float normal_load_N, + const float wheel_angular_velocity_radps) +{ + return { + .wheel_angular_velocity_radps = wheel_angular_velocity_radps, + .vehicle_velocity_x_mps = vs.v_x_mps, + .vehicle_velocity_y_mps = vs.v_y_mps, + .yaw_rate_radps = vs.yaw_rate_radps, + .steering_angle_rad = vs.steer_ang_rad, + .normal_load_N = normal_load_N, + }; +} + +} // namespace + +// ---- Construction ---- + +TorqueAllocator::TorqueAllocator(estimation::TireModel::TirePressure pressure) + : fl_tire_(pressure, WheelSide::Left, WheelAxle::Front), + fr_tire_(pressure, WheelSide::Right, WheelAxle::Front), + rl_tire_(pressure, WheelSide::Left, WheelAxle::Rear), + rr_tire_(pressure, WheelSide::Right, WheelAxle::Rear) +{ +} + +// ---- Public interface ---- + +float TorqueAllocator::estimateYawMoment(const datatypes::datatypes::wheel_set& f_x) +{ + // Mz = (t/2) * (Fx_right_total - Fx_left_total) + return (TRACK_WIDTH_m * 0.5f) * ((f_x.fr + f_x.rr) - (f_x.fl + f_x.rl)); +} + +datatypes::datatypes::wheel_set TorqueAllocator::optimize( + const datatypes::datatypes::wheel_set& des_f_x, + const datatypes::datatypes::wheel_set& normal_forces_N, + const datatypes::datatypes::wheel_set& current_slip_ratios, + const datatypes::datatypes::VehicleState& vehicle_state, + float acc_M_z, + const float des_M_z) +{ + // Working copy of slip ratios — warm-started from current estimates + float kappa[4] = { + current_slip_ratios.fl, + current_slip_ratios.fr, + current_slip_ratios.rl, + current_slip_ratios.rr, + }; + + estimation::TireModel* tires[4] = { &fl_tire_, &fr_tire_, &rl_tire_, &rr_tire_ }; + + const float fz[4] = { normal_forces_N.fl, normal_forces_N.fr, + normal_forces_N.rl, normal_forces_N.rr }; + const float des_fx[4] = { des_f_x.fl, des_f_x.fr, des_f_x.rl, des_f_x.rr }; + + const float half_track = TRACK_WIDTH_m * 0.5f; + + for (int iter = 0; iter < MAX_ITER; ++iter) + { + // ---- Evaluate tire model at current trial slip ratios ---- + float acc_fx[4] = {}; + float dFdK[4] = {}; + + for (int i = 0; i < 4; ++i) + { + // TireModel owns the wheel-center kinematics, so the allocator only provides trial kappa + vehicle state. + const float omega = tires[i]->slipRatioToWheelAngularVelocity(kappa[i], vehicle_state); + const auto inputs = buildStateInputs(vehicle_state, fz[i], omega); + const auto outputs = tires[i]->estimate(inputs); + + acc_fx[i] = outputs.longitudinal_force_N; + dFdK[i] = outputs.dFx_dKappa; + } + + // ---- Yaw moment error ---- + const datatypes::datatypes::wheel_set acc_fx_set = { + .fl = acc_fx[FL], .fr = acc_fx[FR], .rl = acc_fx[RL], .rr = acc_fx[RR] }; + const float e_Mz = acc_M_z - des_M_z; + + // ---- Gauss-Newton step per wheel ---- + // + // Cost: J = W_FX * sum_i (Fx_i - des_fx_i)^2 + W_MZ * (Mz - des_Mz)^2 + // + // Gradient: dJ/dkappa_i = W_FX * e_fx_i * dFx/dkappa + // + W_MZ * e_Mz * dMz/dkappa_i + // + // Hessian (diagonal, Gauss-Newton approximation): + // H_ii = W_FX * (dFx/dkappa)^2 + W_MZ * (dMz/dkappa)^2 + // + for (int i = 0; i < 4; ++i) + { + const float e_fx = acc_fx[i] - des_fx[i]; + const float dMz_dK = MZ_SIGNS[i] * half_track * dFdK[i]; + + const float grad = (W_FX * e_fx * dFdK[i]) + (W_MZ * e_Mz * dMz_dK); + const float H_ii = (W_FX * dFdK[i] * dFdK[i]) + (W_MZ * dMz_dK * dMz_dK); + + kappa[i] -= grad / std::fmax(H_ii, MIN_HESSIAN); + kappa[i] = std::clamp(kappa[i], -SLIP_CLAMP, SLIP_CLAMP); + } + + // Use the provided yaw-moment estimate to seed the first iteration, then + // keep the internal solve self-consistent with the tire-model force updates. + acc_M_z = estimateYawMoment(acc_fx_set); + } + + return { .fl = kappa[FL], .fr = kappa[FR], .rl = kappa[RL], .rr = kappa[RR] }; +} + +} // namespace app::tv::controllers::allocator diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp new file mode 100644 index 0000000000..44d9fcd062 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include "torque_vectoring/shared_datatypes/constants.hpp" +#include "torque_vectoring/shared_datatypes/datatypes.hpp" +#include "torque_vectoring/estimation/tire_model.hpp" + +namespace app::tv::controllers::allocator +{ + +/** + * Torque allocator using Gauss-Newton optimization. + * + * Minimizes a weighted cost function over per-wheel slip ratios: + * J(kappa) = W_FX * sum(Fx_i(kappa_i) - des_fx_i)^2 + * + W_MZ * (Mz(kappa) - des_Mz)^2 + * + * Outputs per-wheel optimal slip ratios. Owns internal tire model + * instances separate from the estimation models to avoid state contamination. + */ +class TorqueAllocator +{ +public: + explicit TorqueAllocator( + estimation::TireModel::TirePressure pressure = estimation::TireModel::TirePressure::PSI_12); + + /** + * Run Gauss-Newton iterations to find optimal slip ratios. + * + * @param des_f_x Target per-wheel longitudinal force (pedal request + yaw moment) + * @param normal_forces_N Per-wheel vertical load from dynamics estimator + * @param current_slip_ratios Warm start from current cycle estimation + * @param vehicle_state Vehicle-level state (fixed during optimization) + * @param acc_M_z Current yaw moment estimate supplied by the caller (Nm) + * @param des_M_z Target yaw moment from high-level controller (Nm) + * @return Per-wheel optimal slip ratios + */ + [[nodiscard]] datatypes::datatypes::wheel_set optimize( + const datatypes::datatypes::wheel_set& des_f_x, + const datatypes::datatypes::wheel_set& normal_forces_N, + const datatypes::datatypes::wheel_set& current_slip_ratios, + const datatypes::datatypes::VehicleState& vehicle_state, + float acc_M_z, + float des_M_z); + + /** + * Estimate yaw moment from longitudinal wheel forces. + * Mz = (track_width / 2) * ((Fx_fr + Fx_rr) - (Fx_fl + Fx_rl)) + * Temporary fallback for callers until a dedicated yaw-moment estimator is wired in. + */ + [[nodiscard]] static float estimateYawMoment(const datatypes::datatypes::wheel_set& f_x); + +private: + estimation::TireModel fl_tire_; + estimation::TireModel fr_tire_; + estimation::TireModel rl_tire_; + estimation::TireModel rr_tire_; +}; + +} // namespace app::tv::controllers::allocator diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp index 993dc7c7a2..4edf03c063 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp @@ -1,6 +1,6 @@ #include "controllers_dyrc.hpp" #include "torque_vectoring/controllers/controllers_config.hpp" -#include "torque_vectoring/datatypes/datatypes_vd_constants.hpp" +#include "torque_vectoring/shared_datatypes/constants.hpp" using namespace app::tv::datatypes::vd_constants; @@ -17,14 +17,13 @@ static PID pid(PID_DYRC_config); static float yaw_moment_Nm = 0.0f; static float r_ref_rad = 0.0f; -[[nodiscard]] inline float computeRefYawRate(const float steer_ang_rad, const float body_velx_mps) +[[nodiscard]] float computeRefYawRate(const float steer_ang_rad, const float body_velx_mps) { r_ref_rad = (body_velx_mps * steer_ang_rad) / (WHEELBASE_m * (1.0f + ku * body_velx_mps * body_velx_mps)); return r_ref_rad; } -[[nodiscard]] inline float - computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps) +[[nodiscard]] float computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps) { yaw_moment_Nm = pid.compute(computeRefYawRate(steer_ang_rad, body_velx_mps), r_actual_rad, 0.0f); return yaw_moment_Nm; @@ -39,4 +38,4 @@ static float r_ref_rad = 0.0f; { return r_ref_rad; } -} // namespace app::tv::controllers::dyrc \ No newline at end of file +} // namespace app::tv::controllers::dyrc diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp index 1f2ee5f793..431c63ceab 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp @@ -21,7 +21,7 @@ namespace app::tv::controllers::dyrc * * @return The reference yaw rate to target in rad/s */ -[[nodiscard]] inline float computeRefYawRate(const float steer_ang_rad, const float body_velx_mps); +[[nodiscard]] float computeRefYawRate(const float steer_ang_rad, const float body_velx_mps); /** * @brief Computes the corrective yaw moment to apply on the vehicle to target a reference yaw rate @@ -34,8 +34,7 @@ namespace app::tv::controllers::dyrc * * @return The corrective yaw moment in Nm to apply on the vehicle */ -[[nodiscard]] inline float - computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps); +[[nodiscard]] float computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps); // The functions below are getters for CAN debugging @@ -52,4 +51,4 @@ namespace app::tv::controllers::dyrc * @return The desired yaw rate to achieve in radians per second */ [[nodiscard]] float getRefYawRate(); -} // namespace app::tv::controllers::dyrc \ No newline at end of file +} // namespace app::tv::controllers::dyrc diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp index 1e86b7a88f..f74c340ee6 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp @@ -24,7 +24,34 @@ float vehicleDynamics::estimateBodySlip(float v_x_mps, float v_y_mps) const return std::atan2(v_y_mps, safeLongitudinalVelocity(v_x_mps)); } -vehicleDynamics::normal_force_t vehicleDynamics::estimateNormalForce_N( +float vehicleDynamics::estimateYawMoment_Nm( + const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N, + const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N, + const float steering_angle_rad) const +{ + const float cos_delta = std::cos(steering_angle_rad); + const float sin_delta = std::sin(steering_angle_rad); + const float half_track_m = TRACK_WIDTH_m * 0.5f; + + const float fl_fx_body = (cos_delta * longitudinal_forces_N.fl) - (sin_delta * lateral_forces_N.fl); + const float fl_fy_body = (sin_delta * longitudinal_forces_N.fl) + (cos_delta * lateral_forces_N.fl); + const float fr_fx_body = (cos_delta * longitudinal_forces_N.fr) - (sin_delta * lateral_forces_N.fr); + const float fr_fy_body = (sin_delta * longitudinal_forces_N.fr) + (cos_delta * lateral_forces_N.fr); + + const float rl_fx_body = longitudinal_forces_N.rl; + const float rl_fy_body = lateral_forces_N.rl; + const float rr_fx_body = longitudinal_forces_N.rr; + const float rr_fy_body = lateral_forces_N.rr; + + const float fl_moment = (DIST_FRONT_AXLE_CG_m * fl_fy_body) - (half_track_m * fl_fx_body); + const float fr_moment = (DIST_FRONT_AXLE_CG_m * fr_fy_body) + (half_track_m * fr_fx_body); + const float rl_moment = (-DIST_REAR_AXLE_CG_m * rl_fy_body) - (half_track_m * rl_fx_body); + const float rr_moment = (-DIST_REAR_AXLE_CG_m * rr_fy_body) + (half_track_m * rr_fx_body); + + return fl_moment + fr_moment + rl_moment + rr_moment; +} + +app::tv::datatypes::datatypes::wheel_set vehicleDynamics::estimateNormalForce_N( const float a_x_MPS2, const float a_y_MPS2, const float v_x_mps) const { const float long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(a_x_MPS2); @@ -37,18 +64,18 @@ vehicleDynamics::normal_force_t vehicleDynamics::estimateNormalForce_N( const float rear_down_force_n = down_force_n * (1.0f - front_cop); return { - .fz_fl_N = std::fmax( + .fl = std::fmax( 0.0f, STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) - lat_load_tf + (front_down_force_n * (1.0f - right_cop))), - .fz_fr_N = std::fmax( + .fr = std::fmax( 0.0f, STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) + lat_load_tf + (front_down_force_n * right_cop)), - .fz_rl_N = std::fmax( + .rl = std::fmax( 0.0f, STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) - lat_load_tf + (rear_down_force_n * (1.0f - right_cop))), - .fz_rr_N = std::fmax( + .rr = std::fmax( 0.0f, STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) + lat_load_tf + (rear_down_force_n * right_cop)), }; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp index 435dbcb6e4..5d065b851b 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp @@ -1,31 +1,104 @@ #pragma once #include "torque_vectoring/shared_datatypes/constants.hpp" +#include "torque_vectoring/shared_datatypes/datatypes.hpp" namespace app::tv::estimation { +using namespace app::tv::datatypes::vd_constants; class vehicleDynamics { public: - struct normal_force_t - { - float fz_fl_N = 0.0f; - float fz_fr_N = 0.0f; - float fz_rl_N = 0.0f; - float fz_rr_N = 0.0f; - }; - [[nodiscard]] normal_force_t estimateNormalForce_N(float a_x_MPS2, float a_y_MPS2, float v_x_mps) const; + [[nodiscard]] app::tv::datatypes::datatypes::wheel_set estimateNormalForce_N(float a_x_MPS2, float a_y_MPS2, float v_x_mps) const; [[nodiscard]] float estimateBodySlip(float v_x_mps, float v_y_mps) const; + [[nodiscard]] float estimateYawMoment_Nm( + const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N, + const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N, + float steering_angle_rad) const; [[nodiscard]] float estimateDrag_N(float v_x_mps) const; + [[nodiscard]] float estimateDownForce_N(float v_x_mps) const; private: - static constexpr float FRONTAL_AREA_M2 = 0.94f; // m^2 from aero team - static constexpr float AIR_DENSITY_KGPM3 = 1.2205f; // kg/m^3 - static constexpr float LIFT_COEFF = 1.7f; // from aero team - static constexpr float DRAG_COEFF = 0.92f; - static constexpr float COP_REAR = 0.68f; // fraction of aero load acting behind the CG - static constexpr float COP_RIGHT = 0.5f; // fraction of aero load acting on the right side + + // ============================================================================= + // VEHICLE DYNAMICS - VERTICAL LOAD TRANSFER + // Reference: https://www.zotero.org/groups/5809911/vehicle_controls_2024/items/N4TQBR67/reader + // ============================================================================= + + /** + * Longitudinal load transfer component (page 21) + * Positive long_accel transfers load to rear axle + * + * @param long_accel Longitudinal acceleration (m/s^2) + * @return Load transfer force (N) + */ + [[nodiscard]] static inline constexpr float LONG_ACCEL_TERM_VERTICAL_FORCE(const float long_accel) + { + return (CAR_MASS_AT_CG_KG * long_accel * DIST_HEIGHT_CG_m) / WHEELBASE_m; + } + + /** + * Lateral load transfer component (page 21) + * Transfers load to outside wheels during cornering + * + * @param lat_accel Lateral acceleration (m/s^2) + * @return Load transfer force per side (N) + */ + [[nodiscard]] static inline constexpr float LAT_ACCEL_TERM_VERTICAL_FORCE(const float lat_accel) + { + return (CAR_MASS_AT_CG_KG * lat_accel * DIST_HEIGHT_CG_m) / (2.0f * TRACK_WIDTH_m); + } + + [[nodiscard]] static inline constexpr float REAR_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) + { + return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) + + LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); + } + + [[nodiscard]] static inline constexpr float REAR_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) + { + return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) - + LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); + } + + // TODO: Check if front wheels use rear weight or front weight distribution + [[nodiscard]] static inline constexpr float FRONT_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) + { + return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) + + LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); + } + + [[nodiscard]] static inline constexpr float FRONT_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) + { + return REAR_WEIGHT_DISTRIBUTION - LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) - + LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); + } + + /** + * Yaw moment distribution factor Kmz (page 57) + * Accounts for load transfer effect on yaw moment generation capacity + * + * @param long_accel Longitudinal acceleration (m/s^2) + * @return Effective moment arm (m) + */ + [[nodiscard]] static inline constexpr float ACCELERATION_TERM_KMZ(const float long_accel) + { + return DIST_FRONT_AXLE_CG_m + (long_accel * DIST_HEIGHT_CG_m) / GRAVITY; + } + + [[nodiscard]] static inline constexpr float KMZ(const float long_accel) + { + float LONG_ACCEL_TERM = ACCELERATION_TERM_KMZ(long_accel); + return ((CAR_WEIGHT - (CAR_WEIGHT / WHEELBASE_m) * LONG_ACCEL_TERM)) / ((CAR_WEIGHT / WHEELBASE_m) * LONG_ACCEL_TERM); + } + + /** + * Moment scaling factor F (page 58) + * Relates torque differential to yaw moment through track width and effective radius + */ + static constexpr float F = (TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * IN_TO_M)) * GEAR_RATIO; + static constexpr float STATIC_FRONT_AXLE_LOAD_N = datatypes::vd_constants::CAR_WEIGHT * (datatypes::vd_constants::DIST_REAR_AXLE_CG_m / datatypes::vd_constants::WHEELBASE_m); @@ -37,6 +110,5 @@ class vehicleDynamics [[nodiscard]] static float dynamicCopFront(float long_accel_mps2); [[nodiscard]] static float dynamicCopRight(float lat_accel_mps2); - [[nodiscard]] float estimateDownForce_N(float v_x_mps) const; }; } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index c44b97dd5f..434da997d6 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -1,6 +1,7 @@ #include "tire_model.hpp" #include +#include using namespace app::tv::datatypes::vd_constants; @@ -16,7 +17,10 @@ namespace [[nodiscard]] TireModel::Outputs TireModel::estimate(const StateInputs& inputs) { - estimateWheelVelocities(inputs.vehicle_velocity_x_mps, inputs.vehicle_velocity_y_mps, inputs.yaw_rate_radps); + const auto wheel_velocities = + wheelVelocities(inputs.vehicle_velocity_x_mps, inputs.vehicle_velocity_y_mps, inputs.yaw_rate_radps); + wheel_vel_x_mps_ = wheel_velocities.x_mps; + wheel_vel_y_mps_ = wheel_velocities.y_mps; estimateSlipAngle(inputs.steering_angle_rad); estimateSlipRatio(inputs.wheel_angular_velocity_radps); computePureLongitudinalForce(inputs.normal_load_N); @@ -30,6 +34,14 @@ namespace return wheel_surface_speed_mps / safeMagnitude(WHEEL_RADIUS_M); } +[[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity( + const float slip_ratio, const datatypes::datatypes::VehicleState& vehicle_state) const +{ + const auto wheel_velocities = + wheelVelocities(vehicle_state.v_x_mps, vehicle_state.v_y_mps, vehicle_state.yaw_rate_radps); + return slipRatioToWheelAngularVelocity(slip_ratio, wheel_velocities.x_mps); +} + [[nodiscard]] float TireModel::wheelLongOffset_m() const { return wheel_axle_ == WheelAxle::Front ? DIST_FRONT_AXLE_CG_m : -DIST_REAR_AXLE_CG_m; @@ -45,10 +57,15 @@ namespace return wheel_axle_ == WheelAxle::Front ? steering_angle_rad : 0.0f; } -void TireModel::estimateWheelVelocities(float vehicle_velocity_x_mps, float vehicle_velocity_y_mps, float yaw_rate_radps) +[[nodiscard]] TireModel::WheelVelocities TireModel::wheelVelocities( + const float vehicle_velocity_x_mps, const float vehicle_velocity_y_mps, const float yaw_rate_radps) const { - wheel_vel_x_mps_ = vehicle_velocity_x_mps - (yaw_rate_radps * wheelLatOffset_m()); - wheel_vel_y_mps_ = vehicle_velocity_y_mps + (yaw_rate_radps * wheelLongOffset_m()); + // Rigid-body planar kinematics in the body frame: + // v_wheel = v_cg + omega_z x r_wheel, where r_wheel = [x_offset, y_offset, 0]. + return { + .x_mps = vehicle_velocity_x_mps - (yaw_rate_radps * wheelLatOffset_m()), + .y_mps = vehicle_velocity_y_mps + (yaw_rate_radps * wheelLongOffset_m()), + }; } void TireModel::estimateSlipAngle(float steering_angle_rad) @@ -58,8 +75,9 @@ void TireModel::estimateSlipAngle(float steering_angle_rad) void TireModel::estimateSlipRatio(const float wheel_angular_velocity_radps) { - const float wheel_surface_speed_mps = wheel_angular_velocity_radps * WHEEL_RADIUS_M; - const float effective_wheel_speed_mps = std::cos(tire_outputs_.slip_angle_rad) * wheel_vel_x_mps_; + const float wheel_surface_speed_mps = wheel_angular_velocity_radps * WHEEL_RADIUS_M; + const float wheel_speed_magnitude_mps = std::hypot(wheel_vel_x_mps_, wheel_vel_y_mps_); + const float effective_wheel_speed_mps = wheel_speed_magnitude_mps * std::cos(tire_outputs_.slip_angle_rad); tire_outputs_.slip_ratio = (wheel_surface_speed_mps - effective_wheel_speed_mps) / safeMagnitude(effective_wheel_speed_mps); @@ -91,21 +109,6 @@ void TireModel::pureFxMagicFormulaCoefficients(const float normal_load_N) }; } -constexpr float TireModel::pure_Fx() const -{ - // Pacejka Page 179 (4.E9): F_x0 - const float b_x_kappa_x = pure_fx_mj_coefficients_.b_x * pure_fx_mj_coefficients_.kappa_x; - - return pure_fx_mj_coefficients_.d_x * - std::sin( - pure_fx_mj_coefficients_.c_x * - std::atan( - b_x_kappa_x - - (pure_fx_mj_coefficients_.e_x * - (b_x_kappa_x - std::atan(b_x_kappa_x))))) + - pure_fx_mj_coefficients_.s_vx; -} - void TireModel::pureFyMagicFormulaCoefficients(const float normal_load_N) { // Assumes gamma = 0, lambda terms are 1.0, and pressure effects are captured by the discrete pressure row. @@ -147,7 +150,23 @@ constexpr float TireModel::pure_Fy() const void TireModel::computePureLongitudinalForce(const float normal_load_N) { pureFxMagicFormulaCoefficients(normal_load_N); - tire_outputs_.longitudinal_force_N = pure_Fx(); + + // Use forward-mode autodiff to compute Fx and ∂Fx/∂κ simultaneously. + // All Pacejka coefficients are fixed at this operating point; kappa_x is the only variable. + const auto& c = pure_fx_mj_coefficients_; + autodiff::dual kappa_x = c.kappa_x; + + auto fx_formula = [&](autodiff::dual k) -> autodiff::dual { + autodiff::dual u = c.b_x * k; + autodiff::dual phi = u - c.e_x * (u - atan(u)); + return c.d_x * sin(c.c_x * atan(phi)) + c.s_vx; + }; + + const autodiff::dual fx_result = fx_formula(kappa_x); + const auto dFx_dKappa = autodiff::derivative(fx_formula, autodiff::wrt(kappa_x), autodiff::at(kappa_x)); + + tire_outputs_.longitudinal_force_N = static_cast(autodiff::val(fx_result)); + tire_outputs_.dFx_dKappa = static_cast(dFx_dKappa); } void TireModel::computePureLateralForce(const float normal_load_N) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index d520c61aaf..931c0c5de2 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -3,6 +3,7 @@ #include #include "torque_vectoring/shared_datatypes/constants.hpp" +#include "torque_vectoring/shared_datatypes/datatypes.hpp" namespace app::tv::estimation { @@ -43,6 +44,7 @@ class TireModel float slip_angle_rad = 0.0f; float longitudinal_force_N = 0.0f; float lateral_force_N = 0.0f; + float dFx_dKappa = 0.0f; // ∂Fx/∂κ — sensitivity of longitudinal force to slip ratio }; constexpr TireModel(const TirePressure tire_pressure, const WheelSide wheel_side, const WheelAxle wheel_axle) @@ -55,8 +57,16 @@ class TireModel [[nodiscard]] Outputs estimate(const StateInputs& inputs); [[nodiscard]] float slipRatioToWheelAngularVelocity(const float slip_ratio, const float wheel_vel_x_mps) const; + [[nodiscard]] float slipRatioToWheelAngularVelocity( + const float slip_ratio, const datatypes::datatypes::VehicleState& vehicle_state) const; private: + struct WheelVelocities + { + float x_mps = 0.0f; + float y_mps = 0.0f; + }; + struct TireCoefficents_Fy { float cy_1; @@ -237,19 +247,18 @@ class TireModel const float cornering_stiffness, const float shape_factor, const float peak_factor); [[nodiscard]] constexpr float pureFy_Sv(const float normal_load_N, const float normalized_load_delta) const; void pureFxMagicFormulaCoefficients(const float normal_load_N); - [[nodiscard]] constexpr float pure_Fx() const; void pureFyMagicFormulaCoefficients(const float normal_load_N); [[nodiscard]] constexpr float pure_Fy() const; [[nodiscard]] float wheelLongOffset_m() const; [[nodiscard]] float wheelLatOffset_m() const; [[nodiscard]] float wheelSteeringAngle_rad(const float steering_angle_rad) const; - void estimateWheelVelocities(float vehicle_velocity_x_mps, float vehicle_velocity_y_mps, float yaw_rate_radps); + [[nodiscard]] WheelVelocities wheelVelocities( + float vehicle_velocity_x_mps, float vehicle_velocity_y_mps, float yaw_rate_radps) const; void estimateSlipAngle(float steering_angle_rad); void estimateSlipRatio(const float wheel_angular_velocity_radps); void computePureLongitudinalForce(const float normal_load_N); void computePureLateralForce(const float normal_load_N); - void estimateForces(const float normal_load_N); const TireCoefficents_Fx coeff_fx_; const TireCoefficents_Fy coeff_fy_; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp new file mode 100644 index 0000000000..9397693bf4 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp @@ -0,0 +1,288 @@ +#include "vehicle_state_estimator.hpp" + +#include +#include + +#include "torque_vectoring/shared_datatypes/constants.hpp" + +using namespace app::tv::datatypes::vd_constants; + +namespace app::tv::estimation +{ +namespace +{ +using Filter = VehicleStateEstimator::Filter; +using StateInput = Filter::state_inp_mtx; +using State = Filter::state_mtx; +using StateVector = Filter::N_1; +using InputVector = Filter::U_1; +using Measurement = Filter::M_1; + +static constexpr std::size_t VX = 0; +static constexpr std::size_t VY = 1; +static constexpr std::size_t R = 2; +static constexpr std::size_t MZ = 3; + +static constexpr std::size_t AX = 0; +static constexpr std::size_t AY = 1; + +static constexpr float ESTIMATOR_DT_S = 0.01f; // Matches the 100 Hz control task. +static constexpr float ESTIMATOR_YAW_INERTIA = 110.0f; // TODO: Replace with measured Hexray yaw inertia. + +[[nodiscard]] autodiff::dual stateTransitionVx(const StateInput& x) +{ + const autodiff::dual& v_x = x(static_cast(VX)); + const autodiff::dual& v_y = x(static_cast(VY)); + const autodiff::dual& r = x(static_cast(R)); + const autodiff::dual& a_x = x(static_cast(4 + AX)); + + return v_x + (ESTIMATOR_DT_S * (a_x + (v_y * r))); +} + +[[nodiscard]] autodiff::dual stateTransitionVy(const StateInput& x) +{ + const autodiff::dual& v_x = x(static_cast(VX)); + const autodiff::dual& v_y = x(static_cast(VY)); + const autodiff::dual& r = x(static_cast(R)); + const autodiff::dual& a_y = x(static_cast(4 + AY)); + + return v_y + (ESTIMATOR_DT_S * (a_y - (v_x * r))); +} + +[[nodiscard]] autodiff::dual stateTransitionYawRate(const StateInput& x) +{ + const autodiff::dual& r = x(static_cast(R)); + const autodiff::dual& mz = x(static_cast(MZ)); + + return r + (ESTIMATOR_DT_S * (mz / ESTIMATOR_YAW_INERTIA)); +} + +[[nodiscard]] autodiff::dual stateTransitionYawMoment(const StateInput& x) +{ + return x(static_cast(MZ)); +} + +[[nodiscard]] autodiff::dual measurementVx(const State& x) +{ + return x(static_cast(VX)); +} + +[[nodiscard]] autodiff::dual measurementVy(const State& x) +{ + return x(static_cast(VY)); +} + +[[nodiscard]] autodiff::dual measurementYawRate(const State& x) +{ + return x(static_cast(R)); +} + +[[nodiscard]] autodiff::dual measurementYawMoment(const State& x) +{ + return x(static_cast(MZ)); +} + +[[nodiscard]] auto createStateFunctions() -> std::array +{ + return { { + stateTransitionVx, + stateTransitionVy, + stateTransitionYawRate, + stateTransitionYawMoment, + } }; +} + +[[nodiscard]] auto createMeasurementFunctions() -> std::array +{ + return { { + measurementVx, + measurementVy, + measurementYawRate, + measurementYawMoment, + } }; +} + +[[nodiscard]] auto processNoise() -> Filter::N_N +{ + Filter::N_N q = Filter::N_N::Zero(); + q(static_cast(VX), static_cast(VX)) = 0.05f; + q(static_cast(VY), static_cast(VY)) = 0.05f; + q(static_cast(R), static_cast(R)) = 0.10f; + q(static_cast(MZ), static_cast(MZ)) = 150.0f; + return q; +} + +[[nodiscard]] auto measurementNoise() -> Filter::M_M +{ + Filter::M_M r = Filter::M_M::Zero(); + r(static_cast(VX), static_cast(VX)) = 0.75f; + r(static_cast(VY), static_cast(VY)) = 0.75f; + r(static_cast(R), static_cast(R)) = 0.05f; + r(static_cast(MZ), static_cast(MZ)) = 250.0f; + return r; +} + +[[nodiscard]] auto initialState() -> StateVector +{ + return StateVector::Zero(); +} + +[[nodiscard]] auto initialCovariance() -> Filter::N_N +{ + Filter::N_N p0 = Filter::N_N::Identity(); + p0(static_cast(VX), static_cast(VX)) = 5.0f; + p0(static_cast(VY), static_cast(VY)) = 5.0f; + p0(static_cast(R), static_cast(R)) = 1.0f; + p0(static_cast(MZ), static_cast(MZ)) = 400.0f; + return p0; +} + +[[nodiscard]] float readMeasuredLongitudinalAccelMps2(const VehicleStateEstimator::Inputs& inputs) +{ + // TODO: Replace with app_canRx_* when the Hexray signal table is ready. + return inputs.measured_state.a_x_mps2; +} + +[[nodiscard]] float readMeasuredLateralAccelMps2(const VehicleStateEstimator::Inputs& inputs) +{ + // TODO: Replace with app_canRx_* when the Hexray signal table is ready. + return inputs.measured_state.a_y_mps2; +} + +[[nodiscard]] float readMeasuredYawRateRadps(const VehicleStateEstimator::Inputs& inputs) +{ + // TODO: Replace with app_canRx_* when the Hexray signal table is ready. + return inputs.measured_state.yaw_rate_radps; +} + +[[nodiscard]] float readMeasuredSteeringAngleRad(const VehicleStateEstimator::Inputs& inputs) +{ + // TODO: Replace with app_canRx_* when the Hexray signal table is ready. + return inputs.measured_state.steer_ang_rad; +} + +[[nodiscard]] datatypes::datatypes::wheel_set readWheelAngularVelocitiesRadps( + const VehicleStateEstimator::Inputs& inputs) +{ + // TODO: Replace with app_canRx_* when the Hexray signal table is ready. + return inputs.wheel_angular_velocities_radps; +} + +[[nodiscard]] Measurement pseudoMeasurementFromWheelSpeeds( + const datatypes::datatypes::wheel_set& wheel_angular_velocities_radps, + const float yaw_rate_radps, + const float steering_angle_rad, + const StateVector& previous_state) +{ + const float front_cos = std::cos(steering_angle_rad); + const float front_sin = std::sin(steering_angle_rad); + const float half_track_m = TRACK_WIDTH_m * 0.5f; + + const std::array wheel_surface_speeds_mps = { + wheel_angular_velocities_radps.fl * WHEEL_RADIUS_M, + wheel_angular_velocities_radps.fr * WHEEL_RADIUS_M, + wheel_angular_velocities_radps.rl * WHEEL_RADIUS_M, + wheel_angular_velocities_radps.rr * WHEEL_RADIUS_M, + }; + + const float speed_sum_mps = + std::fabs(wheel_surface_speeds_mps[0]) + std::fabs(wheel_surface_speeds_mps[1]) + + std::fabs(wheel_surface_speeds_mps[2]) + std::fabs(wheel_surface_speeds_mps[3]); + + Measurement z = Measurement::Zero(); + z(static_cast(VX)) = previous_state(static_cast(VX)); + z(static_cast(VY)) = previous_state(static_cast(VY)); + + if (speed_sum_mps <= SMALL_EPSILON) + { + z(static_cast(VX)) = 0.0f; + z(static_cast(VY)) = 0.0f; + return z; + } + + const float fl_vx = (wheel_surface_speeds_mps[0] * front_cos) + (yaw_rate_radps * half_track_m); + const float fr_vx = (wheel_surface_speeds_mps[1] * front_cos) - (yaw_rate_radps * half_track_m); + const float rl_vx = wheel_surface_speeds_mps[2] + (yaw_rate_radps * half_track_m); + const float rr_vx = wheel_surface_speeds_mps[3] - (yaw_rate_radps * half_track_m); + + const float fl_vy = (wheel_surface_speeds_mps[0] * front_sin) - (yaw_rate_radps * DIST_FRONT_AXLE_CG_m); + const float fr_vy = (wheel_surface_speeds_mps[1] * front_sin) - (yaw_rate_radps * DIST_FRONT_AXLE_CG_m); + const float rl_vy = yaw_rate_radps * DIST_REAR_AXLE_CG_m; + const float rr_vy = yaw_rate_radps * DIST_REAR_AXLE_CG_m; + + z(static_cast(VX)) = 0.25f * (fl_vx + fr_vx + rl_vx + rr_vx); + z(static_cast(VY)) = 0.25f * (fl_vy + fr_vy + rl_vy + rr_vy); + return z; +} +} // namespace + +VehicleStateEstimator::VehicleStateEstimator() + : filter_(createFilter()) +{ +} + +auto VehicleStateEstimator::createFilter() -> Filter +{ + return Filter( + createStateFunctions(), + createMeasurementFunctions(), + processNoise(), + measurementNoise(), + initialState(), + initialCovariance()); +} + +VehicleStateEstimator::Outputs VehicleStateEstimator::estimate(const Inputs& inputs) +{ + InputVector u = InputVector::Zero(); + u(static_cast(AX)) = readMeasuredLongitudinalAccelMps2(inputs); + u(static_cast(AY)) = readMeasuredLateralAccelMps2(inputs); + + const float measured_yaw_rate_radps = readMeasuredYawRateRadps(inputs); + const float measured_steering_angle = readMeasuredSteeringAngleRad(inputs); + const auto wheel_angular_velocities = readWheelAngularVelocitiesRadps(inputs); + const auto& previous_state = filter_.state(); + + Measurement z = pseudoMeasurementFromWheelSpeeds( + wheel_angular_velocities, + measured_yaw_rate_radps, + measured_steering_angle, + previous_state); + + z(static_cast(R)) = measured_yaw_rate_radps; + z(static_cast(MZ)) = dynamics_estimator_.estimateYawMoment_Nm( + inputs.longitudinal_forces_N, + inputs.lateral_forces_N, + measured_steering_angle); + + const StateVector estimated_state = filter_.estimated_states(u, z); + + outputs_.vehicle_state = { + .v_x_mps = estimated_state(static_cast(VX)), + .v_y_mps = estimated_state(static_cast(VY)), + .yaw_rate_radps = estimated_state(static_cast(R)), + .steer_ang_rad = measured_steering_angle, + .a_x_mps2 = u(static_cast(AX)), + .a_y_mps2 = u(static_cast(AY)), + }; + outputs_.yaw_moment_nm = estimated_state(static_cast(MZ)); + + return outputs_; +} + +const VehicleStateEstimator::Outputs& VehicleStateEstimator::outputs() const +{ + return outputs_; +} + +const VehicleStateEstimator::Covariance& VehicleStateEstimator::covariance() const +{ + return filter_.covariance(); +} + +void VehicleStateEstimator::reset() +{ + filter_ = createFilter(); + outputs_ = {}; +} +} // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp new file mode 100644 index 0000000000..fc83c4f1e5 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include "state_estimation/app_kalman_filter.hpp" +#include "torque_vectoring/estimation/dynamics_estimation.hpp" +#include "torque_vectoring/shared_datatypes/datatypes.hpp" + +namespace app::tv::estimation +{ +class VehicleStateEstimator +{ +public: + struct Inputs + { + datatypes::datatypes::VehicleState measured_state{}; + datatypes::datatypes::wheel_set wheel_angular_velocities_radps{}; + datatypes::datatypes::wheel_set longitudinal_forces_N{}; + datatypes::datatypes::wheel_set lateral_forces_N{}; + }; + + struct Outputs + { + datatypes::datatypes::VehicleState vehicle_state{}; + float yaw_moment_nm = 0.0f; + }; + + using Filter = app::state_estimation::ekf; + using Covariance = Filter::N_N; + + VehicleStateEstimator(); + + [[nodiscard]] Outputs estimate(const Inputs& inputs); + [[nodiscard]] const Outputs& outputs() const; + [[nodiscard]] const Covariance& covariance() const; + void reset(); + +private: + [[nodiscard]] static Filter createFilter(); + + Filter filter_; + vehicleDynamics dynamics_estimator_{}; + Outputs outputs_{}; +}; +} // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index 948649005c..9f024e2101 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -11,6 +11,12 @@ namespace app::tv::datatypes::vd_constants inline constexpr float GRAVITY = 9.81f; // m/s^2 inline constexpr float SMALL_EPSILON = 0.000001f; // Numerical stability for division +inline constexpr float FRONTAL_AREA_M2 = 0.94f; // m^2 from aero team +inline constexpr float AIR_DENSITY_KGPM3 = 1.2205f; // kg/m^3 +inline constexpr float LIFT_COEFF = 1.7f; // from aero team +inline constexpr float DRAG_COEFF = 0.92f; +inline constexpr float COP_REAR = 0.68f; // fraction of aero load acting behind the CG +inline constexpr float COP_RIGHT = 0.5f; // fraction of aero load acting on the right side // ============================================================================= // VEHICLE DIMENSIONS @@ -21,6 +27,7 @@ inline constexpr float WHEELBASE_m = WHEELBASE_mm * MM_TO_M; inline constexpr float TRACK_WIDTH_mm = 1100.0f; inline constexpr float TRACK_WIDTH_m = TRACK_WIDTH_mm * MM_TO_M; +inline constexpr float WHEEL_RADIUS_M = WHEEL_DIAMETER_IN * IN_TO_M / 2.0f; // ============================================================================= // VEHICLE MASS & CENTER OF GRAVITY @@ -119,77 +126,6 @@ inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = return (power * static_cast(POWER_TO_TORQUE_CONVERSION_FACTOR)) / (std::fmax(rpm, 0.00001f) / GEAR_RATIO); } -// ============================================================================= -// VEHICLE DYNAMICS - VERTICAL LOAD TRANSFER -// Reference: https://www.zotero.org/groups/5809911/vehicle_controls_2024/items/N4TQBR67/reader -// ============================================================================= - -/** - * Longitudinal load transfer component (page 21) - * Positive long_accel transfers load to rear axle - * - * @param long_accel Longitudinal acceleration (m/s^2) - * @return Load transfer force (N) - */ -[[nodiscard]] inline constexpr float LONG_ACCEL_TERM_VERTICAL_FORCE(const float long_accel) -{ - return (CAR_MASS_AT_CG_KG * long_accel * DIST_HEIGHT_CG_m) / WHEELBASE_m; -} - -/** - * Lateral load transfer component (page 21) - * Transfers load to outside wheels during cornering - * - * @param lat_accel Lateral acceleration (m/s^2) - * @return Load transfer force per side (N) - */ -[[nodiscard]] inline constexpr float LAT_ACCEL_TERM_VERTICAL_FORCE(const float lat_accel) -{ - return (CAR_MASS_AT_CG_KG * lat_accel * DIST_HEIGHT_CG_m) / (2.0f * TRACK_WIDTH_m); -} - -[[nodiscard]] inline constexpr float REAR_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) -{ - return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) + - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); -} - -[[nodiscard]] inline constexpr float REAR_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) -{ - return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) - - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); -} - -// TODO: Check if front wheels use rear weight or front weight distribution -[[nodiscard]] inline constexpr float FRONT_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) -{ - return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) + - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); -} - -[[nodiscard]] inline constexpr float FRONT_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) -{ - return REAR_WEIGHT_DISTRIBUTION - LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) - - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); -} - -/** - * Yaw moment distribution factor Kmz (page 57) - * Accounts for load transfer effect on yaw moment generation capacity - * - * @param long_accel Longitudinal acceleration (m/s^2) - * @return Effective moment arm (m) - */ -[[nodiscard]] inline constexpr float ACCELERATION_TERM_KMZ(const float long_accel) -{ - return DIST_FRONT_AXLE_CG_m + (long_accel * DIST_HEIGHT_CG_m) / GRAVITY; -} - -/** - * Moment scaling factor F (page 58) - * Relates torque differential to yaw moment through track width and effective radius - */ -inline constexpr float F = (TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * 2.54f)) * GEAR_RATIO; // ============================================================================= // EXTERNAL CONFIGURATION (Commented Out) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp new file mode 100644 index 0000000000..324bfcbc96 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -0,0 +1,22 @@ +#pragma once + +namespace app::tv::datatypes::datatypes +{ +struct wheel_set +{ + float fl = 0.0f; + float fr = 0.0f; + float rl = 0.0f; + float rr = 0.0f; +}; + +struct VehicleState +{ + float v_x_mps = 0.0f; + float v_y_mps = 0.0f; + float yaw_rate_radps = 0.0f; + float steer_ang_rad = 0.0f; + float a_x_mps2 = 0.0f; + float a_y_mps2 = 0.0f; +}; +} // namespace app::tv::datatypes::datatypes diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index cbbd929b30..974219a530 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -1,39 +1,156 @@ #include "torque_vectoring/estimation/dynamics_estimation.hpp" #include "torque_vectoring/estimation/tire_model.hpp" +#include "torque_vectoring/estimation/vehicle_state_estimator.hpp" +#include "torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp" +#include "torque_vectoring/controllers/torque_allocator/torque_allocator.hpp" +#include "torque_vectoring/shared_datatypes/constants.hpp" +#include "torque_vectoring/shared_datatypes/datatypes.hpp" + +#include namespace app::tv { namespace { -[[maybe_unused]] estimation::vehicleDynamics vehicle_dynamics_estimator{}; +using namespace datatypes::datatypes; +using namespace datatypes::vd_constants; + +//------------------------------------- ESTIMATION MODULES ----------------------------------// + +static estimation::vehicleDynamics vehicle_dynamics_estimator{}; +static estimation::VehicleStateEstimator vehicle_state_estimator{}; -estimation::TireModel front_left_tire_model{ +static estimation::TireModel fl_tire_model{ estimation::TireModel::TirePressure::PSI_12, estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Front}; -estimation::TireModel front_right_tire_model{ +static estimation::TireModel fr_tire_model{ estimation::TireModel::TirePressure::PSI_12, estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Front}; -estimation::TireModel rear_left_tire_model{ +static estimation::TireModel rl_tire_model{ estimation::TireModel::TirePressure::PSI_12, estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Rear}; -estimation::TireModel rear_right_tire_model{ +static estimation::TireModel rr_tire_model{ estimation::TireModel::TirePressure::PSI_12, estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Rear}; + +//------------------------------------- CONTROLLERS -----------------------------------------// + +static controllers::allocator::TorqueAllocator torque_allocator{}; + +//------------------------------------- STATE VARIABLES -------------------------------------// + +static wheel_set acc_f_x{}; +static wheel_set acc_f_y{}; +static wheel_set acc_f_z{}; +static wheel_set acc_slip_ratio{}; +static wheel_set acc_slip_angle{}; +static wheel_set slip_ratio_opt{}; +[[maybe_unused]] static float est_yaw_moment_nm{}; + +// Build TireModel::StateInputs from VehicleState + per-wheel data +[[nodiscard]] estimation::TireModel::StateInputs buildTireInputs( + const VehicleState& state, const float omega, const float fz) +{ + return { + .wheel_angular_velocity_radps = omega, + .vehicle_velocity_x_mps = state.v_x_mps, + .vehicle_velocity_y_mps = state.v_y_mps, + .yaw_rate_radps = state.yaw_rate_radps, + .steering_angle_rad = state.steer_ang_rad, + .normal_load_N = fz, + }; +} + +// Unpack four tire model outputs into the module-level wheel_set variables +void unpackTireOutputs( + const estimation::TireModel::Outputs& fl, + const estimation::TireModel::Outputs& fr, + const estimation::TireModel::Outputs& rl, + const estimation::TireModel::Outputs& rr) +{ + acc_f_x = { .fl = fl.longitudinal_force_N, .fr = fr.longitudinal_force_N, + .rl = rl.longitudinal_force_N, .rr = rr.longitudinal_force_N }; + acc_f_y = { .fl = fl.lateral_force_N, .fr = fr.lateral_force_N, + .rl = rl.lateral_force_N, .rr = rr.lateral_force_N }; + acc_slip_ratio = { .fl = fl.slip_ratio, .fr = fr.slip_ratio, + .rl = rl.slip_ratio, .rr = rr.slip_ratio }; + acc_slip_angle = { .fl = fl.slip_angle_rad, .fr = fr.slip_angle_rad, + .rl = rl.slip_angle_rad, .rr = rr.slip_angle_rad }; +} + } // namespace -// optimzier -/* -// yaw rate control and pedal request +//------------------------------------- TORQUE VECTORING UPDATE CYCLE -------------------------// + +void update(const VehicleState& state, const float pedal_percentage, + const float fl_omega, const float fr_omega, + const float rl_omega, const float rr_omega) +{ + //------------------------------------- STATE ESTIMATION --------------------------------// + + const auto state_estimate = vehicle_state_estimator.estimate({ + .measured_state = state, + .wheel_angular_velocities_radps = { .fl = fl_omega, .fr = fr_omega, .rl = rl_omega, .rr = rr_omega }, + .longitudinal_forces_N = acc_f_x, + .lateral_forces_N = acc_f_y, + }); + + const VehicleState estimated_state = state_estimate.vehicle_state; + est_yaw_moment_nm = state_estimate.yaw_moment_nm; + + // Normal forces from longitudinal/lateral load transfer + downforce + acc_f_z = vehicle_dynamics_estimator.estimateNormalForce_N( + estimated_state.a_x_mps2, estimated_state.a_y_mps2, estimated_state.v_x_mps); + + // Tire model estimation from current sensor data using the filtered vehicle state. + const auto fl_out = fl_tire_model.estimate(buildTireInputs(estimated_state, fl_omega, acc_f_z.fl)); + const auto fr_out = fr_tire_model.estimate(buildTireInputs(estimated_state, fr_omega, acc_f_z.fr)); + const auto rl_out = rl_tire_model.estimate(buildTireInputs(estimated_state, rl_omega, acc_f_z.rl)); + const auto rr_out = rr_tire_model.estimate(buildTireInputs(estimated_state, rr_omega, acc_f_z.rr)); + unpackTireOutputs(fl_out, fr_out, rl_out, rr_out); + + const float acc_yaw_moment_nm = vehicle_dynamics_estimator.estimateYawMoment_Nm( + acc_f_x, acc_f_y, estimated_state.steer_ang_rad); + + //------------------------------------- HIGH LEVEL CONTROLLER ----------------------------// + + // Desired per-wheel longitudinal force from pedal request, projected along tire heading + const float per_wheel_tq = MAX_TORQUE_REQUEST_NM * pedal_percentage; + wheel_set des_f_x = { + .fl = per_wheel_tq * std::cos(acc_slip_angle.fl) * WHEEL_RADIUS_M , + .fr = per_wheel_tq * std::cos(acc_slip_angle.fr) * WHEEL_RADIUS_M, + .rl = per_wheel_tq * std::cos(acc_slip_angle.rl) * WHEEL_RADIUS_M, + .rr = per_wheel_tq * std::cos(acc_slip_angle.rr) * WHEEL_RADIUS_M, + }; + + // Direct yaw rate control: corrective yaw moment + const float des_yaw_moment_nm = controllers::dyrc::computeYawMoment( + estimated_state.yaw_rate_radps, estimated_state.steer_ang_rad, estimated_state.v_x_mps); + + // Distribute yaw moment into per-wheel force adjustment + // From Mz = (t/2) * (Fx_right - Fx_left), solve for delta Fx per side: + const float des_fx_delta = des_yaw_moment_nm / TRACK_WIDTH_m; + des_f_x.fl -= des_fx_delta; + des_f_x.fr += des_fx_delta; + des_f_x.rl -= des_fx_delta; + des_f_x.rr += des_fx_delta; + + //------------------------------------- LOW LEVEL CONTROLLER -----------------------------// + // Gauss-Newton optimizer: find slip ratios that minimize combined force + moment tracking error + slip_ratio_opt = torque_allocator.optimize( + des_f_x, acc_f_z, acc_slip_ratio, estimated_state, acc_yaw_moment_nm, des_yaw_moment_nm); + //------------------------------------- POWER LIMITER -----------------------------------// -*/ + // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request +} } // namespace app::tv From f5ef2f3dfa94007e30a21bba7ed8fc93dd97171b Mon Sep 17 00:00:00 2001 From: shayana18 Date: Wed, 8 Apr 2026 14:36:07 -0700 Subject: [PATCH 04/35] logic is there now --- .../controllers/regen/regen.cpp | 0 .../controllers/regen/regen.hpp | 0 .../torque_allocator/torque_allocator.cpp | 287 +++++++----- .../torque_allocator/torque_allocator.hpp | 41 +- .../estimation/dynamics_estimation.cpp | 8 +- .../estimation/dynamics_estimation.hpp | 9 +- .../estimation/tire_model.cpp | 435 ++++++++++++++---- .../estimation/tire_model.hpp | 257 +++++++---- .../estimation/vehicle_state_estimator.cpp | 6 +- .../estimation/vehicle_state_estimator.hpp | 6 +- .../shared_datatypes/constants.hpp | 9 +- .../shared_datatypes/datatypes.hpp | 20 +- .../shared_datatypes/low_speed_blend.hpp | 31 ++ .../app/torque_vectoring/torque_vectoring.cpp | 86 ++-- 14 files changed, 818 insertions(+), 377 deletions(-) delete mode 100644 firmware/hexray/VC/src/app/torque_vectoring/controllers/regen/regen.cpp delete mode 100644 firmware/hexray/VC/src/app/torque_vectoring/controllers/regen/regen.hpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/low_speed_blend.hpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/regen/regen.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/regen/regen.cpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/regen/regen.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/regen/regen.hpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp index 99a36ae2c6..33880fa445 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp @@ -2,10 +2,14 @@ #include #include +#include -using namespace app::tv::datatypes::vd_constants; -using WheelSide = app::tv::estimation::TireModel::WheelSide; -using WheelAxle = app::tv::estimation::TireModel::WheelAxle; +#include +#include +#include + +using namespace app::tv::shared_datatypes::vd_constants; +using namespace app::tv::shared_datatypes::datatypes; namespace app::tv::controllers::allocator { @@ -13,136 +17,193 @@ namespace { // ---- Optimizer tuning (module-private) ---- - -static constexpr float W_FX = 2.0f / 3.0f; // Weight on per-wheel force tracking error -static constexpr float W_MZ = 1.0f / 3.0f; // Weight on yaw moment tracking error -static constexpr int MAX_ITER = 8; // Gauss-Newton iterations per control cycle -static constexpr float SLIP_CLAMP = 0.3f; // Physical slip ratio bounds -static constexpr float MIN_HESSIAN = 1e-6f; // Prevents division by zero in Newton step - -// Wheel array index convention -static constexpr int FL = 0; -static constexpr int FR = 1; -static constexpr int RL = 2; -static constexpr int RR = 3; - -// Mz sign per wheel: right-side forces create positive yaw moment -static constexpr float MZ_SIGNS[4] = { -1.0f, 1.0f, -1.0f, 1.0f }; - -/** - * Build TireModel::StateInputs from shared vehicle state. - * The vehicle state and normal load are fixed during optimization; - * only the angular velocity changes across iterations (via trial slip ratio). - */ -[[nodiscard]] estimation::TireModel::StateInputs buildStateInputs( - const datatypes::datatypes::VehicleState& vs, - const float normal_load_N, - const float wheel_angular_velocity_radps) +static constexpr float W_FX = 2.0f / 3.0f; +static constexpr float W_MZ = 1.0f / 3.0f; +static constexpr int MAX_ITER = 8; +static constexpr float SLIP_CLAMP = 0.3f; +static constexpr float NORMAL_MATRIX_EPS = 1e-6f; +static constexpr float STEP_TOLERANCE = 1e-5f; +static constexpr float COST_TOLERANCE = 1e-6f; + +using Vec4f = Eigen::Matrix; +using Vec5f = Eigen::Matrix; +using Mat54f = Eigen::Matrix; +using Mat44f = Eigen::Matrix; +using DualVec4 = Eigen::Matrix; +using DualVec5 = Eigen::Matrix; + +template +[[nodiscard]] T yawMomentFromTireForces( + const wheel_set& f_x, const wheel_set& f_y, const float steering_angle_rad) { - return { - .wheel_angular_velocity_radps = wheel_angular_velocity_radps, - .vehicle_velocity_x_mps = vs.v_x_mps, - .vehicle_velocity_y_mps = vs.v_y_mps, - .yaw_rate_radps = vs.yaw_rate_radps, - .steering_angle_rad = vs.steer_ang_rad, - .normal_load_N = normal_load_N, - }; -} + const float cos_delta = std::cos(steering_angle_rad); + const float sin_delta = std::sin(steering_angle_rad); + const float half_track_m = TRACK_WIDTH_m * 0.5f; -} // namespace + const T fl_fx_body = (T(cos_delta) * f_x.fl) - (T(sin_delta) * f_y.fl); + const T fl_fy_body = (T(sin_delta) * f_x.fl) + (T(cos_delta) * f_y.fl); + const T fr_fx_body = (T(cos_delta) * f_x.fr) - (T(sin_delta) * f_y.fr); + const T fr_fy_body = (T(sin_delta) * f_x.fr) + (T(cos_delta) * f_y.fr); -// ---- Construction ---- + const T fl_moment = (T(DIST_FRONT_AXLE_CG_m) * fl_fy_body) - (T(half_track_m) * fl_fx_body); + const T fr_moment = (T(DIST_FRONT_AXLE_CG_m) * fr_fy_body) + (T(half_track_m) * fr_fx_body); + const T rl_moment = (T(-DIST_REAR_AXLE_CG_m) * f_y.rl) - (T(half_track_m) * f_x.rl); + const T rr_moment = (T(-DIST_REAR_AXLE_CG_m) * f_y.rr) + (T(half_track_m) * f_x.rr); -TorqueAllocator::TorqueAllocator(estimation::TireModel::TirePressure pressure) - : fl_tire_(pressure, WheelSide::Left, WheelAxle::Front), - fr_tire_(pressure, WheelSide::Right, WheelAxle::Front), - rl_tire_(pressure, WheelSide::Left, WheelAxle::Rear), - rr_tire_(pressure, WheelSide::Right, WheelAxle::Rear) -{ + return fl_moment + fr_moment + rl_moment + rr_moment; } -// ---- Public interface ---- - -float TorqueAllocator::estimateYawMoment(const datatypes::datatypes::wheel_set& f_x) -{ - // Mz = (t/2) * (Fx_right_total - Fx_left_total) - return (TRACK_WIDTH_m * 0.5f) * ((f_x.fr + f_x.rr) - (f_x.fl + f_x.rl)); -} +} // namespace -datatypes::datatypes::wheel_set TorqueAllocator::optimize( - const datatypes::datatypes::wheel_set& des_f_x, - const datatypes::datatypes::wheel_set& normal_forces_N, - const datatypes::datatypes::wheel_set& current_slip_ratios, - const datatypes::datatypes::VehicleState& vehicle_state, - float acc_M_z, +wheel_set TorqueAllocator::optimize( + const wheel_set& tire_models, + const wheel_set& des_f_x, + const wheel_set& normal_forces_N, + const wheel_set& current_slip_ratios, + const wheel_set& current_slip_angles, + const float low_speed_blend, + const float steering_angle_rad, const float des_M_z) { - // Working copy of slip ratios — warm-started from current estimates - float kappa[4] = { - current_slip_ratios.fl, - current_slip_ratios.fr, - current_slip_ratios.rl, - current_slip_ratios.rr, - }; - - estimation::TireModel* tires[4] = { &fl_tire_, &fr_tire_, &rl_tire_, &rr_tire_ }; + // Low-speed safeguard: + // torque_vectoring.cpp computes a single force-availability blend from vehicle speed and passes it + // into the allocator. Keeping that policy decision outside the optimizer makes the heuristic explicit + // at the orchestration layer while the optimizer itself only consumes the already-decided scaling. + // + // Below a very small blend threshold, there is no meaningful traction allocation problem to solve, + // so return zero requested slip immediately. + if (low_speed_blend < 0.05f) + { + return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; + } - const float fz[4] = { normal_forces_N.fl, normal_forces_N.fr, - normal_forces_N.rl, normal_forces_N.rr }; - const float des_fx[4] = { des_f_x.fl, des_f_x.fr, des_f_x.rl, des_f_x.rr }; + const wheel_set blended_des_f_x{ + .fl = low_speed_blend * des_f_x.fl, + .fr = low_speed_blend * des_f_x.fr, + .rl = low_speed_blend * des_f_x.rl, + .rr = low_speed_blend * des_f_x.rr, + }; + const float blended_des_m_z = low_speed_blend * des_M_z; + + Vec4f opt_slip; + opt_slip << current_slip_ratios.fl, + current_slip_ratios.fr, + current_slip_ratios.rl, + current_slip_ratios.rr; + + const float sqrt_w_fx = std::sqrt(W_FX); + const float sqrt_w_mz = std::sqrt(W_MZ); + + // Reference material used to shape this implementation: + // - Video walkthrough: https://www.youtube.com/watch?v=C6DCtQjKkdY + // - Wikipedia summary: https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm + // + // Residual vector for Gauss-Newton: + // r = [sqrt(W_FX) * (Fx_i - des_fx_i), sqrt(W_MZ) * (Mz - des_Mz)]^T + // + // Why this form: + // - The allocator is naturally a least-squares problem: track desired wheel forces while also + // matching a desired yaw moment. + // - Gauss-Newton is a good fit because it works directly on residuals and only needs the + // residual Jacobian J = dr/dkappa, which autodiff can compute for us cleanly. + // + // The solve uses the textbook normal equations: + // (J^T J) delta = -J^T r + // where: + // r = residual vector evaluated at the current trial slip + // J = dr/dkappa evaluated at the current trial slip + // + // We keep all vectors/matrices fixed-size (4 decision variables, 5 residuals) so the optimizer + // stays allocation-free and predictable on embedded targets. + const auto residualVector = [&](const DualVec4& kappa) -> DualVec5 { + const wheel_set predicted_fx{ + .fl = tire_models.fl.computeCombinedFx_N( + normal_forces_N.fl, current_slip_angles.fl, low_speed_blend, kappa(0)), + .fr = tire_models.fr.computeCombinedFx_N( + normal_forces_N.fr, current_slip_angles.fr, low_speed_blend, kappa(1)), + .rl = tire_models.rl.computeCombinedFx_N( + normal_forces_N.rl, current_slip_angles.rl, low_speed_blend, kappa(2)), + .rr = tire_models.rr.computeCombinedFx_N( + normal_forces_N.rr, current_slip_angles.rr, low_speed_blend, kappa(3)), + }; + + const wheel_set predicted_fy{ + .fl = tire_models.fl.computeCombinedFy_N( + normal_forces_N.fl, current_slip_angles.fl, low_speed_blend, kappa(0)), + .fr = tire_models.fr.computeCombinedFy_N( + normal_forces_N.fr, current_slip_angles.fr, low_speed_blend, kappa(1)), + .rl = tire_models.rl.computeCombinedFy_N( + normal_forces_N.rl, current_slip_angles.rl, low_speed_blend, kappa(2)), + .rr = tire_models.rr.computeCombinedFy_N( + normal_forces_N.rr, current_slip_angles.rr, low_speed_blend, kappa(3)), + }; + const autodiff::dual predicted_mz = yawMomentFromTireForces(predicted_fx, predicted_fy, steering_angle_rad); + + DualVec5 residuals; + residuals(0) = autodiff::dual(sqrt_w_fx) * (predicted_fx.fl - blended_des_f_x.fl); + residuals(1) = autodiff::dual(sqrt_w_fx) * (predicted_fx.fr - blended_des_f_x.fr); + residuals(2) = autodiff::dual(sqrt_w_fx) * (predicted_fx.rl - blended_des_f_x.rl); + residuals(3) = autodiff::dual(sqrt_w_fx) * (predicted_fx.rr - blended_des_f_x.rr); + residuals(4) = autodiff::dual(sqrt_w_mz) * (predicted_mz - blended_des_m_z); + return residuals; + }; - const float half_track = TRACK_WIDTH_m * 0.5f; + float previous_cost = std::numeric_limits::infinity(); for (int iter = 0; iter < MAX_ITER; ++iter) { - // ---- Evaluate tire model at current trial slip ratios ---- - float acc_fx[4] = {}; - float dFdK[4] = {}; - + DualVec4 kappa_dual; for (int i = 0; i < 4; ++i) - { - // TireModel owns the wheel-center kinematics, so the allocator only provides trial kappa + vehicle state. - const float omega = tires[i]->slipRatioToWheelAngularVelocity(kappa[i], vehicle_state); - const auto inputs = buildStateInputs(vehicle_state, fz[i], omega); - const auto outputs = tires[i]->estimate(inputs); - - acc_fx[i] = outputs.longitudinal_force_N; - dFdK[i] = outputs.dFx_dKappa; - } - - // ---- Yaw moment error ---- - const datatypes::datatypes::wheel_set acc_fx_set = { - .fl = acc_fx[FL], .fr = acc_fx[FR], .rl = acc_fx[RL], .rr = acc_fx[RR] }; - const float e_Mz = acc_M_z - des_M_z; - - // ---- Gauss-Newton step per wheel ---- - // - // Cost: J = W_FX * sum_i (Fx_i - des_fx_i)^2 + W_MZ * (Mz - des_Mz)^2 - // - // Gradient: dJ/dkappa_i = W_FX * e_fx_i * dFx/dkappa - // + W_MZ * e_Mz * dMz/dkappa_i - // - // Hessian (diagonal, Gauss-Newton approximation): - // H_ii = W_FX * (dFx/dkappa)^2 + W_MZ * (dMz/dkappa)^2 + kappa_dual(i) = autodiff::dual(opt_slip(i)); + + DualVec5 residual_eval_dual; + Mat54f jacobian; + autodiff::jacobian( + residualVector, autodiff::wrt(kappa_dual), autodiff::at(kappa_dual), residual_eval_dual, jacobian); + + Vec5f residuals; + for (int i = 0; i < 5; ++i) + residuals(i) = static_cast(autodiff::val(residual_eval_dual(i))); + + // Gauss-Newton solves: + // (J^T J) * delta = -J^T r + // where r is the residual vector and J is dr/dkappa at the current trial slip. // - for (int i = 0; i < 4; ++i) - { - const float e_fx = acc_fx[i] - des_fx[i]; - const float dMz_dK = MZ_SIGNS[i] * half_track * dFdK[i]; + // J comes directly from autodiff::jacobian(...), so we do not hand-derive per-wheel slopes + // or yaw-moment sensitivities. The optimizer stays readable: define residuals first, then + // let autodiff provide the linearization used by Gauss-Newton. + Mat44f normal_matrix = jacobian.transpose() * jacobian; + normal_matrix.diagonal().array() += NORMAL_MATRIX_EPS; + + const Vec4f rhs = -jacobian.transpose() * residuals; + Eigen::LDLT ldlt(normal_matrix); + + if (ldlt.info() != Eigen::Success) + break; + + const Vec4f delta = ldlt.solve(rhs); + if (!delta.allFinite()) + break; - const float grad = (W_FX * e_fx * dFdK[i]) + (W_MZ * e_Mz * dMz_dK); - const float H_ii = (W_FX * dFdK[i] * dFdK[i]) + (W_MZ * dMz_dK * dMz_dK); + opt_slip += delta; + for (int i = 0; i < 4; ++i) + opt_slip(i) = std::clamp(opt_slip(i), -SLIP_CLAMP, SLIP_CLAMP); - kappa[i] -= grad / std::fmax(H_ii, MIN_HESSIAN); - kappa[i] = std::clamp(kappa[i], -SLIP_CLAMP, SLIP_CLAMP); - } + // Least-squares cost: + // J = 0.5 * r^T r + // This is used only for convergence monitoring; the actual update is driven by J^T J and J^T r above. + const float cost = 0.5f * residuals.squaredNorm(); + if (delta.norm() < STEP_TOLERANCE || std::fabs(previous_cost - cost) < COST_TOLERANCE) + break; - // Use the provided yaw-moment estimate to seed the first iteration, then - // keep the internal solve self-consistent with the tire-model force updates. - acc_M_z = estimateYawMoment(acc_fx_set); + previous_cost = cost; } - return { .fl = kappa[FL], .fr = kappa[FR], .rl = kappa[RL], .rr = kappa[RR] }; + return { + .fl = opt_slip(0), + .fr = opt_slip(1), + .rl = opt_slip(2), + .rr = opt_slip(3), + }; } - } // namespace app::tv::controllers::allocator diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp index 44d9fcd062..1905d70088 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp @@ -3,6 +3,7 @@ #include "torque_vectoring/shared_datatypes/constants.hpp" #include "torque_vectoring/shared_datatypes/datatypes.hpp" #include "torque_vectoring/estimation/tire_model.hpp" +#include namespace app::tv::controllers::allocator { @@ -14,46 +15,34 @@ namespace app::tv::controllers::allocator * J(kappa) = W_FX * sum(Fx_i(kappa_i) - des_fx_i)^2 * + W_MZ * (Mz(kappa) - des_Mz)^2 * - * Outputs per-wheel optimal slip ratios. Owns internal tire model - * instances separate from the estimation models to avoid state contamination. + * Outputs per-wheel optimal slip ratios using the caller-provided per-wheel tire models. */ class TorqueAllocator { public: - explicit TorqueAllocator( - estimation::TireModel::TirePressure pressure = estimation::TireModel::TirePressure::PSI_12); - /** * Run Gauss-Newton iterations to find optimal slip ratios. * - * @param des_f_x Target per-wheel longitudinal force (pedal request + yaw moment) + * @param tire_models Per-wheel tire models used to evaluate candidate slip ratios + * @param des_f_x Target per-wheel longitudinal force (pedal request + yaw split) * @param normal_forces_N Per-wheel vertical load from dynamics estimator * @param current_slip_ratios Warm start from current cycle estimation - * @param vehicle_state Vehicle-level state (fixed during optimization) - * @param acc_M_z Current yaw moment estimate supplied by the caller (Nm) + * @param current_slip_angles Fixed per-wheel slip angles for this optimization tick + * @param low_speed_blend Precomputed low-speed force-availability blend in [0, 1] + * @param steering_angle_rad Steering angle used to rotate front-wheel forces into the body frame * @param des_M_z Target yaw moment from high-level controller (Nm) * @return Per-wheel optimal slip ratios */ - [[nodiscard]] datatypes::datatypes::wheel_set optimize( - const datatypes::datatypes::wheel_set& des_f_x, - const datatypes::datatypes::wheel_set& normal_forces_N, - const datatypes::datatypes::wheel_set& current_slip_ratios, - const datatypes::datatypes::VehicleState& vehicle_state, - float acc_M_z, + [[nodiscard]] shared_datatypes::datatypes::wheel_set optimize( + const shared_datatypes::datatypes::wheel_set& tire_models, + const shared_datatypes::datatypes::wheel_set& des_f_x, + const shared_datatypes::datatypes::wheel_set& normal_forces_N, + const shared_datatypes::datatypes::wheel_set& current_slip_ratios, + const shared_datatypes::datatypes::wheel_set& current_slip_angles, + float low_speed_blend, + float steering_angle_rad, float des_M_z); - /** - * Estimate yaw moment from longitudinal wheel forces. - * Mz = (track_width / 2) * ((Fx_fr + Fx_rr) - (Fx_fl + Fx_rl)) - * Temporary fallback for callers until a dedicated yaw-moment estimator is wired in. - */ - [[nodiscard]] static float estimateYawMoment(const datatypes::datatypes::wheel_set& f_x); - -private: - estimation::TireModel fl_tire_; - estimation::TireModel fr_tire_; - estimation::TireModel rl_tire_; - estimation::TireModel rr_tire_; }; } // namespace app::tv::controllers::allocator diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp index f74c340ee6..62bc91a2f9 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp @@ -2,7 +2,7 @@ #include -using namespace app::tv::datatypes::vd_constants; +using namespace app::tv::shared_datatypes::vd_constants; namespace app::tv::estimation { @@ -25,8 +25,8 @@ float vehicleDynamics::estimateBodySlip(float v_x_mps, float v_y_mps) const } float vehicleDynamics::estimateYawMoment_Nm( - const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N, - const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N, + const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N, + const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N, const float steering_angle_rad) const { const float cos_delta = std::cos(steering_angle_rad); @@ -51,7 +51,7 @@ float vehicleDynamics::estimateYawMoment_Nm( return fl_moment + fr_moment + rl_moment + rr_moment; } -app::tv::datatypes::datatypes::wheel_set vehicleDynamics::estimateNormalForce_N( +app::tv::datatypes::datatypes::wheel_set vehicleDynamics::estimateNormalForce_N( const float a_x_MPS2, const float a_y_MPS2, const float v_x_mps) const { const float long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(a_x_MPS2); diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp index 5d065b851b..7269e3c612 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp @@ -5,16 +5,17 @@ namespace app::tv::estimation { -using namespace app::tv::datatypes::vd_constants; +using namespace app::tv::shared_datatypes::vd_constants; class vehicleDynamics { public: - [[nodiscard]] app::tv::datatypes::datatypes::wheel_set estimateNormalForce_N(float a_x_MPS2, float a_y_MPS2, float v_x_mps) const; + [[nodiscard]] app::tv::datatypes::datatypes::wheel_set estimateNormalForce_N( + float a_x_MPS2, float a_y_MPS2, float v_x_mps) const; [[nodiscard]] float estimateBodySlip(float v_x_mps, float v_y_mps) const; [[nodiscard]] float estimateYawMoment_Nm( - const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N, - const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N, + const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N, + const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N, float steering_angle_rad) const; [[nodiscard]] float estimateDrag_N(float v_x_mps) const; [[nodiscard]] float estimateDownForce_N(float v_x_mps) const; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index 434da997d6..a90697c647 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -3,7 +3,7 @@ #include #include -using namespace app::tv::datatypes::vd_constants; +using namespace app::tv::shared_datatypes::vd_constants; namespace app::tv::estimation { @@ -13,19 +13,50 @@ namespace { return std::fmax(std::fabs(value), SMALL_EPSILON); } + +[[nodiscard]] float primalValue(const float value) +{ + return value; +} + +[[nodiscard]] float primalValue(const autodiff::dual& value) +{ + return static_cast(autodiff::val(value)); +} + +template +[[nodiscard]] T safeTemplateDenominator(const T& value) +{ + const float primal = primalValue(value); + + if (std::fabs(primal) >= SMALL_EPSILON) + { + return value; + } + + return T(primal < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON); +} } // namespace -[[nodiscard]] TireModel::Outputs TireModel::estimate(const StateInputs& inputs) +[[nodiscard]] TireModel::Outputs TireModel::estimate(const StateInputs& inputs) const { + const float vehicle_speed_mps = std::hypot(inputs.vehicle_velocity_x_mps, inputs.vehicle_velocity_y_mps); + const float low_speed_blend = shared_datatypes::velocityBlend(vehicle_speed_mps); const auto wheel_velocities = wheelVelocities(inputs.vehicle_velocity_x_mps, inputs.vehicle_velocity_y_mps, inputs.yaw_rate_radps); - wheel_vel_x_mps_ = wheel_velocities.x_mps; - wheel_vel_y_mps_ = wheel_velocities.y_mps; - estimateSlipAngle(inputs.steering_angle_rad); - estimateSlipRatio(inputs.wheel_angular_velocity_radps); - computePureLongitudinalForce(inputs.normal_load_N); - computePureLateralForce(inputs.normal_load_N); - return tire_outputs_; + const float slip_angle_rad = + estimateSlipAngle(wheel_velocities.x_mps, wheel_velocities.y_mps, inputs.steering_angle_rad); + const float slip_ratio = estimateSlipRatio( + wheel_velocities.x_mps, wheel_velocities.y_mps, slip_angle_rad, inputs.wheel_angular_velocity_radps); + + return { + .slip_ratio = slip_ratio, + .slip_angle_rad = slip_angle_rad, + .longitudinal_force_N = static_cast( + computeCombinedFx_N(inputs.normal_load_N, slip_angle_rad, low_speed_blend, slip_ratio)), + .lateral_force_N = computeCombinedFy_N( + inputs.normal_load_N, slip_angle_rad, low_speed_blend, slip_ratio), + }; } [[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity(const float slip_ratio, const float wheel_vel_x_mps) const @@ -68,111 +99,330 @@ namespace }; } -void TireModel::estimateSlipAngle(float steering_angle_rad) +[[nodiscard]] float TireModel::estimateSlipAngle( + const float wheel_vel_x_mps, const float wheel_vel_y_mps, const float steering_angle_rad) const { - tire_outputs_.slip_angle_rad = std::atan2(wheel_vel_y_mps_, wheel_vel_x_mps_) - wheelSteeringAngle_rad(steering_angle_rad); + return std::atan2(wheel_vel_y_mps, wheel_vel_x_mps) - wheelSteeringAngle_rad(steering_angle_rad); } -void TireModel::estimateSlipRatio(const float wheel_angular_velocity_radps) +[[nodiscard]] float TireModel::estimateSlipRatio( + const float wheel_vel_x_mps, + const float wheel_vel_y_mps, + const float slip_angle_rad, + const float wheel_angular_velocity_radps) const { const float wheel_surface_speed_mps = wheel_angular_velocity_radps * WHEEL_RADIUS_M; - const float wheel_speed_magnitude_mps = std::hypot(wheel_vel_x_mps_, wheel_vel_y_mps_); - const float effective_wheel_speed_mps = wheel_speed_magnitude_mps * std::cos(tire_outputs_.slip_angle_rad); + const float wheel_speed_magnitude_mps = std::hypot(wheel_vel_x_mps, wheel_vel_y_mps); + const float effective_wheel_speed_mps = wheel_speed_magnitude_mps * std::cos(slip_angle_rad); - tire_outputs_.slip_ratio = - (wheel_surface_speed_mps - effective_wheel_speed_mps) / safeMagnitude(effective_wheel_speed_mps); + return (wheel_surface_speed_mps - effective_wheel_speed_mps) / safeMagnitude(effective_wheel_speed_mps); } //-------------------------------------------------------------------- Pacejka 5.2 ----------------------------------------------------------------------// -void TireModel::pureFxMagicFormulaCoefficients(const float normal_load_N) +template +TireModel::PureFxMagicFormulaCoefficients TireModel::pureFxMagicFormulaCoefficients( + const float normal_load_N, const T& slip_ratio) const { - // Assumes lambda terms are 1.0, gamma = 0, and pressure effects are captured by the discrete pressure row. + // Assumes lambda terms are 1.0, gamma = 0, and pressure effects are captured by the fixed 12_PSI fitted row. const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); - const float kappa_x = pureFx_Kappa(normalized_load_delta, tire_outputs_.slip_ratio); - const float c_x = pureFx_C(); - const float d_x = pureFx_D(clamped_normal_load_N, normalized_load_delta); - const float e_x = pureFx_E(normalized_load_delta, kappa_x); - const float k_x = pureFx_K(clamped_normal_load_N, normalized_load_delta); - const float b_x = pureFx_B(k_x, c_x, d_x); - const float s_vx = pureFx_Sv(clamped_normal_load_N, normalized_load_delta); - - pure_fx_mj_coefficients_ = { + const float s_hx = pureFx_Sh(normalized_load_delta); + const T kappa_x = pureFx_Kappa(normalized_load_delta, slip_ratio); + const float c_x = pureFx_C(); + const float d_x = pureFx_D(clamped_normal_load_N, normalized_load_delta); + const T e_x = pureFx_E(normalized_load_delta, kappa_x); + const float k_x = pureFx_K(clamped_normal_load_N, normalized_load_delta); + const float b_x = pureFx_B(k_x, c_x, d_x); + const float s_vx = pureFx_Sv(clamped_normal_load_N, normalized_load_delta); + + return { + .s_hx = T(s_hx), .kappa_x = kappa_x, - .c_x = c_x, - .d_x = d_x, - .b_x = b_x, + .b_x = T(b_x), + .c_x = T(c_x), + .d_x = T(d_x), .e_x = e_x, - .s_vx = s_vx, + .s_vx = T(s_vx), }; } -void TireModel::pureFyMagicFormulaCoefficients(const float normal_load_N) +TireModel::PureFyMagicFormulaCoefficients TireModel::pureFyMagicFormulaCoefficients( + const float normal_load_N, const float slip_angle_rad) const { - // Assumes gamma = 0, lambda terms are 1.0, and pressure effects are captured by the discrete pressure row. + // Assumes gamma = 0, lambda terms are 1.0, and pressure effects are captured by the fixed 12_PSI fitted row. const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); - const float alpha_y = pureFy_Alpha(normalized_load_delta, tire_outputs_.slip_angle_rad); - const float c_y = pureFy_C(); - const float d_y = pureFy_D(clamped_normal_load_N, normalized_load_delta); - const float e_y = pureFy_E(normalized_load_delta, alpha_y); - const float k_y = pureFy_K(clamped_normal_load_N); - const float b_y = pureFy_B(k_y, c_y, d_y); - const float s_vy = pureFy_Sv(clamped_normal_load_N, normalized_load_delta); - - pure_fy_mj_coefficients_ = { + const float s_hy = pureFy_Sh(normalized_load_delta); + const float alpha_y = pureFy_Alpha(normalized_load_delta, slip_angle_rad); + const float c_y = pureFy_C(); + const float d_y = pureFy_D(clamped_normal_load_N, normalized_load_delta); + const float e_y = pureFy_E(normalized_load_delta, alpha_y); + const float k_y = pureFy_K(clamped_normal_load_N); + const float b_y = pureFy_B(k_y, c_y, d_y); + const float s_vy = pureFy_Sv(clamped_normal_load_N, normalized_load_delta); + + return { + .s_hy = s_hy, .alpha_y = alpha_y, + .b_y = b_y, .c_y = c_y, .d_y = d_y, - .b_y = b_y, .e_y = e_y, .s_vy = s_vy, }; } -constexpr float TireModel::pure_Fy() const +template +T TireModel::computePureFx_N(const float normal_load_N, const T& slip_ratio) const { + using std::atan; + using std::sin; + + const auto coefficients = pureFxMagicFormulaCoefficients(normal_load_N, slip_ratio); + const T u = coefficients.b_x * coefficients.kappa_x; + const T phi = u - coefficients.e_x * (u - atan(u)); + return coefficients.d_x * sin(coefficients.c_x * atan(phi)) + coefficients.s_vx; +} + +float TireModel::computePureFy_N(const float normal_load_N, const float slip_angle_rad) const +{ + const auto coefficients = pureFyMagicFormulaCoefficients(normal_load_N, slip_angle_rad); + // Pacejka Page 180 (4.E19): F_y0 - const float b_y_alpha_y = pure_fy_mj_coefficients_.b_y * pure_fy_mj_coefficients_.alpha_y; + const float b_y_alpha_y = coefficients.b_y * coefficients.alpha_y; - return pure_fy_mj_coefficients_.d_y * + return coefficients.d_y * std::sin( - pure_fy_mj_coefficients_.c_y * + coefficients.c_y * std::atan( b_y_alpha_y - - (pure_fy_mj_coefficients_.e_y * + (coefficients.e_y * (b_y_alpha_y - std::atan(b_y_alpha_y))))) + - pure_fy_mj_coefficients_.s_vy; + coefficients.s_vy; } -void TireModel::computePureLongitudinalForce(const float normal_load_N) +template +T TireModel::computeCombinedFx_N( + const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const T& slip_ratio) const { - pureFxMagicFormulaCoefficients(normal_load_N); + const T pure_fx_0 = computePureFx_N(normal_load_N, slip_ratio); + const auto coefficients = combinedFxMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); + const T force_blend = T(low_speed_blend); + + // Low-speed safeguard: + // Below a small vehicle-speed threshold the tire model can predict unrealistically large + // forces because the slip calculation becomes ill-conditioned while the fitted Pacejka + // peak terms remain large. Blend the final combined-slip force output to zero so the model + // fades out smoothly instead of producing unstable low-speed force demands. + // + // Pacejka Page 181 (4.E50): F_x = G_xa * F_x0 + return force_blend * (coefficients.g_xa * pure_fx_0); +} - // Use forward-mode autodiff to compute Fx and ∂Fx/∂κ simultaneously. - // All Pacejka coefficients are fixed at this operating point; kappa_x is the only variable. - const auto& c = pure_fx_mj_coefficients_; - autodiff::dual kappa_x = c.kappa_x; +template +T TireModel::computeCombinedFy_N( + const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const T& slip_ratio) const +{ + const float pure_fy_0 = computePureFy_N(normal_load_N, slip_angle_rad); + const auto coefficients = combinedFyMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); + const T force_blend = T(low_speed_blend); + + // Apply the same low-speed force-availability blend used for Fx so the optimizer sees a + // self-consistent pair of tire forces as the vehicle approaches a stop. + // + // Pacejka Pages 181-182 (4.E58): F_y = G_yk * F_y0 + S_vyk + return force_blend * ((coefficients.g_yk * T(pure_fy_0)) + coefficients.s_vyk); +} + +constexpr float TireModel::combinedFx_SHxa() const +{ + // Pacejka Page 181 (4.E57): S_Hxa + return fit_comb_fx_.rHx1; +} + +constexpr float TireModel::combinedFx_Alpha_s(const float slip_angle_rad) const +{ + // Pacejka Page 181 (4.E53): alpha_s = alpha* + S_Hxa + return slip_angle_rad + combinedFx_SHxa(); +} + +constexpr float TireModel::combinedFx_Cxa() const +{ + // Pacejka Page 181 (4.E55): C_xa + return fit_comb_fx_.rCx1; +} + +float TireModel::combinedFx_Exa(const float normalized_load_delta) const +{ + // Pacejka Page 181 (4.E56): E_xa with reduced-model assumptions and E_xa <= 1. + return std::fmin(fit_comb_fx_.rEx1 + (fit_comb_fx_.rEx2 * normalized_load_delta), 1.0f); +} + +template +T TireModel::combinedFx_Bxa(const T& slip_ratio) const +{ + using std::atan; + using std::cos; + + // Pacejka Page 181 (4.E54): B_xa with gamma* = 0 and lambda_xa = 1. + return T(fit_comb_fx_.rBx1) * cos(atan(T(fit_comb_fx_.rBx2) * slip_ratio)); +} + +template +T TireModel::combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients& coefficients) const +{ + using std::atan; + using std::cos; - auto fx_formula = [&](autodiff::dual k) -> autodiff::dual { - autodiff::dual u = c.b_x * k; - autodiff::dual phi = u - c.e_x * (u - atan(u)); - return c.d_x * sin(c.c_x * atan(phi)) + c.s_vx; + // Pacejka Page 181 (4.E52): G_xao + const T u = coefficients.b_xa * coefficients.s_hxa; + const T phi = u - coefficients.e_xa * (u - atan(u)); + return cos(coefficients.c_xa * atan(phi)); +} + +template +T TireModel::combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients& coefficients) const +{ + using std::atan; + using std::cos; + + // Pacejka Page 181 (4.E51): G_xa with G_xao > 0 protected by a signed epsilon denominator. + const T u = coefficients.b_xa * coefficients.alpha_s; + const T phi = u - coefficients.e_xa * (u - atan(u)); + const T numerator = cos(coefficients.c_xa * atan(phi)); + + return numerator / safeTemplateDenominator(coefficients.g_xao); +} + +template +TireModel::CombinedFxMagicFormulaCoefficients TireModel::combinedFxMagicFormulaCoefficients( + const float normal_load_N, const float slip_angle_rad, const T& slip_ratio) const +{ + const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); + const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); + + CombinedFxMagicFormulaCoefficients coefficients{ + .s_hxa = T(combinedFx_SHxa()), + .alpha_s = T(combinedFx_Alpha_s(slip_angle_rad)), + .b_xa = combinedFx_Bxa(slip_ratio), + .c_xa = T(combinedFx_Cxa()), + .e_xa = T(combinedFx_Exa(normalized_load_delta)), + .g_xao = T(0.0f), + .g_xa = T(0.0f), }; - const autodiff::dual fx_result = fx_formula(kappa_x); - const auto dFx_dKappa = autodiff::derivative(fx_formula, autodiff::wrt(kappa_x), autodiff::at(kappa_x)); + coefficients.g_xao = combinedFx_Gxao(coefficients); + coefficients.g_xa = combinedFx_Gxa(coefficients); + return coefficients; +} + +float TireModel::combinedFy_SHyk(const float normalized_load_delta) const +{ + // Pacejka Page 182 (4.E65): S_Hyk + return fit_comb_fy_.rHy1 + (fit_comb_fy_.rHy2 * normalized_load_delta); +} + +template +T TireModel::combinedFy_Kappa_s(const float normalized_load_delta, const T& slip_ratio) const +{ + // Pacejka Page 181 (4.E61): kappa_s = kappa + S_Hyk + return slip_ratio + T(combinedFy_SHyk(normalized_load_delta)); +} + +constexpr float TireModel::combinedFy_Cyk() const +{ + // Pacejka Page 182 (4.E63): C_yk + return fit_comb_fy_.rCy1; +} + +float TireModel::combinedFy_Eyk(const float normalized_load_delta) const +{ + // Pacejka Page 182 (4.E64): E_yk with reduced-model assumptions and E_yk <= 1. + return std::fmin(fit_comb_fy_.rEy1 + (fit_comb_fy_.rEy2 * normalized_load_delta), 1.0f); +} + +float TireModel::combinedFy_Byk(const float slip_angle_rad) const +{ + using std::atan; + using std::cos; - tire_outputs_.longitudinal_force_N = static_cast(autodiff::val(fx_result)); - tire_outputs_.dFx_dKappa = static_cast(dFx_dKappa); + // Pacejka Page 182 (4.E62): B_yk with gamma* = 0 and lambda_yk = 1. + return fit_comb_fy_.rBy1 * cos(atan(fit_comb_fy_.rBy2 * (slip_angle_rad - fit_comb_fy_.rBy3))); } -void TireModel::computePureLateralForce(const float normal_load_N) +float TireModel::combinedFy_Dvyk( + const float normal_load_N, const float normalized_load_delta, const float slip_angle_rad) const { - pureFyMagicFormulaCoefficients(normal_load_N); - tire_outputs_.lateral_force_N = pure_Fy(); + using std::atan; + using std::cos; + + // Pacejka Page 182 (4.E67): D_vyk with gamma* = 0 and zeta_2 = 1. + return pureFy_mu(normalized_load_delta) * normal_load_N * + (fit_comb_fy_.rVy1 + (fit_comb_fy_.rVy2 * normalized_load_delta)) * + cos(atan(fit_comb_fy_.rVy4 * slip_angle_rad)); +} + +template +T TireModel::combinedFy_Svyk(const T& d_vyk, const T& slip_ratio) const +{ + using std::atan; + using std::sin; + + // Pacejka Page 182 (4.E66): S_vyk with lambda_vyk = 1. + return d_vyk * sin(T(fit_comb_fy_.rVy5) * atan(T(fit_comb_fy_.rVy6) * slip_ratio)); +} + +template +T TireModel::combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients& coefficients) const +{ + using std::atan; + using std::cos; + + // Pacejka Page 181 (4.E60): G_yko + const T u = coefficients.b_yk * coefficients.s_hyk; + const T phi = u - coefficients.e_yk * (u - atan(u)); + return cos(coefficients.c_yk * atan(phi)); +} + +template +T TireModel::combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients& coefficients) const +{ + using std::atan; + using std::cos; + + // Pacejka Page 181 (4.E59): G_yk with G_yko > 0 protected by a signed epsilon denominator. + const T u = coefficients.b_yk * coefficients.kappa_s; + const T phi = u - coefficients.e_yk * (u - atan(u)); + const T numerator = cos(coefficients.c_yk * atan(phi)); + + return numerator / safeTemplateDenominator(coefficients.g_yko); +} + +template +TireModel::CombinedFyMagicFormulaCoefficients TireModel::combinedFyMagicFormulaCoefficients( + const float normal_load_N, const float slip_angle_rad, const T& slip_ratio) const +{ + const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); + const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); + + CombinedFyMagicFormulaCoefficients coefficients{ + .s_hyk = T(combinedFy_SHyk(normalized_load_delta)), + .kappa_s = combinedFy_Kappa_s(normalized_load_delta, slip_ratio), + .b_yk = T(combinedFy_Byk(slip_angle_rad)), + .c_yk = T(combinedFy_Cyk()), + .e_yk = T(combinedFy_Eyk(normalized_load_delta)), + .d_vyk = T(combinedFy_Dvyk(clamped_normal_load_N, normalized_load_delta, slip_angle_rad)), + .s_vyk = T(0.0f), + .g_yko = T(0.0f), + .g_yk = T(0.0f), + }; + + coefficients.s_vyk = combinedFy_Svyk(coefficients.d_vyk, slip_ratio); + coefficients.g_yko = combinedFy_Gyko(coefficients); + coefficients.g_yk = combinedFy_Gyk(coefficients); + return coefficients; } @@ -212,25 +462,26 @@ constexpr float TireModel::normalizedLoadDelta(const float normal_load_N) constexpr float TireModel::pureFx_Sh(const float normalized_load_delta) const { // Pacejka Page 179 (4.E17): S_Hx - return coeff_fx_.hx_1 + (coeff_fx_.hx_2 * normalized_load_delta); + return fit_pure_fx_.hx_1 + (fit_pure_fx_.hx_2 * normalized_load_delta); } -constexpr float TireModel::pureFx_Kappa(const float normalized_load_delta, const float slip_ratio) const +template +T TireModel::pureFx_Kappa(const float normalized_load_delta, const T& slip_ratio) const { // Pacejka Page 179 (4.E10): kappa_x = kappa + S_Hx - return slip_ratio + pureFx_Sh(normalized_load_delta); + return slip_ratio + T(pureFx_Sh(normalized_load_delta)); } constexpr float TireModel::pureFx_C() const { // Pacejka Page 179 (4.E11): C_x - return coeff_fx_.cx_1; + return fit_pure_fx_.cx_1; } constexpr float TireModel::pureFx_mu(const float normalized_load_delta) const { // Pacejka Page 179 (4.E13): mu_x with pressure/camber terms reduced to the current assumptions. - return coeff_fx_.dx_1 + (coeff_fx_.dx_2 * normalized_load_delta); + return fit_pure_fx_.dx_1 + (fit_pure_fx_.dx_2 * normalized_load_delta); } constexpr float TireModel::pureFx_D(const float normal_load_N, const float normalized_load_delta) const @@ -239,20 +490,23 @@ constexpr float TireModel::pureFx_D(const float normal_load_N, const float norma return pureFx_mu(normalized_load_delta) * normal_load_N; } -constexpr float TireModel::pureFx_E(const float normalized_load_delta, const float kappa_x) const +template +T TireModel::pureFx_E(const float normalized_load_delta, const T& kappa_x) const { // Pacejka Page 179 (4.E14): E_x const float normalized_load_delta_squared = normalized_load_delta * normalized_load_delta; - return (coeff_fx_.ex_1 + (coeff_fx_.ex_2 * normalized_load_delta) + - (coeff_fx_.ex_3 * normalized_load_delta_squared)) * - (1.0f - (coeff_fx_.ex_4 * signum(kappa_x))); + const float base_e_x = fit_pure_fx_.ex_1 + (fit_pure_fx_.ex_2 * normalized_load_delta) + + (fit_pure_fx_.ex_3 * normalized_load_delta_squared); + const float kappa_x_value = primalValue(kappa_x); + + return T(base_e_x * (1.0f - (fit_pure_fx_.ex_4 * signum(kappa_x_value)))); } constexpr float TireModel::pureFx_K(const float normal_load_N, const float normalized_load_delta) const { // Pacejka Page 179 (4.E15): K_xk - return normal_load_N * (coeff_fx_.kx_1 + (coeff_fx_.kx_2 * normalized_load_delta)) * - std::exp(coeff_fx_.kx_3 * normalized_load_delta); + return normal_load_N * (fit_pure_fx_.kx_1 + (fit_pure_fx_.kx_2 * normalized_load_delta)) * + std::exp(fit_pure_fx_.kx_3 * normalized_load_delta); } constexpr float TireModel::pureFx_B( @@ -265,13 +519,13 @@ constexpr float TireModel::pureFx_B( constexpr float TireModel::pureFx_Sv(const float normal_load_N, const float normalized_load_delta) const { // Pacejka Page 179 (4.E18): S_Vx - return normal_load_N * (coeff_fx_.vx_1 + (coeff_fx_.vx_2 * normalized_load_delta)); + return normal_load_N * (fit_pure_fx_.vx_1 + (fit_pure_fx_.vx_2 * normalized_load_delta)); } constexpr float TireModel::pureFy_Sh(const float normalized_load_delta) const { // Pacejka Page 180 (4.E27): S_Hy with gamma terms reduced to zero. - return coeff_fy_.hy_1 + (coeff_fy_.hy_2 * normalized_load_delta); + return fit_pure_fy_.hy_1 + (fit_pure_fy_.hy_2 * normalized_load_delta); } constexpr float TireModel::pureFy_Alpha(const float normalized_load_delta, const float slip_angle_rad) const @@ -283,13 +537,13 @@ constexpr float TireModel::pureFy_Alpha(const float normalized_load_delta, const constexpr float TireModel::pureFy_C() const { // Pacejka Page 180 (4.E21): C_y - return coeff_fy_.cy_1; + return fit_pure_fy_.cy_1; } constexpr float TireModel::pureFy_mu(const float normalized_load_delta) const { // Pacejka Page 180 (4.E23): mu_y with pressure/camber terms reduced to the current assumptions. - return coeff_fy_.dy_1 + (coeff_fy_.dy_2 * normalized_load_delta); + return fit_pure_fy_.dy_1 + (fit_pure_fy_.dy_2 * normalized_load_delta); } constexpr float TireModel::pureFy_D(const float normal_load_N, const float normalized_load_delta) const @@ -301,15 +555,15 @@ constexpr float TireModel::pureFy_D(const float normal_load_N, const float norma constexpr float TireModel::pureFy_E(const float normalized_load_delta, const float alpha_y) const { // Pacejka Page 180 (4.E24): E_y with gamma terms reduced to zero. - return (coeff_fy_.ey_1 + (coeff_fy_.ey_2 * normalized_load_delta)) * - (1.0f - (coeff_fy_.ey_3 * signum(alpha_y))); + return (fit_pure_fy_.ey_1 + (fit_pure_fy_.ey_2 * normalized_load_delta)) * + (1.0f - (fit_pure_fy_.ey_3 * signum(alpha_y))); } constexpr float TireModel::pureFy_K(const float normal_load_N) const { // Pacejka Page 180 (4.E25): K_yalpha with gamma/pressure terms reduced to zero. - const float denominator = safeSignedDenominator(coeff_fy_.ky_2 * NOMINAL_FZ_N); - return coeff_fy_.ky_1 * NOMINAL_FZ_N * + const float denominator = safeSignedDenominator(fit_pure_fy_.ky_2 * NOMINAL_FZ_N); + return fit_pure_fy_.ky_1 * NOMINAL_FZ_N * std::sin(2.0f * std::atan(normal_load_N / denominator)); } @@ -323,7 +577,16 @@ constexpr float TireModel::pureFy_B( constexpr float TireModel::pureFy_Sv(const float normal_load_N, const float normalized_load_delta) const { // Pacejka Page 180 (4.E29): S_Vy with gamma terms reduced to zero. - return normal_load_N * (coeff_fy_.vy_1 + (coeff_fy_.vy_2 * normalized_load_delta)); + return normal_load_N * (fit_pure_fy_.vy_1 + (fit_pure_fy_.vy_2 * normalized_load_delta)); } +template float TireModel::computeCombinedFx_N( + const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const float& slip_ratio) const; +template autodiff::dual TireModel::computeCombinedFx_N( + const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const autodiff::dual& slip_ratio) const; +template float TireModel::computeCombinedFy_N( + const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const float& slip_ratio) const; +template autodiff::dual TireModel::computeCombinedFy_N( + const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const autodiff::dual& slip_ratio) const; + } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index 931c0c5de2..fc4697c35b 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -4,6 +4,7 @@ #include "torque_vectoring/shared_datatypes/constants.hpp" #include "torque_vectoring/shared_datatypes/datatypes.hpp" +#include "torque_vectoring/shared_datatypes/low_speed_blend.hpp" namespace app::tv::estimation { @@ -22,12 +23,6 @@ class TireModel Rear }; - enum class TirePressure - { - PSI_10, - PSI_12 - }; - struct StateInputs { float wheel_angular_velocity_radps = 0.0f; @@ -44,18 +39,25 @@ class TireModel float slip_angle_rad = 0.0f; float longitudinal_force_N = 0.0f; float lateral_force_N = 0.0f; - float dFx_dKappa = 0.0f; // ∂Fx/∂κ — sensitivity of longitudinal force to slip ratio }; - constexpr TireModel(const TirePressure tire_pressure, const WheelSide wheel_side, const WheelAxle wheel_axle) - : coeff_fx_(selectFxCoefficients(tire_pressure)), - coeff_fy_(selectFyCoefficients(tire_pressure)), + constexpr TireModel(const WheelSide wheel_side, const WheelAxle wheel_axle) + : fit_pure_fx_(FIT_PURE_FX_12_PSI), + fit_pure_fy_(FIT_PURE_FY_12_PSI), + fit_comb_fx_(FIT_COMB_FX_12_PSI), + fit_comb_fy_(FIT_COMB_FY_12_PSI), wheel_side_(wheel_side), wheel_axle_(wheel_axle) { } - [[nodiscard]] Outputs estimate(const StateInputs& inputs); + [[nodiscard]] Outputs estimate(const StateInputs& inputs) const; + template + [[nodiscard]] T computeCombinedFx_N( + float normal_load_N, float slip_angle_rad, float low_speed_blend, const T& slip_ratio) const; + template + [[nodiscard]] T computeCombinedFy_N( + float normal_load_N, float slip_angle_rad, float low_speed_blend, const T& slip_ratio) const; [[nodiscard]] float slipRatioToWheelAngularVelocity(const float slip_ratio, const float wheel_vel_x_mps) const; [[nodiscard]] float slipRatioToWheelAngularVelocity( const float slip_ratio, const datatypes::datatypes::VehicleState& vehicle_state) const; @@ -67,7 +69,7 @@ class TireModel float y_mps = 0.0f; }; - struct TireCoefficents_Fy + struct TireFitPureParamFy { float cy_1; float dy_1; @@ -89,7 +91,7 @@ class TireModel float vy_4; }; - struct TireCoefficents_Fx + struct TireFitPureParamFx { float cx_1; float dx_1; @@ -108,49 +110,91 @@ class TireModel float vx_2; }; + struct TireFitCombParamFx + { + float rBx1; + float rBx2; + float rBx3; + float rCx1; + float rEx1; + float rEx2; + float rHx1; + }; + + struct TireFitCombParamFy + { + float rBy1; + float rBy2; + float rBy3; + float rBy4; + float rCy1; + float rEy1; + float rEy2; + float rHy1; + float rHy2; + float rVy1; + float rVy2; + float rVy3; + float rVy4; + float rVy5; + float rVy6; + }; + + template struct PureFxMagicFormulaCoefficients { - float kappa_x = 0.0f; - float c_x = 0.0f; - float d_x = 0.0f; - float b_x = 0.0f; - float e_x = 0.0f; - float s_vx = 0.0f; + T s_hx = T(0.0f); + T kappa_x = T(0.0f); + T b_x = T(0.0f); + T c_x = T(0.0f); + T d_x = T(0.0f); + T e_x = T(0.0f); + T s_vx = T(0.0f); + }; + + template + struct CombinedFxMagicFormulaCoefficients + { + T s_hxa = T(0.0f); + T alpha_s = T(0.0f); + T b_xa = T(0.0f); + T c_xa = T(0.0f); + T e_xa = T(0.0f); + T g_xao = T(0.0f); // letter o, not zero + T g_xa = T(0.0f); }; struct PureFyMagicFormulaCoefficients { + float s_hy = 0.0f; float alpha_y = 0.0f; + float b_y = 0.0f; float c_y = 0.0f; float d_y = 0.0f; - float b_y = 0.0f; float e_y = 0.0f; float s_vy = 0.0f; }; - /* - These are discrete pressure snapshots from the coefficient workbook. If pressure becomes a continuous input later, - replace this with interpolation or a refit against the raw tire data. - */ - static constexpr TireCoefficents_Fx Fx_10_PSI = { - 0.01284039486f, - 262.5114145f, - -41.08996459f, - 8.15087112f, - 1.175111664f, - 0.04701116929f, - -0.009561725595f, - -0.01302948897f, - 62.43954722f, - -0.05063343595f, - -0.3634867697f, - -0.0003767549902f, - 0.0008216827314f, - -0.002877423451f, - -0.01695834766f, + template + struct CombinedFyMagicFormulaCoefficients + { + T s_hyk = T(0.0f); + T kappa_s = T(0.0f); + T b_yk = T(0.0f); + T c_yk = T(0.0f); + T e_yk = T(0.0f); + T d_vyk = T(0.0f); + T s_vyk = T(0.0f); + T g_yko = T(0.0f); + T g_yk = T(0.0f); }; - static constexpr TireCoefficents_Fx Fx_12_PSI = { + + /* + Current model uses the 12_PSI fitted workbook row as a fixed-pressure operating assumption. + If pressure becomes a runtime input later, add pressure interpolation or a refit against the raw tire data. + */ + static constexpr TireFitPureParamFx FIT_PURE_FX_12_PSI = { 0.008656294312f, 366.1925838f, -57.2826791f, @@ -168,28 +212,7 @@ class TireModel 0.004135955934f, }; - static constexpr TireCoefficents_Fy Fy_10_PSI = { - 1.652843597f, - -2.803116154f, - -0.2320399036f, - 17.97429428f, - 1.450330291f, - 0.5012143046f, - -0.02726754434f, - -0.7645474843f, - -52.17053158f, - 1.886718468f, - 1.530410198f, - -0.003839066017f, - -0.003970614639f, - -0.1017943816f, - -0.08574689113f, - -0.02575321928f, - 0.5309737209f, - 1.493197148f, - }; - - static constexpr TireCoefficents_Fy Fy_12_PSI = { + static constexpr TireFitPureParamFy FIT_PURE_FY_12_PSI = { 0.90721342f, -3.824114982f, -0.4280518359f, @@ -210,28 +233,54 @@ class TireModel 1.494231303f, }; - static constexpr float NOMINAL_FZ_N = 750.0f; - static constexpr float WHEEL_RADIUS_M = (WHEEL_DIAMETER_IN * IN_TO_M) * 0.5f; - - [[nodiscard]] static constexpr TireCoefficents_Fx selectFxCoefficients(const TirePressure tire_pressure) + static constexpr TireFitCombParamFx FIT_COMB_FX_12_PSI = { - return tire_pressure == TirePressure::PSI_10 ? Fx_10_PSI : Fx_12_PSI; - } + .rBx1 = 13.046f, + .rBx2 = 9.718f, + .rBx3 = 0.0f, + .rCx1 = 0.9995f, + .rEx1 = -0.4403f, + .rEx2 = -0.4663f, + .rHx1 = -0.0001f, + }; - [[nodiscard]] static constexpr TireCoefficents_Fy selectFyCoefficients(const TirePressure tire_pressure) + static constexpr TireFitCombParamFy FIT_COMB_FY_12_PSI = { - return tire_pressure == TirePressure::PSI_10 ? Fy_10_PSI : Fy_12_PSI; - } + .rBy1 = 10.622f, + .rBy2 = 7.82f, + .rBy3 = 0.00204f, + .rBy4 = 0.0f, + .rCy1 = 1.0587f, + .rEy1 = 0.3148f, + .rEy2 = 0.00487f, + .rHy1 = 0.00947f, + .rHy2 = 0.00975f, + .rVy1 = 0.05187f, + .rVy2 = 0.00049f, + .rVy3 = 0.0f, + .rVy4 = 94.63f, + .rVy5 = 1.8914f, + .rVy6 = 23.8f, + }; + + static constexpr float NOMINAL_FZ_N = 750.0f; + //-------------------------------------------------------------------- Class Helpers ----------------------------------------------------------------------// [[nodiscard]] static constexpr float safeSignedDenominator(const float value); [[nodiscard]] static constexpr float signum(const float value); [[nodiscard]] static constexpr float normalizedLoadDelta(const float normal_load_N); + // Reduced-model assumptions for combined slip in this pass: + // gamma* = 0, lambda_xa = 1, lambda_yk = 1, lambda_vyk = 1, zeta_2 = 1. + // Pressure dependence is captured by the fixed 12_PSI fitted parameter row. + //-------------------------------------------------------------------- Pure Pacejka 5.2 Helpers ----------------------------------------------------------------------// [[nodiscard]] constexpr float pureFx_Sh(const float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFx_Kappa(const float normalized_load_delta, const float slip_ratio) const; + template + [[nodiscard]] T pureFx_Kappa(const float normalized_load_delta, const T& slip_ratio) const; [[nodiscard]] constexpr float pureFx_C() const; [[nodiscard]] constexpr float pureFx_mu(const float normalized_load_delta) const; [[nodiscard]] constexpr float pureFx_D(const float normal_load_N, const float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFx_E(const float normalized_load_delta, const float kappa_x) const; + template + [[nodiscard]] T pureFx_E(const float normalized_load_delta, const T& kappa_x) const; [[nodiscard]] constexpr float pureFx_K(const float normal_load_N, const float normalized_load_delta) const; [[nodiscard]] static constexpr float pureFx_B( const float slip_stiffness, const float shape_factor, const float peak_factor); @@ -246,28 +295,60 @@ class TireModel [[nodiscard]] static constexpr float pureFy_B( const float cornering_stiffness, const float shape_factor, const float peak_factor); [[nodiscard]] constexpr float pureFy_Sv(const float normal_load_N, const float normalized_load_delta) const; - void pureFxMagicFormulaCoefficients(const float normal_load_N); - void pureFyMagicFormulaCoefficients(const float normal_load_N); - [[nodiscard]] constexpr float pure_Fy() const; + template + [[nodiscard]] PureFxMagicFormulaCoefficients pureFxMagicFormulaCoefficients( + float normal_load_N, const T& slip_ratio) const; + [[nodiscard]] PureFyMagicFormulaCoefficients pureFyMagicFormulaCoefficients( + float normal_load_N, float slip_angle_rad) const; + //-------------------------------------------------------------------- Combined Pacejka 5.2 Helpers ----------------------------------------------------------------------// + [[nodiscard]] constexpr float combinedFx_SHxa() const; + [[nodiscard]] constexpr float combinedFx_Alpha_s(float slip_angle_rad) const; + [[nodiscard]] constexpr float combinedFx_Cxa() const; + [[nodiscard]] float combinedFx_Exa(float normalized_load_delta) const; + template + [[nodiscard]] T combinedFx_Bxa(const T& slip_ratio) const; + template + [[nodiscard]] T combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients& coefficients) const; + template + [[nodiscard]] T combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients& coefficients) const; + template + [[nodiscard]] CombinedFxMagicFormulaCoefficients combinedFxMagicFormulaCoefficients( + float normal_load_N, float slip_angle_rad, const T& slip_ratio) const; + [[nodiscard]] float combinedFy_SHyk(float normalized_load_delta) const; + template + [[nodiscard]] T combinedFy_Kappa_s(float normalized_load_delta, const T& slip_ratio) const; + [[nodiscard]] constexpr float combinedFy_Cyk() const; + [[nodiscard]] float combinedFy_Eyk(float normalized_load_delta) const; + [[nodiscard]] float combinedFy_Byk(float slip_angle_rad) const; + [[nodiscard]] float combinedFy_Dvyk(float normal_load_N, float normalized_load_delta, float slip_angle_rad) const; + template + [[nodiscard]] T combinedFy_Svyk(const T& d_vyk, const T& slip_ratio) const; + template + [[nodiscard]] T combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients& coefficients) const; + template + [[nodiscard]] T combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients& coefficients) const; + template + [[nodiscard]] CombinedFyMagicFormulaCoefficients combinedFyMagicFormulaCoefficients( + float normal_load_N, float slip_angle_rad, const T& slip_ratio) const; [[nodiscard]] float wheelLongOffset_m() const; [[nodiscard]] float wheelLatOffset_m() const; [[nodiscard]] float wheelSteeringAngle_rad(const float steering_angle_rad) const; [[nodiscard]] WheelVelocities wheelVelocities( float vehicle_velocity_x_mps, float vehicle_velocity_y_mps, float yaw_rate_radps) const; - void estimateSlipAngle(float steering_angle_rad); - void estimateSlipRatio(const float wheel_angular_velocity_radps); - void computePureLongitudinalForce(const float normal_load_N); - void computePureLateralForce(const float normal_load_N); + [[nodiscard]] float estimateSlipAngle( + float wheel_vel_x_mps, float wheel_vel_y_mps, float steering_angle_rad) const; + [[nodiscard]] float estimateSlipRatio( + float wheel_vel_x_mps, float wheel_vel_y_mps, float slip_angle_rad, float wheel_angular_velocity_radps) const; + template + [[nodiscard]] T computePureFx_N(float normal_load_N, const T& slip_ratio) const; + [[nodiscard]] float computePureFy_N(float normal_load_N, float slip_angle_rad) const; - const TireCoefficents_Fx coeff_fx_; - const TireCoefficents_Fy coeff_fy_; + const TireFitPureParamFx fit_pure_fx_; + const TireFitPureParamFy fit_pure_fy_; + const TireFitCombParamFx fit_comb_fx_; + const TireFitCombParamFy fit_comb_fy_; const WheelSide wheel_side_; const WheelAxle wheel_axle_; - PureFxMagicFormulaCoefficients pure_fx_mj_coefficients_{}; - PureFyMagicFormulaCoefficients pure_fy_mj_coefficients_{}; - float wheel_vel_x_mps_ = 0.0f; - float wheel_vel_y_mps_ = 0.0f; - Outputs tire_outputs_{}; }; } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp index 9397693bf4..00b824594c 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp @@ -5,7 +5,7 @@ #include "torque_vectoring/shared_datatypes/constants.hpp" -using namespace app::tv::datatypes::vd_constants; +using namespace app::tv::shared_datatypes::vd_constants; namespace app::tv::estimation { @@ -161,7 +161,7 @@ static constexpr float ESTIMATOR_YAW_INERTIA = 110.0f; // TODO: Replace with me return inputs.measured_state.steer_ang_rad; } -[[nodiscard]] datatypes::datatypes::wheel_set readWheelAngularVelocitiesRadps( +[[nodiscard]] datatypes::datatypes::wheel_set readWheelAngularVelocitiesRadps( const VehicleStateEstimator::Inputs& inputs) { // TODO: Replace with app_canRx_* when the Hexray signal table is ready. @@ -169,7 +169,7 @@ static constexpr float ESTIMATOR_YAW_INERTIA = 110.0f; // TODO: Replace with me } [[nodiscard]] Measurement pseudoMeasurementFromWheelSpeeds( - const datatypes::datatypes::wheel_set& wheel_angular_velocities_radps, + const datatypes::datatypes::wheel_set& wheel_angular_velocities_radps, const float yaw_rate_radps, const float steering_angle_rad, const StateVector& previous_state) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp index fc83c4f1e5..faf36df3d1 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp @@ -14,9 +14,9 @@ class VehicleStateEstimator struct Inputs { datatypes::datatypes::VehicleState measured_state{}; - datatypes::datatypes::wheel_set wheel_angular_velocities_radps{}; - datatypes::datatypes::wheel_set longitudinal_forces_N{}; - datatypes::datatypes::wheel_set lateral_forces_N{}; + datatypes::datatypes::wheel_set wheel_angular_velocities_radps{}; + datatypes::datatypes::wheel_set longitudinal_forces_N{}; + datatypes::datatypes::wheel_set lateral_forces_N{}; }; struct Outputs diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index 9f024e2101..1da9e65fa6 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -2,7 +2,7 @@ #include "util_units.hpp" -namespace app::tv::datatypes::vd_constants +namespace app::tv::shared_datatypes::vd_constants { // ============================================================================= @@ -135,4 +135,9 @@ inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = // extern const PID_Config PID_TRACTION_CONTROL_CONFIG; // extern const PID_Config PID_YAW_RATE_CONTROLLER_CONFIG; // extern const YawRateController_Config YAW_RATE_CONTROLLER_CONFIG; -} // namespace app::tv::datatypes::vd_constants \ No newline at end of file +} // namespace app::tv::shared_datatypes::vd_constants + +namespace app::tv::datatypes +{ +namespace vd_constants = app::tv::shared_datatypes::vd_constants; +} diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 324bfcbc96..28768d5c37 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -1,13 +1,15 @@ #pragma once -namespace app::tv::datatypes::datatypes +namespace app::tv::shared_datatypes::datatypes { + +template struct wheel_set { - float fl = 0.0f; - float fr = 0.0f; - float rl = 0.0f; - float rr = 0.0f; + T fl; + T fr; + T rl; + T rr; }; struct VehicleState @@ -19,4 +21,10 @@ struct VehicleState float a_x_mps2 = 0.0f; float a_y_mps2 = 0.0f; }; -} // namespace app::tv::datatypes::datatypes + +} // namespace app::tv::shared_datatypes::datatypes + +namespace app::tv::datatypes +{ +namespace datatypes = app::tv::shared_datatypes::datatypes; +} diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/low_speed_blend.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/low_speed_blend.hpp new file mode 100644 index 0000000000..cf9834e514 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/low_speed_blend.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include + +namespace app::tv::shared_datatypes +{ + +// Below this speed, tire-force-based optimization is not meaningful because the wheel-speed/slip +// calculation becomes numerically fragile and the tire model can predict unrealistically large forces. +static constexpr float SPEED_MIN_MPS = 0.5f; + +// Above this speed, the low-speed safeguard is fully inactive and the nominal model is used. +static constexpr float SPEED_MAX_MPS = 3.0f; + +// Returns 0.0 at or below SPEED_MIN_MPS, 1.0 at or above SPEED_MAX_MPS, +// and linearly blends between them in the transition region. +[[nodiscard]] inline float velocityBlend(const float vehicle_speed_mps) +{ + if (vehicle_speed_mps <= SPEED_MIN_MPS) + return 0.0f; + + if (vehicle_speed_mps >= SPEED_MAX_MPS) + return 1.0f; + + return std::clamp( + (vehicle_speed_mps - SPEED_MIN_MPS) / (SPEED_MAX_MPS - SPEED_MIN_MPS), + 0.0f, + 1.0f); +} + +} // namespace app::tv::shared_datatypes diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index 974219a530..bd3ff211d8 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -5,6 +5,7 @@ #include "torque_vectoring/controllers/torque_allocator/torque_allocator.hpp" #include "torque_vectoring/shared_datatypes/constants.hpp" #include "torque_vectoring/shared_datatypes/datatypes.hpp" +#include "torque_vectoring/shared_datatypes/low_speed_blend.hpp" #include @@ -12,33 +13,19 @@ namespace app::tv { namespace { -using namespace datatypes::datatypes; -using namespace datatypes::vd_constants; +using namespace shared_datatypes::datatypes; +using namespace shared_datatypes::vd_constants; //------------------------------------- ESTIMATION MODULES ----------------------------------// static estimation::vehicleDynamics vehicle_dynamics_estimator{}; static estimation::VehicleStateEstimator vehicle_state_estimator{}; - -static estimation::TireModel fl_tire_model{ - estimation::TireModel::TirePressure::PSI_12, - estimation::TireModel::WheelSide::Left, - estimation::TireModel::WheelAxle::Front}; - -static estimation::TireModel fr_tire_model{ - estimation::TireModel::TirePressure::PSI_12, - estimation::TireModel::WheelSide::Right, - estimation::TireModel::WheelAxle::Front}; - -static estimation::TireModel rl_tire_model{ - estimation::TireModel::TirePressure::PSI_12, - estimation::TireModel::WheelSide::Left, - estimation::TireModel::WheelAxle::Rear}; - -static estimation::TireModel rr_tire_model{ - estimation::TireModel::TirePressure::PSI_12, - estimation::TireModel::WheelSide::Right, - estimation::TireModel::WheelAxle::Rear}; +static wheel_set tire_models{ + .fl = estimation::TireModel(estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Front), + .fr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Front), + .rl = estimation::TireModel(estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Rear), + .rr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Rear), +}; //------------------------------------- CONTROLLERS -----------------------------------------// @@ -46,13 +33,14 @@ static controllers::allocator::TorqueAllocator torque_allocator{}; //------------------------------------- STATE VARIABLES -------------------------------------// -static wheel_set acc_f_x{}; -static wheel_set acc_f_y{}; -static wheel_set acc_f_z{}; -static wheel_set acc_slip_ratio{}; -static wheel_set acc_slip_angle{}; -static wheel_set slip_ratio_opt{}; -[[maybe_unused]] static float est_yaw_moment_nm{}; +// These wheel-set caches carry the previous update's estimated tire state/forces into the next cycle. +// The state estimator and optimizer both consume them as warm starts / feedback, so they remain module-local. +static wheel_set acc_f_x{}; +static wheel_set acc_f_y{}; +static wheel_set acc_f_z{}; +static wheel_set acc_slip_ratio{}; +static wheel_set acc_slip_angle{}; +static wheel_set slip_ratio_opt{}; // Build TireModel::StateInputs from VehicleState + per-wheel data [[nodiscard]] estimation::TireModel::StateInputs buildTireInputs( @@ -68,7 +56,8 @@ static wheel_set slip_ratio_opt{}; }; } -// Unpack four tire model outputs into the module-level wheel_set variables +// Copy the current tire estimates into the module-local wheel-set caches so the rest of the +// pipeline can use a consistent per-wheel state snapshot for this update tick. void unpackTireOutputs( const estimation::TireModel::Outputs& fl, const estimation::TireModel::Outputs& fr, @@ -94,6 +83,8 @@ void update(const VehicleState& state, const float pedal_percentage, const float rl_omega, const float rr_omega) { //------------------------------------- STATE ESTIMATION --------------------------------// + // Fuse the measured chassis state with the previous tire-force estimate to obtain the + // filtered vehicle state used consistently by all downstream control blocks this tick. const auto state_estimate = vehicle_state_estimator.estimate({ .measured_state = state, @@ -103,27 +94,24 @@ void update(const VehicleState& state, const float pedal_percentage, }); const VehicleState estimated_state = state_estimate.vehicle_state; - est_yaw_moment_nm = state_estimate.yaw_moment_nm; // Normal forces from longitudinal/lateral load transfer + downforce acc_f_z = vehicle_dynamics_estimator.estimateNormalForce_N( estimated_state.a_x_mps2, estimated_state.a_y_mps2, estimated_state.v_x_mps); // Tire model estimation from current sensor data using the filtered vehicle state. - const auto fl_out = fl_tire_model.estimate(buildTireInputs(estimated_state, fl_omega, acc_f_z.fl)); - const auto fr_out = fr_tire_model.estimate(buildTireInputs(estimated_state, fr_omega, acc_f_z.fr)); - const auto rl_out = rl_tire_model.estimate(buildTireInputs(estimated_state, rl_omega, acc_f_z.rl)); - const auto rr_out = rr_tire_model.estimate(buildTireInputs(estimated_state, rr_omega, acc_f_z.rr)); + const auto fl_out = tire_models.fl.estimate(buildTireInputs(estimated_state, fl_omega, acc_f_z.fl)); + const auto fr_out = tire_models.fr.estimate(buildTireInputs(estimated_state, fr_omega, acc_f_z.fr)); + const auto rl_out = tire_models.rl.estimate(buildTireInputs(estimated_state, rl_omega, acc_f_z.rl)); + const auto rr_out = tire_models.rr.estimate(buildTireInputs(estimated_state, rr_omega, acc_f_z.rr)); unpackTireOutputs(fl_out, fr_out, rl_out, rr_out); - const float acc_yaw_moment_nm = vehicle_dynamics_estimator.estimateYawMoment_Nm( - acc_f_x, acc_f_y, estimated_state.steer_ang_rad); - //------------------------------------- HIGH LEVEL CONTROLLER ----------------------------// - // Desired per-wheel longitudinal force from pedal request, projected along tire heading + // Convert the driver's pedal request into a nominal per-wheel longitudinal force target. + // The cosine projection keeps the request aligned with the tire heading under nonzero slip angle. const float per_wheel_tq = MAX_TORQUE_REQUEST_NM * pedal_percentage; - wheel_set des_f_x = { + wheel_set des_f_x = { .fl = per_wheel_tq * std::cos(acc_slip_angle.fl) * WHEEL_RADIUS_M , .fr = per_wheel_tq * std::cos(acc_slip_angle.fr) * WHEEL_RADIUS_M, .rl = per_wheel_tq * std::cos(acc_slip_angle.rl) * WHEEL_RADIUS_M, @@ -144,9 +132,23 @@ void update(const VehicleState& state, const float pedal_percentage, //------------------------------------- LOW LEVEL CONTROLLER -----------------------------// - // Gauss-Newton optimizer: find slip ratios that minimize combined force + moment tracking error + // Compute the low-speed blend once at the orchestration layer and pass it down explicitly. + // This keeps the low-speed force-availability heuristic visible in one place instead of + // recomputing it independently inside the optimizer. + const float vehicle_speed_mps = std::hypot(estimated_state.v_x_mps, estimated_state.v_y_mps); + const float low_speed_blend = shared_datatypes::velocityBlend(vehicle_speed_mps); + + // Gauss-Newton optimizer: invert the combined-slip tire model to find the slip ratios whose + // predicted forces best match the desired per-wheel force split and desired yaw moment. slip_ratio_opt = torque_allocator.optimize( - des_f_x, acc_f_z, acc_slip_ratio, estimated_state, acc_yaw_moment_nm, des_yaw_moment_nm); + tire_models, + des_f_x, + acc_f_z, + acc_slip_ratio, + acc_slip_angle, + low_speed_blend, + estimated_state.steer_ang_rad, + des_yaw_moment_nm); //------------------------------------- POWER LIMITER -----------------------------------// From 9bcb1ba831f0d63a8f9bf3b70f4897e7f28e86bf Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Wed, 8 Apr 2026 16:32:44 -0700 Subject: [PATCH 05/35] fahh --- .../estimation/dynamics_estimation.cpp | 101 +++----- .../estimation/dynamics_estimation.hpp | 146 +++++------ .../estimation/tire_model.cpp | 226 +++++++++--------- .../estimation/tire_model.hpp | 225 ++++++++--------- .../estimation/vehicle_state_estimator.cpp | 2 +- .../estimation/vehicle_state_estimator.hpp | 22 +- .../shared_datatypes/constants.hpp | 25 +- .../shared_datatypes/datatypes.hpp | 21 +- .../app/torque_vectoring/torque_vectoring.cpp | 164 ++++++------- .../app/torque_vectoring/torque_vectoring.hpp | 4 + 10 files changed, 435 insertions(+), 501 deletions(-) create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp index 62bc91a2f9..e7f4a43c14 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp @@ -8,98 +8,69 @@ namespace app::tv::estimation { namespace { -[[nodiscard]] float safeLongitudinalVelocity(const float v_x_mps) -{ - if (std::fabs(v_x_mps) >= SMALL_EPSILON) + [[nodiscard]] float safe_vx(const float v_x_mps) { - return v_x_mps; - } + if (std::fabs(v_x_mps) >= SMALL_EPSILON) + { + return v_x_mps; + } - return v_x_mps < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; -} + return v_x_mps < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; + } } // namespace -float vehicleDynamics::estimateBodySlip(float v_x_mps, float v_y_mps) const +float vehicleDynamics::est_beta_rad(const float v_x_mps, const float v_y_mps) { - return std::atan2(v_y_mps, safeLongitudinalVelocity(v_x_mps)); + return std::atan2(v_y_mps, safe_vx(v_x_mps)); } -float vehicleDynamics::estimateYawMoment_Nm( - const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N, - const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N, - const float steering_angle_rad) const +float vehicleDynamics::est_Mz_N( + const app::tv::datatypes::datatypes::wheel_set &tires_Fx_N, + const app::tv::datatypes::datatypes::wheel_set &tires_Fy_N, + const float delta) { - const float cos_delta = std::cos(steering_angle_rad); - const float sin_delta = std::sin(steering_angle_rad); - const float half_track_m = TRACK_WIDTH_m * 0.5f; - - const float fl_fx_body = (cos_delta * longitudinal_forces_N.fl) - (sin_delta * lateral_forces_N.fl); - const float fl_fy_body = (sin_delta * longitudinal_forces_N.fl) + (cos_delta * lateral_forces_N.fl); - const float fr_fx_body = (cos_delta * longitudinal_forces_N.fr) - (sin_delta * lateral_forces_N.fr); - const float fr_fy_body = (sin_delta * longitudinal_forces_N.fr) + (cos_delta * lateral_forces_N.fr); + constexpr float half_track_m = TRACK_WIDTH_m * 0.5f; - const float rl_fx_body = longitudinal_forces_N.rl; - const float rl_fy_body = lateral_forces_N.rl; - const float rr_fx_body = longitudinal_forces_N.rr; - const float rr_fy_body = lateral_forces_N.rr; + // rotate front wheels into body frame + const float cos_delta = std::cos(delta); + const float sin_delta = std::sin(delta); + const float fl_fx_body = (cos_delta * tires_Fx_N.fl) - (sin_delta * tires_Fy_N.fl); + const float fl_fy_body = (sin_delta * tires_Fx_N.fl) + (cos_delta * tires_Fy_N.fl); + const float fr_fx_body = (cos_delta * tires_Fx_N.fr) - (sin_delta * tires_Fy_N.fr); + const float fr_fy_body = (sin_delta * tires_Fx_N.fr) + (cos_delta * tires_Fy_N.fr); + // contributions to moment of each tire const float fl_moment = (DIST_FRONT_AXLE_CG_m * fl_fy_body) - (half_track_m * fl_fx_body); const float fr_moment = (DIST_FRONT_AXLE_CG_m * fr_fy_body) + (half_track_m * fr_fx_body); - const float rl_moment = (-DIST_REAR_AXLE_CG_m * rl_fy_body) - (half_track_m * rl_fx_body); - const float rr_moment = (-DIST_REAR_AXLE_CG_m * rr_fy_body) + (half_track_m * rr_fx_body); + const float rl_moment = (-DIST_REAR_AXLE_CG_m * tires_Fy_N.rl) - (half_track_m * tires_Fx_N.rl); + const float rr_moment = (-DIST_REAR_AXLE_CG_m * tires_Fy_N.rr) + (half_track_m * tires_Fx_N.rr); return fl_moment + fr_moment + rl_moment + rr_moment; } -app::tv::datatypes::datatypes::wheel_set vehicleDynamics::estimateNormalForce_N( - const float a_x_MPS2, const float a_y_MPS2, const float v_x_mps) const +app::tv::datatypes::datatypes::wheel_set + vehicleDynamics::est_Fz_N(const float a_x_MPS2, const float a_y_MPS2, const float v_x_mps) { + // load transfer const float long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(a_x_MPS2); const float lat_load_tf = LAT_ACCEL_TERM_VERTICAL_FORCE(a_y_MPS2); - const float down_force_n = estimateDownForce_N(v_x_mps); - const float front_cop = dynamicCopFront(a_x_MPS2); - const float right_cop = dynamicCopRight(a_y_MPS2); - - const float front_down_force_n = down_force_n * front_cop; - const float rear_down_force_n = down_force_n * (1.0f - front_cop); + // downforce and cop components + const float down_force_n = est_downforceFz_N(v_x_mps); + // Assuming COP is a fraction of the total downforce + const float front_cop = dynamicCOPFront(a_x_MPS2), rear_cop = 1.0f - front_cop, + right_cop = dynamicCOPRight(a_y_MPS2), left_cop = 1.0f - right_cop; return { .fl = std::fmax( 0.0f, - STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) - lat_load_tf + - (front_down_force_n * (1.0f - right_cop))), + STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) - lat_load_tf + down_force_n * front_cop * left_cop), .fr = std::fmax( 0.0f, - STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) + lat_load_tf + (front_down_force_n * right_cop)), + STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) + lat_load_tf + down_force_n * front_cop * right_cop), .rl = std::fmax( - 0.0f, - STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) - lat_load_tf + - (rear_down_force_n * (1.0f - right_cop))), + 0.0f, STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) - lat_load_tf + down_force_n * rear_cop * left_cop), .rr = std::fmax( - 0.0f, - STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) + lat_load_tf + (rear_down_force_n * right_cop)), + 0.0f, STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) + lat_load_tf + down_force_n * rear_cop * right_cop), }; } - -float vehicleDynamics::dynamicCopFront(const float long_accel_mps2) -{ - (void)long_accel_mps2; - return 1.0f - COP_REAR; -} - -float vehicleDynamics::dynamicCopRight(const float lat_accel_mps2) -{ - (void)lat_accel_mps2; - return COP_RIGHT; -} - -float vehicleDynamics::estimateDrag_N(const float v_x_mps) const -{ - return 0.5f * AIR_DENSITY_KGPM3 * FRONTAL_AREA_M2 * DRAG_COEFF * v_x_mps * v_x_mps; -} - -float vehicleDynamics::estimateDownForce_N(const float v_x_mps) const -{ - return 0.5f * AIR_DENSITY_KGPM3 * FRONTAL_AREA_M2 * LIFT_COEFF * v_x_mps * v_x_mps; -} } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp index 7269e3c612..e28cc4ea5d 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp @@ -6,21 +6,23 @@ namespace app::tv::estimation { using namespace app::tv::shared_datatypes::vd_constants; -class vehicleDynamics +namespace vehicleDynamics { -public: - - [[nodiscard]] app::tv::datatypes::datatypes::wheel_set estimateNormalForce_N( - float a_x_MPS2, float a_y_MPS2, float v_x_mps) const; - [[nodiscard]] float estimateBodySlip(float v_x_mps, float v_y_mps) const; - [[nodiscard]] float estimateYawMoment_Nm( - const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N, - const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N, - float steering_angle_rad) const; - [[nodiscard]] float estimateDrag_N(float v_x_mps) const; - [[nodiscard]] float estimateDownForce_N(float v_x_mps) const; - -private: + [[nodiscard]] app::tv::datatypes::datatypes::wheel_set + est_Fz_N(float a_x_MPS2, float a_y_MPS2, float v_x_mps); + [[nodiscard]] float est_beta_rad(float v_x_mps, float v_y_mps); + [[nodiscard]] float est_Mz_N( + const app::tv::datatypes::datatypes::wheel_set &tires_Fx_N, + const app::tv::datatypes::datatypes::wheel_set &tires_Fy_N, + float delta); + [[nodiscard]] constexpr float est_dragFx_N(const float v_x_mps) + { + return 0.5f * AIR_DENSITY_KGPM3 * FRONTAL_AREA_M2 * DRAG_COEFF * v_x_mps * v_x_mps; + } + [[nodiscard]] constexpr float est_downforceFz_N(const float v_x_mps) + { + return 0.5f * AIR_DENSITY_KGPM3 * FRONTAL_AREA_M2 * LIFT_COEFF * v_x_mps * v_x_mps; + } // ============================================================================= // VEHICLE DYNAMICS - VERTICAL LOAD TRANSFER @@ -28,88 +30,90 @@ class vehicleDynamics // ============================================================================= /** - * Longitudinal load transfer component (page 21) - * Positive long_accel transfers load to rear axle - * - * @param long_accel Longitudinal acceleration (m/s^2) - * @return Load transfer force (N) - */ - [[nodiscard]] static inline constexpr float LONG_ACCEL_TERM_VERTICAL_FORCE(const float long_accel) + * Longitudinal load transfer component (page 21) + * Positive long_accel transfers load to rear axle + * + * @param long_accel Longitudinal acceleration (m/s^2) + * @return Load transfer force (N) + */ + [[nodiscard]] constexpr float LONG_ACCEL_TERM_VERTICAL_FORCE(const float long_accel) { return (CAR_MASS_AT_CG_KG * long_accel * DIST_HEIGHT_CG_m) / WHEELBASE_m; } - /** - * Lateral load transfer component (page 21) - * Transfers load to outside wheels during cornering - * - * @param lat_accel Lateral acceleration (m/s^2) - * @return Load transfer force per side (N) - */ - [[nodiscard]] static inline constexpr float LAT_ACCEL_TERM_VERTICAL_FORCE(const float lat_accel) + * Lateral load transfer component (page 21) + * Transfers load to outside wheels during cornering + * + * @param lat_accel Lateral acceleration (m/s^2) + * @return Load transfer force per side (N) + */ + [[nodiscard]] constexpr float LAT_ACCEL_TERM_VERTICAL_FORCE(const float lat_accel) { return (CAR_MASS_AT_CG_KG * lat_accel * DIST_HEIGHT_CG_m) / (2.0f * TRACK_WIDTH_m); } - [[nodiscard]] static inline constexpr float REAR_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) + [[nodiscard]] constexpr float REAR_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) { return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) + - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); + LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); } - - [[nodiscard]] static inline constexpr float REAR_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) + [[nodiscard]] constexpr float REAR_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) { return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) - - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); + LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); } - - // TODO: Check if front wheels use rear weight or front weight distribution - [[nodiscard]] static inline constexpr float FRONT_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) + [[nodiscard]] constexpr float FRONT_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) { return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) + - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); + LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); } - - [[nodiscard]] static inline constexpr float FRONT_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) + [[nodiscard]] constexpr float FRONT_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) { return REAR_WEIGHT_DISTRIBUTION - LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) - - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); + LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); } /** - * Yaw moment distribution factor Kmz (page 57) - * Accounts for load transfer effect on yaw moment generation capacity - * - * @param long_accel Longitudinal acceleration (m/s^2) - * @return Effective moment arm (m) - */ - [[nodiscard]] static inline constexpr float ACCELERATION_TERM_KMZ(const float long_accel) + * Yaw moment distribution factor Kmz (page 57) + * Accounts for load transfer effect on yaw moment generation capacity + * + * @param a_x Longitudinal acceleration (m/s^2) + * @return Effective moment arm (m) + */ + [[nodiscard]] constexpr float ACCELERATION_TERM_KMZ(const float a_x) { - return DIST_FRONT_AXLE_CG_m + (long_accel * DIST_HEIGHT_CG_m) / GRAVITY; + return DIST_FRONT_AXLE_CG_m + (a_x * DIST_HEIGHT_CG_m) / GRAVITY; } - - [[nodiscard]] static inline constexpr float KMZ(const float long_accel) + [[nodiscard]] constexpr float KMZ(const float a_x) { - float LONG_ACCEL_TERM = ACCELERATION_TERM_KMZ(long_accel); - return ((CAR_WEIGHT - (CAR_WEIGHT / WHEELBASE_m) * LONG_ACCEL_TERM)) / ((CAR_WEIGHT / WHEELBASE_m) * LONG_ACCEL_TERM); + const float LONG_ACCEL_TERM = ACCELERATION_TERM_KMZ(a_x); + return ((CAR_WEIGHT - (CAR_WEIGHT / WHEELBASE_m) * LONG_ACCEL_TERM)) / + ((CAR_WEIGHT / WHEELBASE_m) * LONG_ACCEL_TERM); } - /** - * Moment scaling factor F (page 58) - * Relates torque differential to yaw moment through track width and effective radius - */ - static constexpr float F = (TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * IN_TO_M)) * GEAR_RATIO; - - static constexpr float STATIC_FRONT_AXLE_LOAD_N = datatypes::vd_constants::CAR_WEIGHT * - (datatypes::vd_constants::DIST_REAR_AXLE_CG_m / - datatypes::vd_constants::WHEELBASE_m); - static constexpr float STATIC_REAR_AXLE_LOAD_N = datatypes::vd_constants::CAR_WEIGHT * - (datatypes::vd_constants::DIST_FRONT_AXLE_CG_m / - datatypes::vd_constants::WHEELBASE_m); - static constexpr float STATIC_FRONT_WHEEL_LOAD_N = 0.5f * STATIC_FRONT_AXLE_LOAD_N; - static constexpr float STATIC_REAR_WHEEL_LOAD_N = 0.5f * STATIC_REAR_AXLE_LOAD_N; - - [[nodiscard]] static float dynamicCopFront(float long_accel_mps2); - [[nodiscard]] static float dynamicCopRight(float lat_accel_mps2); -}; + * Moment scaling factor F (page 58) + * Relates torque differential to yaw moment through track width and effective radius + */ + inline constexpr float F = (TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * IN_TO_M)) * GEAR_RATIO; + + inline constexpr float STATIC_FRONT_AXLE_LOAD_N = + datatypes::vd_constants::CAR_WEIGHT * + (datatypes::vd_constants::DIST_REAR_AXLE_CG_m / datatypes::vd_constants::WHEELBASE_m); + inline constexpr float STATIC_REAR_AXLE_LOAD_N = + datatypes::vd_constants::CAR_WEIGHT * + (datatypes::vd_constants::DIST_FRONT_AXLE_CG_m / datatypes::vd_constants::WHEELBASE_m); + inline constexpr float STATIC_FRONT_WHEEL_LOAD_N = 0.5f * STATIC_FRONT_AXLE_LOAD_N; + inline constexpr float STATIC_REAR_WHEEL_LOAD_N = 0.5f * STATIC_REAR_AXLE_LOAD_N; + + [[nodiscard]] constexpr float dynamicCOPFront(const float long_accel_mps2) + { + (void)long_accel_mps2; + return 1.0f - COP_REAR; + } + [[nodiscard]] constexpr float dynamicCOPRight(const float lat_accel_mps2) + { + (void)lat_accel_mps2; + return COP_RIGHT; + } +}; // namespace vehicleDynamics } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index a90697c647..e70ae071f2 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -1,4 +1,6 @@ #include "tire_model.hpp" +#include "torque_vectoring/shared_datatypes/constants.hpp" +#include "torque_vectoring/shared_datatypes/low_speed_blend.hpp" #include #include @@ -9,68 +11,65 @@ namespace app::tv::estimation { namespace { -[[nodiscard]] float safeMagnitude(const float value) -{ - return std::fmax(std::fabs(value), SMALL_EPSILON); -} - -[[nodiscard]] float primalValue(const float value) -{ - return value; -} - -[[nodiscard]] float primalValue(const autodiff::dual& value) -{ - return static_cast(autodiff::val(value)); -} - -template -[[nodiscard]] T safeTemplateDenominator(const T& value) -{ - const float primal = primalValue(value); + [[nodiscard]] float safeMagnitude(const float value) + { + return std::fmax(std::fabs(value), SMALL_EPSILON); + } - if (std::fabs(primal) >= SMALL_EPSILON) + [[nodiscard]] float primalValue(const float value) { return value; } - return T(primal < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON); -} + [[nodiscard]] float primalValue(const autodiff::dual &value) + { + return static_cast(autodiff::val(value)); + } + + template [[nodiscard]] T safeTemplateDenominator(const T &value) + { + const float primal = primalValue(value); + + if (std::fabs(primal) >= SMALL_EPSILON) + { + return value; + } + + return T(primal < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON); + } } // namespace -[[nodiscard]] TireModel::Outputs TireModel::estimate(const StateInputs& inputs) const +[[nodiscard]] TireModel::Outputs TireModel::estimate(const StateInputs &inputs) const { const float vehicle_speed_mps = std::hypot(inputs.vehicle_velocity_x_mps, inputs.vehicle_velocity_y_mps); const float low_speed_blend = shared_datatypes::velocityBlend(vehicle_speed_mps); - const auto wheel_velocities = + const auto [x_mps, y_mps] = wheelVelocities(inputs.vehicle_velocity_x_mps, inputs.vehicle_velocity_y_mps, inputs.yaw_rate_radps); - const float slip_angle_rad = - estimateSlipAngle(wheel_velocities.x_mps, wheel_velocities.y_mps, inputs.steering_angle_rad); - const float slip_ratio = estimateSlipRatio( - wheel_velocities.x_mps, wheel_velocities.y_mps, slip_angle_rad, inputs.wheel_angular_velocity_radps); + const float slip_angle_rad = estimateSlipAngle(x_mps, y_mps, inputs.steering_angle_rad); + const float slip_ratio = estimateSlipRatio(x_mps, y_mps, slip_angle_rad, inputs.wheel_angular_velocity_radps); return { .slip_ratio = slip_ratio, .slip_angle_rad = slip_angle_rad, .longitudinal_force_N = static_cast( computeCombinedFx_N(inputs.normal_load_N, slip_angle_rad, low_speed_blend, slip_ratio)), - .lateral_force_N = computeCombinedFy_N( - inputs.normal_load_N, slip_angle_rad, low_speed_blend, slip_ratio), + .lateral_force_N = computeCombinedFy_N(inputs.normal_load_N, slip_angle_rad, low_speed_blend, slip_ratio), }; } -[[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity(const float slip_ratio, const float wheel_vel_x_mps) const +[[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity(const float slip_ratio, const float wheel_vel_x_mps) { const float wheel_surface_speed_mps = wheel_vel_x_mps * (1.0f + slip_ratio); return wheel_surface_speed_mps / safeMagnitude(WHEEL_RADIUS_M); } [[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity( - const float slip_ratio, const datatypes::datatypes::VehicleState& vehicle_state) const + const float slip_ratio, + const datatypes::datatypes::VehicleState &vehicle_state) const { - const auto wheel_velocities = + const auto [x_mps, y_mps] = wheelVelocities(vehicle_state.v_x_mps, vehicle_state.v_y_mps, vehicle_state.yaw_rate_radps); - return slipRatioToWheelAngularVelocity(slip_ratio, wheel_velocities.x_mps); + return slipRatioToWheelAngularVelocity(slip_ratio, x_mps); } [[nodiscard]] float TireModel::wheelLongOffset_m() const @@ -89,7 +88,9 @@ template } [[nodiscard]] TireModel::WheelVelocities TireModel::wheelVelocities( - const float vehicle_velocity_x_mps, const float vehicle_velocity_y_mps, const float yaw_rate_radps) const + const float vehicle_velocity_x_mps, + const float vehicle_velocity_y_mps, + const float yaw_rate_radps) const { // Rigid-body planar kinematics in the body frame: // v_wheel = v_cg + omega_z x r_wheel, where r_wheel = [x_offset, y_offset, 0]. @@ -100,7 +101,9 @@ template } [[nodiscard]] float TireModel::estimateSlipAngle( - const float wheel_vel_x_mps, const float wheel_vel_y_mps, const float steering_angle_rad) const + const float wheel_vel_x_mps, + const float wheel_vel_y_mps, + const float steering_angle_rad) const { return std::atan2(wheel_vel_y_mps, wheel_vel_x_mps) - wheelSteeringAngle_rad(steering_angle_rad); } @@ -109,7 +112,7 @@ template const float wheel_vel_x_mps, const float wheel_vel_y_mps, const float slip_angle_rad, - const float wheel_angular_velocity_radps) const + const float wheel_angular_velocity_radps) { const float wheel_surface_speed_mps = wheel_angular_velocity_radps * WHEEL_RADIUS_M; const float wheel_speed_magnitude_mps = std::hypot(wheel_vel_x_mps, wheel_vel_y_mps); @@ -118,24 +121,24 @@ template return (wheel_surface_speed_mps - effective_wheel_speed_mps) / safeMagnitude(effective_wheel_speed_mps); } -//-------------------------------------------------------------------- Pacejka 5.2 ----------------------------------------------------------------------// - +//-------------------------------------------------------------------- Pacejka 5.2 +//----------------------------------------------------------------------// template -TireModel::PureFxMagicFormulaCoefficients TireModel::pureFxMagicFormulaCoefficients( - const float normal_load_N, const T& slip_ratio) const +TireModel::PureFxMagicFormulaCoefficients + TireModel::pureFxMagicFormulaCoefficients(const float normal_load_N, const T &slip_ratio) const { // Assumes lambda terms are 1.0, gamma = 0, and pressure effects are captured by the fixed 12_PSI fitted row. const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); - const float s_hx = pureFx_Sh(normalized_load_delta); - const T kappa_x = pureFx_Kappa(normalized_load_delta, slip_ratio); - const float c_x = pureFx_C(); - const float d_x = pureFx_D(clamped_normal_load_N, normalized_load_delta); - const T e_x = pureFx_E(normalized_load_delta, kappa_x); - const float k_x = pureFx_K(clamped_normal_load_N, normalized_load_delta); - const float b_x = pureFx_B(k_x, c_x, d_x); - const float s_vx = pureFx_Sv(clamped_normal_load_N, normalized_load_delta); + const float s_hx = pureFx_Sh(normalized_load_delta); + const T kappa_x = pureFx_Kappa(normalized_load_delta, slip_ratio); + const float c_x = pureFx_C(); + const float d_x = pureFx_D(clamped_normal_load_N, normalized_load_delta); + const T e_x = pureFx_E(normalized_load_delta, kappa_x); + const float k_x = pureFx_K(clamped_normal_load_N, normalized_load_delta); + const float b_x = pureFx_B(k_x, c_x, d_x); + const float s_vx = pureFx_Sv(clamped_normal_load_N, normalized_load_delta); return { .s_hx = T(s_hx), @@ -148,20 +151,20 @@ TireModel::PureFxMagicFormulaCoefficients TireModel::pureFxMagicFormulaCoeffi }; } -TireModel::PureFyMagicFormulaCoefficients TireModel::pureFyMagicFormulaCoefficients( - const float normal_load_N, const float slip_angle_rad) const +TireModel::PureFyMagicFormulaCoefficients + TireModel::pureFyMagicFormulaCoefficients(const float normal_load_N, const float slip_angle_rad) const { // Assumes gamma = 0, lambda terms are 1.0, and pressure effects are captured by the fixed 12_PSI fitted row. const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); - const float s_hy = pureFy_Sh(normalized_load_delta); - const float alpha_y = pureFy_Alpha(normalized_load_delta, slip_angle_rad); - const float c_y = pureFy_C(); - const float d_y = pureFy_D(clamped_normal_load_N, normalized_load_delta); - const float e_y = pureFy_E(normalized_load_delta, alpha_y); - const float k_y = pureFy_K(clamped_normal_load_N); - const float b_y = pureFy_B(k_y, c_y, d_y); - const float s_vy = pureFy_Sv(clamped_normal_load_N, normalized_load_delta); + const float s_hy = pureFy_Sh(normalized_load_delta); + const float alpha_y = pureFy_Alpha(normalized_load_delta, slip_angle_rad); + const float c_y = pureFy_C(); + const float d_y = pureFy_D(clamped_normal_load_N, normalized_load_delta); + const float e_y = pureFy_E(normalized_load_delta, alpha_y); + const float k_y = pureFy_K(clamped_normal_load_N); + const float b_y = pureFy_B(k_y, c_y, d_y); + const float s_vy = pureFy_Sv(clamped_normal_load_N, normalized_load_delta); return { .s_hy = s_hy, @@ -174,15 +177,14 @@ TireModel::PureFyMagicFormulaCoefficients TireModel::pureFyMagicFormulaCoefficie }; } -template -T TireModel::computePureFx_N(const float normal_load_N, const T& slip_ratio) const +template T TireModel::computePureFx_N(const float normal_load_N, const T &slip_ratio) const { using std::atan; using std::sin; const auto coefficients = pureFxMagicFormulaCoefficients(normal_load_N, slip_ratio); - const T u = coefficients.b_x * coefficients.kappa_x; - const T phi = u - coefficients.e_x * (u - atan(u)); + const T u = coefficients.b_x * coefficients.kappa_x; + const T phi = u - coefficients.e_x * (u - atan(u)); return coefficients.d_x * sin(coefficients.c_x * atan(phi)) + coefficients.s_vx; } @@ -196,20 +198,20 @@ float TireModel::computePureFy_N(const float normal_load_N, const float slip_ang return coefficients.d_y * std::sin( coefficients.c_y * - std::atan( - b_y_alpha_y - - (coefficients.e_y * - (b_y_alpha_y - std::atan(b_y_alpha_y))))) + + std::atan(b_y_alpha_y - (coefficients.e_y * (b_y_alpha_y - std::atan(b_y_alpha_y))))) + coefficients.s_vy; } template T TireModel::computeCombinedFx_N( - const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const T& slip_ratio) const + const float normal_load_N, + const float slip_angle_rad, + const float low_speed_blend, + const T &slip_ratio) const { - const T pure_fx_0 = computePureFx_N(normal_load_N, slip_ratio); + const T pure_fx_0 = computePureFx_N(normal_load_N, slip_ratio); const auto coefficients = combinedFxMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); - const T force_blend = T(low_speed_blend); + const T force_blend = T(low_speed_blend); // Low-speed safeguard: // Below a small vehicle-speed threshold the tire model can predict unrealistically large @@ -223,11 +225,14 @@ T TireModel::computeCombinedFx_N( template T TireModel::computeCombinedFy_N( - const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const T& slip_ratio) const + const float normal_load_N, + const float slip_angle_rad, + const float low_speed_blend, + const T &slip_ratio) const { - const float pure_fy_0 = computePureFy_N(normal_load_N, slip_angle_rad); + const float pure_fy_0 = computePureFy_N(normal_load_N, slip_angle_rad); const auto coefficients = combinedFyMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); - const T force_blend = T(low_speed_blend); + const T force_blend = T(low_speed_blend); // Apply the same low-speed force-availability blend used for Fx so the optimizer sees a // self-consistent pair of tire forces as the vehicle approaches a stop. @@ -260,8 +265,7 @@ float TireModel::combinedFx_Exa(const float normalized_load_delta) const return std::fmin(fit_comb_fx_.rEx1 + (fit_comb_fx_.rEx2 * normalized_load_delta), 1.0f); } -template -T TireModel::combinedFx_Bxa(const T& slip_ratio) const +template T TireModel::combinedFx_Bxa(const T &slip_ratio) const { using std::atan; using std::cos; @@ -270,8 +274,7 @@ T TireModel::combinedFx_Bxa(const T& slip_ratio) const return T(fit_comb_fx_.rBx1) * cos(atan(T(fit_comb_fx_.rBx2) * slip_ratio)); } -template -T TireModel::combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients& coefficients) const +template T TireModel::combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients &coefficients) const { using std::atan; using std::cos; @@ -282,8 +285,7 @@ T TireModel::combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients& coeffi return cos(coefficients.c_xa * atan(phi)); } -template -T TireModel::combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients& coefficients) const +template T TireModel::combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients &coefficients) const { using std::atan; using std::cos; @@ -298,7 +300,9 @@ T TireModel::combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients& coeffic template TireModel::CombinedFxMagicFormulaCoefficients TireModel::combinedFxMagicFormulaCoefficients( - const float normal_load_N, const float slip_angle_rad, const T& slip_ratio) const + const float normal_load_N, + const float slip_angle_rad, + const T &slip_ratio) const { const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); @@ -324,8 +328,7 @@ float TireModel::combinedFy_SHyk(const float normalized_load_delta) const return fit_comb_fy_.rHy1 + (fit_comb_fy_.rHy2 * normalized_load_delta); } -template -T TireModel::combinedFy_Kappa_s(const float normalized_load_delta, const T& slip_ratio) const +template T TireModel::combinedFy_Kappa_s(const float normalized_load_delta, const T &slip_ratio) const { // Pacejka Page 181 (4.E61): kappa_s = kappa + S_Hyk return slip_ratio + T(combinedFy_SHyk(normalized_load_delta)); @@ -353,7 +356,9 @@ float TireModel::combinedFy_Byk(const float slip_angle_rad) const } float TireModel::combinedFy_Dvyk( - const float normal_load_N, const float normalized_load_delta, const float slip_angle_rad) const + const float normal_load_N, + const float normalized_load_delta, + const float slip_angle_rad) const { using std::atan; using std::cos; @@ -364,8 +369,7 @@ float TireModel::combinedFy_Dvyk( cos(atan(fit_comb_fy_.rVy4 * slip_angle_rad)); } -template -T TireModel::combinedFy_Svyk(const T& d_vyk, const T& slip_ratio) const +template T TireModel::combinedFy_Svyk(const T &d_vyk, const T &slip_ratio) const { using std::atan; using std::sin; @@ -374,8 +378,7 @@ T TireModel::combinedFy_Svyk(const T& d_vyk, const T& slip_ratio) const return d_vyk * sin(T(fit_comb_fy_.rVy5) * atan(T(fit_comb_fy_.rVy6) * slip_ratio)); } -template -T TireModel::combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients& coefficients) const +template T TireModel::combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients &coefficients) const { using std::atan; using std::cos; @@ -386,8 +389,7 @@ T TireModel::combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients& coeffi return cos(coefficients.c_yk * atan(phi)); } -template -T TireModel::combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients& coefficients) const +template T TireModel::combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients &coefficients) const { using std::atan; using std::cos; @@ -402,7 +404,9 @@ T TireModel::combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients& coeffic template TireModel::CombinedFyMagicFormulaCoefficients TireModel::combinedFyMagicFormulaCoefficients( - const float normal_load_N, const float slip_angle_rad, const T& slip_ratio) const + const float normal_load_N, + const float slip_angle_rad, + const T &slip_ratio) const { const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); @@ -425,7 +429,6 @@ TireModel::CombinedFyMagicFormulaCoefficients TireModel::combinedFyMagicFormu return coefficients; } - constexpr float TireModel::safeSignedDenominator(const float value) { if (std::fabs(value) >= SMALL_EPSILON) @@ -451,8 +454,8 @@ constexpr float TireModel::signum(const float value) return 0.0f; } -//-------------------------------------------------------------------- MJ Pure Coefficents 5.2 ----------------------------------------------------------------------// - +//-------------------------------------------------------------------- MJ Pure Coefficents 5.2 +//----------------------------------------------------------------------// constexpr float TireModel::normalizedLoadDelta(const float normal_load_N) { @@ -465,8 +468,7 @@ constexpr float TireModel::pureFx_Sh(const float normalized_load_delta) const return fit_pure_fx_.hx_1 + (fit_pure_fx_.hx_2 * normalized_load_delta); } -template -T TireModel::pureFx_Kappa(const float normalized_load_delta, const T& slip_ratio) const +template T TireModel::pureFx_Kappa(const float normalized_load_delta, const T &slip_ratio) const { // Pacejka Page 179 (4.E10): kappa_x = kappa + S_Hx return slip_ratio + T(pureFx_Sh(normalized_load_delta)); @@ -490,12 +492,11 @@ constexpr float TireModel::pureFx_D(const float normal_load_N, const float norma return pureFx_mu(normalized_load_delta) * normal_load_N; } -template -T TireModel::pureFx_E(const float normalized_load_delta, const T& kappa_x) const +template T TireModel::pureFx_E(const float normalized_load_delta, const T &kappa_x) const { // Pacejka Page 179 (4.E14): E_x const float normalized_load_delta_squared = normalized_load_delta * normalized_load_delta; - const float base_e_x = fit_pure_fx_.ex_1 + (fit_pure_fx_.ex_2 * normalized_load_delta) + + const float base_e_x = fit_pure_fx_.ex_1 + (fit_pure_fx_.ex_2 * normalized_load_delta) + (fit_pure_fx_.ex_3 * normalized_load_delta_squared); const float kappa_x_value = primalValue(kappa_x); @@ -509,8 +510,7 @@ constexpr float TireModel::pureFx_K(const float normal_load_N, const float norma std::exp(fit_pure_fx_.kx_3 * normalized_load_delta); } -constexpr float TireModel::pureFx_B( - const float slip_stiffness, const float shape_factor, const float peak_factor) +constexpr float TireModel::pureFx_B(const float slip_stiffness, const float shape_factor, const float peak_factor) { // Pacejka Page 179 (4.E16): B_x = K_xk / (C_x * D_x + epsilon_x) return slip_stiffness / safeSignedDenominator(shape_factor * peak_factor); @@ -563,12 +563,10 @@ constexpr float TireModel::pureFy_K(const float normal_load_N) const { // Pacejka Page 180 (4.E25): K_yalpha with gamma/pressure terms reduced to zero. const float denominator = safeSignedDenominator(fit_pure_fy_.ky_2 * NOMINAL_FZ_N); - return fit_pure_fy_.ky_1 * NOMINAL_FZ_N * - std::sin(2.0f * std::atan(normal_load_N / denominator)); + return fit_pure_fy_.ky_1 * NOMINAL_FZ_N * std::sin(2.0f * std::atan(normal_load_N / denominator)); } -constexpr float TireModel::pureFy_B( - const float cornering_stiffness, const float shape_factor, const float peak_factor) +constexpr float TireModel::pureFy_B(const float cornering_stiffness, const float shape_factor, const float peak_factor) { // Pacejka Page 180 (4.E26): B_y = K_yalpha / (C_y * D_y + epsilon_y) return cornering_stiffness / safeSignedDenominator(shape_factor * peak_factor); @@ -581,12 +579,24 @@ constexpr float TireModel::pureFy_Sv(const float normal_load_N, const float norm } template float TireModel::computeCombinedFx_N( - const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const float& slip_ratio) const; + const float normal_load_N, + const float slip_angle_rad, + const float low_speed_blend, + const float &slip_ratio) const; template autodiff::dual TireModel::computeCombinedFx_N( - const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const autodiff::dual& slip_ratio) const; + const float normal_load_N, + const float slip_angle_rad, + const float low_speed_blend, + const autodiff::dual &slip_ratio) const; template float TireModel::computeCombinedFy_N( - const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const float& slip_ratio) const; + const float normal_load_N, + const float slip_angle_rad, + const float low_speed_blend, + const float &slip_ratio) const; template autodiff::dual TireModel::computeCombinedFy_N( - const float normal_load_N, const float slip_angle_rad, const float low_speed_blend, const autodiff::dual& slip_ratio) const; + const float normal_load_N, + const float slip_angle_rad, + const float low_speed_blend, + const autodiff::dual &slip_ratio) const; } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index fc4697c35b..520c693175 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -1,16 +1,12 @@ #pragma once -#include - -#include "torque_vectoring/shared_datatypes/constants.hpp" #include "torque_vectoring/shared_datatypes/datatypes.hpp" -#include "torque_vectoring/shared_datatypes/low_speed_blend.hpp" namespace app::tv::estimation { class TireModel { -public: + public: enum class WheelSide { Left, @@ -42,27 +38,30 @@ class TireModel }; constexpr TireModel(const WheelSide wheel_side, const WheelAxle wheel_axle) - : fit_pure_fx_(FIT_PURE_FX_12_PSI), - fit_pure_fy_(FIT_PURE_FY_12_PSI), - fit_comb_fx_(FIT_COMB_FX_12_PSI), - fit_comb_fy_(FIT_COMB_FY_12_PSI), - wheel_side_(wheel_side), - wheel_axle_(wheel_axle) + : fit_pure_fx_(FIT_PURE_FX_12_PSI), + fit_pure_fy_(FIT_PURE_FY_12_PSI), + fit_comb_fx_(FIT_COMB_FX_12_PSI), + fit_comb_fy_(FIT_COMB_FY_12_PSI), + wheel_side_(wheel_side), + wheel_axle_(wheel_axle) { } - [[nodiscard]] Outputs estimate(const StateInputs& inputs) const; + [[nodiscard]] Outputs estimate(const StateInputs &inputs) const; template - [[nodiscard]] T computeCombinedFx_N( - float normal_load_N, float slip_angle_rad, float low_speed_blend, const T& slip_ratio) const; + [[nodiscard]] T + computeCombinedFx_N(float normal_load_N, float slip_angle_rad, float low_speed_blend, const T &slip_ratio) + const; template - [[nodiscard]] T computeCombinedFy_N( - float normal_load_N, float slip_angle_rad, float low_speed_blend, const T& slip_ratio) const; - [[nodiscard]] float slipRatioToWheelAngularVelocity(const float slip_ratio, const float wheel_vel_x_mps) const; - [[nodiscard]] float slipRatioToWheelAngularVelocity( - const float slip_ratio, const datatypes::datatypes::VehicleState& vehicle_state) const; + [[nodiscard]] T + computeCombinedFy_N(float normal_load_N, float slip_angle_rad, float low_speed_blend, const T &slip_ratio) + const; + [[nodiscard]] static float slipRatioToWheelAngularVelocity(float slip_ratio, float wheel_vel_x_mps); + [[nodiscard]] float slipRatioToWheelAngularVelocity( + float slip_ratio, + const datatypes::datatypes::VehicleState &vehicle_state) const; -private: + private: struct WheelVelocities { float x_mps = 0.0f; @@ -140,8 +139,7 @@ class TireModel float rVy6; }; - template - struct PureFxMagicFormulaCoefficients + template struct PureFxMagicFormulaCoefficients { T s_hx = T(0.0f); T kappa_x = T(0.0f); @@ -152,8 +150,7 @@ class TireModel T s_vx = T(0.0f); }; - template - struct CombinedFxMagicFormulaCoefficients + template struct CombinedFxMagicFormulaCoefficients { T s_hxa = T(0.0f); T alpha_s = T(0.0f); @@ -175,8 +172,7 @@ class TireModel float s_vy = 0.0f; }; - template - struct CombinedFyMagicFormulaCoefficients + template struct CombinedFyMagicFormulaCoefficients { T s_hyk = T(0.0f); T kappa_s = T(0.0f); @@ -189,52 +185,23 @@ class TireModel T g_yk = T(0.0f); }; - /* Current model uses the 12_PSI fitted workbook row as a fixed-pressure operating assumption. If pressure becomes a runtime input later, add pressure interpolation or a refit against the raw tire data. */ static constexpr TireFitPureParamFx FIT_PURE_FX_12_PSI = { - 0.008656294312f, - 366.1925838f, - -57.2826791f, - 8.208916842f, - 1.134744942f, - 0.04728092792f, - -0.02191230313f, - -0.01799018414f, - 62.46585932f, - 1.390484252f, - -0.3909753489f, - 0.0005429909716f, - -0.0007019708842f, - 0.01286770906f, - 0.004135955934f, + 0.008656294312f, 366.1925838f, -57.2826791f, 8.208916842f, 1.134744942f, + 0.04728092792f, -0.02191230313f, -0.01799018414f, 62.46585932f, 1.390484252f, + -0.3909753489f, 0.0005429909716f, -0.0007019708842f, 0.01286770906f, 0.004135955934f, }; static constexpr TireFitPureParamFy FIT_PURE_FY_12_PSI = { - 0.90721342f, - -3.824114982f, - -0.4280518359f, - 15.56383821f, - 1.218066219f, - 0.3097318297f, - -0.02930740966f, - 0.2068388689f, - -54.60640233f, - 2.122730258f, - 1.671281037f, - -0.004171875264f, - -0.00360720284f, - -0.0787609381f, - -0.1057253032f, - -0.02043816084f, - 0.83972231f, - 1.494231303f, + 0.90721342f, -3.824114982f, -0.4280518359f, 15.56383821f, 1.218066219f, 0.3097318297f, + -0.02930740966f, 0.2068388689f, -54.60640233f, 2.122730258f, 1.671281037f, -0.004171875264f, + -0.00360720284f, -0.0787609381f, -0.1057253032f, -0.02043816084f, 0.83972231f, 1.494231303f, }; - static constexpr TireFitCombParamFx FIT_COMB_FX_12_PSI = - { + static constexpr TireFitCombParamFx FIT_COMB_FX_12_PSI = { .rBx1 = 13.046f, .rBx2 = 9.718f, .rBx3 = 0.0f, @@ -244,8 +211,7 @@ class TireModel .rHx1 = -0.0001f, }; - static constexpr TireFitCombParamFy FIT_COMB_FY_12_PSI = - { + static constexpr TireFitCombParamFy FIT_COMB_FY_12_PSI = { .rBy1 = 10.622f, .rBy2 = 7.82f, .rBy3 = 0.00204f, @@ -263,92 +229,89 @@ class TireModel .rVy6 = 23.8f, }; - static constexpr float NOMINAL_FZ_N = 750.0f; - //-------------------------------------------------------------------- Class Helpers ----------------------------------------------------------------------// + static constexpr float NOMINAL_FZ_N = 750.0f; + //-------------------------------------------------------------------- Class Helpers + //----------------------------------------------------------------------// - [[nodiscard]] static constexpr float safeSignedDenominator(const float value); - [[nodiscard]] static constexpr float signum(const float value); - [[nodiscard]] static constexpr float normalizedLoadDelta(const float normal_load_N); + [[nodiscard]] static constexpr float safeSignedDenominator(float value); + [[nodiscard]] static constexpr float signum(float value); + [[nodiscard]] static constexpr float normalizedLoadDelta(float normal_load_N); // Reduced-model assumptions for combined slip in this pass: // gamma* = 0, lambda_xa = 1, lambda_yk = 1, lambda_vyk = 1, zeta_2 = 1. // Pressure dependence is captured by the fixed 12_PSI fitted parameter row. - //-------------------------------------------------------------------- Pure Pacejka 5.2 Helpers ----------------------------------------------------------------------// - [[nodiscard]] constexpr float pureFx_Sh(const float normalized_load_delta) const; + //-------------------------------------------------------------------- Pure Pacejka 5.2 Helpers + //----------------------------------------------------------------------// + [[nodiscard]] constexpr float pureFx_Sh(float normalized_load_delta) const; + template [[nodiscard]] T pureFx_Kappa(float normalized_load_delta, const T &slip_ratio) const; + [[nodiscard]] constexpr float pureFx_C() const; + [[nodiscard]] constexpr float pureFx_mu(float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFx_D(float normal_load_N, float normalized_load_delta) const; + template [[nodiscard]] T pureFx_E(float normalized_load_delta, const T &kappa_x) const; + [[nodiscard]] constexpr float pureFx_K(float normal_load_N, float normalized_load_delta) const; + [[nodiscard]] static constexpr float pureFx_B(float slip_stiffness, float shape_factor, float peak_factor); + [[nodiscard]] constexpr float pureFx_Sv(float normal_load_N, float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_Sh(float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_Alpha(float normalized_load_delta, float slip_angle_rad) const; + [[nodiscard]] constexpr float pureFy_C() const; + [[nodiscard]] constexpr float pureFy_mu(float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_D(float normal_load_N, float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_E(float normalized_load_delta, float alpha_y) const; + [[nodiscard]] constexpr float pureFy_K(float normal_load_N) const; + [[nodiscard]] static constexpr float pureFy_B(float cornering_stiffness, float shape_factor, float peak_factor); + [[nodiscard]] constexpr float pureFy_Sv(float normal_load_N, float normalized_load_delta) const; template - [[nodiscard]] T pureFx_Kappa(const float normalized_load_delta, const T& slip_ratio) const; - [[nodiscard]] constexpr float pureFx_C() const; - [[nodiscard]] constexpr float pureFx_mu(const float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFx_D(const float normal_load_N, const float normalized_load_delta) const; + [[nodiscard]] PureFxMagicFormulaCoefficients + pureFxMagicFormulaCoefficients(float normal_load_N, const T &slip_ratio) const; + [[nodiscard]] PureFyMagicFormulaCoefficients + pureFyMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad) const; + //-------------------------------------------------------------------- Combined Pacejka 5.2 Helpers + //----------------------------------------------------------------------// + [[nodiscard]] constexpr float combinedFx_SHxa() const; + [[nodiscard]] constexpr float combinedFx_Alpha_s(float slip_angle_rad) const; + [[nodiscard]] constexpr float combinedFx_Cxa() const; + [[nodiscard]] float combinedFx_Exa(float normalized_load_delta) const; + template [[nodiscard]] T combinedFx_Bxa(const T &slip_ratio) const; template - [[nodiscard]] T pureFx_E(const float normalized_load_delta, const T& kappa_x) const; - [[nodiscard]] constexpr float pureFx_K(const float normal_load_N, const float normalized_load_delta) const; - [[nodiscard]] static constexpr float pureFx_B( - const float slip_stiffness, const float shape_factor, const float peak_factor); - [[nodiscard]] constexpr float pureFx_Sv(const float normal_load_N, const float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFy_Sh(const float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFy_Alpha(const float normalized_load_delta, const float slip_angle_rad) const; - [[nodiscard]] constexpr float pureFy_C() const; - [[nodiscard]] constexpr float pureFy_mu(const float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFy_D(const float normal_load_N, const float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFy_E(const float normalized_load_delta, const float alpha_y) const; - [[nodiscard]] constexpr float pureFy_K(const float normal_load_N) const; - [[nodiscard]] static constexpr float pureFy_B( - const float cornering_stiffness, const float shape_factor, const float peak_factor); - [[nodiscard]] constexpr float pureFy_Sv(const float normal_load_N, const float normalized_load_delta) const; + [[nodiscard]] T combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients &coefficients) const; template - [[nodiscard]] PureFxMagicFormulaCoefficients pureFxMagicFormulaCoefficients( - float normal_load_N, const T& slip_ratio) const; - [[nodiscard]] PureFyMagicFormulaCoefficients pureFyMagicFormulaCoefficients( - float normal_load_N, float slip_angle_rad) const; - //-------------------------------------------------------------------- Combined Pacejka 5.2 Helpers ----------------------------------------------------------------------// - [[nodiscard]] constexpr float combinedFx_SHxa() const; - [[nodiscard]] constexpr float combinedFx_Alpha_s(float slip_angle_rad) const; - [[nodiscard]] constexpr float combinedFx_Cxa() const; - [[nodiscard]] float combinedFx_Exa(float normalized_load_delta) const; + [[nodiscard]] T combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients &coefficients) const; template - [[nodiscard]] T combinedFx_Bxa(const T& slip_ratio) const; - template - [[nodiscard]] T combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients& coefficients) const; - template - [[nodiscard]] T combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients& coefficients) const; - template - [[nodiscard]] CombinedFxMagicFormulaCoefficients combinedFxMagicFormulaCoefficients( - float normal_load_N, float slip_angle_rad, const T& slip_ratio) const; - [[nodiscard]] float combinedFy_SHyk(float normalized_load_delta) const; - template - [[nodiscard]] T combinedFy_Kappa_s(float normalized_load_delta, const T& slip_ratio) const; - [[nodiscard]] constexpr float combinedFy_Cyk() const; - [[nodiscard]] float combinedFy_Eyk(float normalized_load_delta) const; - [[nodiscard]] float combinedFy_Byk(float slip_angle_rad) const; + [[nodiscard]] CombinedFxMagicFormulaCoefficients + combinedFxMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; + [[nodiscard]] float combinedFy_SHyk(float normalized_load_delta) const; + template [[nodiscard]] T combinedFy_Kappa_s(float normalized_load_delta, const T &slip_ratio) const; + [[nodiscard]] constexpr float combinedFy_Cyk() const; + [[nodiscard]] float combinedFy_Eyk(float normalized_load_delta) const; + [[nodiscard]] float combinedFy_Byk(float slip_angle_rad) const; [[nodiscard]] float combinedFy_Dvyk(float normal_load_N, float normalized_load_delta, float slip_angle_rad) const; + template [[nodiscard]] T combinedFy_Svyk(const T &d_vyk, const T &slip_ratio) const; template - [[nodiscard]] T combinedFy_Svyk(const T& d_vyk, const T& slip_ratio) const; - template - [[nodiscard]] T combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients& coefficients) const; + [[nodiscard]] T combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients &coefficients) const; template - [[nodiscard]] T combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients& coefficients) const; + [[nodiscard]] T combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients &coefficients) const; template - [[nodiscard]] CombinedFyMagicFormulaCoefficients combinedFyMagicFormulaCoefficients( - float normal_load_N, float slip_angle_rad, const T& slip_ratio) const; + [[nodiscard]] CombinedFyMagicFormulaCoefficients + combinedFyMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; [[nodiscard]] float wheelLongOffset_m() const; [[nodiscard]] float wheelLatOffset_m() const; - [[nodiscard]] float wheelSteeringAngle_rad(const float steering_angle_rad) const; - [[nodiscard]] WheelVelocities wheelVelocities( - float vehicle_velocity_x_mps, float vehicle_velocity_y_mps, float yaw_rate_radps) const; - [[nodiscard]] float estimateSlipAngle( - float wheel_vel_x_mps, float wheel_vel_y_mps, float steering_angle_rad) const; - [[nodiscard]] float estimateSlipRatio( - float wheel_vel_x_mps, float wheel_vel_y_mps, float slip_angle_rad, float wheel_angular_velocity_radps) const; - template - [[nodiscard]] T computePureFx_N(float normal_load_N, const T& slip_ratio) const; - [[nodiscard]] float computePureFy_N(float normal_load_N, float slip_angle_rad) const; + [[nodiscard]] float wheelSteeringAngle_rad(float steering_angle_rad) const; + [[nodiscard]] WheelVelocities + wheelVelocities(float vehicle_velocity_x_mps, float vehicle_velocity_y_mps, float yaw_rate_radps) const; + [[nodiscard]] float estimateSlipAngle(float wheel_vel_x_mps, float wheel_vel_y_mps, float steering_angle_rad) const; + [[nodiscard]] static float estimateSlipRatio( + float wheel_vel_x_mps, + float wheel_vel_y_mps, + float slip_angle_rad, + float wheel_angular_velocity_radps); + template [[nodiscard]] T computePureFx_N(float normal_load_N, const T &slip_ratio) const; + [[nodiscard]] float computePureFy_N(float normal_load_N, float slip_angle_rad) const; const TireFitPureParamFx fit_pure_fx_; const TireFitPureParamFy fit_pure_fy_; const TireFitCombParamFx fit_comb_fx_; const TireFitCombParamFy fit_comb_fy_; - const WheelSide wheel_side_; - const WheelAxle wheel_axle_; + const WheelSide wheel_side_; + const WheelAxle wheel_axle_; }; } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp index 00b824594c..8a6086edfb 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp @@ -250,7 +250,7 @@ VehicleStateEstimator::Outputs VehicleStateEstimator::estimate(const Inputs& inp previous_state); z(static_cast(R)) = measured_yaw_rate_radps; - z(static_cast(MZ)) = dynamics_estimator_.estimateYawMoment_Nm( + z(static_cast(MZ)) = dynamics_estimator_.est_Mz_N( inputs.longitudinal_forces_N, inputs.lateral_forces_N, measured_steering_angle); diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp index faf36df3d1..c74e81eb38 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp @@ -10,32 +10,24 @@ namespace app::tv::estimation { class VehicleStateEstimator { -public: - struct Inputs - { - datatypes::datatypes::VehicleState measured_state{}; - datatypes::datatypes::wheel_set wheel_angular_velocities_radps{}; - datatypes::datatypes::wheel_set longitudinal_forces_N{}; - datatypes::datatypes::wheel_set lateral_forces_N{}; - }; - + public: struct Outputs { datatypes::datatypes::VehicleState vehicle_state{}; float yaw_moment_nm = 0.0f; }; - using Filter = app::state_estimation::ekf; + using Filter = app::state_estimation::ekf; using Covariance = Filter::N_N; VehicleStateEstimator(); - [[nodiscard]] Outputs estimate(const Inputs& inputs); - [[nodiscard]] const Outputs& outputs() const; - [[nodiscard]] const Covariance& covariance() const; - void reset(); + [[nodiscard]] Outputs estimate(const shared_datatypes::datatypes::VehicleState &state); + [[nodiscard]] const Outputs &outputs() const; + [[nodiscard]] const Covariance &covariance() const; + void reset(); -private: + private: [[nodiscard]] static Filter createFilter(); Filter filter_; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index 1da9e65fa6..61f405eeaf 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -9,14 +9,14 @@ namespace app::tv::shared_datatypes::vd_constants // PHYSICAL CONSTANTS // ============================================================================= -inline constexpr float GRAVITY = 9.81f; // m/s^2 -inline constexpr float SMALL_EPSILON = 0.000001f; // Numerical stability for division -inline constexpr float FRONTAL_AREA_M2 = 0.94f; // m^2 from aero team -inline constexpr float AIR_DENSITY_KGPM3 = 1.2205f; // kg/m^3 -inline constexpr float LIFT_COEFF = 1.7f; // from aero team -inline constexpr float DRAG_COEFF = 0.92f; -inline constexpr float COP_REAR = 0.68f; // fraction of aero load acting behind the CG -inline constexpr float COP_RIGHT = 0.5f; // fraction of aero load acting on the right side +inline constexpr float GRAVITY = 9.81f; // m/s^2 +inline constexpr float SMALL_EPSILON = 0.000001f; // Numerical stability for division +inline constexpr float FRONTAL_AREA_M2 = 0.94f; // m^2 from aero team +inline constexpr float AIR_DENSITY_KGPM3 = 1.2205f; // kg/m^3 +inline constexpr float LIFT_COEFF = 1.7f; // from aero team +inline constexpr float DRAG_COEFF = 0.92f; +inline constexpr float COP_REAR = 0.68f; // fraction of aero load acting behind the CG +inline constexpr float COP_RIGHT = 0.5f; // fraction of aero load acting on the right side // ============================================================================= // VEHICLE DIMENSIONS @@ -41,7 +41,7 @@ inline constexpr float DIST_REAR_AXLE_CG_m = inline constexpr float DIST_HEIGHT_CG_m = 30.0f * CM_TO_M; // CG height (from suspension team) // Derived weight distribution properties -inline constexpr float CAR_WEIGHT = CAR_MASS_AT_CG_KG * GRAVITY; +inline constexpr float CAR_WEIGHT = CAR_MASS_AT_CG_KG * GRAVITY; inline constexpr float WEIGHT_ACROSS_BODY = CAR_MASS_AT_CG_KG * GRAVITY / WHEELBASE_m; inline constexpr float REAR_WEIGHT_DISTRIBUTION = WEIGHT_ACROSS_BODY * DIST_REAR_AXLE_CG_m; inline constexpr float FRONT_WEIGHT_DISTRIBUTION = WEIGHT_ACROSS_BODY * DIST_FRONT_AXLE_CG_m; @@ -104,7 +104,7 @@ inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = * Input: torque in Nm * Output: int16_t representing (torque/nominal) * 1000 */ -[[nodiscard]] inline constexpr int16_t MOTOR_TORQUE_REQUEST(const float torque) +[[nodiscard]] constexpr int16_t MOTOR_TORQUE_REQUEST(const float torque) { return static_cast((torque / NOMINAL_TORQUE_REQUEST_NM) * 1000.0f); } @@ -112,7 +112,7 @@ inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = /** * Convert torque and RPM to power (kW) */ -[[nodiscard]] inline constexpr float TORQUE_TO_POWER(const float torque, const float rpm) +[[nodiscard]] constexpr float TORQUE_TO_POWER(const float torque, const float rpm) { return (torque * (rpm / GEAR_RATIO)) / static_cast(POWER_TO_TORQUE_CONVERSION_FACTOR); } @@ -121,12 +121,11 @@ inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = * Convert power (kW) and RPM to torque (Nm) * Includes safety guard against division by zero */ -[[nodiscard]] inline constexpr float POWER_TO_TORQUE(const float power, const float rpm) +[[nodiscard]] constexpr float POWER_TO_TORQUE(const float power, const float rpm) { return (power * static_cast(POWER_TO_TORQUE_CONVERSION_FACTOR)) / (std::fmax(rpm, 0.00001f) / GEAR_RATIO); } - // ============================================================================= // EXTERNAL CONFIGURATION (Commented Out) // ============================================================================= diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 28768d5c37..9d21eefb9d 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -2,9 +2,7 @@ namespace app::tv::shared_datatypes::datatypes { - -template -struct wheel_set +template struct wheel_set { T fl; T fr; @@ -14,12 +12,17 @@ struct wheel_set struct VehicleState { - float v_x_mps = 0.0f; - float v_y_mps = 0.0f; - float yaw_rate_radps = 0.0f; - float steer_ang_rad = 0.0f; - float a_x_mps2 = 0.0f; - float a_y_mps2 = 0.0f; + float v_x_mps = 0.0f; + float v_y_mps = 0.0f; + float yaw_rate_radps = 0.0f; + float steer_ang_rad = 0.0f; + float a_x_mps2 = 0.0f; + float a_y_mps2 = 0.0f; + float pedal_percentage = 0.0f; + + wheel_set wheel_angular_velocities_radps{}; + wheel_set longitudinal_forces_N{}; + wheel_set lateral_forces_N{}; }; } // namespace app::tv::shared_datatypes::datatypes diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index bd3ff211d8..e3d4c0dd7d 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -13,109 +13,103 @@ namespace app::tv { namespace { -using namespace shared_datatypes::datatypes; -using namespace shared_datatypes::vd_constants; - -//------------------------------------- ESTIMATION MODULES ----------------------------------// - -static estimation::vehicleDynamics vehicle_dynamics_estimator{}; -static estimation::VehicleStateEstimator vehicle_state_estimator{}; -static wheel_set tire_models{ - .fl = estimation::TireModel(estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Front), - .fr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Front), - .rl = estimation::TireModel(estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Rear), - .rr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Rear), -}; - -//------------------------------------- CONTROLLERS -----------------------------------------// - -static controllers::allocator::TorqueAllocator torque_allocator{}; - -//------------------------------------- STATE VARIABLES -------------------------------------// - -// These wheel-set caches carry the previous update's estimated tire state/forces into the next cycle. -// The state estimator and optimizer both consume them as warm starts / feedback, so they remain module-local. -static wheel_set acc_f_x{}; -static wheel_set acc_f_y{}; -static wheel_set acc_f_z{}; -static wheel_set acc_slip_ratio{}; -static wheel_set acc_slip_angle{}; -static wheel_set slip_ratio_opt{}; - -// Build TireModel::StateInputs from VehicleState + per-wheel data -[[nodiscard]] estimation::TireModel::StateInputs buildTireInputs( - const VehicleState& state, const float omega, const float fz) -{ - return { - .wheel_angular_velocity_radps = omega, - .vehicle_velocity_x_mps = state.v_x_mps, - .vehicle_velocity_y_mps = state.v_y_mps, - .yaw_rate_radps = state.yaw_rate_radps, - .steering_angle_rad = state.steer_ang_rad, - .normal_load_N = fz, - }; -} + using namespace shared_datatypes::datatypes; + using namespace shared_datatypes::vd_constants; -// Copy the current tire estimates into the module-local wheel-set caches so the rest of the -// pipeline can use a consistent per-wheel state snapshot for this update tick. -void unpackTireOutputs( - const estimation::TireModel::Outputs& fl, - const estimation::TireModel::Outputs& fr, - const estimation::TireModel::Outputs& rl, - const estimation::TireModel::Outputs& rr) -{ - acc_f_x = { .fl = fl.longitudinal_force_N, .fr = fr.longitudinal_force_N, - .rl = rl.longitudinal_force_N, .rr = rr.longitudinal_force_N }; - acc_f_y = { .fl = fl.lateral_force_N, .fr = fr.lateral_force_N, - .rl = rl.lateral_force_N, .rr = rr.lateral_force_N }; - acc_slip_ratio = { .fl = fl.slip_ratio, .fr = fr.slip_ratio, - .rl = rl.slip_ratio, .rr = rr.slip_ratio }; - acc_slip_angle = { .fl = fl.slip_angle_rad, .fr = fr.slip_angle_rad, - .rl = rl.slip_angle_rad, .rr = rr.slip_angle_rad }; -} + //------------------------------------- ESTIMATION MODULES ----------------------------------// + estimation::vehicleDynamics vehicle_dynamics_estimator{}; + estimation::VehicleStateEstimator vehicle_state_estimator{}; + + wheel_set tire_models{ + .fl = estimation::TireModel(estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Front), + .fr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Front), + .rl = estimation::TireModel(estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Rear), + .rr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Rear), + }; + + //------------------------------------- CONTROLLERS -----------------------------------------// + + controllers::allocator::TorqueAllocator torque_allocator{}; + + //------------------------------------- STATE VARIABLES -------------------------------------// + + // These wheel-set caches carry the previous update's estimated tire state/forces into the next cycle. + // The state estimator and optimizer both consume them as warm starts / feedback, so they remain module-local. + wheel_set acc_f_x{}; + wheel_set acc_f_y{}; + wheel_set acc_f_z{}; + wheel_set acc_slip_ratio{}; + wheel_set acc_slip_angle{}; + wheel_set slip_ratio_opt{}; + + // Build TireModel::StateInputs from VehicleState + per-wheel data + [[nodiscard]] estimation::TireModel::StateInputs + buildTireInputs(const VehicleState &state, const float omega, const float fz) + { + return { + .wheel_angular_velocity_radps = omega, + .vehicle_velocity_x_mps = state.v_x_mps, + .vehicle_velocity_y_mps = state.v_y_mps, + .yaw_rate_radps = state.yaw_rate_radps, + .steering_angle_rad = state.steer_ang_rad, + .normal_load_N = fz, + }; + } + + // Copy the current tire estimates into the module-local wheel-set caches so the rest of the + // pipeline can use a consistent per-wheel state snapshot for this update tick. + void unpackTireOutputs( + const estimation::TireModel::Outputs &fl, + const estimation::TireModel::Outputs &fr, + const estimation::TireModel::Outputs &rl, + const estimation::TireModel::Outputs &rr) + { + acc_f_x = { .fl = fl.longitudinal_force_N, + .fr = fr.longitudinal_force_N, + .rl = rl.longitudinal_force_N, + .rr = rr.longitudinal_force_N }; + acc_f_y = { + .fl = fl.lateral_force_N, .fr = fr.lateral_force_N, .rl = rl.lateral_force_N, .rr = rr.lateral_force_N + }; + acc_slip_ratio = { .fl = fl.slip_ratio, .fr = fr.slip_ratio, .rl = rl.slip_ratio, .rr = rr.slip_ratio }; + acc_slip_angle = { + .fl = fl.slip_angle_rad, .fr = fr.slip_angle_rad, .rl = rl.slip_angle_rad, .rr = rr.slip_angle_rad + }; + } } // namespace //------------------------------------- TORQUE VECTORING UPDATE CYCLE -------------------------// -void update(const VehicleState& state, const float pedal_percentage, - const float fl_omega, const float fr_omega, - const float rl_omega, const float rr_omega) +void update(const VehicleState &state) { //------------------------------------- STATE ESTIMATION --------------------------------// // Fuse the measured chassis state with the previous tire-force estimate to obtain the // filtered vehicle state used consistently by all downstream control blocks this tick. - const auto state_estimate = vehicle_state_estimator.estimate({ - .measured_state = state, - .wheel_angular_velocities_radps = { .fl = fl_omega, .fr = fr_omega, .rl = rl_omega, .rr = rr_omega }, - .longitudinal_forces_N = acc_f_x, - .lateral_forces_N = acc_f_y, - }); - - const VehicleState estimated_state = state_estimate.vehicle_state; + const auto [estimated_state, _yaw_moment_nm] = vehicle_state_estimator.estimate(state); // Normal forces from longitudinal/lateral load transfer + downforce - acc_f_z = vehicle_dynamics_estimator.estimateNormalForce_N( + acc_f_z = vehicle_dynamics_estimator.est_Fz_N( estimated_state.a_x_mps2, estimated_state.a_y_mps2, estimated_state.v_x_mps); // Tire model estimation from current sensor data using the filtered vehicle state. - const auto fl_out = tire_models.fl.estimate(buildTireInputs(estimated_state, fl_omega, acc_f_z.fl)); - const auto fr_out = tire_models.fr.estimate(buildTireInputs(estimated_state, fr_omega, acc_f_z.fr)); - const auto rl_out = tire_models.rl.estimate(buildTireInputs(estimated_state, rl_omega, acc_f_z.rl)); - const auto rr_out = tire_models.rr.estimate(buildTireInputs(estimated_state, rr_omega, acc_f_z.rr)); - unpackTireOutputs(fl_out, fr_out, rl_out, rr_out); + unpackTireOutputs( + tire_models.fl.estimate(buildTireInputs(estimated_state, fl_omega, acc_f_z.fl)), + tire_models.fr.estimate(buildTireInputs(estimated_state, fr_omega, acc_f_z.fr)), + tire_models.rl.estimate(buildTireInputs(estimated_state, rl_omega, acc_f_z.rl)), + tire_models.rr.estimate(buildTireInputs(estimated_state, rr_omega, acc_f_z.rr))); //------------------------------------- HIGH LEVEL CONTROLLER ----------------------------// // Convert the driver's pedal request into a nominal per-wheel longitudinal force target. // The cosine projection keeps the request aligned with the tire heading under nonzero slip angle. const float per_wheel_tq = MAX_TORQUE_REQUEST_NM * pedal_percentage; - wheel_set des_f_x = { - .fl = per_wheel_tq * std::cos(acc_slip_angle.fl) * WHEEL_RADIUS_M , - .fr = per_wheel_tq * std::cos(acc_slip_angle.fr) * WHEEL_RADIUS_M, - .rl = per_wheel_tq * std::cos(acc_slip_angle.rl) * WHEEL_RADIUS_M, - .rr = per_wheel_tq * std::cos(acc_slip_angle.rr) * WHEEL_RADIUS_M, + wheel_set des_f_x = { + .fl = per_wheel_tq * std::cos(acc_slip_angle.fl) * WHEEL_RADIUS_M, + .fr = per_wheel_tq * std::cos(acc_slip_angle.fr) * WHEEL_RADIUS_M, + .rl = per_wheel_tq * std::cos(acc_slip_angle.rl) * WHEEL_RADIUS_M, + .rr = per_wheel_tq * std::cos(acc_slip_angle.rr) * WHEEL_RADIUS_M, }; // Direct yaw rate control: corrective yaw moment @@ -141,13 +135,7 @@ void update(const VehicleState& state, const float pedal_percentage, // Gauss-Newton optimizer: invert the combined-slip tire model to find the slip ratios whose // predicted forces best match the desired per-wheel force split and desired yaw moment. slip_ratio_opt = torque_allocator.optimize( - tire_models, - des_f_x, - acc_f_z, - acc_slip_ratio, - acc_slip_angle, - low_speed_blend, - estimated_state.steer_ang_rad, + tire_models, des_f_x, acc_f_z, acc_slip_ratio, acc_slip_angle, low_speed_blend, estimated_state.steer_ang_rad, des_yaw_moment_nm); //------------------------------------- POWER LIMITER -----------------------------------// diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp new file mode 100644 index 0000000000..7e41d51cbe --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -0,0 +1,4 @@ +#pragma once +#include "shared_datatypes/datatypes.hpp" + +void update(const app::tv::shared_datatypes::datatypes::VehicleState &state); \ No newline at end of file From 3600b8256f0b40ca6131562b7363ad75c1dd24bb Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Wed, 8 Apr 2026 17:54:06 -0700 Subject: [PATCH 06/35] small changes --- .../estimation/vehicle_state_estimator.cpp | 388 ++++++++---------- .../estimation/vehicle_state_estimator.hpp | 31 +- .../shared_datatypes/datatypes.hpp | 15 + .../app/torque_vectoring/torque_vectoring.cpp | 33 +- .../app/torque_vectoring/torque_vectoring.hpp | 25 +- .../state_estimation/app_kalman_filter.hpp | 13 +- 6 files changed, 244 insertions(+), 261 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp index 8a6086edfb..eed19f1d77 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp @@ -11,253 +11,209 @@ namespace app::tv::estimation { namespace { -using Filter = VehicleStateEstimator::Filter; -using StateInput = Filter::state_inp_mtx; -using State = Filter::state_mtx; -using StateVector = Filter::N_1; -using InputVector = Filter::U_1; -using Measurement = Filter::M_1; + using Filter = VehicleStateEstimator::Filter; + using StateInput = Filter::state_inp_mtx; + using State = Filter::state_mtx; + using StateVector = Filter::N_1; + using InputVector = Filter::U_1; + using Measurement = Filter::M_1; -static constexpr std::size_t VX = 0; -static constexpr std::size_t VY = 1; -static constexpr std::size_t R = 2; -static constexpr std::size_t MZ = 3; + constexpr std::size_t VX = 0; + constexpr std::size_t VY = 1; + constexpr std::size_t R = 2; + constexpr std::size_t MZ = 3; -static constexpr std::size_t AX = 0; -static constexpr std::size_t AY = 1; + constexpr std::size_t AX = 0; + constexpr std::size_t AY = 1; -static constexpr float ESTIMATOR_DT_S = 0.01f; // Matches the 100 Hz control task. -static constexpr float ESTIMATOR_YAW_INERTIA = 110.0f; // TODO: Replace with measured Hexray yaw inertia. + constexpr float ESTIMATOR_DT_S = 0.01f; // Matches the 100 Hz control task. + constexpr float ESTIMATOR_YAW_INERTIA = 110.0f; // TODO: Replace with measured Hexray yaw inertia. -[[nodiscard]] autodiff::dual stateTransitionVx(const StateInput& x) -{ - const autodiff::dual& v_x = x(static_cast(VX)); - const autodiff::dual& v_y = x(static_cast(VY)); - const autodiff::dual& r = x(static_cast(R)); - const autodiff::dual& a_x = x(static_cast(4 + AX)); - - return v_x + (ESTIMATOR_DT_S * (a_x + (v_y * r))); -} - -[[nodiscard]] autodiff::dual stateTransitionVy(const StateInput& x) -{ - const autodiff::dual& v_x = x(static_cast(VX)); - const autodiff::dual& v_y = x(static_cast(VY)); - const autodiff::dual& r = x(static_cast(R)); - const autodiff::dual& a_y = x(static_cast(4 + AY)); - - return v_y + (ESTIMATOR_DT_S * (a_y - (v_x * r))); -} - -[[nodiscard]] autodiff::dual stateTransitionYawRate(const StateInput& x) -{ - const autodiff::dual& r = x(static_cast(R)); - const autodiff::dual& mz = x(static_cast(MZ)); - - return r + (ESTIMATOR_DT_S * (mz / ESTIMATOR_YAW_INERTIA)); -} - -[[nodiscard]] autodiff::dual stateTransitionYawMoment(const StateInput& x) -{ - return x(static_cast(MZ)); -} - -[[nodiscard]] autodiff::dual measurementVx(const State& x) -{ - return x(static_cast(VX)); -} - -[[nodiscard]] autodiff::dual measurementVy(const State& x) -{ - return x(static_cast(VY)); -} + [[nodiscard]] constexpr autodiff::dual stateTransitionVx(const StateInput &x) + { + const autodiff::dual &v_x = x(static_cast(VX)); + const autodiff::dual &v_y = x(static_cast(VY)); + const autodiff::dual &r = x(static_cast(R)); + const autodiff::dual &a_x = x(static_cast(4 + AX)); -[[nodiscard]] autodiff::dual measurementYawRate(const State& x) -{ - return x(static_cast(R)); -} + return v_x + (ESTIMATOR_DT_S * (a_x + (v_y * r))); + } -[[nodiscard]] autodiff::dual measurementYawMoment(const State& x) -{ - return x(static_cast(MZ)); -} + [[nodiscard]] constexpr autodiff::dual stateTransitionVy(const StateInput &x) + { + const autodiff::dual &v_x = x(static_cast(VX)); + const autodiff::dual &v_y = x(static_cast(VY)); + const autodiff::dual &r = x(static_cast(R)); + const autodiff::dual &a_y = x(static_cast(4 + AY)); -[[nodiscard]] auto createStateFunctions() -> std::array -{ - return { { - stateTransitionVx, - stateTransitionVy, - stateTransitionYawRate, - stateTransitionYawMoment, - } }; -} + return v_y + (ESTIMATOR_DT_S * (a_y - (v_x * r))); + } -[[nodiscard]] auto createMeasurementFunctions() -> std::array -{ - return { { - measurementVx, - measurementVy, - measurementYawRate, - measurementYawMoment, - } }; -} + [[nodiscard]] constexpr autodiff::dual stateTransitionYawRate(const StateInput &x) + { + const autodiff::dual &r = x(static_cast(R)); + const autodiff::dual &mz = x(static_cast(MZ)); -[[nodiscard]] auto processNoise() -> Filter::N_N -{ - Filter::N_N q = Filter::N_N::Zero(); - q(static_cast(VX), static_cast(VX)) = 0.05f; - q(static_cast(VY), static_cast(VY)) = 0.05f; - q(static_cast(R), static_cast(R)) = 0.10f; - q(static_cast(MZ), static_cast(MZ)) = 150.0f; - return q; -} + return r + (ESTIMATOR_DT_S * (mz / ESTIMATOR_YAW_INERTIA)); + } -[[nodiscard]] auto measurementNoise() -> Filter::M_M -{ - Filter::M_M r = Filter::M_M::Zero(); - r(static_cast(VX), static_cast(VX)) = 0.75f; - r(static_cast(VY), static_cast(VY)) = 0.75f; - r(static_cast(R), static_cast(R)) = 0.05f; - r(static_cast(MZ), static_cast(MZ)) = 250.0f; - return r; -} + [[nodiscard]] constexpr autodiff::dual stateTransitionYawMoment(const StateInput &x) + { + return x(static_cast(MZ)); + } -[[nodiscard]] auto initialState() -> StateVector -{ - return StateVector::Zero(); -} + [[nodiscard]] constexpr autodiff::dual measurementVx(const State &x) + { + return x(static_cast(VX)); + } -[[nodiscard]] auto initialCovariance() -> Filter::N_N -{ - Filter::N_N p0 = Filter::N_N::Identity(); - p0(static_cast(VX), static_cast(VX)) = 5.0f; - p0(static_cast(VY), static_cast(VY)) = 5.0f; - p0(static_cast(R), static_cast(R)) = 1.0f; - p0(static_cast(MZ), static_cast(MZ)) = 400.0f; - return p0; -} + [[nodiscard]] constexpr autodiff::dual measurementVy(const State &x) + { + return x(static_cast(VY)); + } -[[nodiscard]] float readMeasuredLongitudinalAccelMps2(const VehicleStateEstimator::Inputs& inputs) -{ - // TODO: Replace with app_canRx_* when the Hexray signal table is ready. - return inputs.measured_state.a_x_mps2; -} + [[nodiscard]] constexpr autodiff::dual measurementYawRate(const State &x) + { + return x(static_cast(R)); + } -[[nodiscard]] float readMeasuredLateralAccelMps2(const VehicleStateEstimator::Inputs& inputs) -{ - // TODO: Replace with app_canRx_* when the Hexray signal table is ready. - return inputs.measured_state.a_y_mps2; -} + [[nodiscard]] constexpr autodiff::dual measurementYawMoment(const State &x) + { + return x(static_cast(MZ)); + } -[[nodiscard]] float readMeasuredYawRateRadps(const VehicleStateEstimator::Inputs& inputs) -{ - // TODO: Replace with app_canRx_* when the Hexray signal table is ready. - return inputs.measured_state.yaw_rate_radps; -} + [[nodiscard]] consteval std::array createStateFunctions() + { + return { { + stateTransitionVx, + stateTransitionVy, + stateTransitionYawRate, + stateTransitionYawMoment, + } }; + } -[[nodiscard]] float readMeasuredSteeringAngleRad(const VehicleStateEstimator::Inputs& inputs) -{ - // TODO: Replace with app_canRx_* when the Hexray signal table is ready. - return inputs.measured_state.steer_ang_rad; -} + [[nodiscard]] consteval std::array createMeasurementFunctions() + { + return { { + measurementVx, + measurementVy, + measurementYawRate, + measurementYawMoment, + } }; + } -[[nodiscard]] datatypes::datatypes::wheel_set readWheelAngularVelocitiesRadps( - const VehicleStateEstimator::Inputs& inputs) -{ - // TODO: Replace with app_canRx_* when the Hexray signal table is ready. - return inputs.wheel_angular_velocities_radps; -} + [[nodiscard]] consteval Filter::N_N processNoise() + { + Filter::N_N q = Filter::N_N::Zero(); + q(static_cast(VX), static_cast(VX)) = 0.05f; + q(static_cast(VY), static_cast(VY)) = 0.05f; + q(static_cast(R), static_cast(R)) = 0.10f; + q(static_cast(MZ), static_cast(MZ)) = 150.0f; + return q; + } -[[nodiscard]] Measurement pseudoMeasurementFromWheelSpeeds( - const datatypes::datatypes::wheel_set& wheel_angular_velocities_radps, - const float yaw_rate_radps, - const float steering_angle_rad, - const StateVector& previous_state) -{ - const float front_cos = std::cos(steering_angle_rad); - const float front_sin = std::sin(steering_angle_rad); - const float half_track_m = TRACK_WIDTH_m * 0.5f; - - const std::array wheel_surface_speeds_mps = { - wheel_angular_velocities_radps.fl * WHEEL_RADIUS_M, - wheel_angular_velocities_radps.fr * WHEEL_RADIUS_M, - wheel_angular_velocities_radps.rl * WHEEL_RADIUS_M, - wheel_angular_velocities_radps.rr * WHEEL_RADIUS_M, - }; + [[nodiscard]] consteval Filter::M_M measurementNoise() + { + Filter::M_M r = Filter::M_M::Zero(); + r(static_cast(VX), static_cast(VX)) = 0.75f; + r(static_cast(VY), static_cast(VY)) = 0.75f; + r(static_cast(R), static_cast(R)) = 0.05f; + r(static_cast(MZ), static_cast(MZ)) = 250.0f; + return r; + } - const float speed_sum_mps = - std::fabs(wheel_surface_speeds_mps[0]) + std::fabs(wheel_surface_speeds_mps[1]) + - std::fabs(wheel_surface_speeds_mps[2]) + std::fabs(wheel_surface_speeds_mps[3]); + [[nodiscard]] consteval StateVector initialState() + { + return StateVector::Zero(); + } - Measurement z = Measurement::Zero(); - z(static_cast(VX)) = previous_state(static_cast(VX)); - z(static_cast(VY)) = previous_state(static_cast(VY)); + [[nodiscard]] consteval Filter::N_N initialCovariance() + { + Filter::N_N p0 = Filter::N_N::Identity(); + p0(static_cast(VX), static_cast(VX)) = 5.0f; + p0(static_cast(VY), static_cast(VY)) = 5.0f; + p0(static_cast(R), static_cast(R)) = 1.0f; + p0(static_cast(MZ), static_cast(MZ)) = 400.0f; + return p0; + } - if (speed_sum_mps <= SMALL_EPSILON) + [[nodiscard]] Measurement pseudoMeasurementFromWheelSpeeds( + const datatypes::datatypes::wheel_set &wheel_angular_velocities_radps, + const float yaw_rate_radps, + const float steering_angle_rad, + const StateVector &previous_state) { - z(static_cast(VX)) = 0.0f; - z(static_cast(VY)) = 0.0f; + const float front_cos = std::cos(steering_angle_rad); + const float front_sin = std::sin(steering_angle_rad); + constexpr float half_track_m = TRACK_WIDTH_m * 0.5f; + + const std::array wheel_surface_speeds_mps = { { + wheel_angular_velocities_radps.fl * WHEEL_RADIUS_M, + wheel_angular_velocities_radps.fr * WHEEL_RADIUS_M, + wheel_angular_velocities_radps.rl * WHEEL_RADIUS_M, + wheel_angular_velocities_radps.rr * WHEEL_RADIUS_M, + } }; + + const float speed_sum_mps = std::fabs(wheel_surface_speeds_mps[0]) + std::fabs(wheel_surface_speeds_mps[1]) + + std::fabs(wheel_surface_speeds_mps[2]) + std::fabs(wheel_surface_speeds_mps[3]); + + Measurement z = Measurement::Zero(); + z(static_cast(VX)) = previous_state(static_cast(VX)); + z(static_cast(VY)) = previous_state(static_cast(VY)); + + if (speed_sum_mps <= SMALL_EPSILON) + { + z(static_cast(VX)) = 0.0f; + z(static_cast(VY)) = 0.0f; + return z; + } + + const float fl_vx = (wheel_surface_speeds_mps[0] * front_cos) + (yaw_rate_radps * half_track_m); + const float fr_vx = (wheel_surface_speeds_mps[1] * front_cos) - (yaw_rate_radps * half_track_m); + const float rl_vx = wheel_surface_speeds_mps[2] + (yaw_rate_radps * half_track_m); + const float rr_vx = wheel_surface_speeds_mps[3] - (yaw_rate_radps * half_track_m); + + const float fl_vy = (wheel_surface_speeds_mps[0] * front_sin) - (yaw_rate_radps * DIST_FRONT_AXLE_CG_m); + const float fr_vy = (wheel_surface_speeds_mps[1] * front_sin) - (yaw_rate_radps * DIST_FRONT_AXLE_CG_m); + const float rl_vy = yaw_rate_radps * DIST_REAR_AXLE_CG_m; + const float rr_vy = yaw_rate_radps * DIST_REAR_AXLE_CG_m; + + z(static_cast(VX)) = 0.25f * (fl_vx + fr_vx + rl_vx + rr_vx); + z(static_cast(VY)) = 0.25f * (fl_vy + fr_vy + rl_vy + rr_vy); return z; } - - const float fl_vx = (wheel_surface_speeds_mps[0] * front_cos) + (yaw_rate_radps * half_track_m); - const float fr_vx = (wheel_surface_speeds_mps[1] * front_cos) - (yaw_rate_radps * half_track_m); - const float rl_vx = wheel_surface_speeds_mps[2] + (yaw_rate_radps * half_track_m); - const float rr_vx = wheel_surface_speeds_mps[3] - (yaw_rate_radps * half_track_m); - - const float fl_vy = (wheel_surface_speeds_mps[0] * front_sin) - (yaw_rate_radps * DIST_FRONT_AXLE_CG_m); - const float fr_vy = (wheel_surface_speeds_mps[1] * front_sin) - (yaw_rate_radps * DIST_FRONT_AXLE_CG_m); - const float rl_vy = yaw_rate_radps * DIST_REAR_AXLE_CG_m; - const float rr_vy = yaw_rate_radps * DIST_REAR_AXLE_CG_m; - - z(static_cast(VX)) = 0.25f * (fl_vx + fr_vx + rl_vx + rr_vx); - z(static_cast(VY)) = 0.25f * (fl_vy + fr_vy + rl_vy + rr_vy); - return z; -} } // namespace -VehicleStateEstimator::VehicleStateEstimator() - : filter_(createFilter()) -{ -} - -auto VehicleStateEstimator::createFilter() -> Filter +constexpr Filter VehicleStateEstimator::createFilter() { return Filter( - createStateFunctions(), - createMeasurementFunctions(), - processNoise(), - measurementNoise(), - initialState(), + createStateFunctions(), createMeasurementFunctions(), processNoise(), measurementNoise(), initialState(), initialCovariance()); } -VehicleStateEstimator::Outputs VehicleStateEstimator::estimate(const Inputs& inputs) +[[nodiscard]] app::tv::shared_datatypes::datatypes::VehicleState + VehicleStateEstimator::estimate(const Measurements &state) const { - InputVector u = InputVector::Zero(); - u(static_cast(AX)) = readMeasuredLongitudinalAccelMps2(inputs); - u(static_cast(AY)) = readMeasuredLateralAccelMps2(inputs); + InputVector u = InputVector::Zero(); + u(static_cast(AX)) = state.ax; + u(static_cast(AY)) = state.ay; - const float measured_yaw_rate_radps = readMeasuredYawRateRadps(inputs); - const float measured_steering_angle = readMeasuredSteeringAngleRad(inputs); - const auto wheel_angular_velocities = readWheelAngularVelocitiesRadps(inputs); - const auto& previous_state = filter_.state(); + const float measured_yaw_rate_radps = state.yaw_rate; + const float measured_steering_angle = state.delta; + const datatypes::datatypes::wheel_set wheel_angular_velocities = state.omegas; + const auto &previous_state = filter_.state(); Measurement z = pseudoMeasurementFromWheelSpeeds( - wheel_angular_velocities, - measured_yaw_rate_radps, - measured_steering_angle, - previous_state); + wheel_angular_velocities, measured_yaw_rate_radps, measured_steering_angle, previous_state); - z(static_cast(R)) = measured_yaw_rate_radps; - z(static_cast(MZ)) = dynamics_estimator_.est_Mz_N( - inputs.longitudinal_forces_N, - inputs.lateral_forces_N, - measured_steering_angle); + z(static_cast(R)) = measured_yaw_rate_radps; + // z(static_cast(MZ)) = + // dynamics_estimator_.est_Mz_N(inputs.longitudinal_forces_N, inputs.lateral_forces_N, measured_steering_angle); const StateVector estimated_state = filter_.estimated_states(u, z); - outputs_.vehicle_state = { + // outputs_.yaw_moment_nm = estimated_state(static_cast(MZ)); + return { .v_x_mps = estimated_state(static_cast(VX)), .v_y_mps = estimated_state(static_cast(VY)), .yaw_rate_radps = estimated_state(static_cast(R)), @@ -265,24 +221,10 @@ VehicleStateEstimator::Outputs VehicleStateEstimator::estimate(const Inputs& inp .a_x_mps2 = u(static_cast(AX)), .a_y_mps2 = u(static_cast(AY)), }; - outputs_.yaw_moment_nm = estimated_state(static_cast(MZ)); - - return outputs_; } -const VehicleStateEstimator::Outputs& VehicleStateEstimator::outputs() const -{ - return outputs_; -} - -const VehicleStateEstimator::Covariance& VehicleStateEstimator::covariance() const +const VehicleStateEstimator::Covariance &VehicleStateEstimator::covariance() const { return filter_.covariance(); } - -void VehicleStateEstimator::reset() -{ - filter_ = createFilter(); - outputs_ = {}; -} } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp index c74e81eb38..adfe4707b0 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp @@ -11,27 +11,32 @@ namespace app::tv::estimation class VehicleStateEstimator { public: - struct Outputs + struct Measurements { - datatypes::datatypes::VehicleState vehicle_state{}; - float yaw_moment_nm = 0.0f; + // sensor measurements + const float ax; + const float ay; + const float yaw_rate; + + // driver controls + const float delta; + const float apps; + + const datatypes::datatypes::wheel_set omegas; }; using Filter = app::state_estimation::ekf; using Covariance = Filter::N_N; - VehicleStateEstimator(); + // constexpr VehicleStateEstimator() : filter_(createFilter()) {} + constexpr VehicleStateEstimator() : filter_(createFilter()) {} - [[nodiscard]] Outputs estimate(const shared_datatypes::datatypes::VehicleState &state); - [[nodiscard]] const Outputs &outputs() const; - [[nodiscard]] const Covariance &covariance() const; - void reset(); + [[nodiscard]] shared_datatypes::datatypes::VehicleState estimate(const Measurements &state) const; + [[nodiscard]] const Covariance &covariance() const; + // if you want to reset, just reconstruct the object private: - [[nodiscard]] static Filter createFilter(); - - Filter filter_; - vehicleDynamics dynamics_estimator_{}; - Outputs outputs_{}; + [[nodiscard]] static constexpr Filter createFilter(); + const Filter filter_; }; } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 9d21eefb9d..537957e28c 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -25,6 +25,21 @@ struct VehicleState wheel_set lateral_forces_N{}; }; +struct ControlOutput +{ + const float fl_omega_radps; + const float fr_omega_radps; + const float rl_omega_radps; + const float rr_omega_radps; +}; +struct ControlOutputAutonomous +{ + const float fl_omega_radps; + const float fr_omega_radps; + const float rl_omega_radps; + const float rr_omega_radps; + const float delta; +}; } // namespace app::tv::shared_datatypes::datatypes namespace app::tv::datatypes diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index e3d4c0dd7d..4e117d5046 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -17,8 +17,6 @@ namespace using namespace shared_datatypes::vd_constants; //------------------------------------- ESTIMATION MODULES ----------------------------------// - - estimation::vehicleDynamics vehicle_dynamics_estimator{}; estimation::VehicleStateEstimator vehicle_state_estimator{}; wheel_set tire_models{ @@ -34,15 +32,6 @@ namespace //------------------------------------- STATE VARIABLES -------------------------------------// - // These wheel-set caches carry the previous update's estimated tire state/forces into the next cycle. - // The state estimator and optimizer both consume them as warm starts / feedback, so they remain module-local. - wheel_set acc_f_x{}; - wheel_set acc_f_y{}; - wheel_set acc_f_z{}; - wheel_set acc_slip_ratio{}; - wheel_set acc_slip_angle{}; - wheel_set slip_ratio_opt{}; - // Build TireModel::StateInputs from VehicleState + per-wheel data [[nodiscard]] estimation::TireModel::StateInputs buildTireInputs(const VehicleState &state, const float omega, const float fz) @@ -57,6 +46,14 @@ namespace }; } + // These wheel-set caches carry the previous update's estimated tire state/forces into the next cycle. + // The state estimator and optimizer both consume them as warm starts / feedback, so they remain module-local. + wheel_set acc_f_x{}; + wheel_set acc_f_y{}; + wheel_set acc_f_z{}; + wheel_set acc_slip_ratio{}; + wheel_set acc_slip_angle{}; + wheel_set slip_ratio_opt{}; // Copy the current tire estimates into the module-local wheel-set caches so the rest of the // pipeline can use a consistent per-wheel state snapshot for this update tick. void unpackTireOutputs( @@ -80,18 +77,19 @@ namespace } // namespace //------------------------------------- TORQUE VECTORING UPDATE CYCLE -------------------------// - -void update(const VehicleState &state) +ControlOutput + update(const estimation::VehicleStateEstimator::Measurements &measurement, const float ax, const float omega_dot) { //------------------------------------- STATE ESTIMATION --------------------------------// // Fuse the measured chassis state with the previous tire-force estimate to obtain the // filtered vehicle state used consistently by all downstream control blocks this tick. - const auto [estimated_state, _yaw_moment_nm] = vehicle_state_estimator.estimate(state); + // const auto [estimated_state, _yaw_moment_nm] = vehicle_state_estimator.estimate(state); + const VehicleState state = vehicle_state_estimator.estimate(measurement); // Normal forces from longitudinal/lateral load transfer + downforce - acc_f_z = vehicle_dynamics_estimator.est_Fz_N( - estimated_state.a_x_mps2, estimated_state.a_y_mps2, estimated_state.v_x_mps); + // acc_f_z = vehicle_dynamics_estimator.est_Fz_N( + // estimated_state.a_x_mps2, estimated_state.a_y_mps2, estimated_state.v_x_mps); // Tire model estimation from current sensor data using the filtered vehicle state. unpackTireOutputs( @@ -139,8 +137,9 @@ void update(const VehicleState &state) des_yaw_moment_nm); //------------------------------------- POWER LIMITER -----------------------------------// - // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request + + return {}; } } // namespace app::tv diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp index 7e41d51cbe..aea9ec9f70 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -1,4 +1,27 @@ #pragma once +#include "estimation/vehicle_state_estimator.hpp" #include "shared_datatypes/datatypes.hpp" +#include "estimation/vehicle_state_estimator.hpp" -void update(const app::tv::shared_datatypes::datatypes::VehicleState &state); \ No newline at end of file +// app::tv::shared_datatypes::datatypes::VehicleState +// estimateVehicleState(); + +/** + * This is the main entrypoint into the low level vehicle controls algorithm + * @param state The current measured vehicle state + * @param ax Requested Longitudinal acceleration (m/s^2) + * @param omega_dot Requested Yaw acceleration (rad/s^2) + * @return The per-wheel torque requests to achieve the desired accelerations, in Newton-meters + */ +app::tv::shared_datatypes::datatypes::ControlOutput + update(const app::tv::estimation::VehicleStateEstimator::Measurements &state, float ax, float omega_dot); + +/** + * This is the main entrypoint into the low level vehicle controls algorithm for autonomous + * @param state The current measured vehicle state + * @param ax Requested Longitudinal acceleration (m/s^2) + * @param omega_dot Requested Yaw acceleration (rad/s^2) + * @return + */ +app::tv::shared_datatypes::datatypes::ControlOutputAutonomous + update_autonomous(const app::tv::estimation::VehicleStateEstimator::Measurements &state, float ax, float omega_dot); diff --git a/firmware/shared/srcpp/app/state_estimation/app_kalman_filter.hpp b/firmware/shared/srcpp/app/state_estimation/app_kalman_filter.hpp index 135c316fcf..0b323de7d8 100644 --- a/firmware/shared/srcpp/app/state_estimation/app_kalman_filter.hpp +++ b/firmware/shared/srcpp/app/state_estimation/app_kalman_filter.hpp @@ -24,7 +24,7 @@ namespace detail */ template auto symmetrize(const Mat &matrix) -> Mat { - return (matrix + matrix.transpose()) * static_cast(0.5); + return (matrix + matrix.transpose()) * static_cast(0.5); } /** @@ -34,7 +34,7 @@ namespace detail * in constructors before the filter is used. */ template - auto is_symmetric(const Mat &matrix, typename Mat::Scalar tol = static_cast(1e-6)) -> bool + auto is_symmetric(const Mat &matrix, typename Mat::Scalar tol = static_cast(1e-6)) -> bool { return matrix.isApprox(matrix.transpose(), tol); } @@ -47,8 +47,7 @@ namespace detail * a small negative tolerance to handle numerical round-off. */ template - auto is_positive_semidefinite(const Mat &matrix, typename Mat::Scalar tol = static_cast(1e-8)) - -> bool + auto is_positive_semidefinite(const Mat &matrix, typename Mat::Scalar tol = static_cast(1e-8)) -> bool { const Mat symmetric = symmetrize(matrix); Eigen::LDLT ldlt(symmetric); @@ -152,7 +151,7 @@ template Date: Thu, 9 Apr 2026 10:07:12 -0700 Subject: [PATCH 07/35] a few more small changes --- .../controllers/controllers_config.hpp | 1 + .../torque_allocator/torque_allocator.cpp | 135 ++++++------ .../torque_allocator/torque_allocator.hpp | 33 +-- .../yaw_rate_control/controllers_dyrc.cpp | 7 +- .../estimation/dynamics_estimation.cpp | 76 ------- .../estimation/dynamics_estimation.hpp | 119 ---------- .../estimation/vehicle_state_estimator.cpp | 99 +++++---- .../estimation/vehicle_state_estimator.hpp | 42 ++-- .../shared_datatypes/constants.hpp | 19 +- .../shared_datatypes/datatypes.hpp | 203 ++++++++++++++++-- .../app/torque_vectoring/torque_vectoring.cpp | 138 ++++++------ .../app/torque_vectoring/torque_vectoring.hpp | 17 +- 12 files changed, 400 insertions(+), 489 deletions(-) delete mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp delete mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_config.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_config.hpp index c740af82d4..1834ccb5b0 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_config.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_config.hpp @@ -1,3 +1,4 @@ +#pragma once #include "app_pid.hpp" namespace app::tv::controllers::dyrc diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp index 33880fa445..84e3c8e8bf 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp @@ -8,91 +8,81 @@ #include #include +#include "torque_vectoring/shared_datatypes/constants.hpp" +using namespace app::tv::shared_datatypes; using namespace app::tv::shared_datatypes::vd_constants; -using namespace app::tv::shared_datatypes::datatypes; namespace app::tv::controllers::allocator { namespace { - -// ---- Optimizer tuning (module-private) ---- -static constexpr float W_FX = 2.0f / 3.0f; -static constexpr float W_MZ = 1.0f / 3.0f; -static constexpr int MAX_ITER = 8; -static constexpr float SLIP_CLAMP = 0.3f; -static constexpr float NORMAL_MATRIX_EPS = 1e-6f; -static constexpr float STEP_TOLERANCE = 1e-5f; -static constexpr float COST_TOLERANCE = 1e-6f; - -using Vec4f = Eigen::Matrix; -using Vec5f = Eigen::Matrix; -using Mat54f = Eigen::Matrix; -using Mat44f = Eigen::Matrix; -using DualVec4 = Eigen::Matrix; -using DualVec5 = Eigen::Matrix; - -template -[[nodiscard]] T yawMomentFromTireForces( - const wheel_set& f_x, const wheel_set& f_y, const float steering_angle_rad) -{ - const float cos_delta = std::cos(steering_angle_rad); - const float sin_delta = std::sin(steering_angle_rad); - const float half_track_m = TRACK_WIDTH_m * 0.5f; - - const T fl_fx_body = (T(cos_delta) * f_x.fl) - (T(sin_delta) * f_y.fl); - const T fl_fy_body = (T(sin_delta) * f_x.fl) + (T(cos_delta) * f_y.fl); - const T fr_fx_body = (T(cos_delta) * f_x.fr) - (T(sin_delta) * f_y.fr); - const T fr_fy_body = (T(sin_delta) * f_x.fr) + (T(cos_delta) * f_y.fr); - - const T fl_moment = (T(DIST_FRONT_AXLE_CG_m) * fl_fy_body) - (T(half_track_m) * fl_fx_body); - const T fr_moment = (T(DIST_FRONT_AXLE_CG_m) * fr_fy_body) + (T(half_track_m) * fr_fx_body); - const T rl_moment = (T(-DIST_REAR_AXLE_CG_m) * f_y.rl) - (T(half_track_m) * f_x.rl); - const T rr_moment = (T(-DIST_REAR_AXLE_CG_m) * f_y.rr) + (T(half_track_m) * f_x.rr); - - return fl_moment + fr_moment + rl_moment + rr_moment; -} - + // ---- Optimizer tuning ---- + constexpr float W_FX = 2.0f / 3.0f; + constexpr float W_MZ = 1.0f / 3.0f; + constexpr int MAX_ITER = 8; + constexpr float SLIP_CLAMP = 0.3f; + constexpr float NORMAL_MATRIX_EPS = 1e-6f; + constexpr float STEP_TOLERANCE = 1e-5f; + constexpr float COST_TOLERANCE = 1e-6f; + + using Vec4f = Eigen::Matrix; + using Vec5f = Eigen::Matrix; + using Mat54f = Eigen::Matrix; + using Mat44f = Eigen::Matrix; + using DualVec4 = Eigen::Matrix; + using DualVec5 = Eigen::Matrix; + + // template + // [[nodiscard]] T yawMomentFromTireForces( + // const shared_datatypes::wheel_set &f_x, + // const shared_datatypes::wheel_set &f_y, + // const float steering_angle_rad) + // { + // constexpr float half_track_m = TRACK_WIDTH_m * 0.5f; + // + // const float cos_delta = std::cos(steering_angle_rad); + // const float sin_delta = std::sin(steering_angle_rad); + // + // const T fl_fx_body = (T(cos_delta) * f_x.fl) - (T(sin_delta) * f_y.fl); + // const T fl_fy_body = (T(sin_delta) * f_x.fl) + (T(cos_delta) * f_y.fl); + // const T fr_fx_body = (T(cos_delta) * f_x.fr) - (T(sin_delta) * f_y.fr); + // const T fr_fy_body = (T(sin_delta) * f_x.fr) + (T(cos_delta) * f_y.fr); + // const T fl_moment = (T(DIST_FRONT_AXLE_CG_m) * fl_fy_body) - (T(half_track_m) * fl_fx_body); + // const T fr_moment = (T(DIST_FRONT_AXLE_CG_m) * fr_fy_body) + (T(half_track_m) * fr_fx_body); + // const T rl_moment = (T(-DIST_REAR_AXLE_CG_m) * f_y.rl) - (T(half_track_m) * f_x.rl); + // const T rr_moment = (T(-DIST_REAR_AXLE_CG_m) * f_y.rr) + (T(half_track_m) * f_x.rr); + // return fl_moment + fr_moment + rl_moment + rr_moment; + // } } // namespace -wheel_set TorqueAllocator::optimize( - const wheel_set& tire_models, - const wheel_set& des_f_x, - const wheel_set& normal_forces_N, - const wheel_set& current_slip_ratios, - const wheel_set& current_slip_angles, - const float low_speed_blend, - const float steering_angle_rad, - const float des_M_z) +[[nodiscard]] wheel_set optimize(const VehicleState &state, float ax_setpoint, float omegadot_setpoint) { // Low-speed safeguard: // torque_vectoring.cpp computes a single force-availability blend from vehicle speed and passes it // into the allocator. Keeping that policy decision outside the optimizer makes the heuristic explicit // at the orchestration layer while the optimizer itself only consumes the already-decided scaling. - // + // Below a very small blend threshold, there is no meaningful traction allocation problem to solve, // so return zero requested slip immediately. - if (low_speed_blend < 0.05f) - { - return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; - } - - const wheel_set blended_des_f_x{ - .fl = low_speed_blend * des_f_x.fl, - .fr = low_speed_blend * des_f_x.fr, - .rl = low_speed_blend * des_f_x.rl, - .rr = low_speed_blend * des_f_x.rr, - }; - const float blended_des_m_z = low_speed_blend * des_M_z; - - Vec4f opt_slip; - opt_slip << current_slip_ratios.fl, - current_slip_ratios.fr, - current_slip_ratios.rl, - current_slip_ratios.rr; - - const float sqrt_w_fx = std::sqrt(W_FX); - const float sqrt_w_mz = std::sqrt(W_MZ); + // if (low_speed_blend < 0.05f) + // { + // return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; + // } + + // const wheel_set blended_des_f_x{ + // .fl = low_speed_blend * des_f_x.fl, + // .fr = low_speed_blend * des_f_x.fr, + // .rl = low_speed_blend * des_f_x.rl, + // .rr = low_speed_blend * des_f_x.rr, + // }; + // const float blended_des_m_z = low_speed_blend * des_M_z; + + Vec4f opt_slip; // output variable + const auto [kappa_fl, kappa_fr, kappa_rl, kappa_rr] = state.kappas(); + opt_slip << kappa_fl, kappa_fr, kappa_rl, kappa_rr; + + // const float sqrt_w_fx = std::sqrt(W_FX); + // const float sqrt_w_mz = std::sqrt(W_MZ); // Reference material used to shape this implementation: // - Video walkthrough: https://www.youtube.com/watch?v=C6DCtQjKkdY @@ -115,7 +105,8 @@ wheel_set TorqueAllocator::optimize( // // We keep all vectors/matrices fixed-size (4 decision variables, 5 residuals) so the optimizer // stays allocation-free and predictable on embedded targets. - const auto residualVector = [&](const DualVec4& kappa) -> DualVec5 { + const auto residualVector = [&](const DualVec4 &kappa) -> DualVec5 + { const wheel_set predicted_fx{ .fl = tire_models.fl.computeCombinedFx_N( normal_forces_N.fl, current_slip_angles.fl, low_speed_blend, kappa(0)), @@ -175,7 +166,7 @@ wheel_set TorqueAllocator::optimize( Mat44f normal_matrix = jacobian.transpose() * jacobian; normal_matrix.diagonal().array() += NORMAL_MATRIX_EPS; - const Vec4f rhs = -jacobian.transpose() * residuals; + const Vec4f rhs = -jacobian.transpose() * residuals; Eigen::LDLT ldlt(normal_matrix); if (ldlt.info() != Eigen::Success) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp index 1905d70088..4c827ec861 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp @@ -1,11 +1,8 @@ #pragma once -#include "torque_vectoring/shared_datatypes/constants.hpp" #include "torque_vectoring/shared_datatypes/datatypes.hpp" -#include "torque_vectoring/estimation/tire_model.hpp" -#include -namespace app::tv::controllers::allocator +namespace app::tv::controllers { /** @@ -17,32 +14,14 @@ namespace app::tv::controllers::allocator * * Outputs per-wheel optimal slip ratios using the caller-provided per-wheel tire models. */ -class TorqueAllocator +namespace TorqueAllocator { -public: /** * Run Gauss-Newton iterations to find optimal slip ratios. - * - * @param tire_models Per-wheel tire models used to evaluate candidate slip ratios - * @param des_f_x Target per-wheel longitudinal force (pedal request + yaw split) - * @param normal_forces_N Per-wheel vertical load from dynamics estimator - * @param current_slip_ratios Warm start from current cycle estimation - * @param current_slip_angles Fixed per-wheel slip angles for this optimization tick - * @param low_speed_blend Precomputed low-speed force-availability blend in [0, 1] - * @param steering_angle_rad Steering angle used to rotate front-wheel forces into the body frame - * @param des_M_z Target yaw moment from high-level controller (Nm) * @return Per-wheel optimal slip ratios */ - [[nodiscard]] shared_datatypes::datatypes::wheel_set optimize( - const shared_datatypes::datatypes::wheel_set& tire_models, - const shared_datatypes::datatypes::wheel_set& des_f_x, - const shared_datatypes::datatypes::wheel_set& normal_forces_N, - const shared_datatypes::datatypes::wheel_set& current_slip_ratios, - const shared_datatypes::datatypes::wheel_set& current_slip_angles, - float low_speed_blend, - float steering_angle_rad, - float des_M_z); + [[nodiscard]] shared_datatypes::wheel_set + optimize(const shared_datatypes::VehicleState &state, float ax_setpoint, float omegadot_setpoint); -}; - -} // namespace app::tv::controllers::allocator +}; // namespace TorqueAllocator +} // namespace app::tv::controllers diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp index 4edf03c063..fb2fe85652 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp @@ -19,14 +19,13 @@ static float r_ref_rad = 0.0f; [[nodiscard]] float computeRefYawRate(const float steer_ang_rad, const float body_velx_mps) { - r_ref_rad = (body_velx_mps * steer_ang_rad) / (WHEELBASE_m * (1.0f + ku * body_velx_mps * body_velx_mps)); - return r_ref_rad; + return (body_velx_mps * steer_ang_rad) / (WHEELBASE_m * (1.0f + ku * body_velx_mps * body_velx_mps)); } [[nodiscard]] float computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps) { - yaw_moment_Nm = pid.compute(computeRefYawRate(steer_ang_rad, body_velx_mps), r_actual_rad, 0.0f); - return yaw_moment_Nm; + r_ref_rad = computeRefYawRate(steer_ang_rad, body_velx_mps); + return yaw_moment_Nm = pid.compute(r_ref_rad, r_actual_rad, 0.0f); } [[nodiscard]] float getYawMoment() diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp deleted file mode 100644 index e7f4a43c14..0000000000 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#include "dynamics_estimation.hpp" - -#include - -using namespace app::tv::shared_datatypes::vd_constants; - -namespace app::tv::estimation -{ -namespace -{ - [[nodiscard]] float safe_vx(const float v_x_mps) - { - if (std::fabs(v_x_mps) >= SMALL_EPSILON) - { - return v_x_mps; - } - - return v_x_mps < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; - } -} // namespace - -float vehicleDynamics::est_beta_rad(const float v_x_mps, const float v_y_mps) -{ - return std::atan2(v_y_mps, safe_vx(v_x_mps)); -} - -float vehicleDynamics::est_Mz_N( - const app::tv::datatypes::datatypes::wheel_set &tires_Fx_N, - const app::tv::datatypes::datatypes::wheel_set &tires_Fy_N, - const float delta) -{ - constexpr float half_track_m = TRACK_WIDTH_m * 0.5f; - - // rotate front wheels into body frame - const float cos_delta = std::cos(delta); - const float sin_delta = std::sin(delta); - const float fl_fx_body = (cos_delta * tires_Fx_N.fl) - (sin_delta * tires_Fy_N.fl); - const float fl_fy_body = (sin_delta * tires_Fx_N.fl) + (cos_delta * tires_Fy_N.fl); - const float fr_fx_body = (cos_delta * tires_Fx_N.fr) - (sin_delta * tires_Fy_N.fr); - const float fr_fy_body = (sin_delta * tires_Fx_N.fr) + (cos_delta * tires_Fy_N.fr); - - // contributions to moment of each tire - const float fl_moment = (DIST_FRONT_AXLE_CG_m * fl_fy_body) - (half_track_m * fl_fx_body); - const float fr_moment = (DIST_FRONT_AXLE_CG_m * fr_fy_body) + (half_track_m * fr_fx_body); - const float rl_moment = (-DIST_REAR_AXLE_CG_m * tires_Fy_N.rl) - (half_track_m * tires_Fx_N.rl); - const float rr_moment = (-DIST_REAR_AXLE_CG_m * tires_Fy_N.rr) + (half_track_m * tires_Fx_N.rr); - - return fl_moment + fr_moment + rl_moment + rr_moment; -} - -app::tv::datatypes::datatypes::wheel_set - vehicleDynamics::est_Fz_N(const float a_x_MPS2, const float a_y_MPS2, const float v_x_mps) -{ - // load transfer - const float long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(a_x_MPS2); - const float lat_load_tf = LAT_ACCEL_TERM_VERTICAL_FORCE(a_y_MPS2); - - // downforce and cop components - const float down_force_n = est_downforceFz_N(v_x_mps); - // Assuming COP is a fraction of the total downforce - const float front_cop = dynamicCOPFront(a_x_MPS2), rear_cop = 1.0f - front_cop, - right_cop = dynamicCOPRight(a_y_MPS2), left_cop = 1.0f - right_cop; - return { - .fl = std::fmax( - 0.0f, - STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) - lat_load_tf + down_force_n * front_cop * left_cop), - .fr = std::fmax( - 0.0f, - STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) + lat_load_tf + down_force_n * front_cop * right_cop), - .rl = std::fmax( - 0.0f, STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) - lat_load_tf + down_force_n * rear_cop * left_cop), - .rr = std::fmax( - 0.0f, STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) + lat_load_tf + down_force_n * rear_cop * right_cop), - }; -} -} // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp deleted file mode 100644 index e28cc4ea5d..0000000000 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/dynamics_estimation.hpp +++ /dev/null @@ -1,119 +0,0 @@ -#pragma once - -#include "torque_vectoring/shared_datatypes/constants.hpp" -#include "torque_vectoring/shared_datatypes/datatypes.hpp" - -namespace app::tv::estimation -{ -using namespace app::tv::shared_datatypes::vd_constants; -namespace vehicleDynamics -{ - [[nodiscard]] app::tv::datatypes::datatypes::wheel_set - est_Fz_N(float a_x_MPS2, float a_y_MPS2, float v_x_mps); - [[nodiscard]] float est_beta_rad(float v_x_mps, float v_y_mps); - [[nodiscard]] float est_Mz_N( - const app::tv::datatypes::datatypes::wheel_set &tires_Fx_N, - const app::tv::datatypes::datatypes::wheel_set &tires_Fy_N, - float delta); - [[nodiscard]] constexpr float est_dragFx_N(const float v_x_mps) - { - return 0.5f * AIR_DENSITY_KGPM3 * FRONTAL_AREA_M2 * DRAG_COEFF * v_x_mps * v_x_mps; - } - [[nodiscard]] constexpr float est_downforceFz_N(const float v_x_mps) - { - return 0.5f * AIR_DENSITY_KGPM3 * FRONTAL_AREA_M2 * LIFT_COEFF * v_x_mps * v_x_mps; - } - - // ============================================================================= - // VEHICLE DYNAMICS - VERTICAL LOAD TRANSFER - // Reference: https://www.zotero.org/groups/5809911/vehicle_controls_2024/items/N4TQBR67/reader - // ============================================================================= - - /** - * Longitudinal load transfer component (page 21) - * Positive long_accel transfers load to rear axle - * - * @param long_accel Longitudinal acceleration (m/s^2) - * @return Load transfer force (N) - */ - [[nodiscard]] constexpr float LONG_ACCEL_TERM_VERTICAL_FORCE(const float long_accel) - { - return (CAR_MASS_AT_CG_KG * long_accel * DIST_HEIGHT_CG_m) / WHEELBASE_m; - } - /** - * Lateral load transfer component (page 21) - * Transfers load to outside wheels during cornering - * - * @param lat_accel Lateral acceleration (m/s^2) - * @return Load transfer force per side (N) - */ - [[nodiscard]] constexpr float LAT_ACCEL_TERM_VERTICAL_FORCE(const float lat_accel) - { - return (CAR_MASS_AT_CG_KG * lat_accel * DIST_HEIGHT_CG_m) / (2.0f * TRACK_WIDTH_m); - } - - [[nodiscard]] constexpr float REAR_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) - { - return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) + - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); - } - [[nodiscard]] constexpr float REAR_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) - { - return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) - - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); - } - [[nodiscard]] constexpr float FRONT_RIGHT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) - { - return REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) + - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); - } - [[nodiscard]] constexpr float FRONT_LEFT_WHEEL_VERTICAL_FORCE(const float long_accel, const float lat_accel) - { - return REAR_WEIGHT_DISTRIBUTION - LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel / 4.0f) - - LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel); - } - - /** - * Yaw moment distribution factor Kmz (page 57) - * Accounts for load transfer effect on yaw moment generation capacity - * - * @param a_x Longitudinal acceleration (m/s^2) - * @return Effective moment arm (m) - */ - [[nodiscard]] constexpr float ACCELERATION_TERM_KMZ(const float a_x) - { - return DIST_FRONT_AXLE_CG_m + (a_x * DIST_HEIGHT_CG_m) / GRAVITY; - } - [[nodiscard]] constexpr float KMZ(const float a_x) - { - const float LONG_ACCEL_TERM = ACCELERATION_TERM_KMZ(a_x); - return ((CAR_WEIGHT - (CAR_WEIGHT / WHEELBASE_m) * LONG_ACCEL_TERM)) / - ((CAR_WEIGHT / WHEELBASE_m) * LONG_ACCEL_TERM); - } - /** - * Moment scaling factor F (page 58) - * Relates torque differential to yaw moment through track width and effective radius - */ - inline constexpr float F = (TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * IN_TO_M)) * GEAR_RATIO; - - inline constexpr float STATIC_FRONT_AXLE_LOAD_N = - datatypes::vd_constants::CAR_WEIGHT * - (datatypes::vd_constants::DIST_REAR_AXLE_CG_m / datatypes::vd_constants::WHEELBASE_m); - inline constexpr float STATIC_REAR_AXLE_LOAD_N = - datatypes::vd_constants::CAR_WEIGHT * - (datatypes::vd_constants::DIST_FRONT_AXLE_CG_m / datatypes::vd_constants::WHEELBASE_m); - inline constexpr float STATIC_FRONT_WHEEL_LOAD_N = 0.5f * STATIC_FRONT_AXLE_LOAD_N; - inline constexpr float STATIC_REAR_WHEEL_LOAD_N = 0.5f * STATIC_REAR_AXLE_LOAD_N; - - [[nodiscard]] constexpr float dynamicCOPFront(const float long_accel_mps2) - { - (void)long_accel_mps2; - return 1.0f - COP_REAR; - } - [[nodiscard]] constexpr float dynamicCOPRight(const float lat_accel_mps2) - { - (void)lat_accel_mps2; - return COP_RIGHT; - } -}; // namespace vehicleDynamics -} // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp index eed19f1d77..8c699a5354 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp @@ -102,7 +102,7 @@ namespace } }; } - [[nodiscard]] consteval Filter::N_N processNoise() + [[nodiscard]] constexpr Filter::N_N processNoise() { Filter::N_N q = Filter::N_N::Zero(); q(static_cast(VX), static_cast(VX)) = 0.05f; @@ -112,7 +112,7 @@ namespace return q; } - [[nodiscard]] consteval Filter::M_M measurementNoise() + [[nodiscard]] constexpr Filter::M_M measurementNoise() { Filter::M_M r = Filter::M_M::Zero(); r(static_cast(VX), static_cast(VX)) = 0.75f; @@ -122,12 +122,12 @@ namespace return r; } - [[nodiscard]] consteval StateVector initialState() + [[nodiscard]] constexpr StateVector initialState() { return StateVector::Zero(); } - [[nodiscard]] consteval Filter::N_N initialCovariance() + [[nodiscard]] constexpr Filter::N_N initialCovariance() { Filter::N_N p0 = Filter::N_N::Identity(); p0(static_cast(VX), static_cast(VX)) = 5.0f; @@ -182,49 +182,58 @@ namespace z(static_cast(VY)) = 0.25f * (fl_vy + fr_vy + rl_vy + rr_vy); return z; } + + [[nodiscard]] constexpr Filter createFilter() + { + return Filter( + createStateFunctions(), createMeasurementFunctions(), processNoise(), measurementNoise(), initialState(), + initialCovariance()); + } } // namespace -constexpr Filter VehicleStateEstimator::createFilter() +namespace VehicleStateEstimator { - return Filter( - createStateFunctions(), createMeasurementFunctions(), processNoise(), measurementNoise(), initialState(), - initialCovariance()); -} + Filter filter_ = createFilter(); + void reset_filter() + { + filter_ = createFilter(); + } -[[nodiscard]] app::tv::shared_datatypes::datatypes::VehicleState - VehicleStateEstimator::estimate(const Measurements &state) const -{ - InputVector u = InputVector::Zero(); - u(static_cast(AX)) = state.ax; - u(static_cast(AY)) = state.ay; - - const float measured_yaw_rate_radps = state.yaw_rate; - const float measured_steering_angle = state.delta; - const datatypes::datatypes::wheel_set wheel_angular_velocities = state.omegas; - const auto &previous_state = filter_.state(); - - Measurement z = pseudoMeasurementFromWheelSpeeds( - wheel_angular_velocities, measured_yaw_rate_radps, measured_steering_angle, previous_state); - - z(static_cast(R)) = measured_yaw_rate_radps; - // z(static_cast(MZ)) = - // dynamics_estimator_.est_Mz_N(inputs.longitudinal_forces_N, inputs.lateral_forces_N, measured_steering_angle); - - const StateVector estimated_state = filter_.estimated_states(u, z); - - // outputs_.yaw_moment_nm = estimated_state(static_cast(MZ)); - return { - .v_x_mps = estimated_state(static_cast(VX)), - .v_y_mps = estimated_state(static_cast(VY)), - .yaw_rate_radps = estimated_state(static_cast(R)), - .steer_ang_rad = measured_steering_angle, - .a_x_mps2 = u(static_cast(AX)), - .a_y_mps2 = u(static_cast(AY)), - }; -} - -const VehicleStateEstimator::Covariance &VehicleStateEstimator::covariance() const -{ - return filter_.covariance(); -} + [[nodiscard]] shared_datatypes::datatypes::VehicleState estimate(const Measurements &state) + { + InputVector u = InputVector::Zero(); + u(static_cast(AX)) = state.ax; + u(static_cast(AY)) = state.ay; + + const float measured_yaw_rate_radps = state.yaw_rate; + const float measured_steering_angle = state.delta; + const datatypes::datatypes::wheel_set wheel_angular_velocities = state.omegas; + const auto &previous_state = filter_.state(); + + Measurement z = pseudoMeasurementFromWheelSpeeds( + wheel_angular_velocities, measured_yaw_rate_radps, measured_steering_angle, previous_state); + + z(static_cast(R)) = measured_yaw_rate_radps; + // z(static_cast(MZ)) = + // dynamics_estimator_.est_Mz_N(inputs.longitudinal_forces_N, inputs.lateral_forces_N, + // measured_steering_angle); + + const StateVector estimated_state = filter_.estimated_states(u, z); + + // outputs_.yaw_moment_nm = estimated_state(static_cast(MZ)); + return { + .v_x_mps = estimated_state(static_cast(VX)), + .v_y_mps = estimated_state(static_cast(VY)), + .yaw_rate_radps = estimated_state(static_cast(R)), + .steer_ang_rad = measured_steering_angle, + .a_x_mps2 = u(static_cast(AX)), + .a_y_mps2 = u(static_cast(AY)), + }; + } + + const Covariance &covariance() + { + return filter_.covariance(); + } +} // namespace VehicleStateEstimator } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp index adfe4707b0..b8495d9e09 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp @@ -1,42 +1,32 @@ #pragma once -#include - #include "state_estimation/app_kalman_filter.hpp" #include "torque_vectoring/estimation/dynamics_estimation.hpp" #include "torque_vectoring/shared_datatypes/datatypes.hpp" namespace app::tv::estimation { -class VehicleStateEstimator +struct Measurements { - public: - struct Measurements - { - // sensor measurements - const float ax; - const float ay; - const float yaw_rate; + // sensor measurements + const float ax; + const float ay; + const float yaw_rate; - // driver controls - const float delta; - const float apps; + // driver controls + const float delta; + const float apps; - const datatypes::datatypes::wheel_set omegas; - }; + const datatypes::datatypes::wheel_set omegas; +}; +namespace VehicleStateEstimator +{ using Filter = app::state_estimation::ekf; using Covariance = Filter::N_N; - - // constexpr VehicleStateEstimator() : filter_(createFilter()) {} - constexpr VehicleStateEstimator() : filter_(createFilter()) {} - - [[nodiscard]] shared_datatypes::datatypes::VehicleState estimate(const Measurements &state) const; - [[nodiscard]] const Covariance &covariance() const; + [[nodiscard]] shared_datatypes::datatypes::VehicleState estimate(const Measurements &state); + [[nodiscard]] const Covariance &covariance(); + void reset_filter(); // if you want to reset, just reconstruct the object - - private: - [[nodiscard]] static constexpr Filter createFilter(); - const Filter filter_; -}; +}; // namespace VehicleStateEstimator } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index 61f405eeaf..f11ce9e417 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -4,7 +4,6 @@ namespace app::tv::shared_datatypes::vd_constants { - // ============================================================================= // PHYSICAL CONSTANTS // ============================================================================= @@ -88,9 +87,10 @@ inline constexpr float PID_POWER_FACTOR_MAX = 0.1f; // TODO: May need adjustm // ============================================================================= inline constexpr float SLIP_RATIO_IDEAL = 0.05f; // Ideal slip ratio for maximum traction -inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = - 0.3f; // TODO: Replace with reverse/anti-Ackermann model - // Note: Underestimate for wheel angles > 40° (see Confluence/Steering System) +// TODO: Replace with reverse/anti-Ackermann model +inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = 0.3f; +// Note: Underestimate for wheel angles > 40° (see Confluence/Steering System) +inline constexpr float MAX_AX = 10; // TODO idk this number bruh // ============================================================================= // UTILITY FUNCTIONS & CONVERSION HELPERS @@ -106,7 +106,7 @@ inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = */ [[nodiscard]] constexpr int16_t MOTOR_TORQUE_REQUEST(const float torque) { - return static_cast((torque / NOMINAL_TORQUE_REQUEST_NM) * 1000.0f); + return static_cast(torque / NOMINAL_TORQUE_REQUEST_NM * 1000.0f); } /** @@ -114,7 +114,7 @@ inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = */ [[nodiscard]] constexpr float TORQUE_TO_POWER(const float torque, const float rpm) { - return (torque * (rpm / GEAR_RATIO)) / static_cast(POWER_TO_TORQUE_CONVERSION_FACTOR); + return torque * (rpm / GEAR_RATIO) / static_cast(POWER_TO_TORQUE_CONVERSION_FACTOR); } /** @@ -134,9 +134,4 @@ inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = // extern const PID_Config PID_TRACTION_CONTROL_CONFIG; // extern const PID_Config PID_YAW_RATE_CONTROLLER_CONFIG; // extern const YawRateController_Config YAW_RATE_CONTROLLER_CONFIG; -} // namespace app::tv::shared_datatypes::vd_constants - -namespace app::tv::datatypes -{ -namespace vd_constants = app::tv::shared_datatypes::vd_constants; -} +} // namespace app::tv::shared_datatypes::vd_constants \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 537957e28c..026c4e7f62 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -1,6 +1,10 @@ #pragma once -namespace app::tv::shared_datatypes::datatypes +#include "constants.hpp" + +#include + +namespace app::tv::shared_datatypes { template struct wheel_set { @@ -10,8 +14,24 @@ template struct wheel_set T rr; }; +inline float slipRatioToWheelAngularVelocity(const float slip_ratio, const float v_x_mps) +{ + // Avoid division by zero at very low speeds + if (std::fabs(v_x_mps) < vd_constants::SMALL_EPSILON) + return 0.0f; + return (1.0f + slip_ratio) * (v_x_mps / vd_constants::WHEEL_RADIUS_M); +} + +[[nodiscard]] inline float safe_vx(const float v_x_mps) +{ + if (std::fabs(v_x_mps) >= vd_constants::SMALL_EPSILON) + return v_x_mps; + return v_x_mps < 0.0f ? -vd_constants::SMALL_EPSILON : vd_constants::SMALL_EPSILON; +} + struct VehicleState { + // state variables float v_x_mps = 0.0f; float v_y_mps = 0.0f; float yaw_rate_radps = 0.0f; @@ -20,29 +40,176 @@ struct VehicleState float a_y_mps2 = 0.0f; float pedal_percentage = 0.0f; - wheel_set wheel_angular_velocities_radps{}; - wheel_set longitudinal_forces_N{}; - wheel_set lateral_forces_N{}; + wheel_set omegas_radps{}; + wheel_set Fxs_N{}; + wheel_set Fys_N{}; + + [[nodiscard]] wheel_set get_tire_angle() const + { + // todo steering model + return { steer_ang_rad, steer_ang_rad, 0, 0 }; + } + + [[nodiscard]] wheel_set kappas() const + { + const auto tire_angles = get_tire_angle(); + return { + .fl = slipRatioToWheelAngularVelocity(omegas_radps.fl, std::cos(tire_angles.fl) * v_x_mps), + .fr = slipRatioToWheelAngularVelocity(omegas_radps.fr, std::cos(tire_angles.fr) * v_x_mps), + .rl = slipRatioToWheelAngularVelocity(omegas_radps.rl, v_x_mps), + .rr = slipRatioToWheelAngularVelocity(omegas_radps.rr, v_x_mps), + }; + } + + // AERODYNAMIC EFFECTS + // these might use internal state later?? + // ReSharper disable once CppMemberFunctionMayBeStatic + [[nodiscard]] constexpr float dynamicCOPFront() const { return 1.0f - vd_constants::COP_REAR; } + // ReSharper disable once CppMemberFunctionMayBeStatic + [[nodiscard]] constexpr float dynamicCOPRight() const { return vd_constants::COP_RIGHT; } + [[nodiscard]] constexpr float est_dragFx_N() const + { + return 0.5f * vd_constants::AIR_DENSITY_KGPM3 * vd_constants::FRONTAL_AREA_M2 * vd_constants::DRAG_COEFF * + v_x_mps * v_x_mps; + } + [[nodiscard]] constexpr float est_downforceFz_N() const + { + return 0.5f * vd_constants::AIR_DENSITY_KGPM3 * vd_constants::FRONTAL_AREA_M2 * vd_constants::LIFT_COEFF * + v_x_mps * v_x_mps; + } + + // ============================================================================= + // VEHICLE DYNAMICS - VERTICAL LOAD TRANSFER + // Reference: https://www.zotero.org/groups/5809911/vehicle_controls_2024/items/N4TQBR67/reader + // ============================================================================= + + /** + * Longitudinal load transfer component (page 21) + * Positive long_accel transfers load to rear axle + * @return Load transfer force (N) + */ + [[nodiscard]] constexpr float LONG_ACCEL_TERM_VERTICAL_FORCE() const + { + return (vd_constants::CAR_MASS_AT_CG_KG * a_x_mps2 * vd_constants::DIST_HEIGHT_CG_m) / + vd_constants::WHEELBASE_m; + } + /** + * Lateral load transfer component (page 21) + * Transfers load to outside wheels during cornering + * @return Load transfer force per side (N) + */ + [[nodiscard]] constexpr float LAT_ACCEL_TERM_VERTICAL_FORCE() const + { + return (vd_constants::CAR_MASS_AT_CG_KG * a_y_mps2 * vd_constants::DIST_HEIGHT_CG_m) / + (2.0f * vd_constants::TRACK_WIDTH_m); + } + + [[nodiscard]] wheel_set est_Fz_N() const + { + // static loads + static constexpr float STATIC_FRONT_AXLE_LOAD_N = + vd_constants::CAR_WEIGHT * (vd_constants::DIST_REAR_AXLE_CG_m / vd_constants::WHEELBASE_m); + static constexpr float STATIC_REAR_AXLE_LOAD_N = + vd_constants::CAR_WEIGHT * (vd_constants::DIST_FRONT_AXLE_CG_m / vd_constants::WHEELBASE_m); + static constexpr float STATIC_FRONT_WHEEL_LOAD_N = 0.5f * STATIC_FRONT_AXLE_LOAD_N; + static constexpr float STATIC_REAR_WHEEL_LOAD_N = 0.5f * STATIC_REAR_AXLE_LOAD_N; + + // load transfer + const float long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(); + const float lat_load_tf = LAT_ACCEL_TERM_VERTICAL_FORCE(); + + // downforce and cop components + const float down_force_n = est_downforceFz_N(); + // Assuming COP is a fraction of the total downforce + const float front_cop = dynamicCOPFront(), rear_cop = 1.0f - front_cop, right_cop = dynamicCOPRight(), + left_cop = 1.0f - right_cop; + return { + .fl = std::fmax( + 0.0f, + STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) - lat_load_tf + down_force_n * front_cop * left_cop), + .fr = std::fmax( + 0.0f, + STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) + lat_load_tf + down_force_n * front_cop * right_cop), + .rl = std::fmax( + 0.0f, + STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) - lat_load_tf + down_force_n * rear_cop * left_cop), + .rr = std::fmax( + 0.0f, + STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) + lat_load_tf + down_force_n * rear_cop * right_cop), + }; + } + + /** + * Get body slip + * @return + */ + [[nodiscard]] float est_beta_rad() const { return std::atan2(v_y_mps, safe_vx(v_x_mps)); } + + /** + * @param tires_Fx_N tire longitudinal forces in Newtons + * @param tires_Fy_N tire lateral forces in Newtons + * @return Given certain tire forces, what would be the resulting yaw moment Mz about the CG? + */ + [[nodiscard]] float est_Mz_N(const wheel_set &tires_Fx_N, const wheel_set &tires_Fy_N) const + { + constexpr float half_track_m = vd_constants::TRACK_WIDTH_m * 0.5f; + + // rotate front wheels into body frame + const wheel_set tire_angles = get_tire_angle(); + + const float cos_delta_fl = std::cos(tire_angles.fl); + const float sin_delta_fl = std::sin(tire_angles.fl); + const float fl_fx_body = (cos_delta_fl * tires_Fx_N.fl) - (sin_delta_fl * tires_Fy_N.fl); + const float fl_fy_body = (sin_delta_fl * tires_Fx_N.fl) + (cos_delta_fl * tires_Fy_N.fl); + + const float cos_delta_fr = std::cos(tire_angles.fr); + const float sin_delta_fr = std::sin(tire_angles.fr); + const float fr_fx_body = (cos_delta_fr * tires_Fx_N.fr) - (sin_delta_fr * tires_Fy_N.fr); + const float fr_fy_body = (sin_delta_fr * tires_Fx_N.fr) + (cos_delta_fr * tires_Fy_N.fr); + + // contributions to moment of each tire + const float fl_moment = (vd_constants::DIST_FRONT_AXLE_CG_m * fl_fy_body) - (half_track_m * fl_fx_body); + const float fr_moment = (vd_constants::DIST_FRONT_AXLE_CG_m * fr_fy_body) + (half_track_m * fr_fx_body); + const float rl_moment = (-vd_constants::DIST_REAR_AXLE_CG_m * tires_Fy_N.rl) - (half_track_m * tires_Fx_N.rl); + const float rr_moment = (-vd_constants::DIST_REAR_AXLE_CG_m * tires_Fy_N.rr) + (half_track_m * tires_Fx_N.rr); + + return fl_moment + fr_moment + rl_moment + rr_moment; + } + /** + * Yaw moment distribution factor Kmz (page 57) + * Accounts for load transfer effect on yaw moment generation capacity + * @return Effective moment arm (m) + */ + [[nodiscard]] constexpr float ACCELERATION_TERM_KMZ() const + { + return vd_constants::DIST_FRONT_AXLE_CG_m + (a_x_mps2 * vd_constants::DIST_HEIGHT_CG_m) / vd_constants::GRAVITY; + } + [[nodiscard]] constexpr float KMZ() const + { + const float LONG_ACCEL_TERM = ACCELERATION_TERM_KMZ(); + return ((vd_constants::CAR_WEIGHT - (vd_constants::CAR_WEIGHT / vd_constants::WHEELBASE_m) * LONG_ACCEL_TERM)) / + ((vd_constants::CAR_WEIGHT / vd_constants::WHEELBASE_m) * LONG_ACCEL_TERM); + } + /** + * Moment scaling factor F (page 58) + * Relates torque differential to yaw moment through track width and effective radius + */ + static constexpr float F = (vd_constants::TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * IN_TO_M)) * GEAR_RATIO; }; struct ControlOutput { - const float fl_omega_radps; - const float fr_omega_radps; - const float rl_omega_radps; - const float rr_omega_radps; + const float fl_kappa; + const float fr_kappa; + const float rl_kappa; + const float rr_kappa; }; struct ControlOutputAutonomous { - const float fl_omega_radps; - const float fr_omega_radps; - const float rl_omega_radps; - const float rr_omega_radps; + const float fl_kappa; + const float fr_kappa; + const float rl_kappa; + const float rr_kappa; const float delta; }; -} // namespace app::tv::shared_datatypes::datatypes - -namespace app::tv::datatypes -{ -namespace datatypes = app::tv::shared_datatypes::datatypes; -} +} // namespace app::tv::shared_datatypes diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index 4e117d5046..475512dc8d 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -17,8 +17,6 @@ namespace using namespace shared_datatypes::vd_constants; //------------------------------------- ESTIMATION MODULES ----------------------------------// - estimation::VehicleStateEstimator vehicle_state_estimator{}; - wheel_set tire_models{ .fl = estimation::TireModel(estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Front), .fr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Front), @@ -26,101 +24,76 @@ namespace .rr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Rear), }; - //------------------------------------- CONTROLLERS -----------------------------------------// - - controllers::allocator::TorqueAllocator torque_allocator{}; - //------------------------------------- STATE VARIABLES -------------------------------------// // Build TireModel::StateInputs from VehicleState + per-wheel data - [[nodiscard]] estimation::TireModel::StateInputs - buildTireInputs(const VehicleState &state, const float omega, const float fz) - { - return { - .wheel_angular_velocity_radps = omega, - .vehicle_velocity_x_mps = state.v_x_mps, - .vehicle_velocity_y_mps = state.v_y_mps, - .yaw_rate_radps = state.yaw_rate_radps, - .steering_angle_rad = state.steer_ang_rad, - .normal_load_N = fz, - }; - } + // [[nodiscard]] estimation::TireModel::StateInputs + // buildTireInputs(const VehicleState &state, const float omega, const float fz) + // { + // return { + // .wheel_angular_velocity_radps = omega, + // .vehicle_velocity_x_mps = state.v_x_mps, + // .vehicle_velocity_y_mps = state.v_y_mps, + // .yaw_rate_radps = state.yaw_rate_radps, + // .steering_angle_rad = state.steer_ang_rad, + // .normal_load_N = fz, + // }; + // } // These wheel-set caches carry the previous update's estimated tire state/forces into the next cycle. // The state estimator and optimizer both consume them as warm starts / feedback, so they remain module-local. - wheel_set acc_f_x{}; - wheel_set acc_f_y{}; - wheel_set acc_f_z{}; - wheel_set acc_slip_ratio{}; - wheel_set acc_slip_angle{}; - wheel_set slip_ratio_opt{}; + // wheel_set acc_f_x{}; + // wheel_set acc_f_y{}; + // wheel_set acc_f_z{}; + // wheel_set acc_slip_ratio{}; + // wheel_set acc_slip_angle{}; + // wheel_set slip_ratio_opt{}; // Copy the current tire estimates into the module-local wheel-set caches so the rest of the // pipeline can use a consistent per-wheel state snapshot for this update tick. - void unpackTireOutputs( - const estimation::TireModel::Outputs &fl, - const estimation::TireModel::Outputs &fr, - const estimation::TireModel::Outputs &rl, - const estimation::TireModel::Outputs &rr) - { - acc_f_x = { .fl = fl.longitudinal_force_N, - .fr = fr.longitudinal_force_N, - .rl = rl.longitudinal_force_N, - .rr = rr.longitudinal_force_N }; - acc_f_y = { - .fl = fl.lateral_force_N, .fr = fr.lateral_force_N, .rl = rl.lateral_force_N, .rr = rr.lateral_force_N - }; - acc_slip_ratio = { .fl = fl.slip_ratio, .fr = fr.slip_ratio, .rl = rl.slip_ratio, .rr = rr.slip_ratio }; - acc_slip_angle = { - .fl = fl.slip_angle_rad, .fr = fr.slip_angle_rad, .rl = rl.slip_angle_rad, .rr = rr.slip_angle_rad - }; - } + // void unpackTireOutputs( + // const estimation::TireModel::Outputs &fl, + // const estimation::TireModel::Outputs &fr, + // const estimation::TireModel::Outputs &rl, + // const estimation::TireModel::Outputs &rr) + // { + // acc_f_x = { .fl = fl.longitudinal_force_N, + // .fr = fr.longitudinal_force_N, + // .rl = rl.longitudinal_force_N, + // .rr = rr.longitudinal_force_N }; + // acc_f_y = { + // .fl = fl.lateral_force_N, .fr = fr.lateral_force_N, .rl = rl.lateral_force_N, .rr = rr.lateral_force_N + // }; + // acc_slip_ratio = { .fl = fl.slip_ratio, .fr = fr.slip_ratio, .rl = rl.slip_ratio, .rr = rr.slip_ratio }; + // acc_slip_angle = { + // .fl = fl.slip_angle_rad, .fr = fr.slip_angle_rad, .rl = rl.slip_angle_rad, .rr = rr.slip_angle_rad + // }; + // } } // namespace //------------------------------------- TORQUE VECTORING UPDATE CYCLE -------------------------// -ControlOutput - update(const estimation::VehicleStateEstimator::Measurements &measurement, const float ax, const float omega_dot) +ControlOutput update(const estimation::Measurements &measurement) { //------------------------------------- STATE ESTIMATION --------------------------------// // Fuse the measured chassis state with the previous tire-force estimate to obtain the // filtered vehicle state used consistently by all downstream control blocks this tick. - - // const auto [estimated_state, _yaw_moment_nm] = vehicle_state_estimator.estimate(state); - const VehicleState state = vehicle_state_estimator.estimate(measurement); + const VehicleState state = estimation::VehicleStateEstimator::estimate(measurement); // Normal forces from longitudinal/lateral load transfer + downforce // acc_f_z = vehicle_dynamics_estimator.est_Fz_N( // estimated_state.a_x_mps2, estimated_state.a_y_mps2, estimated_state.v_x_mps); - // Tire model estimation from current sensor data using the filtered vehicle state. - unpackTireOutputs( - tire_models.fl.estimate(buildTireInputs(estimated_state, fl_omega, acc_f_z.fl)), - tire_models.fr.estimate(buildTireInputs(estimated_state, fr_omega, acc_f_z.fr)), - tire_models.rl.estimate(buildTireInputs(estimated_state, rl_omega, acc_f_z.rl)), - tire_models.rr.estimate(buildTireInputs(estimated_state, rr_omega, acc_f_z.rr))); + // unpackTireOutputs( + // tire_models.fl.estimate(buildTireInputs(estimated_state, fl_omega, acc_f_z.fl)), + // tire_models.fr.estimate(buildTireInputs(estimated_state, fr_omega, acc_f_z.fr)), + // tire_models.rl.estimate(buildTireInputs(estimated_state, rl_omega, acc_f_z.rl)), + // tire_models.rr.estimate(buildTireInputs(estimated_state, rr_omega, acc_f_z.rr))); //------------------------------------- HIGH LEVEL CONTROLLER ----------------------------// - // Convert the driver's pedal request into a nominal per-wheel longitudinal force target. - // The cosine projection keeps the request aligned with the tire heading under nonzero slip angle. - const float per_wheel_tq = MAX_TORQUE_REQUEST_NM * pedal_percentage; - wheel_set des_f_x = { - .fl = per_wheel_tq * std::cos(acc_slip_angle.fl) * WHEEL_RADIUS_M, - .fr = per_wheel_tq * std::cos(acc_slip_angle.fr) * WHEEL_RADIUS_M, - .rl = per_wheel_tq * std::cos(acc_slip_angle.rl) * WHEEL_RADIUS_M, - .rr = per_wheel_tq * std::cos(acc_slip_angle.rr) * WHEEL_RADIUS_M, - }; - + const float ax_mps2_setpoint = MAX_AX * measurement.apps; // Direct yaw rate control: corrective yaw moment - const float des_yaw_moment_nm = controllers::dyrc::computeYawMoment( - estimated_state.yaw_rate_radps, estimated_state.steer_ang_rad, estimated_state.v_x_mps); - - // Distribute yaw moment into per-wheel force adjustment - // From Mz = (t/2) * (Fx_right - Fx_left), solve for delta Fx per side: - const float des_fx_delta = des_yaw_moment_nm / TRACK_WIDTH_m; - des_f_x.fl -= des_fx_delta; - des_f_x.fr += des_fx_delta; - des_f_x.rl -= des_fx_delta; - des_f_x.rr += des_fx_delta; + const float dotomega_radps2_setpoint = + controllers::dyrc::computeYawMoment(state.yaw_rate_radps, state.steer_ang_rad, state.v_x_mps); //------------------------------------- LOW LEVEL CONTROLLER -----------------------------// @@ -132,14 +105,25 @@ ControlOutput // Gauss-Newton optimizer: invert the combined-slip tire model to find the slip ratios whose // predicted forces best match the desired per-wheel force split and desired yaw moment. - slip_ratio_opt = torque_allocator.optimize( - tire_models, des_f_x, acc_f_z, acc_slip_ratio, acc_slip_angle, low_speed_blend, estimated_state.steer_ang_rad, - des_yaw_moment_nm); + // ReSharper disable once CppUseStructuredBinding + const wheel_set kappa_opt = + controllers::TorqueAllocator::optimize(state, ax_mps2_setpoint, dotomega_radps2_setpoint); //------------------------------------- POWER LIMITER -----------------------------------// // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request - return {}; + return { + .fl_kappa = kappa_opt.fl, + .fr_kappa = kappa_opt.fr, + .rl_kappa = kappa_opt.rl, + .rr_kappa = kappa_opt.rr, + }; } +ControlOutputAutonomous + update_autonomous(const estimation::Measurements &measurement, const float ax, const float omega_dot) +{ + // TODO inshallah one day + return {}; +} } // namespace app::tv diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp index aea9ec9f70..287017e50c 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -1,27 +1,18 @@ #pragma once #include "estimation/vehicle_state_estimator.hpp" #include "shared_datatypes/datatypes.hpp" -#include "estimation/vehicle_state_estimator.hpp" - -// app::tv::shared_datatypes::datatypes::VehicleState -// estimateVehicleState(); /** * This is the main entrypoint into the low level vehicle controls algorithm - * @param state The current measured vehicle state - * @param ax Requested Longitudinal acceleration (m/s^2) - * @param omega_dot Requested Yaw acceleration (rad/s^2) + * @param state The current measured vehicle state, note that intent is in here as well * @return The per-wheel torque requests to achieve the desired accelerations, in Newton-meters */ -app::tv::shared_datatypes::datatypes::ControlOutput - update(const app::tv::estimation::VehicleStateEstimator::Measurements &state, float ax, float omega_dot); +app::tv::shared_datatypes::datatypes::ControlOutput update(const app::tv::estimation::Measurements &state); /** * This is the main entrypoint into the low level vehicle controls algorithm for autonomous - * @param state The current measured vehicle state - * @param ax Requested Longitudinal acceleration (m/s^2) - * @param omega_dot Requested Yaw acceleration (rad/s^2) + * @param state The current measured vehicle state, note that intent is in here as well * @return */ app::tv::shared_datatypes::datatypes::ControlOutputAutonomous - update_autonomous(const app::tv::estimation::VehicleStateEstimator::Measurements &state, float ax, float omega_dot); + update_autonomous(const app::tv::estimation::Measurements &state); From 1e509fe6cdc02ef7f605f4f0f6b50529d09a0f71 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 9 Apr 2026 12:28:17 -0700 Subject: [PATCH 08/35] kms hours --- .../torque_allocator/torque_allocator.cpp | 36 +- .../estimation/tire_model.cpp | 336 ++++++++---------- .../estimation/tire_model.hpp | 263 +++++++------- .../shared_datatypes/datatypes.hpp | 6 + .../shared_datatypes/low_speed_blend.hpp | 5 +- .../app/torque_vectoring/torque_vectoring.cpp | 34 +- .../app/torque_vectoring/torque_vectoring.hpp | 5 +- 7 files changed, 321 insertions(+), 364 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp index 84e3c8e8bf..7ad98150d0 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp @@ -9,6 +9,7 @@ #include #include "torque_vectoring/shared_datatypes/constants.hpp" +#include "torque_vectoring/estimation/tire_model.hpp" using namespace app::tv::shared_datatypes; using namespace app::tv::shared_datatypes::vd_constants; @@ -107,26 +108,29 @@ namespace // stays allocation-free and predictable on embedded targets. const auto residualVector = [&](const DualVec4 &kappa) -> DualVec5 { + const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); + const auto alphas = state.alphas(); + const wheel_set predicted_fx{ - .fl = tire_models.fl.computeCombinedFx_N( - normal_forces_N.fl, current_slip_angles.fl, low_speed_blend, kappa(0)), - .fr = tire_models.fr.computeCombinedFx_N( - normal_forces_N.fr, current_slip_angles.fr, low_speed_blend, kappa(1)), - .rl = tire_models.rl.computeCombinedFx_N( - normal_forces_N.rl, current_slip_angles.rl, low_speed_blend, kappa(2)), - .rr = tire_models.rr.computeCombinedFx_N( - normal_forces_N.rr, current_slip_angles.rr, low_speed_blend, kappa(3)), + .fl = estimation::tire_models.fl.computeCombinedFx_N( + fz_fl, current_slip_angles.fl, low_speed_blend, kappa(0)), + .fr = estimation::tire_models.fr.computeCombinedFx_N( + fz_fr, current_slip_angles.fr, low_speed_blend, kappa(1)), + .rl = estimation::tire_models.rl.computeCombinedFx_N( + fz_rl, current_slip_angles.rl, low_speed_blend, kappa(2)), + .rr = estimation::tire_models.rr.computeCombinedFx_N( + fz_rr, current_slip_angles.rr, low_speed_blend, kappa(3)), }; const wheel_set predicted_fy{ - .fl = tire_models.fl.computeCombinedFy_N( - normal_forces_N.fl, current_slip_angles.fl, low_speed_blend, kappa(0)), - .fr = tire_models.fr.computeCombinedFy_N( - normal_forces_N.fr, current_slip_angles.fr, low_speed_blend, kappa(1)), - .rl = tire_models.rl.computeCombinedFy_N( - normal_forces_N.rl, current_slip_angles.rl, low_speed_blend, kappa(2)), - .rr = tire_models.rr.computeCombinedFy_N( - normal_forces_N.rr, current_slip_angles.rr, low_speed_blend, kappa(3)), + .fl = estimation::tire_models.fl.computeCombinedFy_N( + fz_fl, current_slip_angles.fl, low_speed_blend, kappa(0)), + .fr = estimation::tire_models.fr.computeCombinedFy_N( + fz_fr, current_slip_angles.fr, low_speed_blend, kappa(1)), + .rl = estimation::tire_models.rl.computeCombinedFy_N( + fz_rl, current_slip_angles.rl, low_speed_blend, kappa(2)), + .rr = estimation::tire_models.rr.computeCombinedFy_N( + fz_rr, current_slip_angles.rr, low_speed_blend, kappa(3)), }; const autodiff::dual predicted_mz = yawMomentFromTireForces(predicted_fx, predicted_fy, steering_angle_rad); diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index e70ae071f2..f4d84bd4a8 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -1,9 +1,6 @@ #include "tire_model.hpp" #include "torque_vectoring/shared_datatypes/constants.hpp" -#include "torque_vectoring/shared_datatypes/low_speed_blend.hpp" - #include -#include using namespace app::tv::shared_datatypes::vd_constants; @@ -16,115 +13,153 @@ namespace return std::fmax(std::fabs(value), SMALL_EPSILON); } - [[nodiscard]] float primalValue(const float value) - { - return value; - } + // [[nodiscard]] float primalValue(const float value) + // { + // return value; + // } + // + // [[nodiscard]] float primalValue(const autodiff::dual &value) + // { + // return static_cast(autodiff::val(value)); + // } - [[nodiscard]] float primalValue(const autodiff::dual &value) + [[nodiscard]] float safeTemplateDenominator(const float value) { - return static_cast(autodiff::val(value)); - } + if (std::fabs(value) >= SMALL_EPSILON) + { + return value; + } - template [[nodiscard]] T safeTemplateDenominator(const T &value) + return value < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; + } + [[nodiscard]] autodiff::dual safeTemplateDenominator(const autodiff::dual &value) { - const float primal = primalValue(value); - - if (std::fabs(primal) >= SMALL_EPSILON) + if (std::fabs(static_cast(autodiff::val(value))) >= SMALL_EPSILON) { return value; } - - return T(primal < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON); + if (static_cast(autodiff::val(value)) < 0.0f) + { + return -SMALL_EPSILON; + } + return SMALL_EPSILON; } } // namespace -[[nodiscard]] TireModel::Outputs TireModel::estimate(const StateInputs &inputs) const -{ - const float vehicle_speed_mps = std::hypot(inputs.vehicle_velocity_x_mps, inputs.vehicle_velocity_y_mps); - const float low_speed_blend = shared_datatypes::velocityBlend(vehicle_speed_mps); - const auto [x_mps, y_mps] = - wheelVelocities(inputs.vehicle_velocity_x_mps, inputs.vehicle_velocity_y_mps, inputs.yaw_rate_radps); - const float slip_angle_rad = estimateSlipAngle(x_mps, y_mps, inputs.steering_angle_rad); - const float slip_ratio = estimateSlipRatio(x_mps, y_mps, slip_angle_rad, inputs.wheel_angular_velocity_radps); - - return { - .slip_ratio = slip_ratio, - .slip_angle_rad = slip_angle_rad, - .longitudinal_force_N = static_cast( - computeCombinedFx_N(inputs.normal_load_N, slip_angle_rad, low_speed_blend, slip_ratio)), - .lateral_force_N = computeCombinedFy_N(inputs.normal_load_N, slip_angle_rad, low_speed_blend, slip_ratio), - }; -} - -[[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity(const float slip_ratio, const float wheel_vel_x_mps) -{ - const float wheel_surface_speed_mps = wheel_vel_x_mps * (1.0f + slip_ratio); - return wheel_surface_speed_mps / safeMagnitude(WHEEL_RADIUS_M); -} - -[[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity( - const float slip_ratio, - const datatypes::datatypes::VehicleState &vehicle_state) const -{ - const auto [x_mps, y_mps] = - wheelVelocities(vehicle_state.v_x_mps, vehicle_state.v_y_mps, vehicle_state.yaw_rate_radps); - return slipRatioToWheelAngularVelocity(slip_ratio, x_mps); -} - -[[nodiscard]] float TireModel::wheelLongOffset_m() const -{ - return wheel_axle_ == WheelAxle::Front ? DIST_FRONT_AXLE_CG_m : -DIST_REAR_AXLE_CG_m; -} - -[[nodiscard]] float TireModel::wheelLatOffset_m() const -{ - return wheel_side_ == WheelSide::Left ? TRACK_WIDTH_m * 0.5f : -TRACK_WIDTH_m * 0.5f; -} - -[[nodiscard]] float TireModel::wheelSteeringAngle_rad(const float steering_angle_rad) const -{ - return wheel_axle_ == WheelAxle::Front ? steering_angle_rad : 0.0f; -} - -[[nodiscard]] TireModel::WheelVelocities TireModel::wheelVelocities( - const float vehicle_velocity_x_mps, - const float vehicle_velocity_y_mps, - const float yaw_rate_radps) const +template +[[nodiscard]] T TireModel::computeCombinedFx_N( + const float normal_load_N, + const float slip_angle_rad, + // float low_speed_blend, + const T &slip_ratio) const { - // Rigid-body planar kinematics in the body frame: - // v_wheel = v_cg + omega_z x r_wheel, where r_wheel = [x_offset, y_offset, 0]. - return { - .x_mps = vehicle_velocity_x_mps - (yaw_rate_radps * wheelLatOffset_m()), - .y_mps = vehicle_velocity_y_mps + (yaw_rate_radps * wheelLongOffset_m()), - }; -} + const T pure_fx_0 = computePureFx_N(normal_load_N, slip_ratio); + const auto coefficients = combinedFxMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); + const T force_blend = T(1); // T(low_speed_blend); -[[nodiscard]] float TireModel::estimateSlipAngle( - const float wheel_vel_x_mps, - const float wheel_vel_y_mps, - const float steering_angle_rad) const -{ - return std::atan2(wheel_vel_y_mps, wheel_vel_x_mps) - wheelSteeringAngle_rad(steering_angle_rad); + // Low-speed safeguard: + // Below a small vehicle-speed threshold the tire model can predict unrealistically large + // forces because the slip calculation becomes ill-conditioned while the fitted Pacejka + // peak terms remain large. Blend the final combined-slip force output to zero so the model + // fades out smoothly instead of producing unstable low-speed force demands. + // + // Pacejka Page 181 (4.E50): F_x = G_xa * F_x0 + return force_blend * (coefficients.g_xa * pure_fx_0); } +template float TireModel::computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const float &slip_ratio) const; +template autodiff::dual TireModel::computeCombinedFx_N( + float normal_load_N, + float slip_angle_rad, + const autodiff::dual &slip_ratio) const; -[[nodiscard]] float TireModel::estimateSlipRatio( - const float wheel_vel_x_mps, - const float wheel_vel_y_mps, +template +[[nodiscard]] T TireModel::computeCombinedFy_N( + const float normal_load_N, const float slip_angle_rad, - const float wheel_angular_velocity_radps) + // float low_speed_blend, + const T &slip_ratio) const { - const float wheel_surface_speed_mps = wheel_angular_velocity_radps * WHEEL_RADIUS_M; - const float wheel_speed_magnitude_mps = std::hypot(wheel_vel_x_mps, wheel_vel_y_mps); - const float effective_wheel_speed_mps = wheel_speed_magnitude_mps * std::cos(slip_angle_rad); + const float pure_fy_0 = computePureFy_N(normal_load_N, slip_angle_rad); + const auto coefficients = combinedFyMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); + const T force_blend = T(1); // T(low_speed_blend); - return (wheel_surface_speed_mps - effective_wheel_speed_mps) / safeMagnitude(effective_wheel_speed_mps); + // Apply the same low-speed force-availability blend used for Fx so the optimizer sees a + // self-consistent pair of tire forces as the vehicle approaches a stop. + // + // Pacejka Pages 181-182 (4.E58): F_y = G_yk * F_y0 + S_vyk + return force_blend * ((coefficients.g_yk * T(pure_fy_0)) + coefficients.s_vyk); } +template float + TireModel::computeCombinedFy_N(float normal_load_N, float slip_angle_rad, const float &slip_ratio) const; +template autodiff::dual TireModel::computeCombinedFy_N( + float normal_load_N, + float slip_angle_rad, + const autodiff::dual &slip_ratio) const; -//-------------------------------------------------------------------- Pacejka 5.2 -//----------------------------------------------------------------------// - -template +// [[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity(const float slip_ratio, const float +// wheel_vel_x_mps) +// { +// const float wheel_surface_speed_mps = wheel_vel_x_mps * (1.0f + slip_ratio); +// return wheel_surface_speed_mps / safeMagnitude(WHEEL_RADIUS_M); +// } +// +// [[nodiscard]] float TireModel::slipRatioToWheelAngularVelocity( +// const float slip_ratio, +// const shared_datatypes::VehicleState &vehicle_state) const +// { +// const auto [x_mps, y_mps] = +// wheelVelocities(vehicle_state.v_x_mps, vehicle_state.v_y_mps, vehicle_state.yaw_rate_radps); +// return slipRatioToWheelAngularVelocity(slip_ratio, x_mps); +// } +// [[nodiscard]] float TireModel::wheelLongOffset_m() const +// { +// return wheel_axle_ == WheelAxle::Front ? DIST_FRONT_AXLE_CG_m : -DIST_REAR_AXLE_CG_m; +// } +// +// [[nodiscard]] float TireModel::wheelLatOffset_m() const +// { +// return wheel_side_ == WheelSide::Left ? TRACK_WIDTH_m * 0.5f : -TRACK_WIDTH_m * 0.5f; +// } +// [[nodiscard]] float TireModel::wheelSteeringAngle_rad(const float steering_angle_rad) const +// { +// return wheel_axle_ == WheelAxle::Front ? steering_angle_rad : 0.0f; +// } +// [[nodiscard]] TireModel::WheelVelocities TireModel::wheelVelocities( +// const float vehicle_velocity_x_mps, +// const float vehicle_velocity_y_mps, +// const float yaw_rate_radps) const +// { +// // Rigid-body planar kinematics in the body frame: +// // v_wheel = v_cg + omega_z x r_wheel, where r_wheel = [x_offset, y_offset, 0]. +// return { +// .x_mps = vehicle_velocity_x_mps - (yaw_rate_radps * wheelLatOffset_m()), +// .y_mps = vehicle_velocity_y_mps + (yaw_rate_radps * wheelLongOffset_m()), +// }; +// } +// [[nodiscard]] float TireModel::estimateSlipAngle( +// const float wheel_vel_x_mps, +// const float wheel_vel_y_mps, +// const float steering_angle_rad) const +// { +// return std::atan2(wheel_vel_y_mps, wheel_vel_x_mps) - wheelSteeringAngle_rad(steering_angle_rad); +// } +// +// [[nodiscard]] float TireModel::estimateSlipRatio( +// const float wheel_vel_x_mps, +// const float wheel_vel_y_mps, +// const float slip_angle_rad, +// const float wheel_angular_velocity_radps) +// { +// const float wheel_surface_speed_mps = wheel_angular_velocity_radps * WHEEL_RADIUS_M; +// const float wheel_speed_magnitude_mps = std::hypot(wheel_vel_x_mps, wheel_vel_y_mps); +// const float effective_wheel_speed_mps = wheel_speed_magnitude_mps * std::cos(slip_angle_rad); +// +// return (wheel_surface_speed_mps - effective_wheel_speed_mps) / safeMagnitude(effective_wheel_speed_mps); +// } + +//------------------------------------------ Pacejka 5.2 ----------------------------------// + +template TireModel::PureFxMagicFormulaCoefficients TireModel::pureFxMagicFormulaCoefficients(const float normal_load_N, const T &slip_ratio) const { @@ -177,10 +212,9 @@ TireModel::PureFyMagicFormulaCoefficients }; } -template T TireModel::computePureFx_N(const float normal_load_N, const T &slip_ratio) const +template T TireModel::computePureFx_N(const float normal_load_N, const T &slip_ratio) const { - using std::atan; - using std::sin; + using std::sin, std::atan; const auto coefficients = pureFxMagicFormulaCoefficients(normal_load_N, slip_ratio); const T u = coefficients.b_x * coefficients.kappa_x; @@ -202,45 +236,6 @@ float TireModel::computePureFy_N(const float normal_load_N, const float slip_ang coefficients.s_vy; } -template -T TireModel::computeCombinedFx_N( - const float normal_load_N, - const float slip_angle_rad, - const float low_speed_blend, - const T &slip_ratio) const -{ - const T pure_fx_0 = computePureFx_N(normal_load_N, slip_ratio); - const auto coefficients = combinedFxMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); - const T force_blend = T(low_speed_blend); - - // Low-speed safeguard: - // Below a small vehicle-speed threshold the tire model can predict unrealistically large - // forces because the slip calculation becomes ill-conditioned while the fitted Pacejka - // peak terms remain large. Blend the final combined-slip force output to zero so the model - // fades out smoothly instead of producing unstable low-speed force demands. - // - // Pacejka Page 181 (4.E50): F_x = G_xa * F_x0 - return force_blend * (coefficients.g_xa * pure_fx_0); -} - -template -T TireModel::computeCombinedFy_N( - const float normal_load_N, - const float slip_angle_rad, - const float low_speed_blend, - const T &slip_ratio) const -{ - const float pure_fy_0 = computePureFy_N(normal_load_N, slip_angle_rad); - const auto coefficients = combinedFyMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); - const T force_blend = T(low_speed_blend); - - // Apply the same low-speed force-availability blend used for Fx so the optimizer sees a - // self-consistent pair of tire forces as the vehicle approaches a stop. - // - // Pacejka Pages 181-182 (4.E58): F_y = G_yk * F_y0 + S_vyk - return force_blend * ((coefficients.g_yk * T(pure_fy_0)) + coefficients.s_vyk); -} - constexpr float TireModel::combinedFx_SHxa() const { // Pacejka Page 181 (4.E57): S_Hxa @@ -265,7 +260,7 @@ float TireModel::combinedFx_Exa(const float normalized_load_delta) const return std::fmin(fit_comb_fx_.rEx1 + (fit_comb_fx_.rEx2 * normalized_load_delta), 1.0f); } -template T TireModel::combinedFx_Bxa(const T &slip_ratio) const +template T TireModel::combinedFx_Bxa(const T &slip_ratio) const { using std::atan; using std::cos; @@ -274,7 +269,7 @@ template T TireModel::combinedFx_Bxa(const T &slip_ratio) const return T(fit_comb_fx_.rBx1) * cos(atan(T(fit_comb_fx_.rBx2) * slip_ratio)); } -template T TireModel::combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients &coefficients) const +template T TireModel::combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients &coefficients) const { using std::atan; using std::cos; @@ -285,7 +280,7 @@ template T TireModel::combinedFx_Gxao(const CombinedFxMagicFormulaC return cos(coefficients.c_xa * atan(phi)); } -template T TireModel::combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients &coefficients) const +template T TireModel::combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients &coefficients) const { using std::atan; using std::cos; @@ -298,7 +293,7 @@ template T TireModel::combinedFx_Gxa(const CombinedFxMagicFormulaCo return numerator / safeTemplateDenominator(coefficients.g_xao); } -template +template TireModel::CombinedFxMagicFormulaCoefficients TireModel::combinedFxMagicFormulaCoefficients( const float normal_load_N, const float slip_angle_rad, @@ -328,7 +323,7 @@ float TireModel::combinedFy_SHyk(const float normalized_load_delta) const return fit_comb_fy_.rHy1 + (fit_comb_fy_.rHy2 * normalized_load_delta); } -template T TireModel::combinedFy_Kappa_s(const float normalized_load_delta, const T &slip_ratio) const +template T TireModel::combinedFy_Kappa_s(const float normalized_load_delta, const T &slip_ratio) const { // Pacejka Page 181 (4.E61): kappa_s = kappa + S_Hyk return slip_ratio + T(combinedFy_SHyk(normalized_load_delta)); @@ -348,11 +343,8 @@ float TireModel::combinedFy_Eyk(const float normalized_load_delta) const float TireModel::combinedFy_Byk(const float slip_angle_rad) const { - using std::atan; - using std::cos; - // Pacejka Page 182 (4.E62): B_yk with gamma* = 0 and lambda_yk = 1. - return fit_comb_fy_.rBy1 * cos(atan(fit_comb_fy_.rBy2 * (slip_angle_rad - fit_comb_fy_.rBy3))); + return fit_comb_fy_.rBy1 * std::cos(std::atan(fit_comb_fy_.rBy2 * (slip_angle_rad - fit_comb_fy_.rBy3))); } float TireModel::combinedFy_Dvyk( @@ -360,39 +352,31 @@ float TireModel::combinedFy_Dvyk( const float normalized_load_delta, const float slip_angle_rad) const { - using std::atan; - using std::cos; - // Pacejka Page 182 (4.E67): D_vyk with gamma* = 0 and zeta_2 = 1. return pureFy_mu(normalized_load_delta) * normal_load_N * (fit_comb_fy_.rVy1 + (fit_comb_fy_.rVy2 * normalized_load_delta)) * - cos(atan(fit_comb_fy_.rVy4 * slip_angle_rad)); + std::cos(std::atan(fit_comb_fy_.rVy4 * slip_angle_rad)); } -template T TireModel::combinedFy_Svyk(const T &d_vyk, const T &slip_ratio) const +template T TireModel::combinedFy_Svyk(const T &d_vyk, const T &slip_ratio) const { - using std::atan; - using std::sin; - + using std::sin, std::atan; // Pacejka Page 182 (4.E66): S_vyk with lambda_vyk = 1. return d_vyk * sin(T(fit_comb_fy_.rVy5) * atan(T(fit_comb_fy_.rVy6) * slip_ratio)); } -template T TireModel::combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients &coefficients) const +template T TireModel::combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients &coefficients) const { - using std::atan; - using std::cos; - + using std::cos, std::atan; // Pacejka Page 181 (4.E60): G_yko const T u = coefficients.b_yk * coefficients.s_hyk; const T phi = u - coefficients.e_yk * (u - atan(u)); return cos(coefficients.c_yk * atan(phi)); } -template T TireModel::combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients &coefficients) const +template T TireModel::combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients &coefficients) const { - using std::atan; - using std::cos; + using std::cos, std::atan; // Pacejka Page 181 (4.E59): G_yk with G_yko > 0 protected by a signed epsilon denominator. const T u = coefficients.b_yk * coefficients.kappa_s; @@ -402,7 +386,7 @@ template T TireModel::combinedFy_Gyk(const CombinedFyMagicFormulaCo return numerator / safeTemplateDenominator(coefficients.g_yko); } -template +template TireModel::CombinedFyMagicFormulaCoefficients TireModel::combinedFyMagicFormulaCoefficients( const float normal_load_N, const float slip_angle_rad, @@ -439,7 +423,7 @@ constexpr float TireModel::safeSignedDenominator(const float value) return value < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; } -constexpr float TireModel::signum(const float value) +constexpr float TireModel::sign(const float value) { if (value > 0.0f) { @@ -468,7 +452,7 @@ constexpr float TireModel::pureFx_Sh(const float normalized_load_delta) const return fit_pure_fx_.hx_1 + (fit_pure_fx_.hx_2 * normalized_load_delta); } -template T TireModel::pureFx_Kappa(const float normalized_load_delta, const T &slip_ratio) const +template T TireModel::pureFx_Kappa(const float normalized_load_delta, const T &slip_ratio) const { // Pacejka Page 179 (4.E10): kappa_x = kappa + S_Hx return slip_ratio + T(pureFx_Sh(normalized_load_delta)); @@ -492,7 +476,7 @@ constexpr float TireModel::pureFx_D(const float normal_load_N, const float norma return pureFx_mu(normalized_load_delta) * normal_load_N; } -template T TireModel::pureFx_E(const float normalized_load_delta, const T &kappa_x) const +template T TireModel::pureFx_E(const float normalized_load_delta, const T &kappa_x) const { // Pacejka Page 179 (4.E14): E_x const float normalized_load_delta_squared = normalized_load_delta * normalized_load_delta; @@ -500,7 +484,7 @@ template T TireModel::pureFx_E(const float normalized_load_delta, c (fit_pure_fx_.ex_3 * normalized_load_delta_squared); const float kappa_x_value = primalValue(kappa_x); - return T(base_e_x * (1.0f - (fit_pure_fx_.ex_4 * signum(kappa_x_value)))); + return T(base_e_x * (1.0f - (fit_pure_fx_.ex_4 * sign(kappa_x_value)))); } constexpr float TireModel::pureFx_K(const float normal_load_N, const float normalized_load_delta) const @@ -556,7 +540,7 @@ constexpr float TireModel::pureFy_E(const float normalized_load_delta, const flo { // Pacejka Page 180 (4.E24): E_y with gamma terms reduced to zero. return (fit_pure_fy_.ey_1 + (fit_pure_fy_.ey_2 * normalized_load_delta)) * - (1.0f - (fit_pure_fy_.ey_3 * signum(alpha_y))); + (1.0f - (fit_pure_fy_.ey_3 * sign(alpha_y))); } constexpr float TireModel::pureFy_K(const float normal_load_N) const @@ -577,26 +561,4 @@ constexpr float TireModel::pureFy_Sv(const float normal_load_N, const float norm // Pacejka Page 180 (4.E29): S_Vy with gamma terms reduced to zero. return normal_load_N * (fit_pure_fy_.vy_1 + (fit_pure_fy_.vy_2 * normalized_load_delta)); } - -template float TireModel::computeCombinedFx_N( - const float normal_load_N, - const float slip_angle_rad, - const float low_speed_blend, - const float &slip_ratio) const; -template autodiff::dual TireModel::computeCombinedFx_N( - const float normal_load_N, - const float slip_angle_rad, - const float low_speed_blend, - const autodiff::dual &slip_ratio) const; -template float TireModel::computeCombinedFy_N( - const float normal_load_N, - const float slip_angle_rad, - const float low_speed_blend, - const float &slip_ratio) const; -template autodiff::dual TireModel::computeCombinedFy_N( - const float normal_load_N, - const float slip_angle_rad, - const float low_speed_blend, - const autodiff::dual &slip_ratio) const; - } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index 520c693175..c9614e33d4 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -1,9 +1,12 @@ #pragma once -#include "torque_vectoring/shared_datatypes/datatypes.hpp" +#include namespace app::tv::estimation { +template +concept DecimalOrDual = std::same_as || std::same_as || std::same_as; + class TireModel { public: @@ -19,54 +22,23 @@ class TireModel Rear }; - struct StateInputs + template struct Forces { - float wheel_angular_velocity_radps = 0.0f; - float vehicle_velocity_x_mps = 0.0f; - float vehicle_velocity_y_mps = 0.0f; - float yaw_rate_radps = 0.0f; - float steering_angle_rad = 0.0f; - float normal_load_N = 0.0f; + T fx_N; + T fy_N; }; - - struct Outputs - { - float slip_ratio = 0.0f; - float slip_angle_rad = 0.0f; - float longitudinal_force_N = 0.0f; - float lateral_force_N = 0.0f; - }; - - constexpr TireModel(const WheelSide wheel_side, const WheelAxle wheel_axle) - : fit_pure_fx_(FIT_PURE_FX_12_PSI), - fit_pure_fy_(FIT_PURE_FY_12_PSI), - fit_comb_fx_(FIT_COMB_FX_12_PSI), - fit_comb_fy_(FIT_COMB_FY_12_PSI), - wheel_side_(wheel_side), - wheel_axle_(wheel_axle) + template + [[nodiscard]] Forces estimate(const T &kappa, const float alpha_rad, const float fz_N) const { + return { + computeCombinedFx_N(fz_N, alpha_rad, kappa), + computeCombinedFy_N(fz_N, alpha_rad, kappa), + }; } - [[nodiscard]] Outputs estimate(const StateInputs &inputs) const; - template - [[nodiscard]] T - computeCombinedFx_N(float normal_load_N, float slip_angle_rad, float low_speed_blend, const T &slip_ratio) - const; - template - [[nodiscard]] T - computeCombinedFy_N(float normal_load_N, float slip_angle_rad, float low_speed_blend, const T &slip_ratio) - const; - [[nodiscard]] static float slipRatioToWheelAngularVelocity(float slip_ratio, float wheel_vel_x_mps); - [[nodiscard]] float slipRatioToWheelAngularVelocity( - float slip_ratio, - const datatypes::datatypes::VehicleState &vehicle_state) const; - - private: - struct WheelVelocities - { - float x_mps = 0.0f; - float y_mps = 0.0f; - }; + // [[nodiscard]] static float slipRatioToWheelAngularVelocity(float slip_ratio, float wheel_vel_x_mps); + // [[nodiscard]] float + // slipRatioToWheelAngularVelocity(float slip_ratio, const shared_datatypes::VehicleState &vehicle_state) const; struct TireFitPureParamFy { @@ -139,7 +111,26 @@ class TireModel float rVy6; }; - template struct PureFxMagicFormulaCoefficients + TireModel() = delete; + + protected: + constexpr TireModel( + const TireFitPureParamFx &fit_pure_fx, + const TireFitPureParamFy &fit_pure_fy, + const TireFitCombParamFx &fit_comb_fx, + const TireFitCombParamFy &fit_comb_fy) + : fit_pure_fx_(fit_pure_fx), fit_pure_fy_(fit_pure_fy), fit_comb_fx_(fit_comb_fx), fit_comb_fy_(fit_comb_fy) + { + } + + private: + // note that these only exist for float, dual + template + [[nodiscard]] T computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; + template + [[nodiscard]] T computeCombinedFy_N(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; + + template struct PureFxMagicFormulaCoefficients { T s_hx = T(0.0f); T kappa_x = T(0.0f); @@ -150,7 +141,7 @@ class TireModel T s_vx = T(0.0f); }; - template struct CombinedFxMagicFormulaCoefficients + template struct CombinedFxMagicFormulaCoefficients { T s_hxa = T(0.0f); T alpha_s = T(0.0f); @@ -172,7 +163,7 @@ class TireModel float s_vy = 0.0f; }; - template struct CombinedFyMagicFormulaCoefficients + template struct CombinedFyMagicFormulaCoefficients { T s_hyk = T(0.0f); T kappa_s = T(0.0f); @@ -185,23 +176,99 @@ class TireModel T g_yk = T(0.0f); }; + static constexpr float NOMINAL_FZ_N = 750.0f; + //-------------------------------------------------------------------- Class Helpers + //----------------------------------------------------------------------// + + [[nodiscard]] static constexpr float safeSignedDenominator(float value); + [[nodiscard]] static constexpr float sign(float value); + [[nodiscard]] static constexpr float normalizedLoadDelta(float normal_load_N); + // Reduced-model assumptions for combined slip in this pass: + // gamma* = 0, lambda_xa = 1, lambda_yk = 1, lambda_vyk = 1, zeta_2 = 1. + // Pressure dependence is captured by the fixed 12_PSI fitted parameter row. + //-------------------------------------------------------------------- Pure Pacejka 5.2 Helpers + //----------------------------------------------------------------------// + [[nodiscard]] constexpr float pureFx_Sh(float normalized_load_delta) const; + template [[nodiscard]] T pureFx_Kappa(float normalized_load_delta, const T &slip_ratio) const; + [[nodiscard]] constexpr float pureFx_C() const; + [[nodiscard]] constexpr float pureFx_mu(float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFx_D(float normal_load_N, float normalized_load_delta) const; + template [[nodiscard]] T pureFx_E(float normalized_load_delta, const T &kappa_x) const; + [[nodiscard]] constexpr float pureFx_K(float normal_load_N, float normalized_load_delta) const; + [[nodiscard]] static constexpr float pureFx_B(float slip_stiffness, float shape_factor, float peak_factor); + [[nodiscard]] constexpr float pureFx_Sv(float normal_load_N, float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_Sh(float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_Alpha(float normalized_load_delta, float slip_angle_rad) const; + [[nodiscard]] constexpr float pureFy_C() const; + [[nodiscard]] constexpr float pureFy_mu(float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_D(float normal_load_N, float normalized_load_delta) const; + [[nodiscard]] constexpr float pureFy_E(float normalized_load_delta, float alpha_y) const; + [[nodiscard]] constexpr float pureFy_K(float normal_load_N) const; + [[nodiscard]] static constexpr float pureFy_B(float cornering_stiffness, float shape_factor, float peak_factor); + [[nodiscard]] constexpr float pureFy_Sv(float normal_load_N, float normalized_load_delta) const; + template + [[nodiscard]] PureFxMagicFormulaCoefficients + pureFxMagicFormulaCoefficients(float normal_load_N, const T &slip_ratio) const; + [[nodiscard]] PureFyMagicFormulaCoefficients + pureFyMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad) const; + //-------------------------------------------------------------------- Combined Pacejka 5.2 Helpers + //----------------------------------------------------------------------// + [[nodiscard]] constexpr float combinedFx_SHxa() const; + [[nodiscard]] constexpr float combinedFx_Alpha_s(float slip_angle_rad) const; + [[nodiscard]] constexpr float combinedFx_Cxa() const; + [[nodiscard]] float combinedFx_Exa(float normalized_load_delta) const; + template [[nodiscard]] T combinedFx_Bxa(const T &slip_ratio) const; + template + [[nodiscard]] T combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients &coefficients) const; + template + [[nodiscard]] T combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients &coefficients) const; + template + [[nodiscard]] CombinedFxMagicFormulaCoefficients + combinedFxMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; + [[nodiscard]] float combinedFy_SHyk(float normalized_load_delta) const; + template + [[nodiscard]] T combinedFy_Kappa_s(float normalized_load_delta, const T &slip_ratio) const; + [[nodiscard]] constexpr float combinedFy_Cyk() const; + [[nodiscard]] float combinedFy_Eyk(float normalized_load_delta) const; + [[nodiscard]] float combinedFy_Byk(float slip_angle_rad) const; + [[nodiscard]] float combinedFy_Dvyk(float normal_load_N, float normalized_load_delta, float slip_angle_rad) const; + template [[nodiscard]] T combinedFy_Svyk(const T &d_vyk, const T &slip_ratio) const; + template + [[nodiscard]] T combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients &coefficients) const; + template + [[nodiscard]] T combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients &coefficients) const; + template + [[nodiscard]] CombinedFyMagicFormulaCoefficients + combinedFyMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; + + template [[nodiscard]] T computePureFx_N(float normal_load_N, const T &slip_ratio) const; + [[nodiscard]] float computePureFy_N(float normal_load_N, float slip_angle_rad) const; + + const TireFitPureParamFx &fit_pure_fx_; + const TireFitPureParamFy &fit_pure_fy_; + const TireFitCombParamFx &fit_comb_fx_; + const TireFitCombParamFy &fit_comb_fy_; +}; + +class HoosierTireModel : public TireModel +{ /* Current model uses the 12_PSI fitted workbook row as a fixed-pressure operating assumption. If pressure becomes a runtime input later, add pressure interpolation or a refit against the raw tire data. */ - static constexpr TireFitPureParamFx FIT_PURE_FX_12_PSI = { + static constexpr TireFitPureParamFx HOOSIER_FIT_PURE_FX_12_PSI = { 0.008656294312f, 366.1925838f, -57.2826791f, 8.208916842f, 1.134744942f, 0.04728092792f, -0.02191230313f, -0.01799018414f, 62.46585932f, 1.390484252f, -0.3909753489f, 0.0005429909716f, -0.0007019708842f, 0.01286770906f, 0.004135955934f, }; - static constexpr TireFitPureParamFy FIT_PURE_FY_12_PSI = { + static constexpr TireFitPureParamFy HOOSIER_FIT_PURE_FY_12_PSI = { 0.90721342f, -3.824114982f, -0.4280518359f, 15.56383821f, 1.218066219f, 0.3097318297f, -0.02930740966f, 0.2068388689f, -54.60640233f, 2.122730258f, 1.671281037f, -0.004171875264f, -0.00360720284f, -0.0787609381f, -0.1057253032f, -0.02043816084f, 0.83972231f, 1.494231303f, }; - static constexpr TireFitCombParamFx FIT_COMB_FX_12_PSI = { + static constexpr TireFitCombParamFx HOOSIER_FIT_COMB_FX_12_PSI = { .rBx1 = 13.046f, .rBx2 = 9.718f, .rBx3 = 0.0f, @@ -211,7 +278,7 @@ class TireModel .rHx1 = -0.0001f, }; - static constexpr TireFitCombParamFy FIT_COMB_FY_12_PSI = { + static constexpr TireFitCombParamFy HOOSIER_FIT_COMB_FY_12_PSI = { .rBy1 = 10.622f, .rBy2 = 7.82f, .rBy3 = 0.00204f, @@ -229,89 +296,17 @@ class TireModel .rVy6 = 23.8f, }; - static constexpr float NOMINAL_FZ_N = 750.0f; - //-------------------------------------------------------------------- Class Helpers - //----------------------------------------------------------------------// - - [[nodiscard]] static constexpr float safeSignedDenominator(float value); - [[nodiscard]] static constexpr float signum(float value); - [[nodiscard]] static constexpr float normalizedLoadDelta(float normal_load_N); - // Reduced-model assumptions for combined slip in this pass: - // gamma* = 0, lambda_xa = 1, lambda_yk = 1, lambda_vyk = 1, zeta_2 = 1. - // Pressure dependence is captured by the fixed 12_PSI fitted parameter row. - //-------------------------------------------------------------------- Pure Pacejka 5.2 Helpers - //----------------------------------------------------------------------// - [[nodiscard]] constexpr float pureFx_Sh(float normalized_load_delta) const; - template [[nodiscard]] T pureFx_Kappa(float normalized_load_delta, const T &slip_ratio) const; - [[nodiscard]] constexpr float pureFx_C() const; - [[nodiscard]] constexpr float pureFx_mu(float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFx_D(float normal_load_N, float normalized_load_delta) const; - template [[nodiscard]] T pureFx_E(float normalized_load_delta, const T &kappa_x) const; - [[nodiscard]] constexpr float pureFx_K(float normal_load_N, float normalized_load_delta) const; - [[nodiscard]] static constexpr float pureFx_B(float slip_stiffness, float shape_factor, float peak_factor); - [[nodiscard]] constexpr float pureFx_Sv(float normal_load_N, float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFy_Sh(float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFy_Alpha(float normalized_load_delta, float slip_angle_rad) const; - [[nodiscard]] constexpr float pureFy_C() const; - [[nodiscard]] constexpr float pureFy_mu(float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFy_D(float normal_load_N, float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFy_E(float normalized_load_delta, float alpha_y) const; - [[nodiscard]] constexpr float pureFy_K(float normal_load_N) const; - [[nodiscard]] static constexpr float pureFy_B(float cornering_stiffness, float shape_factor, float peak_factor); - [[nodiscard]] constexpr float pureFy_Sv(float normal_load_N, float normalized_load_delta) const; - template - [[nodiscard]] PureFxMagicFormulaCoefficients - pureFxMagicFormulaCoefficients(float normal_load_N, const T &slip_ratio) const; - [[nodiscard]] PureFyMagicFormulaCoefficients - pureFyMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad) const; - //-------------------------------------------------------------------- Combined Pacejka 5.2 Helpers - //----------------------------------------------------------------------// - [[nodiscard]] constexpr float combinedFx_SHxa() const; - [[nodiscard]] constexpr float combinedFx_Alpha_s(float slip_angle_rad) const; - [[nodiscard]] constexpr float combinedFx_Cxa() const; - [[nodiscard]] float combinedFx_Exa(float normalized_load_delta) const; - template [[nodiscard]] T combinedFx_Bxa(const T &slip_ratio) const; - template - [[nodiscard]] T combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients &coefficients) const; - template - [[nodiscard]] T combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients &coefficients) const; - template - [[nodiscard]] CombinedFxMagicFormulaCoefficients - combinedFxMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; - [[nodiscard]] float combinedFy_SHyk(float normalized_load_delta) const; - template [[nodiscard]] T combinedFy_Kappa_s(float normalized_load_delta, const T &slip_ratio) const; - [[nodiscard]] constexpr float combinedFy_Cyk() const; - [[nodiscard]] float combinedFy_Eyk(float normalized_load_delta) const; - [[nodiscard]] float combinedFy_Byk(float slip_angle_rad) const; - [[nodiscard]] float combinedFy_Dvyk(float normal_load_N, float normalized_load_delta, float slip_angle_rad) const; - template [[nodiscard]] T combinedFy_Svyk(const T &d_vyk, const T &slip_ratio) const; - template - [[nodiscard]] T combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients &coefficients) const; - template - [[nodiscard]] T combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients &coefficients) const; - template - [[nodiscard]] CombinedFyMagicFormulaCoefficients - combinedFyMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; + public: + constexpr HoosierTireModel() + : TireModel( + HOOSIER_FIT_PURE_FX_12_PSI, + HOOSIER_FIT_PURE_FY_12_PSI, + HOOSIER_FIT_COMB_FX_12_PSI, + HOOSIER_FIT_COMB_FY_12_PSI) + { + } +}; - [[nodiscard]] float wheelLongOffset_m() const; - [[nodiscard]] float wheelLatOffset_m() const; - [[nodiscard]] float wheelSteeringAngle_rad(float steering_angle_rad) const; - [[nodiscard]] WheelVelocities - wheelVelocities(float vehicle_velocity_x_mps, float vehicle_velocity_y_mps, float yaw_rate_radps) const; - [[nodiscard]] float estimateSlipAngle(float wheel_vel_x_mps, float wheel_vel_y_mps, float steering_angle_rad) const; - [[nodiscard]] static float estimateSlipRatio( - float wheel_vel_x_mps, - float wheel_vel_y_mps, - float slip_angle_rad, - float wheel_angular_velocity_radps); - template [[nodiscard]] T computePureFx_N(float normal_load_N, const T &slip_ratio) const; - [[nodiscard]] float computePureFy_N(float normal_load_N, float slip_angle_rad) const; +inline constexpr HoosierTireModel tire_model{}; - const TireFitPureParamFx fit_pure_fx_; - const TireFitPureParamFy fit_pure_fy_; - const TireFitCombParamFx fit_comb_fx_; - const TireFitCombParamFy fit_comb_fy_; - const WheelSide wheel_side_; - const WheelAxle wheel_axle_; -}; } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 026c4e7f62..3be76136a8 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -50,6 +50,8 @@ struct VehicleState return { steer_ang_rad, steer_ang_rad, 0, 0 }; } + wheel_set alphas() const {} + [[nodiscard]] wheel_set kappas() const { const auto tire_angles = get_tire_angle(); @@ -203,6 +205,8 @@ struct ControlOutput const float fr_kappa; const float rl_kappa; const float rr_kappa; + const float max_torque; + const float min_torque; }; struct ControlOutputAutonomous { @@ -211,5 +215,7 @@ struct ControlOutputAutonomous const float rl_kappa; const float rr_kappa; const float delta; + const float max_torque; + const float min_torque; }; } // namespace app::tv::shared_datatypes diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/low_speed_blend.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/low_speed_blend.hpp index cf9834e514..18fc4372e0 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/low_speed_blend.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/low_speed_blend.hpp @@ -22,10 +22,7 @@ static constexpr float SPEED_MAX_MPS = 3.0f; if (vehicle_speed_mps >= SPEED_MAX_MPS) return 1.0f; - return std::clamp( - (vehicle_speed_mps - SPEED_MIN_MPS) / (SPEED_MAX_MPS - SPEED_MIN_MPS), - 0.0f, - 1.0f); + return std::clamp((vehicle_speed_mps - SPEED_MIN_MPS) / (SPEED_MAX_MPS - SPEED_MIN_MPS), 0.0f, 1.0f); } } // namespace app::tv::shared_datatypes diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index 475512dc8d..01c0603ad3 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -1,29 +1,18 @@ -#include "torque_vectoring/estimation/dynamics_estimation.hpp" -#include "torque_vectoring/estimation/tire_model.hpp" +#include "torque_vectoring.hpp" + #include "torque_vectoring/estimation/vehicle_state_estimator.hpp" #include "torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp" #include "torque_vectoring/controllers/torque_allocator/torque_allocator.hpp" #include "torque_vectoring/shared_datatypes/constants.hpp" #include "torque_vectoring/shared_datatypes/datatypes.hpp" -#include "torque_vectoring/shared_datatypes/low_speed_blend.hpp" - -#include namespace app::tv { namespace { - using namespace shared_datatypes::datatypes; + using namespace shared_datatypes; using namespace shared_datatypes::vd_constants; - //------------------------------------- ESTIMATION MODULES ----------------------------------// - wheel_set tire_models{ - .fl = estimation::TireModel(estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Front), - .fr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Front), - .rl = estimation::TireModel(estimation::TireModel::WheelSide::Left, estimation::TireModel::WheelAxle::Rear), - .rr = estimation::TireModel(estimation::TireModel::WheelSide::Right, estimation::TireModel::WheelAxle::Rear), - }; - //------------------------------------- STATE VARIABLES -------------------------------------// // Build TireModel::StateInputs from VehicleState + per-wheel data @@ -100,8 +89,8 @@ ControlOutput update(const estimation::Measurements &measurement) // Compute the low-speed blend once at the orchestration layer and pass it down explicitly. // This keeps the low-speed force-availability heuristic visible in one place instead of // recomputing it independently inside the optimizer. - const float vehicle_speed_mps = std::hypot(estimated_state.v_x_mps, estimated_state.v_y_mps); - const float low_speed_blend = shared_datatypes::velocityBlend(vehicle_speed_mps); + // const float vehicle_speed_mps = std::hypot(estimated_state.v_x_mps, estimated_state.v_y_mps); + // const float low_speed_blend = shared_datatypes::velocityBlend(vehicle_speed_mps); // Gauss-Newton optimizer: invert the combined-slip tire model to find the slip ratios whose // predicted forces best match the desired per-wheel force split and desired yaw moment. @@ -113,16 +102,21 @@ ControlOutput update(const estimation::Measurements &measurement) // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request return { - .fl_kappa = kappa_opt.fl, - .fr_kappa = kappa_opt.fr, - .rl_kappa = kappa_opt.rl, - .rr_kappa = kappa_opt.rr, + .fl_kappa = kappa_opt.fl, + .fr_kappa = kappa_opt.fr, + .rl_kappa = kappa_opt.rl, + .rr_kappa = kappa_opt.rr, + .max_torque = 0, + .min_torque = 0, }; } ControlOutputAutonomous update_autonomous(const estimation::Measurements &measurement, const float ax, const float omega_dot) { + (void)measurement; + (void)ax; + (void)omega_dot; // TODO inshallah one day return {}; } diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp index 287017e50c..89f265fe8a 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -7,12 +7,11 @@ * @param state The current measured vehicle state, note that intent is in here as well * @return The per-wheel torque requests to achieve the desired accelerations, in Newton-meters */ -app::tv::shared_datatypes::datatypes::ControlOutput update(const app::tv::estimation::Measurements &state); +app::tv::shared_datatypes::ControlOutput update(const app::tv::estimation::Measurements &state); /** * This is the main entrypoint into the low level vehicle controls algorithm for autonomous * @param state The current measured vehicle state, note that intent is in here as well * @return */ -app::tv::shared_datatypes::datatypes::ControlOutputAutonomous - update_autonomous(const app::tv::estimation::Measurements &state); +app::tv::shared_datatypes::ControlOutputAutonomous update_autonomous(const app::tv::estimation::Measurements &state); From 8e44ed235ffd8894e8fd91ada8e7f3a2b941ca1b Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:48:00 -0700 Subject: [PATCH 09/35] rotations --- .../torque_allocator/torque_allocator.cpp | 71 +++------ .../yaw_rate_control/controllers_dyrc.cpp | 2 +- .../estimation/tire_model.cpp | 140 +++++------------- .../estimation/tire_model.hpp | 23 +-- .../estimation/vehicle_state_estimator.cpp | 18 +-- .../estimation/vehicle_state_estimator.hpp | 9 +- .../shared_datatypes/constants.hpp | 1 + .../shared_datatypes/datatypes.hpp | 114 ++++++++++---- 8 files changed, 169 insertions(+), 209 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp index 7ad98150d0..d5c7ba8975 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp @@ -32,28 +32,6 @@ namespace using Mat44f = Eigen::Matrix; using DualVec4 = Eigen::Matrix; using DualVec5 = Eigen::Matrix; - - // template - // [[nodiscard]] T yawMomentFromTireForces( - // const shared_datatypes::wheel_set &f_x, - // const shared_datatypes::wheel_set &f_y, - // const float steering_angle_rad) - // { - // constexpr float half_track_m = TRACK_WIDTH_m * 0.5f; - // - // const float cos_delta = std::cos(steering_angle_rad); - // const float sin_delta = std::sin(steering_angle_rad); - // - // const T fl_fx_body = (T(cos_delta) * f_x.fl) - (T(sin_delta) * f_y.fl); - // const T fl_fy_body = (T(sin_delta) * f_x.fl) + (T(cos_delta) * f_y.fl); - // const T fr_fx_body = (T(cos_delta) * f_x.fr) - (T(sin_delta) * f_y.fr); - // const T fr_fy_body = (T(sin_delta) * f_x.fr) + (T(cos_delta) * f_y.fr); - // const T fl_moment = (T(DIST_FRONT_AXLE_CG_m) * fl_fy_body) - (T(half_track_m) * fl_fx_body); - // const T fr_moment = (T(DIST_FRONT_AXLE_CG_m) * fr_fy_body) + (T(half_track_m) * fr_fx_body); - // const T rl_moment = (T(-DIST_REAR_AXLE_CG_m) * f_y.rl) - (T(half_track_m) * f_x.rl); - // const T rr_moment = (T(-DIST_REAR_AXLE_CG_m) * f_y.rr) + (T(half_track_m) * f_x.rr); - // return fl_moment + fr_moment + rl_moment + rr_moment; - // } } // namespace [[nodiscard]] wheel_set optimize(const VehicleState &state, float ax_setpoint, float omegadot_setpoint) @@ -108,44 +86,35 @@ namespace // stays allocation-free and predictable on embedded targets. const auto residualVector = [&](const DualVec4 &kappa) -> DualVec5 { - const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); - const auto alphas = state.alphas(); - - const wheel_set predicted_fx{ - .fl = estimation::tire_models.fl.computeCombinedFx_N( - fz_fl, current_slip_angles.fl, low_speed_blend, kappa(0)), - .fr = estimation::tire_models.fr.computeCombinedFx_N( - fz_fr, current_slip_angles.fr, low_speed_blend, kappa(1)), - .rl = estimation::tire_models.rl.computeCombinedFx_N( - fz_rl, current_slip_angles.rl, low_speed_blend, kappa(2)), - .rr = estimation::tire_models.rr.computeCombinedFx_N( - fz_rr, current_slip_angles.rr, low_speed_blend, kappa(3)), + const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); + const auto [alpha_fl, alpha_fr, alpha_rl, alpha_rr] = state.alphas(); + + const wheel_set predicted_fx{ + .fl = estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)), + .fr = estimation::tire_model.computeCombinedFx_N(fz_fr, alpha_fr, kappa(1)), + .rl = estimation::tire_model.computeCombinedFx_N(fz_rl, alpha_rl, kappa(2)), + .rr = estimation::tire_model.computeCombinedFx_N(fz_rr, alpha_rr, kappa(3)), }; - - const wheel_set predicted_fy{ - .fl = estimation::tire_models.fl.computeCombinedFy_N( - fz_fl, current_slip_angles.fl, low_speed_blend, kappa(0)), - .fr = estimation::tire_models.fr.computeCombinedFy_N( - fz_fr, current_slip_angles.fr, low_speed_blend, kappa(1)), - .rl = estimation::tire_models.rl.computeCombinedFy_N( - fz_rl, current_slip_angles.rl, low_speed_blend, kappa(2)), - .rr = estimation::tire_models.rr.computeCombinedFy_N( - fz_rr, current_slip_angles.rr, low_speed_blend, kappa(3)), + const wheel_set predicted_fy{ + .fl = estimation::tire_model.computeCombinedFy_N(fz_fl, alpha_fl, kappa(0)), + .fr = estimation::tire_model.computeCombinedFy_N(fz_fr, alpha_fr, kappa(1)), + .rl = estimation::tire_model.computeCombinedFy_N(fz_rl, alpha_rl, kappa(2)), + .rr = estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)), }; - const autodiff::dual predicted_mz = yawMomentFromTireForces(predicted_fx, predicted_fy, steering_angle_rad); + [[maybe_unused]] const autodiff::dual predicted_mz = state.est_Mz_N(predicted_fx, predicted_fy); DualVec5 residuals; - residuals(0) = autodiff::dual(sqrt_w_fx) * (predicted_fx.fl - blended_des_f_x.fl); - residuals(1) = autodiff::dual(sqrt_w_fx) * (predicted_fx.fr - blended_des_f_x.fr); - residuals(2) = autodiff::dual(sqrt_w_fx) * (predicted_fx.rl - blended_des_f_x.rl); - residuals(3) = autodiff::dual(sqrt_w_fx) * (predicted_fx.rr - blended_des_f_x.rr); - residuals(4) = autodiff::dual(sqrt_w_mz) * (predicted_mz - blended_des_m_z); + // residuals(0) = autodiff::dual(sqrt_w_fx) * (predicted_fx.fl - blended_des_f_x.fl); + // residuals(1) = autodiff::dual(sqrt_w_fx) * (predicted_fx.fr - blended_des_f_x.fr); + // residuals(2) = autodiff::dual(sqrt_w_fx) * (predicted_fx.rl - blended_des_f_x.rl); + // residuals(3) = autodiff::dual(sqrt_w_fx) * (predicted_fx.rr - blended_des_f_x.rr); + // residuals(4) = autodiff::dual(sqrt_w_mz) * (predicted_mz - blended_des_m_z); return residuals; }; float previous_cost = std::numeric_limits::infinity(); - for (int iter = 0; iter < MAX_ITER; ++iter) + for (uint32_t iter = 0; iter < MAX_ITER; ++iter) { DualVec4 kappa_dual; for (int i = 0; i < 4; ++i) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp index fb2fe85652..736a3ee146 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp @@ -2,7 +2,7 @@ #include "torque_vectoring/controllers/controllers_config.hpp" #include "torque_vectoring/shared_datatypes/constants.hpp" -using namespace app::tv::datatypes::vd_constants; +using namespace app::tv::shared_datatypes::vd_constants; namespace app::tv::controllers::dyrc { diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index f4d84bd4a8..97b7637bad 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -1,28 +1,20 @@ #include "tire_model.hpp" + #include "torque_vectoring/shared_datatypes/constants.hpp" +#include "dual.hpp" #include using namespace app::tv::shared_datatypes::vd_constants; namespace app::tv::estimation { +// static namespace :) namespace { - [[nodiscard]] float safeMagnitude(const float value) - { - return std::fmax(std::fabs(value), SMALL_EPSILON); - } - - // [[nodiscard]] float primalValue(const float value) - // { - // return value; - // } - // - // [[nodiscard]] float primalValue(const autodiff::dual &value) + // [[nodiscard]] float safeMagnitude(const float value) // { - // return static_cast(autodiff::val(value)); + // return std::fmax(std::fabs(value), SMALL_EPSILON); // } - [[nodiscard]] float safeTemplateDenominator(const float value) { if (std::fabs(value) >= SMALL_EPSILON) @@ -34,29 +26,41 @@ namespace } [[nodiscard]] autodiff::dual safeTemplateDenominator(const autodiff::dual &value) { - if (std::fabs(static_cast(autodiff::val(value))) >= SMALL_EPSILON) - { + const double primal = autodiff::val(value); + if (std::fabs(static_cast(primal)) >= SMALL_EPSILON) return value; - } - if (static_cast(autodiff::val(value)) < 0.0f) - { - return -SMALL_EPSILON; - } - return SMALL_EPSILON; + return primal < 0.0 ? -SMALL_EPSILON : SMALL_EPSILON; + } + constexpr float safeSignedDenominator(const float value) + { + if (std::fabs(value) >= SMALL_EPSILON) + return value; + return value < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; + } + constexpr float sign(const float value) + { + if (value > 0.0f) + return 1.0f; + if (value < 0.0f) + return -1.0f; + return 0.0f; + } + constexpr autodiff::dual sign(const autodiff::dual &value) + { + if (value > 0.0f) + return 1.0f; + if (value < 0.0f) + return -1.0f; + return 0.0f; } } // namespace template -[[nodiscard]] T TireModel::computeCombinedFx_N( - const float normal_load_N, - const float slip_angle_rad, - // float low_speed_blend, - const T &slip_ratio) const +[[nodiscard]] T + TireModel::computeCombinedFx_N(const float normal_load_N, const float slip_angle_rad, const T &slip_ratio) const { const T pure_fx_0 = computePureFx_N(normal_load_N, slip_ratio); const auto coefficients = combinedFxMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); - const T force_blend = T(1); // T(low_speed_blend); - // Low-speed safeguard: // Below a small vehicle-speed threshold the tire model can predict unrealistically large // forces because the slip calculation becomes ill-conditioned while the fitted Pacejka @@ -64,7 +68,7 @@ template // fades out smoothly instead of producing unstable low-speed force demands. // // Pacejka Page 181 (4.E50): F_x = G_xa * F_x0 - return force_blend * (coefficients.g_xa * pure_fx_0); + return coefficients.g_xa * pure_fx_0; } template float TireModel::computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const float &slip_ratio) const; template autodiff::dual TireModel::computeCombinedFx_N( @@ -73,21 +77,17 @@ template autodiff::dual TireModel::computeCombinedFx_N( const autodiff::dual &slip_ratio) const; template -[[nodiscard]] T TireModel::computeCombinedFy_N( - const float normal_load_N, - const float slip_angle_rad, - // float low_speed_blend, - const T &slip_ratio) const +[[nodiscard]] T + TireModel::computeCombinedFy_N(const float normal_load_N, const float slip_angle_rad, const T &slip_ratio) const { const float pure_fy_0 = computePureFy_N(normal_load_N, slip_angle_rad); const auto coefficients = combinedFyMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); - const T force_blend = T(1); // T(low_speed_blend); // Apply the same low-speed force-availability blend used for Fx so the optimizer sees a // self-consistent pair of tire forces as the vehicle approaches a stop. // // Pacejka Pages 181-182 (4.E58): F_y = G_yk * F_y0 + S_vyk - return force_blend * ((coefficients.g_yk * T(pure_fy_0)) + coefficients.s_vyk); + return coefficients.g_yk * T(pure_fy_0) + coefficients.s_vyk; } template float TireModel::computeCombinedFy_N(float normal_load_N, float slip_angle_rad, const float &slip_ratio) const; @@ -111,39 +111,6 @@ template autodiff::dual TireModel::computeCombinedFy_N( // wheelVelocities(vehicle_state.v_x_mps, vehicle_state.v_y_mps, vehicle_state.yaw_rate_radps); // return slipRatioToWheelAngularVelocity(slip_ratio, x_mps); // } -// [[nodiscard]] float TireModel::wheelLongOffset_m() const -// { -// return wheel_axle_ == WheelAxle::Front ? DIST_FRONT_AXLE_CG_m : -DIST_REAR_AXLE_CG_m; -// } -// -// [[nodiscard]] float TireModel::wheelLatOffset_m() const -// { -// return wheel_side_ == WheelSide::Left ? TRACK_WIDTH_m * 0.5f : -TRACK_WIDTH_m * 0.5f; -// } -// [[nodiscard]] float TireModel::wheelSteeringAngle_rad(const float steering_angle_rad) const -// { -// return wheel_axle_ == WheelAxle::Front ? steering_angle_rad : 0.0f; -// } -// [[nodiscard]] TireModel::WheelVelocities TireModel::wheelVelocities( -// const float vehicle_velocity_x_mps, -// const float vehicle_velocity_y_mps, -// const float yaw_rate_radps) const -// { -// // Rigid-body planar kinematics in the body frame: -// // v_wheel = v_cg + omega_z x r_wheel, where r_wheel = [x_offset, y_offset, 0]. -// return { -// .x_mps = vehicle_velocity_x_mps - (yaw_rate_radps * wheelLatOffset_m()), -// .y_mps = vehicle_velocity_y_mps + (yaw_rate_radps * wheelLongOffset_m()), -// }; -// } -// [[nodiscard]] float TireModel::estimateSlipAngle( -// const float wheel_vel_x_mps, -// const float wheel_vel_y_mps, -// const float steering_angle_rad) const -// { -// return std::atan2(wheel_vel_y_mps, wheel_vel_x_mps) - wheelSteeringAngle_rad(steering_angle_rad); -// } -// // [[nodiscard]] float TireModel::estimateSlipRatio( // const float wheel_vel_x_mps, // const float wheel_vel_y_mps, @@ -413,31 +380,6 @@ TireModel::CombinedFyMagicFormulaCoefficients TireModel::combinedFyMagicFormu return coefficients; } -constexpr float TireModel::safeSignedDenominator(const float value) -{ - if (std::fabs(value) >= SMALL_EPSILON) - { - return value; - } - - return value < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; -} - -constexpr float TireModel::sign(const float value) -{ - if (value > 0.0f) - { - return 1.0f; - } - - if (value < 0.0f) - { - return -1.0f; - } - - return 0.0f; -} - //-------------------------------------------------------------------- MJ Pure Coefficents 5.2 //----------------------------------------------------------------------// @@ -480,17 +422,15 @@ template T TireModel::pureFx_E(const float normalized_load_del { // Pacejka Page 179 (4.E14): E_x const float normalized_load_delta_squared = normalized_load_delta * normalized_load_delta; - const float base_e_x = fit_pure_fx_.ex_1 + (fit_pure_fx_.ex_2 * normalized_load_delta) + - (fit_pure_fx_.ex_3 * normalized_load_delta_squared); - const float kappa_x_value = primalValue(kappa_x); - - return T(base_e_x * (1.0f - (fit_pure_fx_.ex_4 * sign(kappa_x_value)))); + const float base_e_x = fit_pure_fx_.ex_1 + fit_pure_fx_.ex_2 * normalized_load_delta + + fit_pure_fx_.ex_3 * normalized_load_delta_squared; + return base_e_x * (1.0f - fit_pure_fx_.ex_4 * sign(kappa_x)); } constexpr float TireModel::pureFx_K(const float normal_load_N, const float normalized_load_delta) const { // Pacejka Page 179 (4.E15): K_xk - return normal_load_N * (fit_pure_fx_.kx_1 + (fit_pure_fx_.kx_2 * normalized_load_delta)) * + return normal_load_N * (fit_pure_fx_.kx_1 + fit_pure_fx_.kx_2 * normalized_load_delta) * std::exp(fit_pure_fx_.kx_3 * normalized_load_delta); } diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index c9614e33d4..9d20ffe7e2 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -1,12 +1,8 @@ #pragma once - -#include +#include "torque_vectoring/shared_datatypes/datatypes.hpp" namespace app::tv::estimation { -template -concept DecimalOrDual = std::same_as || std::same_as || std::same_as; - class TireModel { public: @@ -36,9 +32,11 @@ class TireModel }; } - // [[nodiscard]] static float slipRatioToWheelAngularVelocity(float slip_ratio, float wheel_vel_x_mps); - // [[nodiscard]] float - // slipRatioToWheelAngularVelocity(float slip_ratio, const shared_datatypes::VehicleState &vehicle_state) const; + // note that these only exist for float, dual + template + [[nodiscard]] T computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; + template + [[nodiscard]] T computeCombinedFy_N(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; struct TireFitPureParamFy { @@ -124,12 +122,6 @@ class TireModel } private: - // note that these only exist for float, dual - template - [[nodiscard]] T computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; - template - [[nodiscard]] T computeCombinedFy_N(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; - template struct PureFxMagicFormulaCoefficients { T s_hx = T(0.0f); @@ -179,9 +171,6 @@ class TireModel static constexpr float NOMINAL_FZ_N = 750.0f; //-------------------------------------------------------------------- Class Helpers //----------------------------------------------------------------------// - - [[nodiscard]] static constexpr float safeSignedDenominator(float value); - [[nodiscard]] static constexpr float sign(float value); [[nodiscard]] static constexpr float normalizedLoadDelta(float normal_load_N); // Reduced-model assumptions for combined slip in this pass: // gamma* = 0, lambda_xa = 1, lambda_yk = 1, lambda_vyk = 1, zeta_2 = 1. diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp index 8c699a5354..191bcd5fe1 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp @@ -138,10 +138,10 @@ namespace } [[nodiscard]] Measurement pseudoMeasurementFromWheelSpeeds( - const datatypes::datatypes::wheel_set &wheel_angular_velocities_radps, - const float yaw_rate_radps, - const float steering_angle_rad, - const StateVector &previous_state) + const shared_datatypes::wheel_set &wheel_angular_velocities_radps, + const float yaw_rate_radps, + const float steering_angle_rad, + const StateVector &previous_state) { const float front_cos = std::cos(steering_angle_rad); const float front_sin = std::sin(steering_angle_rad); @@ -199,16 +199,16 @@ namespace VehicleStateEstimator filter_ = createFilter(); } - [[nodiscard]] shared_datatypes::datatypes::VehicleState estimate(const Measurements &state) + [[nodiscard]] shared_datatypes::VehicleState estimate(const Measurements &state) { InputVector u = InputVector::Zero(); u(static_cast(AX)) = state.ax; u(static_cast(AY)) = state.ay; - const float measured_yaw_rate_radps = state.yaw_rate; - const float measured_steering_angle = state.delta; - const datatypes::datatypes::wheel_set wheel_angular_velocities = state.omegas; - const auto &previous_state = filter_.state(); + const float measured_yaw_rate_radps = state.yaw_rate; + const float measured_steering_angle = state.delta; + const shared_datatypes::wheel_set wheel_angular_velocities = state.omegas; + const auto &previous_state = filter_.state(); Measurement z = pseudoMeasurementFromWheelSpeeds( wheel_angular_velocities, measured_yaw_rate_radps, measured_steering_angle, previous_state); diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp index b8495d9e09..f7fc9a0227 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp @@ -1,7 +1,6 @@ #pragma once #include "state_estimation/app_kalman_filter.hpp" -#include "torque_vectoring/estimation/dynamics_estimation.hpp" #include "torque_vectoring/shared_datatypes/datatypes.hpp" namespace app::tv::estimation @@ -17,16 +16,16 @@ struct Measurements const float delta; const float apps; - const datatypes::datatypes::wheel_set omegas; + const shared_datatypes::wheel_set omegas; }; namespace VehicleStateEstimator { using Filter = app::state_estimation::ekf; using Covariance = Filter::N_N; - [[nodiscard]] shared_datatypes::datatypes::VehicleState estimate(const Measurements &state); - [[nodiscard]] const Covariance &covariance(); - void reset_filter(); + [[nodiscard]] shared_datatypes::VehicleState estimate(const Measurements &state); + [[nodiscard]] const Covariance &covariance(); + void reset_filter(); // if you want to reset, just reconstruct the object }; // namespace VehicleStateEstimator } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index f11ce9e417..cf45de0156 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -26,6 +26,7 @@ inline constexpr float WHEELBASE_m = WHEELBASE_mm * MM_TO_M; inline constexpr float TRACK_WIDTH_mm = 1100.0f; inline constexpr float TRACK_WIDTH_m = TRACK_WIDTH_mm * MM_TO_M; +inline constexpr float HALF_TRACK_M = TRACK_WIDTH_m * 0.5f; inline constexpr float WHEEL_RADIUS_M = WHEEL_DIAMETER_IN * IN_TO_M / 2.0f; // ============================================================================= diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 3be76136a8..0d304341ae 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -1,9 +1,12 @@ #pragma once #include "constants.hpp" - +#include "dual.hpp" #include +template +concept DecimalOrDual = std::same_as || std::same_as || std::same_as; + namespace app::tv::shared_datatypes { template struct wheel_set @@ -12,6 +15,30 @@ template struct wheel_set T fr; T rl; T rr; + + // void rotate(const wheel_set tire_angles) + // { + // const float cos_delta_fl = std::cos(tire_angles.fl); + // const float sin_delta_fl = std::sin(tire_angles.fl); + // fl = (cos_delta_fl * fl) - (sin_delta_fl * fl); + // + // const float cos_delta_fr = std::cos(tire_angles.fr); + // const float sin_delta_fr = std::sin(tire_angles.fr); + // fr = (cos_delta_fr * fr) - (sin_delta_fr * fr); + // + // if (tire_angles.rl != 0.0f) + // { + // const float cos_delta_rl = std::cos(tire_angles.rl); + // const float sin_delta_rl = std::sin(tire_angles.rl); + // rl = (cos_delta_rl * rl) - (sin_delta_rl * rl); + // } + // if (tire_angles.rr != 0.0f) + // { + // const float cos_delta_rr = std::cos(tire_angles.rr); + // const float sin_delta_rr = std::sin(tire_angles.rr); + // rr = (cos_delta_rr * rr) - (sin_delta_rr * rr); + // } + // } }; inline float slipRatioToWheelAngularVelocity(const float slip_ratio, const float v_x_mps) @@ -50,16 +77,59 @@ struct VehicleState return { steer_ang_rad, steer_ang_rad, 0, 0 }; } - wheel_set alphas() const {} + struct Pair + { + float x; + float y; + }; + + /** + * @return vector of vy each in the frame of the respective tire + */ + wheel_set v_in_tire_frame() const + { + const wheel_set vx_s = { + v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M, + v_x_mps - yaw_rate_radps * -vd_constants::HALF_TRACK_M, + v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M, + v_x_mps - yaw_rate_radps * -vd_constants::HALF_TRACK_M, + }; + const wheel_set v_ys = { + v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m, + v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m, + v_y_mps + yaw_rate_radps * -vd_constants::DIST_FRONT_AXLE_CG_m, + v_y_mps + yaw_rate_radps * -vd_constants::DIST_FRONT_AXLE_CG_m, + }; + return { + { vx_s.fl * std::cos(steer_ang_rad) + v_ys.fl * std::sin(steer_ang_rad), + v_ys.fl * std::cos(steer_ang_rad) - vx_s.fl * std::sin(steer_ang_rad) }, + { vx_s.fr * std::cos(steer_ang_rad) + v_ys.fr * std::sin(steer_ang_rad), + v_ys.fr * std::cos(steer_ang_rad) - vx_s.fr * std::sin(steer_ang_rad) }, + { vx_s.rl, v_ys.rl }, + { vx_s.rr, v_ys.rr }, + }; + } + + wheel_set alphas() const + { + const auto [fl_v, fr_v, rl_v, rr_v] = v_in_tire_frame(); + const wheel_set tire_angles = get_tire_angle(); + return { + std::atan2(fl_v.y, safe_vx(fl_v.x)) - tire_angles.fl, + std::atan2(fr_v.y, safe_vx(fr_v.x)) - tire_angles.fr, + std::atan2(rl_v.y, safe_vx(rl_v.x)), + std::atan2(rr_v.y, safe_vx(rr_v.x)), + }; + } [[nodiscard]] wheel_set kappas() const { - const auto tire_angles = get_tire_angle(); + const auto [fl, fr, rl, rr] = v_in_tire_frame(); return { - .fl = slipRatioToWheelAngularVelocity(omegas_radps.fl, std::cos(tire_angles.fl) * v_x_mps), - .fr = slipRatioToWheelAngularVelocity(omegas_radps.fr, std::cos(tire_angles.fr) * v_x_mps), - .rl = slipRatioToWheelAngularVelocity(omegas_radps.rl, v_x_mps), - .rr = slipRatioToWheelAngularVelocity(omegas_radps.rr, v_x_mps), + .fl = slipRatioToWheelAngularVelocity(omegas_radps.fl, fl.x), + .fr = slipRatioToWheelAngularVelocity(omegas_radps.fr, fr.x), + .rl = slipRatioToWheelAngularVelocity(omegas_radps.rl, rl.x), + .rr = slipRatioToWheelAngularVelocity(omegas_radps.rr, rr.x), }; } @@ -152,28 +222,20 @@ struct VehicleState * @param tires_Fy_N tire lateral forces in Newtons * @return Given certain tire forces, what would be the resulting yaw moment Mz about the CG? */ - [[nodiscard]] float est_Mz_N(const wheel_set &tires_Fx_N, const wheel_set &tires_Fy_N) const + template [[nodiscard]] T est_Mz_N(wheel_set tires_Fx_N, wheel_set tires_Fy_N) const { - constexpr float half_track_m = vd_constants::TRACK_WIDTH_m * 0.5f; - - // rotate front wheels into body frame - const wheel_set tire_angles = get_tire_angle(); - - const float cos_delta_fl = std::cos(tire_angles.fl); - const float sin_delta_fl = std::sin(tire_angles.fl); - const float fl_fx_body = (cos_delta_fl * tires_Fx_N.fl) - (sin_delta_fl * tires_Fy_N.fl); - const float fl_fy_body = (sin_delta_fl * tires_Fx_N.fl) + (cos_delta_fl * tires_Fy_N.fl); - - const float cos_delta_fr = std::cos(tire_angles.fr); - const float sin_delta_fr = std::sin(tire_angles.fr); - const float fr_fx_body = (cos_delta_fr * tires_Fx_N.fr) - (sin_delta_fr * tires_Fy_N.fr); - const float fr_fy_body = (sin_delta_fr * tires_Fx_N.fr) + (cos_delta_fr * tires_Fy_N.fr); + tires_Fx_N.rotate(get_tire_angle()); + tires_Fy_N.rotate(get_tire_angle()); // contributions to moment of each tire - const float fl_moment = (vd_constants::DIST_FRONT_AXLE_CG_m * fl_fy_body) - (half_track_m * fl_fx_body); - const float fr_moment = (vd_constants::DIST_FRONT_AXLE_CG_m * fr_fy_body) + (half_track_m * fr_fx_body); - const float rl_moment = (-vd_constants::DIST_REAR_AXLE_CG_m * tires_Fy_N.rl) - (half_track_m * tires_Fx_N.rl); - const float rr_moment = (-vd_constants::DIST_REAR_AXLE_CG_m * tires_Fy_N.rr) + (half_track_m * tires_Fx_N.rr); + const T fl_moment = + (vd_constants::DIST_FRONT_AXLE_CG_m * tires_Fy_N.fl) - (vd_constants::HALF_TRACK_M * tires_Fx_N.fl); + const T fr_moment = + (vd_constants::DIST_FRONT_AXLE_CG_m * tires_Fy_N.fr) + (vd_constants::HALF_TRACK_M * tires_Fx_N.fr); + const T rl_moment = + (-vd_constants::DIST_REAR_AXLE_CG_m * tires_Fy_N.rl) - (vd_constants::HALF_TRACK_M * tires_Fx_N.rl); + const T rr_moment = + (-vd_constants::DIST_REAR_AXLE_CG_m * tires_Fy_N.rr) + (vd_constants::HALF_TRACK_M * tires_Fx_N.rr); return fl_moment + fr_moment + rl_moment + rr_moment; } From 13ced941fe2003867c930e8a3536d84cd7934ab0 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 9 Apr 2026 20:33:02 -0700 Subject: [PATCH 10/35] edwin zheng forgot how to do rotation matrix --- .../shared_datatypes/datatypes.hpp | 112 +++++++++--------- 1 file changed, 56 insertions(+), 56 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 0d304341ae..d806543137 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -9,36 +9,42 @@ concept DecimalOrDual = std::same_as || std::same_as || std namespace app::tv::shared_datatypes { +struct Pair +{ + float x; + float y; +}; + template struct wheel_set { T fl; T fr; T rl; T rr; +}; - // void rotate(const wheel_set tire_angles) - // { - // const float cos_delta_fl = std::cos(tire_angles.fl); - // const float sin_delta_fl = std::sin(tire_angles.fl); - // fl = (cos_delta_fl * fl) - (sin_delta_fl * fl); - // - // const float cos_delta_fr = std::cos(tire_angles.fr); - // const float sin_delta_fr = std::sin(tire_angles.fr); - // fr = (cos_delta_fr * fr) - (sin_delta_fr * fr); - // - // if (tire_angles.rl != 0.0f) - // { - // const float cos_delta_rl = std::cos(tire_angles.rl); - // const float sin_delta_rl = std::sin(tire_angles.rl); - // rl = (cos_delta_rl * rl) - (sin_delta_rl * rl); - // } - // if (tire_angles.rr != 0.0f) - // { - // const float cos_delta_rr = std::cos(tire_angles.rr); - // const float sin_delta_rr = std::sin(tire_angles.rr); - // rr = (cos_delta_rr * rr) - (sin_delta_rr * rr); - // } - // } +template <> struct wheel_set +{ + Pair fl; + Pair fr; + Pair rl; + Pair rr; + + /** + * rotates each pair in the z-axis by the respective angle in z_rot + * @param z_rot_rad rotation angles for each wheel in radians + */ + void rotate(const wheel_set &z_rot_rad) + { + fl = { fl.x * std::cos(z_rot_rad.fl) + fl.y * std::sin(z_rot_rad.fl), + fl.y * std::cos(z_rot_rad.fl) - fl.x * std::sin(z_rot_rad.fl) }; + fr = { fr.x * std::cos(z_rot_rad.fr) + fr.y * std::sin(z_rot_rad.fr), + fr.y * std::cos(z_rot_rad.fr) - fr.x * std::sin(z_rot_rad.fr) }; + rl = { rl.x * std::cos(z_rot_rad.rl) + rl.y * std::sin(z_rot_rad.rl), + rl.y * std::cos(z_rot_rad.rl) - rl.x * std::sin(z_rot_rad.rl) }; + rr = { rr.x * std::cos(z_rot_rad.rr) + rr.y * std::sin(z_rot_rad.rr), + rr.y * std::cos(z_rot_rad.rr) - rr.x * std::sin(z_rot_rad.rr) }; + } }; inline float slipRatioToWheelAngularVelocity(const float slip_ratio, const float v_x_mps) @@ -77,48 +83,42 @@ struct VehicleState return { steer_ang_rad, steer_ang_rad, 0, 0 }; } - struct Pair - { - float x; - float y; - }; - /** * @return vector of vy each in the frame of the respective tire */ wheel_set v_in_tire_frame() const { - const wheel_set vx_s = { - v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M, - v_x_mps - yaw_rate_radps * -vd_constants::HALF_TRACK_M, - v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M, - v_x_mps - yaw_rate_radps * -vd_constants::HALF_TRACK_M, - }; - const wheel_set v_ys = { - v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m, - v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m, - v_y_mps + yaw_rate_radps * -vd_constants::DIST_FRONT_AXLE_CG_m, - v_y_mps + yaw_rate_radps * -vd_constants::DIST_FRONT_AXLE_CG_m, - }; - return { - { vx_s.fl * std::cos(steer_ang_rad) + v_ys.fl * std::sin(steer_ang_rad), - v_ys.fl * std::cos(steer_ang_rad) - vx_s.fl * std::sin(steer_ang_rad) }, - { vx_s.fr * std::cos(steer_ang_rad) + v_ys.fr * std::sin(steer_ang_rad), - v_ys.fr * std::cos(steer_ang_rad) - vx_s.fr * std::sin(steer_ang_rad) }, - { vx_s.rl, v_ys.rl }, - { vx_s.rr, v_ys.rr }, + wheel_set v = { + { + v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M, + v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m, + }, + { + v_x_mps - yaw_rate_radps * -vd_constants::HALF_TRACK_M, + v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m, + }, + { + v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M, + v_y_mps + yaw_rate_radps * -vd_constants::DIST_FRONT_AXLE_CG_m, + }, + { + v_x_mps - yaw_rate_radps * -vd_constants::HALF_TRACK_M, + v_y_mps + yaw_rate_radps * -vd_constants::DIST_FRONT_AXLE_CG_m, + }, }; + v.rotate(get_tire_angle()); + return v; } wheel_set alphas() const { - const auto [fl_v, fr_v, rl_v, rr_v] = v_in_tire_frame(); - const wheel_set tire_angles = get_tire_angle(); + const auto [fl_v, fr_v, rl_v, rr_v] = v_in_tire_frame(); + const auto [fl_rot, fr_rot, rl_rot, rr_rot] = get_tire_angle(); return { - std::atan2(fl_v.y, safe_vx(fl_v.x)) - tire_angles.fl, - std::atan2(fr_v.y, safe_vx(fr_v.x)) - tire_angles.fr, - std::atan2(rl_v.y, safe_vx(rl_v.x)), - std::atan2(rr_v.y, safe_vx(rr_v.x)), + std::atan2(fl_v.y, safe_vx(fl_v.x)) - fl_rot, + std::atan2(fr_v.y, safe_vx(fr_v.x)) - fr_rot, + std::atan2(rl_v.y, safe_vx(rl_v.x)) - rl_rot, + std::atan2(rr_v.y, safe_vx(rr_v.x)) - rr_rot, }; } @@ -224,8 +224,8 @@ struct VehicleState */ template [[nodiscard]] T est_Mz_N(wheel_set tires_Fx_N, wheel_set tires_Fy_N) const { - tires_Fx_N.rotate(get_tire_angle()); - tires_Fy_N.rotate(get_tire_angle()); + // tires_Fx_N.rotate(get_tire_angle()); + // tires_Fy_N.rotate(get_tire_angle()); // contributions to moment of each tire const T fl_moment = From c8a80202ce6baca8bb2d3e17ac270247058555a4 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 9 Apr 2026 20:40:31 -0700 Subject: [PATCH 11/35] template metaprogramming masturbation --- .../torque_allocator/torque_allocator.cpp | 30 ++++++++------ .../shared_datatypes/datatypes.hpp | 41 ++++++++----------- 2 files changed, 36 insertions(+), 35 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp index d5c7ba8975..5ec31800e4 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp @@ -89,19 +89,25 @@ namespace const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); const auto [alpha_fl, alpha_fr, alpha_rl, alpha_rr] = state.alphas(); - const wheel_set predicted_fx{ - .fl = estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)), - .fr = estimation::tire_model.computeCombinedFx_N(fz_fr, alpha_fr, kappa(1)), - .rl = estimation::tire_model.computeCombinedFx_N(fz_rl, alpha_rl, kappa(2)), - .rr = estimation::tire_model.computeCombinedFx_N(fz_rr, alpha_rr, kappa(3)), + const wheel_set> predicted_f{ + { + estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)), + estimation::tire_model.computeCombinedFy_N(fz_fl, alpha_fl, kappa(0)), + }, + { + estimation::tire_model.computeCombinedFx_N(fz_fr, alpha_fr, kappa(1)), + estimation::tire_model.computeCombinedFy_N(fz_fr, alpha_fr, kappa(1)), + }, + { + estimation::tire_model.computeCombinedFx_N(fz_rl, alpha_rl, kappa(2)), + estimation::tire_model.computeCombinedFy_N(fz_rl, alpha_rl, kappa(2)), + }, + { + estimation::tire_model.computeCombinedFx_N(fz_rr, alpha_rr, kappa(3)), + estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)), + }, }; - const wheel_set predicted_fy{ - .fl = estimation::tire_model.computeCombinedFy_N(fz_fl, alpha_fl, kappa(0)), - .fr = estimation::tire_model.computeCombinedFy_N(fz_fr, alpha_fr, kappa(1)), - .rl = estimation::tire_model.computeCombinedFy_N(fz_rl, alpha_rl, kappa(2)), - .rr = estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)), - }; - [[maybe_unused]] const autodiff::dual predicted_mz = state.est_Mz_N(predicted_fx, predicted_fy); + [[maybe_unused]] const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); DualVec5 residuals; // residuals(0) = autodiff::dual(sqrt_w_fx) * (predicted_fx.fl - blended_des_f_x.fl); diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index d806543137..397f420f6a 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -9,10 +9,10 @@ concept DecimalOrDual = std::same_as || std::same_as || std namespace app::tv::shared_datatypes { -struct Pair +template struct Pair { - float x; - float y; + T x; + T y; }; template struct wheel_set @@ -22,13 +22,12 @@ template struct wheel_set T rl; T rr; }; - -template <> struct wheel_set +template struct wheel_set> { - Pair fl; - Pair fr; - Pair rl; - Pair rr; + Pair fl; + Pair fr; + Pair rl; + Pair rr; /** * rotates each pair in the z-axis by the respective angle in z_rot @@ -86,9 +85,9 @@ struct VehicleState /** * @return vector of vy each in the frame of the respective tire */ - wheel_set v_in_tire_frame() const + wheel_set> v_in_tire_frame() const { - wheel_set v = { + wheel_set> v = { { v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M, v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m, @@ -218,27 +217,23 @@ struct VehicleState [[nodiscard]] float est_beta_rad() const { return std::atan2(v_y_mps, safe_vx(v_x_mps)); } /** - * @param tires_Fx_N tire longitudinal forces in Newtons - * @param tires_Fy_N tire lateral forces in Newtons + * @param tires_F_N tire forces * @return Given certain tire forces, what would be the resulting yaw moment Mz about the CG? */ - template [[nodiscard]] T est_Mz_N(wheel_set tires_Fx_N, wheel_set tires_Fy_N) const + template [[nodiscard]] T est_Mz_N(wheel_set> tires_F_N) const { - // tires_Fx_N.rotate(get_tire_angle()); - // tires_Fy_N.rotate(get_tire_angle()); - - // contributions to moment of each tire + tires_F_N.rotate(get_tire_angle()); const T fl_moment = - (vd_constants::DIST_FRONT_AXLE_CG_m * tires_Fy_N.fl) - (vd_constants::HALF_TRACK_M * tires_Fx_N.fl); + (vd_constants::DIST_FRONT_AXLE_CG_m * tires_F_N.fl.y) - (vd_constants::HALF_TRACK_M * tires_F_N.fl.x); const T fr_moment = - (vd_constants::DIST_FRONT_AXLE_CG_m * tires_Fy_N.fr) + (vd_constants::HALF_TRACK_M * tires_Fx_N.fr); + (vd_constants::DIST_FRONT_AXLE_CG_m * tires_F_N.fr.y) + (vd_constants::HALF_TRACK_M * tires_F_N.fr.x); const T rl_moment = - (-vd_constants::DIST_REAR_AXLE_CG_m * tires_Fy_N.rl) - (vd_constants::HALF_TRACK_M * tires_Fx_N.rl); + (-vd_constants::DIST_REAR_AXLE_CG_m * tires_F_N.rl.y) - (vd_constants::HALF_TRACK_M * tires_F_N.rl.x); const T rr_moment = - (-vd_constants::DIST_REAR_AXLE_CG_m * tires_Fy_N.rr) + (vd_constants::HALF_TRACK_M * tires_Fx_N.rr); - + (-vd_constants::DIST_REAR_AXLE_CG_m * tires_F_N.rr.y) + (vd_constants::HALF_TRACK_M * tires_F_N.rr.x); return fl_moment + fr_moment + rl_moment + rr_moment; } + /** * Yaw moment distribution factor Kmz (page 57) * Accounts for load transfer effect on yaw moment generation capacity From ffb80cd2b675543b60fbc2f2a14ca6ca6485eeb3 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 9 Apr 2026 22:05:27 -0700 Subject: [PATCH 12/35] W optimizer? --- .../torque_allocator/torque_allocator.cpp | 96 ++++++++++--------- .../estimation/tire_model.cpp | 30 +++--- .../shared_datatypes/constants.hpp | 2 + .../shared_datatypes/datatypes.hpp | 1 + 4 files changed, 69 insertions(+), 60 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp index 5ec31800e4..bbcff28db2 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp @@ -18,23 +18,25 @@ namespace app::tv::controllers::allocator namespace { // ---- Optimizer tuning ---- - constexpr float W_FX = 2.0f / 3.0f; - constexpr float W_MZ = 1.0f / 3.0f; - constexpr int MAX_ITER = 8; - constexpr float SLIP_CLAMP = 0.3f; - constexpr float NORMAL_MATRIX_EPS = 1e-6f; - constexpr float STEP_TOLERANCE = 1e-5f; - constexpr float COST_TOLERANCE = 1e-6f; + constexpr float W_FX = 2.0f / 3.0f; + constexpr float W_MZ = 1.0f / 3.0f; + constexpr int MAX_ITER = 8; + [[maybe_unused]] constexpr float SLIP_CLAMP = 0.3f; + constexpr float NORMAL_MATRIX_EPS = 1e-6f; + constexpr float STEP_TOLERANCE = 1e-5f; + constexpr float COST_TOLERANCE = 1e-6f; + + using Vec2f = Eigen::Matrix; using Vec4f = Eigen::Matrix; - using Vec5f = Eigen::Matrix; - using Mat54f = Eigen::Matrix; + using Mat24f = Eigen::Matrix; using Mat44f = Eigen::Matrix; + using DualVec2 = Eigen::Matrix; using DualVec4 = Eigen::Matrix; - using DualVec5 = Eigen::Matrix; } // namespace -[[nodiscard]] wheel_set optimize(const VehicleState &state, float ax_setpoint, float omegadot_setpoint) +[[nodiscard]] wheel_set + optimize(const VehicleState &state, const float ax_setpoint, const float omegadot_setpoint) { // Low-speed safeguard: // torque_vectoring.cpp computes a single force-availability blend from vehicle speed and passes it @@ -48,6 +50,9 @@ namespace // return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; // } + static constexpr float SQRT_W_MX = std::sqrt(W_FX); + static constexpr float SQRT_W_MZ = std::sqrt(W_MZ); + // const wheel_set blended_des_f_x{ // .fl = low_speed_blend * des_f_x.fl, // .fr = low_speed_blend * des_f_x.fr, @@ -56,13 +61,6 @@ namespace // }; // const float blended_des_m_z = low_speed_blend * des_M_z; - Vec4f opt_slip; // output variable - const auto [kappa_fl, kappa_fr, kappa_rl, kappa_rr] = state.kappas(); - opt_slip << kappa_fl, kappa_fr, kappa_rl, kappa_rr; - - // const float sqrt_w_fx = std::sqrt(W_FX); - // const float sqrt_w_mz = std::sqrt(W_MZ); - // Reference material used to shape this implementation: // - Video walkthrough: https://www.youtube.com/watch?v=C6DCtQjKkdY // - Wikipedia summary: https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm @@ -82,9 +80,9 @@ namespace // r = residual vector evaluated at the current trial slip // J = dr/dkappa evaluated at the current trial slip // - // We keep all vectors/matrices fixed-size (4 decision variables, 5 residuals) so the optimizer + // We keep all vectors/matrices fixed-size (4 decision variables, 2 residuals) so the optimizer // stays allocation-free and predictable on embedded targets. - const auto residualVector = [&](const DualVec4 &kappa) -> DualVec5 + const auto residualVector = [&](const DualVec4 &kappa) -> DualVec2 { const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); const auto [alpha_fl, alpha_fr, alpha_rl, alpha_rr] = state.alphas(); @@ -109,31 +107,33 @@ namespace }; [[maybe_unused]] const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); - DualVec5 residuals; - // residuals(0) = autodiff::dual(sqrt_w_fx) * (predicted_fx.fl - blended_des_f_x.fl); - // residuals(1) = autodiff::dual(sqrt_w_fx) * (predicted_fx.fr - blended_des_f_x.fr); - // residuals(2) = autodiff::dual(sqrt_w_fx) * (predicted_fx.rl - blended_des_f_x.rl); - // residuals(3) = autodiff::dual(sqrt_w_fx) * (predicted_fx.rr - blended_des_f_x.rr); - // residuals(4) = autodiff::dual(sqrt_w_mz) * (predicted_mz - blended_des_m_z); - return residuals; + const autodiff::dual sum_fx = predicted_f.fl.x + predicted_f.fr.x + predicted_f.rl.x + predicted_f.rr.x; + return DualVec2{ + SQRT_W_MX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint), + SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), + }; }; + Vec4f opt_slip; // output variable + // seed opt_slip + const auto [kappa_fl, kappa_fr, kappa_rl, kappa_rr] = state.kappas(); + opt_slip << kappa_fl, kappa_fr, kappa_rl, kappa_rr; + float previous_cost = std::numeric_limits::infinity(); for (uint32_t iter = 0; iter < MAX_ITER; ++iter) { - DualVec4 kappa_dual; - for (int i = 0; i < 4; ++i) - kappa_dual(i) = autodiff::dual(opt_slip(i)); - - DualVec5 residual_eval_dual; - Mat54f jacobian; + DualVec4 kappa{ + autodiff::dual(opt_slip(0)), + autodiff::dual(opt_slip(1)), + autodiff::dual(opt_slip(2)), + autodiff::dual(opt_slip(3)), + }; + // evaluate and calculate jacobian at kappa + DualVec2 residual_at_kappa; + Mat24f jacobian_residual_at_kappa; autodiff::jacobian( - residualVector, autodiff::wrt(kappa_dual), autodiff::at(kappa_dual), residual_eval_dual, jacobian); - - Vec5f residuals; - for (int i = 0; i < 5; ++i) - residuals(i) = static_cast(autodiff::val(residual_eval_dual(i))); + residualVector, autodiff::wrt(kappa), autodiff::at(kappa), residual_at_kappa, jacobian_residual_at_kappa); // Gauss-Newton solves: // (J^T J) * delta = -J^T r @@ -142,11 +142,15 @@ namespace // J comes directly from autodiff::jacobian(...), so we do not hand-derive per-wheel slopes // or yaw-moment sensitivities. The optimizer stays readable: define residuals first, then // let autodiff provide the linearization used by Gauss-Newton. - Mat44f normal_matrix = jacobian.transpose() * jacobian; + Mat44f normal_matrix = jacobian_residual_at_kappa.transpose() * jacobian_residual_at_kappa; normal_matrix.diagonal().array() += NORMAL_MATRIX_EPS; - const Vec4f rhs = -jacobian.transpose() * residuals; - Eigen::LDLT ldlt(normal_matrix); + const Vec2f residuals_at_kappa_primal{ + static_cast(autodiff::val(residual_at_kappa(0))), + static_cast(autodiff::val(residual_at_kappa(1))), + }; + const Vec4f rhs = -jacobian_residual_at_kappa.transpose() * residuals_at_kappa_primal; + const Eigen::LDLT ldlt(normal_matrix); if (ldlt.info() != Eigen::Success) break; @@ -156,13 +160,17 @@ namespace break; opt_slip += delta; - for (int i = 0; i < 4; ++i) - opt_slip(i) = std::clamp(opt_slip(i), -SLIP_CLAMP, SLIP_CLAMP); + // I would recommend not doing this + // consider the following: a large step is required which leaves the clamp space + // this would cause the loop to never converge as the step size at each iteration would be sufficiently large to + // continue + // for (int i = 0; i < 4; ++i) + // opt_slip(i) = std::clamp(opt_slip(i), -SLIP_CLAMP, SLIP_CLAMP); // Least-squares cost: // J = 0.5 * r^T r // This is used only for convergence monitoring; the actual update is driven by J^T J and J^T r above. - const float cost = 0.5f * residuals.squaredNorm(); + const float cost = 0.5f * residuals_at_kappa_primal.squaredNorm(); if (delta.norm() < STEP_TOLERANCE || std::fabs(previous_cost - cost) < COST_TOLERANCE) break; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index 97b7637bad..760af183f6 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -196,10 +196,9 @@ float TireModel::computePureFy_N(const float normal_load_N, const float slip_ang // Pacejka Page 180 (4.E19): F_y0 const float b_y_alpha_y = coefficients.b_y * coefficients.alpha_y; - return coefficients.d_y * - std::sin( - coefficients.c_y * - std::atan(b_y_alpha_y - (coefficients.e_y * (b_y_alpha_y - std::atan(b_y_alpha_y))))) + + return coefficients.d_y * std::sin( + coefficients.c_y * + std::atan(b_y_alpha_y - coefficients.e_y * (b_y_alpha_y - std::atan(b_y_alpha_y)))) + coefficients.s_vy; } @@ -224,7 +223,7 @@ constexpr float TireModel::combinedFx_Cxa() const float TireModel::combinedFx_Exa(const float normalized_load_delta) const { // Pacejka Page 181 (4.E56): E_xa with reduced-model assumptions and E_xa <= 1. - return std::fmin(fit_comb_fx_.rEx1 + (fit_comb_fx_.rEx2 * normalized_load_delta), 1.0f); + return std::fmin(fit_comb_fx_.rEx1 + fit_comb_fx_.rEx2 * normalized_load_delta, 1.0f); } template T TireModel::combinedFx_Bxa(const T &slip_ratio) const @@ -287,7 +286,7 @@ TireModel::CombinedFxMagicFormulaCoefficients TireModel::combinedFxMagicFormu float TireModel::combinedFy_SHyk(const float normalized_load_delta) const { // Pacejka Page 182 (4.E65): S_Hyk - return fit_comb_fy_.rHy1 + (fit_comb_fy_.rHy2 * normalized_load_delta); + return fit_comb_fy_.rHy1 + fit_comb_fy_.rHy2 * normalized_load_delta; } template T TireModel::combinedFy_Kappa_s(const float normalized_load_delta, const T &slip_ratio) const @@ -305,7 +304,7 @@ constexpr float TireModel::combinedFy_Cyk() const float TireModel::combinedFy_Eyk(const float normalized_load_delta) const { // Pacejka Page 182 (4.E64): E_yk with reduced-model assumptions and E_yk <= 1. - return std::fmin(fit_comb_fy_.rEy1 + (fit_comb_fy_.rEy2 * normalized_load_delta), 1.0f); + return std::fmin(fit_comb_fy_.rEy1 + fit_comb_fy_.rEy2 * normalized_load_delta, 1.0f); } float TireModel::combinedFy_Byk(const float slip_angle_rad) const @@ -321,7 +320,7 @@ float TireModel::combinedFy_Dvyk( { // Pacejka Page 182 (4.E67): D_vyk with gamma* = 0 and zeta_2 = 1. return pureFy_mu(normalized_load_delta) * normal_load_N * - (fit_comb_fy_.rVy1 + (fit_comb_fy_.rVy2 * normalized_load_delta)) * + (fit_comb_fy_.rVy1 + fit_comb_fy_.rVy2 * normalized_load_delta) * std::cos(std::atan(fit_comb_fy_.rVy4 * slip_angle_rad)); } @@ -391,7 +390,7 @@ constexpr float TireModel::normalizedLoadDelta(const float normal_load_N) constexpr float TireModel::pureFx_Sh(const float normalized_load_delta) const { // Pacejka Page 179 (4.E17): S_Hx - return fit_pure_fx_.hx_1 + (fit_pure_fx_.hx_2 * normalized_load_delta); + return fit_pure_fx_.hx_1 + fit_pure_fx_.hx_2 * normalized_load_delta; } template T TireModel::pureFx_Kappa(const float normalized_load_delta, const T &slip_ratio) const @@ -409,7 +408,7 @@ constexpr float TireModel::pureFx_C() const constexpr float TireModel::pureFx_mu(const float normalized_load_delta) const { // Pacejka Page 179 (4.E13): mu_x with pressure/camber terms reduced to the current assumptions. - return fit_pure_fx_.dx_1 + (fit_pure_fx_.dx_2 * normalized_load_delta); + return fit_pure_fx_.dx_1 + fit_pure_fx_.dx_2 * normalized_load_delta; } constexpr float TireModel::pureFx_D(const float normal_load_N, const float normalized_load_delta) const @@ -443,13 +442,13 @@ constexpr float TireModel::pureFx_B(const float slip_stiffness, const float shap constexpr float TireModel::pureFx_Sv(const float normal_load_N, const float normalized_load_delta) const { // Pacejka Page 179 (4.E18): S_Vx - return normal_load_N * (fit_pure_fx_.vx_1 + (fit_pure_fx_.vx_2 * normalized_load_delta)); + return normal_load_N * (fit_pure_fx_.vx_1 + fit_pure_fx_.vx_2 * normalized_load_delta); } constexpr float TireModel::pureFy_Sh(const float normalized_load_delta) const { // Pacejka Page 180 (4.E27): S_Hy with gamma terms reduced to zero. - return fit_pure_fy_.hy_1 + (fit_pure_fy_.hy_2 * normalized_load_delta); + return fit_pure_fy_.hy_1 + fit_pure_fy_.hy_2 * normalized_load_delta; } constexpr float TireModel::pureFy_Alpha(const float normalized_load_delta, const float slip_angle_rad) const @@ -467,7 +466,7 @@ constexpr float TireModel::pureFy_C() const constexpr float TireModel::pureFy_mu(const float normalized_load_delta) const { // Pacejka Page 180 (4.E23): mu_y with pressure/camber terms reduced to the current assumptions. - return fit_pure_fy_.dy_1 + (fit_pure_fy_.dy_2 * normalized_load_delta); + return fit_pure_fy_.dy_1 + fit_pure_fy_.dy_2 * normalized_load_delta; } constexpr float TireModel::pureFy_D(const float normal_load_N, const float normalized_load_delta) const @@ -479,8 +478,7 @@ constexpr float TireModel::pureFy_D(const float normal_load_N, const float norma constexpr float TireModel::pureFy_E(const float normalized_load_delta, const float alpha_y) const { // Pacejka Page 180 (4.E24): E_y with gamma terms reduced to zero. - return (fit_pure_fy_.ey_1 + (fit_pure_fy_.ey_2 * normalized_load_delta)) * - (1.0f - (fit_pure_fy_.ey_3 * sign(alpha_y))); + return (fit_pure_fy_.ey_1 + fit_pure_fy_.ey_2 * normalized_load_delta) * (1.0f - fit_pure_fy_.ey_3 * sign(alpha_y)); } constexpr float TireModel::pureFy_K(const float normal_load_N) const @@ -499,6 +497,6 @@ constexpr float TireModel::pureFy_B(const float cornering_stiffness, const float constexpr float TireModel::pureFy_Sv(const float normal_load_N, const float normalized_load_delta) const { // Pacejka Page 180 (4.E29): S_Vy with gamma terms reduced to zero. - return normal_load_N * (fit_pure_fy_.vy_1 + (fit_pure_fy_.vy_2 * normalized_load_delta)); + return normal_load_N * (fit_pure_fy_.vy_1 + fit_pure_fy_.vy_2 * normalized_load_delta); } } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index cf45de0156..8dc49f8557 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -34,6 +34,8 @@ inline constexpr float WHEEL_RADIUS_M = WHEEL_DIAMETER_IN * IN_TO_M / 2.0f; // ============================================================================= inline constexpr float CAR_MASS_AT_CG_KG = 300.0f; // Mass with driver (verified with suspension team) +// Estimated yaw moment of inertia about CG (TODO: Update with suspension team) +inline constexpr float CAR_YAW_MOMENT_INERTIA_KGM2 = 150.0f; inline constexpr float DIST_FRONT_AXLE_CG_m = 0.837f; // Distance from front axle to CG (parameter 'a') inline constexpr float DIST_REAR_AXLE_CG_m = diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 397f420f6a..4ab79d79ab 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -222,6 +222,7 @@ struct VehicleState */ template [[nodiscard]] T est_Mz_N(wheel_set> tires_F_N) const { + // TODO aligning moment contributions to the yaw moment equation tires_F_N.rotate(get_tire_angle()); const T fl_moment = (vd_constants::DIST_FRONT_AXLE_CG_m * tires_F_N.fl.y) - (vd_constants::HALF_TRACK_M * tires_F_N.fl.x); From 4940e26cd41c50137e86cf1439b0645ec358bb27 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 9 Apr 2026 22:11:27 -0700 Subject: [PATCH 13/35] nuclearize --- .../torque_allocator/torque_allocator.cpp | 8 +-- .../app/torque_vectoring/torque_vectoring.cpp | 68 ++----------------- 2 files changed, 7 insertions(+), 69 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp index bbcff28db2..3826221e50 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp @@ -1,6 +1,5 @@ #include "torque_allocator.hpp" -#include #include #include @@ -105,10 +104,9 @@ namespace estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)), }, }; - [[maybe_unused]] const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); - - const autodiff::dual sum_fx = predicted_f.fl.x + predicted_f.fr.x + predicted_f.rl.x + predicted_f.rr.x; - return DualVec2{ + const autodiff::dual sum_fx = predicted_f.fl.x + predicted_f.fr.x + predicted_f.rl.x + predicted_f.rr.x; + const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); + return DualVec2{ SQRT_W_MX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint), SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), }; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index 01c0603ad3..62074bd50c 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -8,56 +8,8 @@ namespace app::tv { -namespace -{ - using namespace shared_datatypes; - using namespace shared_datatypes::vd_constants; - - //------------------------------------- STATE VARIABLES -------------------------------------// - - // Build TireModel::StateInputs from VehicleState + per-wheel data - // [[nodiscard]] estimation::TireModel::StateInputs - // buildTireInputs(const VehicleState &state, const float omega, const float fz) - // { - // return { - // .wheel_angular_velocity_radps = omega, - // .vehicle_velocity_x_mps = state.v_x_mps, - // .vehicle_velocity_y_mps = state.v_y_mps, - // .yaw_rate_radps = state.yaw_rate_radps, - // .steering_angle_rad = state.steer_ang_rad, - // .normal_load_N = fz, - // }; - // } - - // These wheel-set caches carry the previous update's estimated tire state/forces into the next cycle. - // The state estimator and optimizer both consume them as warm starts / feedback, so they remain module-local. - // wheel_set acc_f_x{}; - // wheel_set acc_f_y{}; - // wheel_set acc_f_z{}; - // wheel_set acc_slip_ratio{}; - // wheel_set acc_slip_angle{}; - // wheel_set slip_ratio_opt{}; - // Copy the current tire estimates into the module-local wheel-set caches so the rest of the - // pipeline can use a consistent per-wheel state snapshot for this update tick. - // void unpackTireOutputs( - // const estimation::TireModel::Outputs &fl, - // const estimation::TireModel::Outputs &fr, - // const estimation::TireModel::Outputs &rl, - // const estimation::TireModel::Outputs &rr) - // { - // acc_f_x = { .fl = fl.longitudinal_force_N, - // .fr = fr.longitudinal_force_N, - // .rl = rl.longitudinal_force_N, - // .rr = rr.longitudinal_force_N }; - // acc_f_y = { - // .fl = fl.lateral_force_N, .fr = fr.lateral_force_N, .rl = rl.lateral_force_N, .rr = rr.lateral_force_N - // }; - // acc_slip_ratio = { .fl = fl.slip_ratio, .fr = fr.slip_ratio, .rl = rl.slip_ratio, .rr = rr.slip_ratio }; - // acc_slip_angle = { - // .fl = fl.slip_angle_rad, .fr = fr.slip_angle_rad, .rl = rl.slip_angle_rad, .rr = rr.slip_angle_rad - // }; - // } -} // namespace +using namespace shared_datatypes; +using namespace vd_constants; //------------------------------------- TORQUE VECTORING UPDATE CYCLE -------------------------// ControlOutput update(const estimation::Measurements &measurement) @@ -67,21 +19,11 @@ ControlOutput update(const estimation::Measurements &measurement) // filtered vehicle state used consistently by all downstream control blocks this tick. const VehicleState state = estimation::VehicleStateEstimator::estimate(measurement); - // Normal forces from longitudinal/lateral load transfer + downforce - // acc_f_z = vehicle_dynamics_estimator.est_Fz_N( - // estimated_state.a_x_mps2, estimated_state.a_y_mps2, estimated_state.v_x_mps); - // Tire model estimation from current sensor data using the filtered vehicle state. - // unpackTireOutputs( - // tire_models.fl.estimate(buildTireInputs(estimated_state, fl_omega, acc_f_z.fl)), - // tire_models.fr.estimate(buildTireInputs(estimated_state, fr_omega, acc_f_z.fr)), - // tire_models.rl.estimate(buildTireInputs(estimated_state, rl_omega, acc_f_z.rl)), - // tire_models.rr.estimate(buildTireInputs(estimated_state, rr_omega, acc_f_z.rr))); - //------------------------------------- HIGH LEVEL CONTROLLER ----------------------------// const float ax_mps2_setpoint = MAX_AX * measurement.apps; // Direct yaw rate control: corrective yaw moment - const float dotomega_radps2_setpoint = + const float omegadot_radps2_setpoint = controllers::dyrc::computeYawMoment(state.yaw_rate_radps, state.steer_ang_rad, state.v_x_mps); //------------------------------------- LOW LEVEL CONTROLLER -----------------------------// @@ -92,11 +34,9 @@ ControlOutput update(const estimation::Measurements &measurement) // const float vehicle_speed_mps = std::hypot(estimated_state.v_x_mps, estimated_state.v_y_mps); // const float low_speed_blend = shared_datatypes::velocityBlend(vehicle_speed_mps); - // Gauss-Newton optimizer: invert the combined-slip tire model to find the slip ratios whose - // predicted forces best match the desired per-wheel force split and desired yaw moment. // ReSharper disable once CppUseStructuredBinding const wheel_set kappa_opt = - controllers::TorqueAllocator::optimize(state, ax_mps2_setpoint, dotomega_radps2_setpoint); + controllers::TorqueAllocator::optimize(state, ax_mps2_setpoint, omegadot_radps2_setpoint); //------------------------------------- POWER LIMITER -----------------------------------// // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request From 9471b20efcbcc25fcf2afa1b5467bdc483abd852 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 9 Apr 2026 22:28:01 -0700 Subject: [PATCH 14/35] penis --- .../estimation/tire_model.hpp | 7 +---- .../shared_datatypes/datatypes.hpp | 31 ++++++++++--------- firmware/shared/srcpp/app/app_pid.hpp | 2 +- 3 files changed, 19 insertions(+), 21 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index 9d20ffe7e2..3d9694d864 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -18,13 +18,8 @@ class TireModel Rear }; - template struct Forces - { - T fx_N; - T fy_N; - }; template - [[nodiscard]] Forces estimate(const T &kappa, const float alpha_rad, const float fz_N) const + [[nodiscard]] shared_datatypes::Pair estimate(const T &kappa, const float alpha_rad, const float fz_N) const { return { computeCombinedFx_N(fz_N, alpha_rad, kappa), diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 4ab79d79ab..45aa4ac1a7 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -46,13 +46,13 @@ template struct wheel_set> } }; -inline float slipRatioToWheelAngularVelocity(const float slip_ratio, const float v_x_mps) -{ - // Avoid division by zero at very low speeds - if (std::fabs(v_x_mps) < vd_constants::SMALL_EPSILON) - return 0.0f; - return (1.0f + slip_ratio) * (v_x_mps / vd_constants::WHEEL_RADIUS_M); -} +// inline float slipRatioToWheelAngularVelocity(const float slip_ratio, const float v_x_mps) +// { +// // Avoid division by zero at very low speeds +// if (std::fabs(v_x_mps) < vd_constants::SMALL_EPSILON) +// return 0.0f; +// return (1.0f + slip_ratio) * (v_x_mps / vd_constants::WHEEL_RADIUS_M); +// } [[nodiscard]] inline float safe_vx(const float v_x_mps) { @@ -61,6 +61,11 @@ inline float slipRatioToWheelAngularVelocity(const float slip_ratio, const float return v_x_mps < 0.0f ? -vd_constants::SMALL_EPSILON : vd_constants::SMALL_EPSILON; } +inline float calculateSlipRatio(const float omega, const float vx) +{ + return (vx - omega * vd_constants::WHEEL_RADIUS_M) / safe_vx(vx); +} + struct VehicleState { // state variables @@ -72,9 +77,7 @@ struct VehicleState float a_y_mps2 = 0.0f; float pedal_percentage = 0.0f; - wheel_set omegas_radps{}; - wheel_set Fxs_N{}; - wheel_set Fys_N{}; + wheel_set motorspeed_radps{}; [[nodiscard]] wheel_set get_tire_angle() const { @@ -125,10 +128,10 @@ struct VehicleState { const auto [fl, fr, rl, rr] = v_in_tire_frame(); return { - .fl = slipRatioToWheelAngularVelocity(omegas_radps.fl, fl.x), - .fr = slipRatioToWheelAngularVelocity(omegas_radps.fr, fr.x), - .rl = slipRatioToWheelAngularVelocity(omegas_radps.rl, rl.x), - .rr = slipRatioToWheelAngularVelocity(omegas_radps.rr, rr.x), + .fl = calculateSlipRatio(motorspeed_radps.fl, fl.x), + .fr = calculateSlipRatio(motorspeed_radps.fr, fr.x), + .rl = calculateSlipRatio(motorspeed_radps.rl, rl.x), + .rr = calculateSlipRatio(motorspeed_radps.rr, rr.x), }; } diff --git a/firmware/shared/srcpp/app/app_pid.hpp b/firmware/shared/srcpp/app/app_pid.hpp index 3c7c0ef3d6..86b9a303ee 100644 --- a/firmware/shared/srcpp/app/app_pid.hpp +++ b/firmware/shared/srcpp/app/app_pid.hpp @@ -52,7 +52,7 @@ class PID assert(sample_time > 0.0f); } - [[nodiscard]] float compute(const float setpoint, const float input, const float disturbance = 0.0f); + [[nodiscard]] float compute(float setpoint, float input, float disturbance = 0.0f); void reset(); [[nodiscard]] float getIntegral(); [[nodiscard]] float getDerivative(); From 731d184598e4f0f0f5bd71abb9502058e7a5a526 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 9 Apr 2026 22:31:29 -0700 Subject: [PATCH 15/35] name --- .../estimation/tire_model.cpp | 36 +++++++++---------- .../estimation/tire_model.hpp | 26 ++++---------- 2 files changed, 24 insertions(+), 38 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index 760af183f6..40b5cfbb5b 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -56,11 +56,10 @@ namespace } // namespace template -[[nodiscard]] T - TireModel::computeCombinedFx_N(const float normal_load_N, const float slip_angle_rad, const T &slip_ratio) const +[[nodiscard]] T TireModel::computeCombinedFx_N(const float fz_N, const float alpha_rad, const T &kappa) const { - const T pure_fx_0 = computePureFx_N(normal_load_N, slip_ratio); - const auto coefficients = combinedFxMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); + const T pure_fx_0 = computePureFx_N(fz_N, kappa); + const auto coefficients = combinedFxMagicFormulaCoefficients(fz_N, alpha_rad, kappa); // Low-speed safeguard: // Below a small vehicle-speed threshold the tire model can predict unrealistically large // forces because the slip calculation becomes ill-conditioned while the fitted Pacejka @@ -77,11 +76,10 @@ template autodiff::dual TireModel::computeCombinedFx_N( const autodiff::dual &slip_ratio) const; template -[[nodiscard]] T - TireModel::computeCombinedFy_N(const float normal_load_N, const float slip_angle_rad, const T &slip_ratio) const +[[nodiscard]] T TireModel::computeCombinedFy_N(const float fz_N, const float alpha_rad, const T &kappa) const { - const float pure_fy_0 = computePureFy_N(normal_load_N, slip_angle_rad); - const auto coefficients = combinedFyMagicFormulaCoefficients(normal_load_N, slip_angle_rad, slip_ratio); + const float pure_fy_0 = computePureFy_N(fz_N, alpha_rad); + const auto coefficients = combinedFyMagicFormulaCoefficients(fz_N, alpha_rad, kappa); // Apply the same low-speed force-availability blend used for Fx so the optimizer sees a // self-consistent pair of tire forces as the vehicle approaches a stop. @@ -154,13 +152,13 @@ TireModel::PureFxMagicFormulaCoefficients } TireModel::PureFyMagicFormulaCoefficients - TireModel::pureFyMagicFormulaCoefficients(const float normal_load_N, const float slip_angle_rad) const + TireModel::pureFyMagicFormulaCoefficients(const float fz, const float alpha) const { // Assumes gamma = 0, lambda terms are 1.0, and pressure effects are captured by the fixed 12_PSI fitted row. - const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); + const float clamped_normal_load_N = std::fmax(fz, 0.0f); const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); const float s_hy = pureFy_Sh(normalized_load_delta); - const float alpha_y = pureFy_Alpha(normalized_load_delta, slip_angle_rad); + const float alpha_y = pureFy_Alpha(normalized_load_delta, alpha); const float c_y = pureFy_C(); const float d_y = pureFy_D(clamped_normal_load_N, normalized_load_delta); const float e_y = pureFy_E(normalized_load_delta, alpha_y); @@ -179,19 +177,19 @@ TireModel::PureFyMagicFormulaCoefficients }; } -template T TireModel::computePureFx_N(const float normal_load_N, const T &slip_ratio) const +template T TireModel::computePureFx_N(const float fz, const T &kappa) const { using std::sin, std::atan; - const auto coefficients = pureFxMagicFormulaCoefficients(normal_load_N, slip_ratio); + const auto coefficients = pureFxMagicFormulaCoefficients(fz, kappa); const T u = coefficients.b_x * coefficients.kappa_x; const T phi = u - coefficients.e_x * (u - atan(u)); return coefficients.d_x * sin(coefficients.c_x * atan(phi)) + coefficients.s_vx; } -float TireModel::computePureFy_N(const float normal_load_N, const float slip_angle_rad) const +float TireModel::computePureFy_N(const float fz_N, const float alpha) const { - const auto coefficients = pureFyMagicFormulaCoefficients(normal_load_N, slip_angle_rad); + const auto coefficients = pureFyMagicFormulaCoefficients(fz_N, alpha); // Pacejka Page 180 (4.E19): F_y0 const float b_y_alpha_y = coefficients.b_y * coefficients.alpha_y; @@ -208,10 +206,10 @@ constexpr float TireModel::combinedFx_SHxa() const return fit_comb_fx_.rHx1; } -constexpr float TireModel::combinedFx_Alpha_s(const float slip_angle_rad) const +constexpr float TireModel::combinedFx_Alpha_s(const float alpha) const { // Pacejka Page 181 (4.E53): alpha_s = alpha* + S_Hxa - return slip_angle_rad + combinedFx_SHxa(); + return alpha + combinedFx_SHxa(); } constexpr float TireModel::combinedFx_Cxa() const @@ -226,13 +224,13 @@ float TireModel::combinedFx_Exa(const float normalized_load_delta) const return std::fmin(fit_comb_fx_.rEx1 + fit_comb_fx_.rEx2 * normalized_load_delta, 1.0f); } -template T TireModel::combinedFx_Bxa(const T &slip_ratio) const +template T TireModel::combinedFx_Bxa(const T &kappa) const { using std::atan; using std::cos; // Pacejka Page 181 (4.E54): B_xa with gamma* = 0 and lambda_xa = 1. - return T(fit_comb_fx_.rBx1) * cos(atan(T(fit_comb_fx_.rBx2) * slip_ratio)); + return T(fit_comb_fx_.rBx1) * cos(atan(T(fit_comb_fx_.rBx2) * kappa)); } template T TireModel::combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients &coefficients) const diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index 3d9694d864..238d45c903 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -18,20 +18,9 @@ class TireModel Rear }; - template - [[nodiscard]] shared_datatypes::Pair estimate(const T &kappa, const float alpha_rad, const float fz_N) const - { - return { - computeCombinedFx_N(fz_N, alpha_rad, kappa), - computeCombinedFy_N(fz_N, alpha_rad, kappa), - }; - } - // note that these only exist for float, dual - template - [[nodiscard]] T computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; - template - [[nodiscard]] T computeCombinedFy_N(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; + template [[nodiscard]] T computeCombinedFx_N(float fz_N, float alpha_rad, const T &kappa) const; + template [[nodiscard]] T computeCombinedFy_N(float fz_N, float alpha_rad, const T &kappa) const; struct TireFitPureParamFy { @@ -193,15 +182,14 @@ class TireModel template [[nodiscard]] PureFxMagicFormulaCoefficients pureFxMagicFormulaCoefficients(float normal_load_N, const T &slip_ratio) const; - [[nodiscard]] PureFyMagicFormulaCoefficients - pureFyMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad) const; + [[nodiscard]] PureFyMagicFormulaCoefficients pureFyMagicFormulaCoefficients(float fz, float alpha) const; //-------------------------------------------------------------------- Combined Pacejka 5.2 Helpers //----------------------------------------------------------------------// [[nodiscard]] constexpr float combinedFx_SHxa() const; - [[nodiscard]] constexpr float combinedFx_Alpha_s(float slip_angle_rad) const; + [[nodiscard]] constexpr float combinedFx_Alpha_s(float alpha) const; [[nodiscard]] constexpr float combinedFx_Cxa() const; [[nodiscard]] float combinedFx_Exa(float normalized_load_delta) const; - template [[nodiscard]] T combinedFx_Bxa(const T &slip_ratio) const; + template [[nodiscard]] T combinedFx_Bxa(const T &kappa) const; template [[nodiscard]] T combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients &coefficients) const; template @@ -225,8 +213,8 @@ class TireModel [[nodiscard]] CombinedFyMagicFormulaCoefficients combinedFyMagicFormulaCoefficients(float normal_load_N, float slip_angle_rad, const T &slip_ratio) const; - template [[nodiscard]] T computePureFx_N(float normal_load_N, const T &slip_ratio) const; - [[nodiscard]] float computePureFy_N(float normal_load_N, float slip_angle_rad) const; + template [[nodiscard]] T computePureFx_N(float fz, const T &kappa) const; + [[nodiscard]] float computePureFy_N(float fz_N, float alpha) const; const TireFitPureParamFx &fit_pure_fx_; const TireFitPureParamFy &fit_pure_fy_; From 479c14ed16fe055ee5999dd6cf4556c1c1acbe1c Mon Sep 17 00:00:00 2001 From: Aditya-Dhiman4 <87353212+Aditya-Dhiman4@users.noreply.github.com> Date: Fri, 10 Apr 2026 00:21:31 -0700 Subject: [PATCH 16/35] I didnt even know this was possible Co-authored-by: Edwin <20777515+Lucien950@users.noreply.github.com> --- .../controllers/torque_allocator/torque_allocator.cpp | 2 +- .../VC/src/app/torque_vectoring/estimation/tire_model.cpp | 4 ++-- .../src/app/torque_vectoring/shared_datatypes/datatypes.hpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp index 3826221e50..42f04638e4 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp @@ -49,7 +49,7 @@ namespace // return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; // } - static constexpr float SQRT_W_MX = std::sqrt(W_FX); + static constexpr float SQRT_W_FX = std::sqrt(W_FX); static constexpr float SQRT_W_MZ = std::sqrt(W_MZ); // const wheel_set blended_des_f_x{ diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index 40b5cfbb5b..49b5d51488 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -69,7 +69,7 @@ template // Pacejka Page 181 (4.E50): F_x = G_xa * F_x0 return coefficients.g_xa * pure_fx_0; } -template float TireModel::computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const float &slip_ratio) const; +template float TireModel::computeCombinedFx_N(const float normal_load_N, const float slip_angle_rad, const float &slip_ratio) const; template autodiff::dual TireModel::computeCombinedFx_N( float normal_load_N, float slip_angle_rad, @@ -88,7 +88,7 @@ template return coefficients.g_yk * T(pure_fy_0) + coefficients.s_vyk; } template float - TireModel::computeCombinedFy_N(float normal_load_N, float slip_angle_rad, const float &slip_ratio) const; + TireModel::computeCombinedFy_N(const float normal_load_N, const float slip_angle_rad, const float &slip_ratio) const; template autodiff::dual TireModel::computeCombinedFy_N( float normal_load_N, float slip_angle_rad, diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index 45aa4ac1a7..e7909ff5dd 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -15,7 +15,7 @@ template struct Pair T y; }; -template struct wheel_set +template struct wheel_set { T fl; T fr; From 3741ff9d085b7141ad93f171aa8fefbe49982077 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Sun, 12 Apr 2026 14:28:23 -0700 Subject: [PATCH 17/35] wow --- .../controllers/{yaw_rate_control => }/controllers_dyrc.cpp | 0 .../controllers/{yaw_rate_control => }/controllers_dyrc.hpp | 0 .../controllers/{power_limiting => }/power_limiting.cpp | 0 .../controllers/{power_limiting => }/power_limiting.hpp | 0 .../controllers/{torque_allocator => }/torque_allocator.cpp | 2 +- .../controllers/{torque_allocator => }/torque_allocator.hpp | 0 .../hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp | 4 ++-- 7 files changed, 3 insertions(+), 3 deletions(-) rename firmware/hexray/VC/src/app/torque_vectoring/controllers/{yaw_rate_control => }/controllers_dyrc.cpp (100%) rename firmware/hexray/VC/src/app/torque_vectoring/controllers/{yaw_rate_control => }/controllers_dyrc.hpp (100%) rename firmware/hexray/VC/src/app/torque_vectoring/controllers/{power_limiting => }/power_limiting.cpp (100%) rename firmware/hexray/VC/src/app/torque_vectoring/controllers/{power_limiting => }/power_limiting.hpp (100%) rename firmware/hexray/VC/src/app/torque_vectoring/controllers/{torque_allocator => }/torque_allocator.cpp (99%) rename firmware/hexray/VC/src/app/torque_vectoring/controllers/{torque_allocator => }/torque_allocator.hpp (100%) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_dyrc.cpp similarity index 100% rename from firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp rename to firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_dyrc.cpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_dyrc.hpp similarity index 100% rename from firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp rename to firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_dyrc.hpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/power_limiting/power_limiting.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/power_limiting.cpp similarity index 100% rename from firmware/hexray/VC/src/app/torque_vectoring/controllers/power_limiting/power_limiting.cpp rename to firmware/hexray/VC/src/app/torque_vectoring/controllers/power_limiting.cpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/power_limiting/power_limiting.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/power_limiting.hpp similarity index 100% rename from firmware/hexray/VC/src/app/torque_vectoring/controllers/power_limiting/power_limiting.hpp rename to firmware/hexray/VC/src/app/torque_vectoring/controllers/power_limiting.hpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp similarity index 99% rename from firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp rename to firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index 42f04638e4..fee00fc5c3 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -49,7 +49,7 @@ namespace // return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; // } - static constexpr float SQRT_W_FX = std::sqrt(W_FX); + static constexpr float SQRT_W_FX = std::sqrt(W_FX); static constexpr float SQRT_W_MZ = std::sqrt(W_MZ); // const wheel_set blended_des_f_x{ diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.hpp similarity index 100% rename from firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator/torque_allocator.hpp rename to firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.hpp diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index 62074bd50c..15749cda43 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -1,8 +1,8 @@ #include "torque_vectoring.hpp" #include "torque_vectoring/estimation/vehicle_state_estimator.hpp" -#include "torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp" -#include "torque_vectoring/controllers/torque_allocator/torque_allocator.hpp" +#include "torque_vectoring/controllers/controllers_dyrc.hpp" +#include "torque_vectoring/controllers/torque_allocator.hpp" #include "torque_vectoring/shared_datatypes/constants.hpp" #include "torque_vectoring/shared_datatypes/datatypes.hpp" From a66fb4c389d46e3ba6939460ead9ca5b0f62e894 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Sun, 12 Apr 2026 14:29:10 -0700 Subject: [PATCH 18/35] L --- .../src/app/torque_vectoring/controllers/torque_allocator.cpp | 2 +- .../VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index fee00fc5c3..7f067b0a7e 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -107,7 +107,7 @@ namespace const autodiff::dual sum_fx = predicted_f.fl.x + predicted_f.fr.x + predicted_f.rl.x + predicted_f.rr.x; const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); return DualVec2{ - SQRT_W_MX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint), + SQRT_W_FX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint), SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), }; }; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp index e7909ff5dd..45aa4ac1a7 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp @@ -15,7 +15,7 @@ template struct Pair T y; }; -template struct wheel_set +template struct wheel_set { T fl; T fr; From 6baefc083b5e84676f30a78ee552e706493e5555 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 13 Apr 2026 14:06:10 -0700 Subject: [PATCH 19/35] maybe everything is fucked idk man, love matlab --- firmware/hexray/VC/CMakeLists.txt | 14 +- .../controllers/torque_allocator.cpp | 16 +- .../controllers/torque_allocator.hpp | 14 +- .../estimation/tire_model.cpp | 25 ++- .../estimation/tire_model.hpp | 14 +- .../estimation/vehicle_state_estimator.cpp | 30 +-- .../estimation/vehicle_state_estimator.hpp | 9 +- .../shared_datatypes/constants.hpp | 3 +- .../shared_datatypes/decimal_dual.hpp | 8 + .../shared_datatypes/pair.hpp | 29 +++ ...atypes.hpp => vehicle_state_estimator.hpp} | 194 +++++------------- .../shared_datatypes/wheel_set.hpp | 38 ++++ .../app/torque_vectoring/torque_vectoring.cpp | 89 +++++--- .../app/torque_vectoring/torque_vectoring.hpp | 50 ++++- .../torque_vectoring_matlab.h | 17 ++ firmware/shared/srcpp/util/util_units.hpp | 4 +- 16 files changed, 322 insertions(+), 232 deletions(-) create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/decimal_dual.hpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/pair.hpp rename firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/{datatypes.hpp => vehicle_state_estimator.hpp} (50%) create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/wheel_set.hpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h diff --git a/firmware/hexray/VC/CMakeLists.txt b/firmware/hexray/VC/CMakeLists.txt index 3b94931d3b..1837be2c5c 100644 --- a/firmware/hexray/VC/CMakeLists.txt +++ b/firmware/hexray/VC/CMakeLists.txt @@ -16,7 +16,9 @@ set(SYSTEM_INCLUDE_DIRS ) file(GLOB_RECURSE APP_SRCS CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/app/*.cpp") -list(APPEND APP_SRCS) +list(APPEND APP_SRCS + "${SHARED_APP_INCLUDE_DIR_CPP}/app_pid.cpp" +) set(APP_INCLUDE_DIRS "${SHARED_APP_INCLUDE_DIR}" "${SHARED_APP_INCLUDE_DIR_CPP}" "${CMAKE_CURRENT_SOURCE_DIR}/src/app/" ) @@ -165,4 +167,14 @@ elseif ("${TARGET}" STREQUAL "test") target_link_libraries("hexray_VC_test" PRIVATE "hexray_VC_commit_info" "hexray_VC_jsoncan" "autodiff_interface") target_compile_definitions("hexray_VC_test" PRIVATE STM32H733xx) + + + file(GLOB_RECURSE TV_SRCS CONFIGURE_DEPENDS + "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/*.cpp" + "${SHARED_APP_INCLUDE_DIR_CPP}/app_pid.cpp" + ) + add_library("hexray_VC_torque_vectoring" STATIC ${TV_SRCS}) + target_include_directories("hexray_VC_torque_vectoring" PUBLIC "${INCLUDE_DIRS}") + target_link_libraries("hexray_VC_torque_vectoring" PUBLIC "autodiff_interface" "eigen_interface" "m") + target_compile_options("hexray_VC_torque_vectoring" PRIVATE /MT) endif () \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index 7f067b0a7e..de28bda2d8 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -34,8 +34,8 @@ namespace using DualVec4 = Eigen::Matrix; } // namespace -[[nodiscard]] wheel_set - optimize(const VehicleState &state, const float ax_setpoint, const float omegadot_setpoint) +template +[[nodiscard]] wheel_set optimize(const VehicleState &state, const T ax_setpoint, const T omegadot_setpoint) { // Low-speed safeguard: // torque_vectoring.cpp computes a single force-availability blend from vehicle speed and passes it @@ -49,8 +49,8 @@ namespace // return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; // } - static constexpr float SQRT_W_FX = std::sqrt(W_FX); - static constexpr float SQRT_W_MZ = std::sqrt(W_MZ); + static const float SQRT_W_FX = std::sqrt(W_FX); + static const float SQRT_W_MZ = std::sqrt(W_MZ); // const wheel_set blended_des_f_x{ // .fl = low_speed_blend * des_f_x.fl, @@ -112,10 +112,7 @@ namespace }; }; - Vec4f opt_slip; // output variable - // seed opt_slip - const auto [kappa_fl, kappa_fr, kappa_rl, kappa_rr] = state.kappas(); - opt_slip << kappa_fl, kappa_fr, kappa_rl, kappa_rr; + Vec4f opt_slip{ 0, 0, 0, 0 }; // output variable float previous_cost = std::numeric_limits::infinity(); @@ -182,4 +179,7 @@ namespace .rr = opt_slip(3), }; } + +template wheel_set optimize(const VehicleState &state, float ax_setpoint, float omegadot_setpoint); +template wheel_set optimize(const VehicleState &state, double ax_setpoint, double omegadot_setpoint); } // namespace app::tv::controllers::allocator diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.hpp index 4c827ec861..3890758918 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.hpp @@ -1,10 +1,10 @@ #pragma once -#include "torque_vectoring/shared_datatypes/datatypes.hpp" +#include "torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp" +#include "torque_vectoring/shared_datatypes/wheel_set.hpp" namespace app::tv::controllers { - /** * Torque allocator using Gauss-Newton optimization. * @@ -14,14 +14,14 @@ namespace app::tv::controllers * * Outputs per-wheel optimal slip ratios using the caller-provided per-wheel tire models. */ -namespace TorqueAllocator +namespace allocator { /** * Run Gauss-Newton iterations to find optimal slip ratios. * @return Per-wheel optimal slip ratios */ - [[nodiscard]] shared_datatypes::wheel_set - optimize(const shared_datatypes::VehicleState &state, float ax_setpoint, float omegadot_setpoint); - -}; // namespace TorqueAllocator + template + [[nodiscard]] shared_datatypes::wheel_set + optimize(const shared_datatypes::VehicleState &state, T ax_setpoint, T omegadot_setpoint); +}; // namespace allocator } // namespace app::tv::controllers diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index 49b5d51488..6283adbab8 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -31,7 +31,7 @@ namespace return value; return primal < 0.0 ? -SMALL_EPSILON : SMALL_EPSILON; } - constexpr float safeSignedDenominator(const float value) + float safeSignedDenominator(const float value) { if (std::fabs(value) >= SMALL_EPSILON) return value; @@ -45,7 +45,7 @@ namespace return -1.0f; return 0.0f; } - constexpr autodiff::dual sign(const autodiff::dual &value) + autodiff::dual sign(const autodiff::dual &value) { if (value > 0.0f) return 1.0f; @@ -69,7 +69,10 @@ template // Pacejka Page 181 (4.E50): F_x = G_xa * F_x0 return coefficients.g_xa * pure_fx_0; } -template float TireModel::computeCombinedFx_N(const float normal_load_N, const float slip_angle_rad, const float &slip_ratio) const; +template float TireModel::computeCombinedFx_N( + const float normal_load_N, + const float slip_angle_rad, + const float &slip_ratio) const; template autodiff::dual TireModel::computeCombinedFx_N( float normal_load_N, float slip_angle_rad, @@ -87,8 +90,10 @@ template // Pacejka Pages 181-182 (4.E58): F_y = G_yk * F_y0 + S_vyk return coefficients.g_yk * T(pure_fy_0) + coefficients.s_vyk; } -template float - TireModel::computeCombinedFy_N(const float normal_load_N, const float slip_angle_rad, const float &slip_ratio) const; +template float TireModel::computeCombinedFy_N( + const float normal_load_N, + const float slip_angle_rad, + const float &slip_ratio) const; template autodiff::dual TireModel::computeCombinedFy_N( float normal_load_N, float slip_angle_rad, @@ -380,7 +385,7 @@ TireModel::CombinedFyMagicFormulaCoefficients TireModel::combinedFyMagicFormu //-------------------------------------------------------------------- MJ Pure Coefficents 5.2 //----------------------------------------------------------------------// -constexpr float TireModel::normalizedLoadDelta(const float normal_load_N) +float TireModel::normalizedLoadDelta(const float normal_load_N) { return (normal_load_N - NOMINAL_FZ_N) / safeSignedDenominator(NOMINAL_FZ_N); } @@ -424,14 +429,14 @@ template T TireModel::pureFx_E(const float normalized_load_del return base_e_x * (1.0f - fit_pure_fx_.ex_4 * sign(kappa_x)); } -constexpr float TireModel::pureFx_K(const float normal_load_N, const float normalized_load_delta) const +float TireModel::pureFx_K(const float normal_load_N, const float normalized_load_delta) const { // Pacejka Page 179 (4.E15): K_xk return normal_load_N * (fit_pure_fx_.kx_1 + fit_pure_fx_.kx_2 * normalized_load_delta) * std::exp(fit_pure_fx_.kx_3 * normalized_load_delta); } -constexpr float TireModel::pureFx_B(const float slip_stiffness, const float shape_factor, const float peak_factor) +float TireModel::pureFx_B(const float slip_stiffness, const float shape_factor, const float peak_factor) { // Pacejka Page 179 (4.E16): B_x = K_xk / (C_x * D_x + epsilon_x) return slip_stiffness / safeSignedDenominator(shape_factor * peak_factor); @@ -479,14 +484,14 @@ constexpr float TireModel::pureFy_E(const float normalized_load_delta, const flo return (fit_pure_fy_.ey_1 + fit_pure_fy_.ey_2 * normalized_load_delta) * (1.0f - fit_pure_fy_.ey_3 * sign(alpha_y)); } -constexpr float TireModel::pureFy_K(const float normal_load_N) const +float TireModel::pureFy_K(const float normal_load_N) const { // Pacejka Page 180 (4.E25): K_yalpha with gamma/pressure terms reduced to zero. const float denominator = safeSignedDenominator(fit_pure_fy_.ky_2 * NOMINAL_FZ_N); return fit_pure_fy_.ky_1 * NOMINAL_FZ_N * std::sin(2.0f * std::atan(normal_load_N / denominator)); } -constexpr float TireModel::pureFy_B(const float cornering_stiffness, const float shape_factor, const float peak_factor) +float TireModel::pureFy_B(const float cornering_stiffness, const float shape_factor, const float peak_factor) { // Pacejka Page 180 (4.E26): B_y = K_yalpha / (C_y * D_y + epsilon_y) return cornering_stiffness / safeSignedDenominator(shape_factor * peak_factor); diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index 238d45c903..7f231a35cb 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -1,5 +1,5 @@ #pragma once -#include "torque_vectoring/shared_datatypes/datatypes.hpp" +#include "torque_vectoring/shared_datatypes/decimal_dual.hpp" namespace app::tv::estimation { @@ -155,7 +155,7 @@ class TireModel static constexpr float NOMINAL_FZ_N = 750.0f; //-------------------------------------------------------------------- Class Helpers //----------------------------------------------------------------------// - [[nodiscard]] static constexpr float normalizedLoadDelta(float normal_load_N); + [[nodiscard]] static float normalizedLoadDelta(float normal_load_N); // Reduced-model assumptions for combined slip in this pass: // gamma* = 0, lambda_xa = 1, lambda_yk = 1, lambda_vyk = 1, zeta_2 = 1. // Pressure dependence is captured by the fixed 12_PSI fitted parameter row. @@ -167,8 +167,8 @@ class TireModel [[nodiscard]] constexpr float pureFx_mu(float normalized_load_delta) const; [[nodiscard]] constexpr float pureFx_D(float normal_load_N, float normalized_load_delta) const; template [[nodiscard]] T pureFx_E(float normalized_load_delta, const T &kappa_x) const; - [[nodiscard]] constexpr float pureFx_K(float normal_load_N, float normalized_load_delta) const; - [[nodiscard]] static constexpr float pureFx_B(float slip_stiffness, float shape_factor, float peak_factor); + [[nodiscard]] float pureFx_K(float normal_load_N, float normalized_load_delta) const; + [[nodiscard]] static float pureFx_B(float slip_stiffness, float shape_factor, float peak_factor); [[nodiscard]] constexpr float pureFx_Sv(float normal_load_N, float normalized_load_delta) const; [[nodiscard]] constexpr float pureFy_Sh(float normalized_load_delta) const; [[nodiscard]] constexpr float pureFy_Alpha(float normalized_load_delta, float slip_angle_rad) const; @@ -176,9 +176,9 @@ class TireModel [[nodiscard]] constexpr float pureFy_mu(float normalized_load_delta) const; [[nodiscard]] constexpr float pureFy_D(float normal_load_N, float normalized_load_delta) const; [[nodiscard]] constexpr float pureFy_E(float normalized_load_delta, float alpha_y) const; - [[nodiscard]] constexpr float pureFy_K(float normal_load_N) const; - [[nodiscard]] static constexpr float pureFy_B(float cornering_stiffness, float shape_factor, float peak_factor); - [[nodiscard]] constexpr float pureFy_Sv(float normal_load_N, float normalized_load_delta) const; + [[nodiscard]] float pureFy_K(float normal_load_N) const; + [[nodiscard]] static float pureFy_B(float cornering_stiffness, float shape_factor, float peak_factor); + [[nodiscard]] constexpr float pureFy_Sv(float normal_load_N, float normalized_load_delta) const; template [[nodiscard]] PureFxMagicFormulaCoefficients pureFxMagicFormulaCoefficients(float normal_load_N, const T &slip_ratio) const; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp index 191bcd5fe1..f8aa56c21a 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp @@ -29,7 +29,7 @@ namespace constexpr float ESTIMATOR_DT_S = 0.01f; // Matches the 100 Hz control task. constexpr float ESTIMATOR_YAW_INERTIA = 110.0f; // TODO: Replace with measured Hexray yaw inertia. - [[nodiscard]] constexpr autodiff::dual stateTransitionVx(const StateInput &x) + [[nodiscard]] autodiff::dual stateTransitionVx(const StateInput &x) { const autodiff::dual &v_x = x(static_cast(VX)); const autodiff::dual &v_y = x(static_cast(VY)); @@ -39,7 +39,7 @@ namespace return v_x + (ESTIMATOR_DT_S * (a_x + (v_y * r))); } - [[nodiscard]] constexpr autodiff::dual stateTransitionVy(const StateInput &x) + [[nodiscard]] autodiff::dual stateTransitionVy(const StateInput &x) { const autodiff::dual &v_x = x(static_cast(VX)); const autodiff::dual &v_y = x(static_cast(VY)); @@ -49,7 +49,7 @@ namespace return v_y + (ESTIMATOR_DT_S * (a_y - (v_x * r))); } - [[nodiscard]] constexpr autodiff::dual stateTransitionYawRate(const StateInput &x) + [[nodiscard]] autodiff::dual stateTransitionYawRate(const StateInput &x) { const autodiff::dual &r = x(static_cast(R)); const autodiff::dual &mz = x(static_cast(MZ)); @@ -57,27 +57,27 @@ namespace return r + (ESTIMATOR_DT_S * (mz / ESTIMATOR_YAW_INERTIA)); } - [[nodiscard]] constexpr autodiff::dual stateTransitionYawMoment(const StateInput &x) + [[nodiscard]] autodiff::dual stateTransitionYawMoment(const StateInput &x) { return x(static_cast(MZ)); } - [[nodiscard]] constexpr autodiff::dual measurementVx(const State &x) + [[nodiscard]] autodiff::dual measurementVx(const State &x) { return x(static_cast(VX)); } - [[nodiscard]] constexpr autodiff::dual measurementVy(const State &x) + [[nodiscard]] autodiff::dual measurementVy(const State &x) { return x(static_cast(VY)); } - [[nodiscard]] constexpr autodiff::dual measurementYawRate(const State &x) + [[nodiscard]] autodiff::dual measurementYawRate(const State &x) { return x(static_cast(R)); } - [[nodiscard]] constexpr autodiff::dual measurementYawMoment(const State &x) + [[nodiscard]] autodiff::dual measurementYawMoment(const State &x) { return x(static_cast(MZ)); } @@ -102,7 +102,7 @@ namespace } }; } - [[nodiscard]] constexpr Filter::N_N processNoise() + [[nodiscard]] Filter::N_N processNoise() { Filter::N_N q = Filter::N_N::Zero(); q(static_cast(VX), static_cast(VX)) = 0.05f; @@ -112,7 +112,7 @@ namespace return q; } - [[nodiscard]] constexpr Filter::M_M measurementNoise() + [[nodiscard]] Filter::M_M measurementNoise() { Filter::M_M r = Filter::M_M::Zero(); r(static_cast(VX), static_cast(VX)) = 0.75f; @@ -122,12 +122,12 @@ namespace return r; } - [[nodiscard]] constexpr StateVector initialState() + [[nodiscard]] StateVector initialState() { return StateVector::Zero(); } - [[nodiscard]] constexpr Filter::N_N initialCovariance() + [[nodiscard]] Filter::N_N initialCovariance() { Filter::N_N p0 = Filter::N_N::Identity(); p0(static_cast(VX), static_cast(VX)) = 5.0f; @@ -183,7 +183,7 @@ namespace return z; } - [[nodiscard]] constexpr Filter createFilter() + [[nodiscard]] Filter createFilter() { return Filter( createStateFunctions(), createMeasurementFunctions(), processNoise(), measurementNoise(), initialState(), @@ -199,7 +199,7 @@ namespace VehicleStateEstimator filter_ = createFilter(); } - [[nodiscard]] shared_datatypes::VehicleState estimate(const Measurements &state) + [[nodiscard]] shared_datatypes::VehicleState estimate(const Measurements &state) { InputVector u = InputVector::Zero(); u(static_cast(AX)) = state.ax; @@ -225,9 +225,9 @@ namespace VehicleStateEstimator .v_x_mps = estimated_state(static_cast(VX)), .v_y_mps = estimated_state(static_cast(VY)), .yaw_rate_radps = estimated_state(static_cast(R)), - .steer_ang_rad = measured_steering_angle, .a_x_mps2 = u(static_cast(AX)), .a_y_mps2 = u(static_cast(AY)), + .delta = { measured_steering_angle, measured_steering_angle, 0, 0 }, }; } diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp index f7fc9a0227..289d88124d 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp @@ -1,7 +1,8 @@ #pragma once #include "state_estimation/app_kalman_filter.hpp" -#include "torque_vectoring/shared_datatypes/datatypes.hpp" +#include "torque_vectoring/torque_vectoring.hpp" +#include "torque_vectoring/shared_datatypes/wheel_set.hpp" namespace app::tv::estimation { @@ -23,9 +24,9 @@ namespace VehicleStateEstimator { using Filter = app::state_estimation::ekf; using Covariance = Filter::N_N; - [[nodiscard]] shared_datatypes::VehicleState estimate(const Measurements &state); - [[nodiscard]] const Covariance &covariance(); - void reset_filter(); + [[nodiscard]] shared_datatypes::VehicleState estimate(const Measurements &state); + [[nodiscard]] const Covariance &covariance(); + void reset_filter(); // if you want to reset, just reconstruct the object }; // namespace VehicleStateEstimator } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index 8dc49f8557..c6e1ea41a0 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -1,6 +1,7 @@ #pragma once #include "util_units.hpp" +#include namespace app::tv::shared_datatypes::vd_constants { @@ -124,7 +125,7 @@ inline constexpr float MAX_AX = 10; // TODO idk this number bruh * Convert power (kW) and RPM to torque (Nm) * Includes safety guard against division by zero */ -[[nodiscard]] constexpr float POWER_TO_TORQUE(const float power, const float rpm) +[[nodiscard]] inline float POWER_TO_TORQUE(const float power, const float rpm) { return (power * static_cast(POWER_TO_TORQUE_CONVERSION_FACTOR)) / (std::fmax(rpm, 0.00001f) / GEAR_RATIO); } diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/decimal_dual.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/decimal_dual.hpp new file mode 100644 index 0000000000..f33bfcfa06 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/decimal_dual.hpp @@ -0,0 +1,8 @@ +#pragma once + +#include "dual.hpp" +template +concept DecimalOrDual = std::same_as || std::same_as || std::same_as; + +template +concept Decimal = std::same_as || std::same_as; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/pair.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/pair.hpp new file mode 100644 index 0000000000..7977d05fab --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/pair.hpp @@ -0,0 +1,29 @@ +#pragma once +#include "decimal_dual.hpp" + +namespace app::tv::shared_datatypes +{ +template struct Pair +{ + T x; + T y; +}; +} // namespace app::tv::shared_datatypes + +// { +// +// // inline float slipRatioToWheelAngularVelocity(const float slip_ratio, const float v_x_mps) +// // { +// // // Avoid division by zero at very low speeds +// // if (std::fabs(v_x_mps) < vd_constants::SMALL_EPSILON) +// // return 0.0f; +// // return (1.0f + slip_ratio) * (v_x_mps / vd_constants::WHEEL_RADIUS_M); +// // } +// + +// +// inline float calculateSlipRatio(const float omega, const float vx) +// { +// return (vx - omega * vd_constants::WHEEL_RADIUS_M) / safe_vx(vx); +// } +// } // namespace app::tv::shared_datatypes diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp similarity index 50% rename from firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp rename to firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp index 45aa4ac1a7..e515da0366 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/datatypes.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp @@ -1,96 +1,27 @@ #pragma once - -#include "constants.hpp" -#include "dual.hpp" -#include - -template -concept DecimalOrDual = std::same_as || std::same_as || std::same_as; +#include "torque_vectoring/shared_datatypes/wheel_set.hpp" +#include "torque_vectoring/shared_datatypes/constants.hpp" +#include "torque_vectoring/shared_datatypes/decimal_dual.hpp" namespace app::tv::shared_datatypes { -template struct Pair -{ - T x; - T y; -}; - -template struct wheel_set -{ - T fl; - T fr; - T rl; - T rr; -}; -template struct wheel_set> -{ - Pair fl; - Pair fr; - Pair rl; - Pair rr; - - /** - * rotates each pair in the z-axis by the respective angle in z_rot - * @param z_rot_rad rotation angles for each wheel in radians - */ - void rotate(const wheel_set &z_rot_rad) - { - fl = { fl.x * std::cos(z_rot_rad.fl) + fl.y * std::sin(z_rot_rad.fl), - fl.y * std::cos(z_rot_rad.fl) - fl.x * std::sin(z_rot_rad.fl) }; - fr = { fr.x * std::cos(z_rot_rad.fr) + fr.y * std::sin(z_rot_rad.fr), - fr.y * std::cos(z_rot_rad.fr) - fr.x * std::sin(z_rot_rad.fr) }; - rl = { rl.x * std::cos(z_rot_rad.rl) + rl.y * std::sin(z_rot_rad.rl), - rl.y * std::cos(z_rot_rad.rl) - rl.x * std::sin(z_rot_rad.rl) }; - rr = { rr.x * std::cos(z_rot_rad.rr) + rr.y * std::sin(z_rot_rad.rr), - rr.y * std::cos(z_rot_rad.rr) - rr.x * std::sin(z_rot_rad.rr) }; - } -}; - -// inline float slipRatioToWheelAngularVelocity(const float slip_ratio, const float v_x_mps) -// { -// // Avoid division by zero at very low speeds -// if (std::fabs(v_x_mps) < vd_constants::SMALL_EPSILON) -// return 0.0f; -// return (1.0f + slip_ratio) * (v_x_mps / vd_constants::WHEEL_RADIUS_M); -// } - -[[nodiscard]] inline float safe_vx(const float v_x_mps) -{ - if (std::fabs(v_x_mps) >= vd_constants::SMALL_EPSILON) - return v_x_mps; - return v_x_mps < 0.0f ? -vd_constants::SMALL_EPSILON : vd_constants::SMALL_EPSILON; -} - -inline float calculateSlipRatio(const float omega, const float vx) -{ - return (vx - omega * vd_constants::WHEEL_RADIUS_M) / safe_vx(vx); -} - -struct VehicleState +template struct VehicleState { // state variables - float v_x_mps = 0.0f; - float v_y_mps = 0.0f; - float yaw_rate_radps = 0.0f; - float steer_ang_rad = 0.0f; - float a_x_mps2 = 0.0f; - float a_y_mps2 = 0.0f; - float pedal_percentage = 0.0f; - - wheel_set motorspeed_radps{}; - - [[nodiscard]] wheel_set get_tire_angle() const - { - // todo steering model - return { steer_ang_rad, steer_ang_rad, 0, 0 }; - } + T v_x_mps = 0.0f; + T v_y_mps = 0.0f; + T yaw_rate_radps = 0.0f; + T a_x_mps2 = 0.0f; + T a_y_mps2 = 0.0f; + T apps = 0.0f; + wheel_set delta{}; /** * @return vector of vy each in the frame of the respective tire */ - wheel_set> v_in_tire_frame() const + [[nodiscard]] wheel_set> v_in_tire_frame() const { - wheel_set> v = { + wheel_set> v = { { v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M, v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m, @@ -108,14 +39,14 @@ struct VehicleState v_y_mps + yaw_rate_radps * -vd_constants::DIST_FRONT_AXLE_CG_m, }, }; - v.rotate(get_tire_angle()); + v.rotate(delta); return v; } - wheel_set alphas() const + [[nodiscard]] wheel_set alphas() const { const auto [fl_v, fr_v, rl_v, rr_v] = v_in_tire_frame(); - const auto [fl_rot, fr_rot, rl_rot, rr_rot] = get_tire_angle(); + const auto [fl_rot, fr_rot, rl_rot, rr_rot] = delta; return { std::atan2(fl_v.y, safe_vx(fl_v.x)) - fl_rot, std::atan2(fr_v.y, safe_vx(fr_v.x)) - fr_rot, @@ -124,29 +55,18 @@ struct VehicleState }; } - [[nodiscard]] wheel_set kappas() const - { - const auto [fl, fr, rl, rr] = v_in_tire_frame(); - return { - .fl = calculateSlipRatio(motorspeed_radps.fl, fl.x), - .fr = calculateSlipRatio(motorspeed_radps.fr, fr.x), - .rl = calculateSlipRatio(motorspeed_radps.rl, rl.x), - .rr = calculateSlipRatio(motorspeed_radps.rr, rr.x), - }; - } - // AERODYNAMIC EFFECTS // these might use internal state later?? // ReSharper disable once CppMemberFunctionMayBeStatic - [[nodiscard]] constexpr float dynamicCOPFront() const { return 1.0f - vd_constants::COP_REAR; } + [[nodiscard]] constexpr T dynamicCOPFront() const { return 1.0f - vd_constants::COP_REAR; } // ReSharper disable once CppMemberFunctionMayBeStatic - [[nodiscard]] constexpr float dynamicCOPRight() const { return vd_constants::COP_RIGHT; } - [[nodiscard]] constexpr float est_dragFx_N() const + [[nodiscard]] constexpr T dynamicCOPRight() const { return vd_constants::COP_RIGHT; } + [[nodiscard]] constexpr T est_dragFx_N() const { return 0.5f * vd_constants::AIR_DENSITY_KGPM3 * vd_constants::FRONTAL_AREA_M2 * vd_constants::DRAG_COEFF * v_x_mps * v_x_mps; } - [[nodiscard]] constexpr float est_downforceFz_N() const + [[nodiscard]] constexpr T est_downforceFz_N() const { return 0.5f * vd_constants::AIR_DENSITY_KGPM3 * vd_constants::FRONTAL_AREA_M2 * vd_constants::LIFT_COEFF * v_x_mps * v_x_mps; @@ -162,7 +82,7 @@ struct VehicleState * Positive long_accel transfers load to rear axle * @return Load transfer force (N) */ - [[nodiscard]] constexpr float LONG_ACCEL_TERM_VERTICAL_FORCE() const + [[nodiscard]] constexpr T LONG_ACCEL_TERM_VERTICAL_FORCE() const { return (vd_constants::CAR_MASS_AT_CG_KG * a_x_mps2 * vd_constants::DIST_HEIGHT_CG_m) / vd_constants::WHEELBASE_m; @@ -172,31 +92,31 @@ struct VehicleState * Transfers load to outside wheels during cornering * @return Load transfer force per side (N) */ - [[nodiscard]] constexpr float LAT_ACCEL_TERM_VERTICAL_FORCE() const + [[nodiscard]] constexpr T LAT_ACCEL_TERM_VERTICAL_FORCE() const { return (vd_constants::CAR_MASS_AT_CG_KG * a_y_mps2 * vd_constants::DIST_HEIGHT_CG_m) / (2.0f * vd_constants::TRACK_WIDTH_m); } - [[nodiscard]] wheel_set est_Fz_N() const + [[nodiscard]] wheel_set est_Fz_N() const { // static loads - static constexpr float STATIC_FRONT_AXLE_LOAD_N = + static constexpr T STATIC_FRONT_AXLE_LOAD_N = vd_constants::CAR_WEIGHT * (vd_constants::DIST_REAR_AXLE_CG_m / vd_constants::WHEELBASE_m); - static constexpr float STATIC_REAR_AXLE_LOAD_N = + static constexpr T STATIC_REAR_AXLE_LOAD_N = vd_constants::CAR_WEIGHT * (vd_constants::DIST_FRONT_AXLE_CG_m / vd_constants::WHEELBASE_m); - static constexpr float STATIC_FRONT_WHEEL_LOAD_N = 0.5f * STATIC_FRONT_AXLE_LOAD_N; - static constexpr float STATIC_REAR_WHEEL_LOAD_N = 0.5f * STATIC_REAR_AXLE_LOAD_N; + static constexpr T STATIC_FRONT_WHEEL_LOAD_N = 0.5f * STATIC_FRONT_AXLE_LOAD_N; + static constexpr T STATIC_REAR_WHEEL_LOAD_N = 0.5f * STATIC_REAR_AXLE_LOAD_N; // load transfer - const float long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(); - const float lat_load_tf = LAT_ACCEL_TERM_VERTICAL_FORCE(); + const T long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(); + const T lat_load_tf = LAT_ACCEL_TERM_VERTICAL_FORCE(); // downforce and cop components - const float down_force_n = est_downforceFz_N(); + const T down_force_n = est_downforceFz_N(); // Assuming COP is a fraction of the total downforce - const float front_cop = dynamicCOPFront(), rear_cop = 1.0f - front_cop, right_cop = dynamicCOPRight(), - left_cop = 1.0f - right_cop; + const T front_cop = dynamicCOPFront(), rear_cop = 1.0f - front_cop, right_cop = dynamicCOPRight(), + left_cop = 1.0f - right_cop; return { .fl = std::fmax( 0.0f, @@ -217,23 +137,23 @@ struct VehicleState * Get body slip * @return */ - [[nodiscard]] float est_beta_rad() const { return std::atan2(v_y_mps, safe_vx(v_x_mps)); } + [[nodiscard]] T est_beta_rad() const { return std::atan2(v_y_mps, safe_vx(v_x_mps)); } /** * @param tires_F_N tire forces * @return Given certain tire forces, what would be the resulting yaw moment Mz about the CG? */ - template [[nodiscard]] T est_Mz_N(wheel_set> tires_F_N) const + template [[nodiscard]] F est_Mz_N(wheel_set> tires_F_N) const { // TODO aligning moment contributions to the yaw moment equation - tires_F_N.rotate(get_tire_angle()); - const T fl_moment = + tires_F_N.rotate(delta); + const F fl_moment = (vd_constants::DIST_FRONT_AXLE_CG_m * tires_F_N.fl.y) - (vd_constants::HALF_TRACK_M * tires_F_N.fl.x); - const T fr_moment = + const F fr_moment = (vd_constants::DIST_FRONT_AXLE_CG_m * tires_F_N.fr.y) + (vd_constants::HALF_TRACK_M * tires_F_N.fr.x); - const T rl_moment = + const F rl_moment = (-vd_constants::DIST_REAR_AXLE_CG_m * tires_F_N.rl.y) - (vd_constants::HALF_TRACK_M * tires_F_N.rl.x); - const T rr_moment = + const F rr_moment = (-vd_constants::DIST_REAR_AXLE_CG_m * tires_F_N.rr.y) + (vd_constants::HALF_TRACK_M * tires_F_N.rr.x); return fl_moment + fr_moment + rl_moment + rr_moment; } @@ -243,13 +163,13 @@ struct VehicleState * Accounts for load transfer effect on yaw moment generation capacity * @return Effective moment arm (m) */ - [[nodiscard]] constexpr float ACCELERATION_TERM_KMZ() const + [[nodiscard]] constexpr T ACCELERATION_TERM_KMZ() const { return vd_constants::DIST_FRONT_AXLE_CG_m + (a_x_mps2 * vd_constants::DIST_HEIGHT_CG_m) / vd_constants::GRAVITY; } - [[nodiscard]] constexpr float KMZ() const + [[nodiscard]] constexpr T KMZ() const { - const float LONG_ACCEL_TERM = ACCELERATION_TERM_KMZ(); + const T LONG_ACCEL_TERM = ACCELERATION_TERM_KMZ(); return ((vd_constants::CAR_WEIGHT - (vd_constants::CAR_WEIGHT / vd_constants::WHEELBASE_m) * LONG_ACCEL_TERM)) / ((vd_constants::CAR_WEIGHT / vd_constants::WHEELBASE_m) * LONG_ACCEL_TERM); } @@ -257,26 +177,14 @@ struct VehicleState * Moment scaling factor F (page 58) * Relates torque differential to yaw moment through track width and effective radius */ - static constexpr float F = (vd_constants::TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * IN_TO_M)) * GEAR_RATIO; -}; + static constexpr T F = (vd_constants::TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * IN_TO_M)) * GEAR_RATIO; -struct ControlOutput -{ - const float fl_kappa; - const float fr_kappa; - const float rl_kappa; - const float rr_kappa; - const float max_torque; - const float min_torque; -}; -struct ControlOutputAutonomous -{ - const float fl_kappa; - const float fr_kappa; - const float rl_kappa; - const float rr_kappa; - const float delta; - const float max_torque; - const float min_torque; + private: + [[nodiscard]] static T safe_vx(const T v_x_mps) + { + if (std::fabs(v_x_mps) >= vd_constants::SMALL_EPSILON) + return v_x_mps; + return v_x_mps < 0.0f ? -vd_constants::SMALL_EPSILON : vd_constants::SMALL_EPSILON; + } }; -} // namespace app::tv::shared_datatypes +} // namespace app::tv::shared_datatypes \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/wheel_set.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/wheel_set.hpp new file mode 100644 index 0000000000..ea626953da --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/wheel_set.hpp @@ -0,0 +1,38 @@ +#pragma once +#include "decimal_dual.hpp" +#include "pair.hpp" +#include + +namespace app::tv::shared_datatypes +{ +template struct wheel_set +{ + T fl; + T fr; + T rl; + T rr; +}; +template struct wheel_set> +{ + Pair fl; + Pair fr; + Pair rl; + Pair rr; + + /** + * rotates each pair in the z-axis by the respective angle in z_rot + * @param z_rot_rad rotation angles for each wheel in radians + */ + template void rotate(const wheel_set &z_rot_rad) + { + fl = { fl.x * std::cos(z_rot_rad.fl) + fl.y * std::sin(z_rot_rad.fl), + fl.y * std::cos(z_rot_rad.fl) - fl.x * std::sin(z_rot_rad.fl) }; + fr = { fr.x * std::cos(z_rot_rad.fr) + fr.y * std::sin(z_rot_rad.fr), + fr.y * std::cos(z_rot_rad.fr) - fr.x * std::sin(z_rot_rad.fr) }; + rl = { rl.x * std::cos(z_rot_rad.rl) + rl.y * std::sin(z_rot_rad.rl), + rl.y * std::cos(z_rot_rad.rl) - rl.x * std::sin(z_rot_rad.rl) }; + rr = { rr.x * std::cos(z_rot_rad.rr) + rr.y * std::sin(z_rot_rad.rr), + rr.y * std::cos(z_rot_rad.rr) - rr.x * std::sin(z_rot_rad.rr) }; + } +}; +} // namespace app::tv::shared_datatypes \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index 15749cda43..20ec37e665 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -1,30 +1,21 @@ #include "torque_vectoring.hpp" +#include "torque_vectoring_matlab.h" // this is just for matlab interface syncing -#include "torque_vectoring/estimation/vehicle_state_estimator.hpp" +#include "shared_datatypes/vehicle_state_estimator.hpp" #include "torque_vectoring/controllers/controllers_dyrc.hpp" #include "torque_vectoring/controllers/torque_allocator.hpp" #include "torque_vectoring/shared_datatypes/constants.hpp" -#include "torque_vectoring/shared_datatypes/datatypes.hpp" -namespace app::tv -{ -using namespace shared_datatypes; +using namespace app::tv::shared_datatypes; using namespace vd_constants; -//------------------------------------- TORQUE VECTORING UPDATE CYCLE -------------------------// -ControlOutput update(const estimation::Measurements &measurement) +template ControlOutput update(const VehicleState &state) { - //------------------------------------- STATE ESTIMATION --------------------------------// - // Fuse the measured chassis state with the previous tire-force estimate to obtain the - // filtered vehicle state used consistently by all downstream control blocks this tick. - const VehicleState state = estimation::VehicleStateEstimator::estimate(measurement); - //------------------------------------- HIGH LEVEL CONTROLLER ----------------------------// - - const float ax_mps2_setpoint = MAX_AX * measurement.apps; + const T ax_mps2_setpoint = MAX_AX * state.apps; // Direct yaw rate control: corrective yaw moment - const float omegadot_radps2_setpoint = - controllers::dyrc::computeYawMoment(state.yaw_rate_radps, state.steer_ang_rad, state.v_x_mps); + const T omegadot_radps2_setpoint = app::tv::controllers::dyrc::computeYawMoment( + state.yaw_rate_radps, (state.delta.fl + state.delta.fr) / 2, state.v_x_mps); //------------------------------------- LOW LEVEL CONTROLLER -----------------------------// @@ -35,29 +26,67 @@ ControlOutput update(const estimation::Measurements &measurement) // const float low_speed_blend = shared_datatypes::velocityBlend(vehicle_speed_mps); // ReSharper disable once CppUseStructuredBinding - const wheel_set kappa_opt = - controllers::TorqueAllocator::optimize(state, ax_mps2_setpoint, omegadot_radps2_setpoint); + const wheel_set kappa_opt = + app::tv::controllers::allocator::optimize(state, ax_mps2_setpoint, omegadot_radps2_setpoint); //------------------------------------- POWER LIMITER -----------------------------------// // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request return { - .fl_kappa = kappa_opt.fl, - .fr_kappa = kappa_opt.fr, - .rl_kappa = kappa_opt.rl, - .rr_kappa = kappa_opt.rr, - .max_torque = 0, - .min_torque = 0, + kappa_opt.fl, kappa_opt.fr, kappa_opt.rl, kappa_opt.rr, 0, 0, }; } +template ControlOutput update(const VehicleState &state); +template ControlOutput update(const VehicleState &state); + +extern "C" void update_matlab( + const double v_x, + const double v_y, + const double yaw_rate, + const double a_x, + const double a_y, + const double apps, + const double delta_fl, + const double delta_fr, + double out[6]) +{ + const VehicleState state={ .v_x_mps = v_x, + .v_y_mps = v_y, + .yaw_rate_radps = yaw_rate, + .a_x_mps2 = a_x, + .a_y_mps2 = a_y, + .apps = apps, + .delta = { + .fl = delta_fl, + .fr = delta_fr, + .rl = 0.0f, + .rr = 0.0f, + }, }; + + const auto [fl_kappa, fr_kappa, rl_kappa, rr_kappa, max_torque, min_torque] = update(state); + out[0] = fl_kappa; + out[1] = fr_kappa; + out[2] = rl_kappa; + out[3] = rr_kappa; + out[4] = max_torque; + out[5] = min_torque; +} -ControlOutputAutonomous - update_autonomous(const estimation::Measurements &measurement, const float ax, const float omega_dot) +template ControlOutputAutonomous update_autonomous(const VehicleState &state) { - (void)measurement; - (void)ax; - (void)omega_dot; + (void)state; // TODO inshallah one day return {}; } -} // namespace app::tv +template ControlOutputAutonomous update_autonomous(const VehicleState &state); +template ControlOutputAutonomous update_autonomous(const VehicleState &state); + +void kappa_update_matlab(double kappas[4], const double v_x, double out[4]) +{ + const auto [fl, fr, rl, rr] = + kappa_update({ .fl = kappas[0], .fr = kappas[1], .rl = kappas[2], .rr = kappas[3] }, v_x); + out[0] = fl; + out[1] = fr; + out[2] = rl; + out[3] = rr; +} diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp index 89f265fe8a..dc2ec5fb2d 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -1,17 +1,59 @@ #pragma once -#include "estimation/vehicle_state_estimator.hpp" -#include "shared_datatypes/datatypes.hpp" +#include "shared_datatypes/constants.hpp" +#include "shared_datatypes/vehicle_state_estimator.hpp" +#include "shared_datatypes/wheel_set.hpp" + +template struct ControlOutput +{ + const T fl_kappa = 0; + const T fr_kappa = 0; + const T rl_kappa = 0; + const T rr_kappa = 0; + const T max_torque = 0; + const T min_torque = 0; +}; + +template struct ControlOutputAutonomous +{ + const T fl_kappa = 0; + const T fr_kappa = 0; + const T rl_kappa = 0; + const T rr_kappa = 0; + const T delta = 0; + const T max_torque = 0; + const T min_torque = 0; +}; /** * This is the main entrypoint into the low level vehicle controls algorithm * @param state The current measured vehicle state, note that intent is in here as well * @return The per-wheel torque requests to achieve the desired accelerations, in Newton-meters */ -app::tv::shared_datatypes::ControlOutput update(const app::tv::estimation::Measurements &state); +template ControlOutput update(const app::tv::shared_datatypes::VehicleState &state); /** * This is the main entrypoint into the low level vehicle controls algorithm for autonomous * @param state The current measured vehicle state, note that intent is in here as well * @return */ -app::tv::shared_datatypes::ControlOutputAutonomous update_autonomous(const app::tv::estimation::Measurements &state); +template +ControlOutputAutonomous update_autonomous(const app::tv::shared_datatypes::VehicleState &state); + +/** + * Given a slip ratio setpopint, gives wheel angular velocity setpoints to achieve that slip ratio at the current + * vehicle speed + * @param kappas slip ratio setpoints for each wheel, where kappa = (wheel_speed - vehicle_speed) / vehicle_speed + * @param v_x_mps current longitudinal vehicle speed in meters per second + * @return wheel velocity setpoints + */ +template +app::tv::shared_datatypes::wheel_set + kappa_update(const app::tv::shared_datatypes::wheel_set &kappas, const T v_x_mps) +{ + return { + .fl = (1.0f + kappas.fl) * (v_x_mps / app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M), + .fr = (1.0f + kappas.fr) * (v_x_mps / app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M), + .rl = (1.0f + kappas.rl) * (v_x_mps / app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M), + .rr = (1.0f + kappas.rr) * (v_x_mps / app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M), + }; +} diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h new file mode 100644 index 0000000000..9cf4557346 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h @@ -0,0 +1,17 @@ +#pragma once + +extern "C" +{ + void update_matlab( + double v_x, + double v_y, + double yaw_rate, + double a_x, + double a_y, + double apps, + double delta_fl, + double delta_fr, + double out[6]); + + void kappa_update_matlab(double kappas[4], double v_x, double out[4]); +} \ No newline at end of file diff --git a/firmware/shared/srcpp/util/util_units.hpp b/firmware/shared/srcpp/util/util_units.hpp index a1f1d34c74..fb872565b0 100644 --- a/firmware/shared/srcpp/util/util_units.hpp +++ b/firmware/shared/srcpp/util/util_units.hpp @@ -1,12 +1,12 @@ #pragma once -#include +#include #include // ============================================================================= // MATHEMATICAL CONSTANTS // ============================================================================= -inline constexpr float M_PI_F = static_cast(M_PI); +inline constexpr float M_PI_F = std::numbers::pi; // ============================================================================= // TIME CONVERSIONS From ba10cbb9490f6290b9a84841318787b101350ed7 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 13 Apr 2026 14:13:42 -0700 Subject: [PATCH 20/35] clean up matlab interface --- .../app/torque_vectoring/torque_vectoring.cpp | 51 +++++++++++-------- .../app/torque_vectoring/torque_vectoring.hpp | 20 +++----- .../torque_vectoring_matlab.h | 10 +++- 3 files changed, 46 insertions(+), 35 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index 20ec37e665..bb9bdf9c6b 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -48,28 +48,37 @@ extern "C" void update_matlab( const double apps, const double delta_fl, const double delta_fr, - double out[6]) + double kappas[4], + double torque_max[4], + double torque_min[4]) { - const VehicleState state={ .v_x_mps = v_x, - .v_y_mps = v_y, - .yaw_rate_radps = yaw_rate, - .a_x_mps2 = a_x, - .a_y_mps2 = a_y, - .apps = apps, - .delta = { - .fl = delta_fl, - .fr = delta_fr, - .rl = 0.0f, - .rr = 0.0f, - }, }; - - const auto [fl_kappa, fr_kappa, rl_kappa, rr_kappa, max_torque, min_torque] = update(state); - out[0] = fl_kappa; - out[1] = fr_kappa; - out[2] = rl_kappa; - out[3] = rr_kappa; - out[4] = max_torque; - out[5] = min_torque; + const VehicleState state = { .v_x_mps = v_x, + .v_y_mps = v_y, + .yaw_rate_radps = yaw_rate, + .a_x_mps2 = a_x, + .a_y_mps2 = a_y, + .apps = apps, + .delta = { + .fl = delta_fl, + .fr = delta_fr, + .rl = 0.0f, + .rr = 0.0f, + } }; + // bring it in + const auto [k_kappas, k_torque_max, k_torque_min] = update(state); + // update + kappas[0] = k_kappas.fl; + kappas[1] = k_kappas.fr; + kappas[2] = k_kappas.rl; + kappas[3] = k_kappas.rr; + torque_max[0] = k_torque_max.fl; + torque_max[1] = k_torque_max.fr; + torque_max[2] = k_torque_max.rl; + torque_max[3] = k_torque_max.rr; + torque_min[0] = k_torque_min.fl; + torque_min[1] = k_torque_min.fr; + torque_min[2] = k_torque_min.rl; + torque_min[3] = k_torque_min.rr; } template ControlOutputAutonomous update_autonomous(const VehicleState &state) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp index dc2ec5fb2d..e16c396817 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -5,23 +5,17 @@ template struct ControlOutput { - const T fl_kappa = 0; - const T fr_kappa = 0; - const T rl_kappa = 0; - const T rr_kappa = 0; - const T max_torque = 0; - const T min_torque = 0; + app::tv::shared_datatypes::wheel_set kappas; + app::tv::shared_datatypes::wheel_set torque_ub; + app::tv::shared_datatypes::wheel_set torque_lb; }; template struct ControlOutputAutonomous { - const T fl_kappa = 0; - const T fr_kappa = 0; - const T rl_kappa = 0; - const T rr_kappa = 0; - const T delta = 0; - const T max_torque = 0; - const T min_torque = 0; + app::tv::shared_datatypes::wheel_set kappas; + app::tv::shared_datatypes::wheel_set torque_ub; + app::tv::shared_datatypes::wheel_set torque_lb; + const T delta = 0; }; /** diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h index 9cf4557346..746b676140 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h @@ -2,6 +2,9 @@ extern "C" { + /** + * Matlab Wrapper for update + */ void update_matlab( double v_x, double v_y, @@ -11,7 +14,12 @@ extern "C" double apps, double delta_fl, double delta_fr, - double out[6]); + double kappas[4], + double torque_max[4], + double torque_min[4]); + /** + * Matlab wrapper for kappa_update + */ void kappa_update_matlab(double kappas[4], double v_x, double out[4]); } \ No newline at end of file From 32ceac5b79c0df0d3092dc54c87ce1d8c4341710 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 13 Apr 2026 14:30:35 -0700 Subject: [PATCH 21/35] small mistake --- .../hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index bb9bdf9c6b..560c7a2eb4 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -32,9 +32,7 @@ template ControlOutput update(const VehicleState &state) //------------------------------------- POWER LIMITER -----------------------------------// // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request - return { - kappa_opt.fl, kappa_opt.fr, kappa_opt.rl, kappa_opt.rr, 0, 0, - }; + return { { kappa_opt.fl, kappa_opt.fr, kappa_opt.rl, kappa_opt.rr }, { 0, 0, 0, 0 }, { 0, 0, 0, 0 } }; } template ControlOutput update(const VehicleState &state); template ControlOutput update(const VehicleState &state); From f2d8942ea9263cc99f23f1f97d7f99bc7260efa4 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 13 Apr 2026 16:20:23 -0700 Subject: [PATCH 22/35] matplotlib and others --- firmware/CMakeLists.txt | 3 +- firmware/cmake/dependencies.cmake | 56 +++++++++++-------- firmware/cmake/matplotlib.cmake | 20 +++++++ firmware/hexray/VC/CMakeLists.txt | 19 ++++++- firmware/hexray/VC/plot_tests.cpp | 48 ++++++++++++++++ .../controllers/torque_allocator.cpp | 7 +-- .../estimation/tire_model.cpp | 15 +++-- .../shared_datatypes/constants.hpp | 2 +- .../app/torque_vectoring/torque_vectoring.cpp | 6 +- .../app/torque_vectoring/torque_vectoring.hpp | 8 +-- firmware/shared/srcpp/util/util_utils.hpp | 14 ++++- 11 files changed, 152 insertions(+), 46 deletions(-) create mode 100644 firmware/cmake/matplotlib.cmake create mode 100644 firmware/hexray/VC/plot_tests.cpp diff --git a/firmware/CMakeLists.txt b/firmware/CMakeLists.txt index 8b96a2d5d7..841e35eb13 100644 --- a/firmware/CMakeLists.txt +++ b/firmware/CMakeLists.txt @@ -1,7 +1,8 @@ include(cmake/dependencies.cmake) include(../scripts/code_generation/commit_info_gen/commit_info.cmake) -include(cmake/eigenlib.cmake) +include(cmake/eigenlib.cmake) include(cmake/autodiff.cmake) +include(cmake/matplotlib.cmake) IF ("${TARGET}" STREQUAL "binary") option(BOOTLOAD "Build the bootloader" OFF) diff --git a/firmware/cmake/dependencies.cmake b/firmware/cmake/dependencies.cmake index ce7e654bc4..6a36a068d9 100644 --- a/firmware/cmake/dependencies.cmake +++ b/firmware/cmake/dependencies.cmake @@ -66,19 +66,19 @@ IF ("${TARGET}" STREQUAL "binary") ) # Autodiff library: contains header only apis to automatically compute derrivatives of functions CPMAddPackage( - NAME AUTO_DIFF - GITHUB_REPOSITORY autodiff/autodiff - GIT_TAG 2e2f3a2b16afcd9c04e76c8a689e9fd23ff78679 - GIT_SHALLOW TRUE - DOWNLOAD_ONLY TRUE + NAME AUTO_DIFF + GITHUB_REPOSITORY autodiff/autodiff + GIT_TAG 2e2f3a2b16afcd9c04e76c8a689e9fd23ff78679 + GIT_SHALLOW TRUE + DOWNLOAD_ONLY TRUE ) # Eigen Library: provides support for matrices and matrix algebra CPMAddPackage( - NAME EIGEN - GIT_REPOSITORY https://gitlab.com/libeigen/eigen - GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c - GIT_SHALLOW TRUE - DOWNLOAD_ONLY TRUE + NAME EIGEN + GIT_REPOSITORY https://gitlab.com/libeigen/eigen + GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c + GIT_SHALLOW TRUE + DOWNLOAD_ONLY TRUE ) ELSEIF ("${TARGET}" STREQUAL "test") # Fetch GoogleTest for unit testing. @@ -90,29 +90,37 @@ ELSEIF ("${TARGET}" STREQUAL "test") ) # Autodiff library: contains header only apis to automatically compute derrivatives of functions CPMAddPackage( - NAME AUTO_DIFF - GITHUB_REPOSITORY autodiff/autodiff - GIT_TAG 2e2f3a2b16afcd9c04e76c8a689e9fd23ff78679 - GIT_SHALLOW TRUE - DOWNLOAD_ONLY TRUE + NAME AUTO_DIFF + GITHUB_REPOSITORY autodiff/autodiff + GIT_TAG 2e2f3a2b16afcd9c04e76c8a689e9fd23ff78679 + GIT_SHALLOW TRUE + DOWNLOAD_ONLY TRUE ) # Eigen Library: provides support for matrices and matrix algebra CPMAddPackage( - NAME EIGEN - GIT_REPOSITORY https://gitlab.com/libeigen/eigen - GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c - GIT_SHALLOW TRUE - DOWNLOAD_ONLY TRUE + NAME EIGEN + GIT_REPOSITORY https://gitlab.com/libeigen/eigen + GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c + GIT_SHALLOW TRUE + DOWNLOAD_ONLY TRUE ) # pybind 11: library for creating python bindings for C++ code CPMAddPackage( - NAME pybind11 - GIT_REPOSITORY https://github.com/pybind/pybind11.git - VERSION 3.0.2 - DOWNLOAD_ONLY TRUE + NAME pybind11 + GIT_REPOSITORY https://github.com/pybind/pybind11.git + VERSION 3.0.2 + DOWNLOAD_ONLY TRUE ) add_subdirectory(${pybind11_SOURCE_DIR} ${pybind11_BINARY_DIR}) + + CPMAddPackage( + NAME matplotlibcpp + GITHUB_REPOSITORY lava/matplotlib-cpp + GIT_TAG ef0383f1315d32e0156335e10b82e90b334f6d9f + GIT_SHALLOW TRUE + DOWNLOAD_ONLY TRUE + ) ENDIF () # protobufs diff --git a/firmware/cmake/matplotlib.cmake b/firmware/cmake/matplotlib.cmake new file mode 100644 index 0000000000..351e6efd80 --- /dev/null +++ b/firmware/cmake/matplotlib.cmake @@ -0,0 +1,20 @@ +message("") +message("Configuring matplotlib-cpp...") + +# Library target +find_package(Python3 COMPONENTS Development REQUIRED) +add_library(matplotlib_cpp INTERFACE) +target_include_directories(matplotlib_cpp INTERFACE ${matplotlibcpp_SOURCE_DIR}) +target_link_libraries(matplotlib_cpp INTERFACE + Python3::Python + Python3::Module +) +#find_package(Python3 COMPONENTS NumPy) +#if (Python3_NumPy_FOUND) +# target_link_libraries(matplotlib_cpp INTERFACE +# Python3::NumPy +# ) +#else () +message(WARNING "NumPy not found. Matplotlib-cpp will be built without NumPy support.") +target_compile_definitions(matplotlib_cpp INTERFACE WITHOUT_NUMPY) +#endif () \ No newline at end of file diff --git a/firmware/hexray/VC/CMakeLists.txt b/firmware/hexray/VC/CMakeLists.txt index 1837be2c5c..854e5987b9 100644 --- a/firmware/hexray/VC/CMakeLists.txt +++ b/firmware/hexray/VC/CMakeLists.txt @@ -145,7 +145,8 @@ elseif ("${TARGET}" STREQUAL "test") ) set(INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/test" - ${APP_INCLUDE_DIRS} ${SHARED_FAKES_DIR_CPP} ${IO_INCLUDE_DIRS} ${TEST_INCLUDE_DIRS} ${SYSTEM_INCLUDE_DIRS} ${SHARED_UTIL_INCLUDE_DIR_CPP} + ${APP_INCLUDE_DIRS} ${SHARED_FAKES_DIR_CPP} ${IO_INCLUDE_DIRS} ${TEST_INCLUDE_DIRS} + ${SYSTEM_INCLUDE_DIRS} ${SHARED_UTIL_INCLUDE_DIR_CPP} ) compile_gtest_executable( "hexray_VC_test" @@ -169,6 +170,7 @@ elseif ("${TARGET}" STREQUAL "test") target_compile_definitions("hexray_VC_test" PRIVATE STM32H733xx) + # MATLAB STATIC LIBRARY file(GLOB_RECURSE TV_SRCS CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/*.cpp" "${SHARED_APP_INCLUDE_DIR_CPP}/app_pid.cpp" @@ -177,4 +179,19 @@ elseif ("${TARGET}" STREQUAL "test") target_include_directories("hexray_VC_torque_vectoring" PUBLIC "${INCLUDE_DIRS}") target_link_libraries("hexray_VC_torque_vectoring" PUBLIC "autodiff_interface" "eigen_interface" "m") target_compile_options("hexray_VC_torque_vectoring" PRIVATE /MT) + + + # MATPLOTLIB TESTING + add_executable(test_controls) + target_sources(test_controls PRIVATE + "${CMAKE_CURRENT_SOURCE_DIR}/plot_tests.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/estimation/tire_model.cpp" + ) + target_include_directories(test_controls PRIVATE + "${CMAKE_CURRENT_SOURCE_DIR}/src/app" + ${SHARED_UTIL_INCLUDE_DIR_CPP} + ) + target_link_libraries(test_controls PRIVATE "matplotlib_cpp" "autodiff_interface") + target_compile_options(test_controls PRIVATE /Zi) + target_link_options(test_controls PRIVATE /DEBUG /INCREMENTAL:NO) endif () \ No newline at end of file diff --git a/firmware/hexray/VC/plot_tests.cpp b/firmware/hexray/VC/plot_tests.cpp new file mode 100644 index 0000000000..93161e146f --- /dev/null +++ b/firmware/hexray/VC/plot_tests.cpp @@ -0,0 +1,48 @@ +// ReSharper disable CppTooWideScope +#include "matplotlibcpp.h" +#include "torque_vectoring/estimation/tire_model.hpp" + +#include +#include +#include + +namespace plt = matplotlibcpp; + +void plot_combined_fx() +{ + constexpr int n_points = 301; // includes endpoints + constexpr float fz_N = 1000.0f; + + std::vector kappas; + kappas.reserve(n_points); + for (int i = 0; i < n_points; ++i) + { + constexpr double kappa_max = 0.6f; + constexpr double kappa_min = -0.6f; + const double t = static_cast(i) / static_cast(n_points - 1); + const double kappa = kappa_min + t * (kappa_max - kappa_min); + kappas.push_back(kappa); + } + + for (const float alpha_rad : { 0.3f, 0.25f, 0.2f, 0.15f, 0.1f, 0.05f, 0.0f }) + { + std::vector fxs; + fxs.reserve(kappas.size()); + for (const double kappa : kappas) + { + fxs.push_back(app::tv::estimation::tire_model.computeCombinedFx_N(fz_N, alpha_rad, kappa)); + } + std::stringstream ss; + ss << "\\alpha = " << std::setprecision(2) << alpha_rad; + plt::named_plot(ss.str(), kappas, fxs); + } + + plt::title("F_x as a function of \\kappa and \\alpha"); + plt::xlabel("\\kappa"); + plt::ylabel("F_x (N)"); + plt::grid(true); + plt::legend(); + plt::show(); +} + +int main() {} \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index de28bda2d8..9b169d63eb 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -81,11 +81,10 @@ template // // We keep all vectors/matrices fixed-size (4 decision variables, 2 residuals) so the optimizer // stays allocation-free and predictable on embedded targets. - const auto residualVector = [&](const DualVec4 &kappa) -> DualVec2 + const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); + const auto [alpha_fl, alpha_fr, alpha_rl, alpha_rr] = state.alphas(); + const auto residualVector = [&](const DualVec4 &kappa) -> DualVec2 { - const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); - const auto [alpha_fl, alpha_fr, alpha_rl, alpha_rr] = state.alphas(); - const wheel_set> predicted_f{ { estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)), diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index 6283adbab8..a9c5a9b27e 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -69,10 +69,9 @@ template // Pacejka Page 181 (4.E50): F_x = G_xa * F_x0 return coefficients.g_xa * pure_fx_0; } -template float TireModel::computeCombinedFx_N( - const float normal_load_N, - const float slip_angle_rad, - const float &slip_ratio) const; +template float TireModel::computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const float &slip_ratio) const; +template double + TireModel::computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const double &slip_ratio) const; template autodiff::dual TireModel::computeCombinedFx_N( float normal_load_N, float slip_angle_rad, @@ -90,10 +89,10 @@ template // Pacejka Pages 181-182 (4.E58): F_y = G_yk * F_y0 + S_vyk return coefficients.g_yk * T(pure_fy_0) + coefficients.s_vyk; } -template float TireModel::computeCombinedFy_N( - const float normal_load_N, - const float slip_angle_rad, - const float &slip_ratio) const; +template float + TireModel::computeCombinedFy_N(float normal_load_N, float slip_angle_rad, const float &slip_ratio) const; +template double + TireModel::computeCombinedFy_N(float normal_load_N, float slip_angle_rad, const double &slip_ratio) const; template autodiff::dual TireModel::computeCombinedFy_N( float normal_load_N, float slip_angle_rad, diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index c6e1ea41a0..f9cac0de5f 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -94,7 +94,7 @@ inline constexpr float SLIP_RATIO_IDEAL = 0.05f; // Ideal slip ratio for maximum // TODO: Replace with reverse/anti-Ackermann model inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = 0.3f; // Note: Underestimate for wheel angles > 40° (see Confluence/Steering System) -inline constexpr float MAX_AX = 10; // TODO idk this number bruh +inline constexpr float MAX_AX_MPS2 = 10; // TODO idk this number bruh // ============================================================================= // UTILITY FUNCTIONS & CONVERSION HELPERS diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index 560c7a2eb4..bcc01c5900 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -12,7 +12,7 @@ using namespace vd_constants; template ControlOutput update(const VehicleState &state) { //------------------------------------- HIGH LEVEL CONTROLLER ----------------------------// - const T ax_mps2_setpoint = MAX_AX * state.apps; + const T ax_mps2_setpoint = MAX_AX_MPS2 * state.apps; // Direct yaw rate control: corrective yaw moment const T omegadot_radps2_setpoint = app::tv::controllers::dyrc::computeYawMoment( state.yaw_rate_radps, (state.delta.fl + state.delta.fr) / 2, state.v_x_mps); @@ -32,7 +32,9 @@ template ControlOutput update(const VehicleState &state) //------------------------------------- POWER LIMITER -----------------------------------// // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request - return { { kappa_opt.fl, kappa_opt.fr, kappa_opt.rl, kappa_opt.rr }, { 0, 0, 0, 0 }, { 0, 0, 0, 0 } }; + return { { kappa_opt.fl, kappa_opt.fr, kappa_opt.rl, kappa_opt.rr }, + { ax_mps2_setpoint, omegadot_radps2_setpoint, 0, 0 }, + { 0, 0, 0, 0 } }; } template ControlOutput update(const VehicleState &state); template ControlOutput update(const VehicleState &state); diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp index e16c396817..35bae0337e 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -6,15 +6,15 @@ template struct ControlOutput { app::tv::shared_datatypes::wheel_set kappas; - app::tv::shared_datatypes::wheel_set torque_ub; - app::tv::shared_datatypes::wheel_set torque_lb; + app::tv::shared_datatypes::wheel_set torque_max; + app::tv::shared_datatypes::wheel_set torque_min; }; template struct ControlOutputAutonomous { app::tv::shared_datatypes::wheel_set kappas; - app::tv::shared_datatypes::wheel_set torque_ub; - app::tv::shared_datatypes::wheel_set torque_lb; + app::tv::shared_datatypes::wheel_set torque_max; + app::tv::shared_datatypes::wheel_set torque_min; const T delta = 0; }; diff --git a/firmware/shared/srcpp/util/util_utils.hpp b/firmware/shared/srcpp/util/util_utils.hpp index 5634841192..93792c59cf 100644 --- a/firmware/shared/srcpp/util/util_utils.hpp +++ b/firmware/shared/srcpp/util/util_utils.hpp @@ -18,7 +18,19 @@ #define IS_IN_RANGE(min, max, val) (((val) > (min)) && ((val) < (max))) /* @brief Extract the basename from a file path */ -#define __BASENAME__(path) (__builtin_strrchr(path, '/') ? __builtin_strrchr(path, '/') + 1 : path) +constexpr const char *filename_only(const char *path) +{ + const char *last_slash = path; + for (const char *it = path; *it != '\0'; ++it) + { + if (*it == '/' || *it == '\\') + { + last_slash = it + 1; + } + } + return last_slash; +} +#define __BASENAME__(x) filename_only(x) // Extra guard because HAL defines the same macro #ifndef UNUSED From 2dd61e9b3e954433af55db465826533ab6e11a58 Mon Sep 17 00:00:00 2001 From: Aditya-Dhiman4 Date: Wed, 15 Apr 2026 01:34:19 -0700 Subject: [PATCH 23/35] fixed torque bounds plus multiplied speed request by gear ratio to make it a motor speed request --- .../controllers/torque_allocator.cpp | 21 +++++++++++++++---- .../app/torque_vectoring/torque_vectoring.cpp | 5 +++-- .../app/torque_vectoring/torque_vectoring.hpp | 14 +++++++++---- 3 files changed, 30 insertions(+), 10 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index 9b169d63eb..2de3356878 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "torque_vectoring/shared_datatypes/constants.hpp" #include "torque_vectoring/estimation/tire_model.hpp" @@ -17,8 +18,9 @@ namespace app::tv::controllers::allocator namespace { // ---- Optimizer tuning ---- - constexpr float W_FX = 2.0f / 3.0f; - constexpr float W_MZ = 1.0f / 3.0f; + constexpr float ALPHA = 0.0f; + constexpr float W_FX = 1.0f - ALPHA; + constexpr float W_MZ = ALPHA; constexpr int MAX_ITER = 8; [[maybe_unused]] constexpr float SLIP_CLAMP = 0.3f; @@ -147,11 +149,19 @@ template const Eigen::LDLT ldlt(normal_matrix); if (ldlt.info() != Eigen::Success) - break; + { + std::printf("optimizer :( 1\n"); + std::fflush(stdout); + break; + } const Vec4f delta = ldlt.solve(rhs); if (!delta.allFinite()) - break; + { + std::printf("optimizer :( 2\n"); + std::fflush(stdout); + break; + } opt_slip += delta; // I would recommend not doing this @@ -171,6 +181,9 @@ template previous_cost = cost; } + std::printf("optimized :)\n"); + std::fflush(stdout); + return { .fl = opt_slip(0), .fr = opt_slip(1), diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index bcc01c5900..d9e20d4358 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -33,8 +33,8 @@ template ControlOutput update(const VehicleState &state) // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request return { { kappa_opt.fl, kappa_opt.fr, kappa_opt.rl, kappa_opt.rr }, - { ax_mps2_setpoint, omegadot_radps2_setpoint, 0, 0 }, - { 0, 0, 0, 0 } }; + { 5, 5, 5, 5 }, + { -5, -5, -5, -5 } }; } template ControlOutput update(const VehicleState &state); template ControlOutput update(const VehicleState &state); @@ -66,6 +66,7 @@ extern "C" void update_matlab( } }; // bring it in const auto [k_kappas, k_torque_max, k_torque_min] = update(state); + // std::cout << "DIH" << std::endl; // update kappas[0] = k_kappas.fl; kappas[1] = k_kappas.fr; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp index 35bae0337e..e39bb141da 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -2,6 +2,7 @@ #include "shared_datatypes/constants.hpp" #include "shared_datatypes/vehicle_state_estimator.hpp" #include "shared_datatypes/wheel_set.hpp" +// #include template struct ControlOutput { @@ -44,10 +45,15 @@ template app::tv::shared_datatypes::wheel_set kappa_update(const app::tv::shared_datatypes::wheel_set &kappas, const T v_x_mps) { + // std::cout << "Vx: " << v_x_mps << std::endl; + // std::cout << "Kappas: " << kappas.fl << kappas.fr << kappas.rl << kappas.rr << std::endl; + + T v_x_mps_capped = std::max(v_x_mps, static_cast(1)); + return { - .fl = (1.0f + kappas.fl) * (v_x_mps / app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M), - .fr = (1.0f + kappas.fr) * (v_x_mps / app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M), - .rl = (1.0f + kappas.rl) * (v_x_mps / app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M), - .rr = (1.0f + kappas.rr) * (v_x_mps / app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M), + .fl = GEAR_RATIO * (static_cast(1) + kappas.fl) * (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), + .fr = GEAR_RATIO * (static_cast(1) + kappas.fr) * (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), + .rl = GEAR_RATIO * (static_cast(1) + kappas.rl) * (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), + .rr = GEAR_RATIO * (static_cast(1) + kappas.rr) * (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), }; } From a182398a03af8f7c96966bd613bcbd36c26f994b Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 16 Apr 2026 00:17:49 -0700 Subject: [PATCH 24/35] tweaks for ta --- firmware/hexray/VC/CMakeLists.txt | 1 + firmware/hexray/VC/plot_tests.cpp | 12 ++- .../controllers/torque_allocator.cpp | 102 +++++++++++------- firmware/shared/srcpp/util/util_utils.hpp | 4 + 4 files changed, 77 insertions(+), 42 deletions(-) diff --git a/firmware/hexray/VC/CMakeLists.txt b/firmware/hexray/VC/CMakeLists.txt index 854e5987b9..e853f5f232 100644 --- a/firmware/hexray/VC/CMakeLists.txt +++ b/firmware/hexray/VC/CMakeLists.txt @@ -186,6 +186,7 @@ elseif ("${TARGET}" STREQUAL "test") target_sources(test_controls PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/plot_tests.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/estimation/tire_model.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/controllers/torque_allocator.cpp" ) target_include_directories(test_controls PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/src/app" diff --git a/firmware/hexray/VC/plot_tests.cpp b/firmware/hexray/VC/plot_tests.cpp index 93161e146f..fed2969248 100644 --- a/firmware/hexray/VC/plot_tests.cpp +++ b/firmware/hexray/VC/plot_tests.cpp @@ -1,6 +1,7 @@ // ReSharper disable CppTooWideScope #include "matplotlibcpp.h" #include "torque_vectoring/estimation/tire_model.hpp" +#include "torque_vectoring/controllers/torque_allocator.hpp" #include #include @@ -8,7 +9,7 @@ namespace plt = matplotlibcpp; -void plot_combined_fx() +static void plot_combined_fx() { constexpr int n_points = 301; // includes endpoints constexpr float fz_N = 1000.0f; @@ -45,4 +46,11 @@ void plot_combined_fx() plt::show(); } -int main() {} \ No newline at end of file +int main() +{ + constexpr app::tv::shared_datatypes::VehicleState state{ + .v_x_mps = 0, .v_y_mps = 0, .yaw_rate_radps = 0, .a_x_mps2 = 1, .a_y_mps2 = 0, .apps = 0.2, .delta = 0 + }; + const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0, 0.0); + std::cout << fl << " " << fr << " " << rl << " " << rr << std::endl; +} \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index 2de3356878..f3ecff2a26 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -10,6 +10,8 @@ #include "torque_vectoring/shared_datatypes/constants.hpp" #include "torque_vectoring/estimation/tire_model.hpp" + +#include using namespace app::tv::shared_datatypes; using namespace app::tv::shared_datatypes::vd_constants; @@ -18,9 +20,11 @@ namespace app::tv::controllers::allocator namespace { // ---- Optimizer tuning ---- - constexpr float ALPHA = 0.0f; - constexpr float W_FX = 1.0f - ALPHA; - constexpr float W_MZ = ALPHA; + constexpr float W_FX = 0.5f; + constexpr float W_MZ = 0.0f; + constexpr float W_R = 0.5f; // regularization weight on slip sum to encourage convergence in edge cases where the + // problem is underdetermined + static_assert(W_FX + W_MZ + W_R == 1.0f, "Weights must sum to 1"); constexpr int MAX_ITER = 8; [[maybe_unused]] constexpr float SLIP_CLAMP = 0.3f; @@ -28,12 +32,15 @@ namespace constexpr float STEP_TOLERANCE = 1e-5f; constexpr float COST_TOLERANCE = 1e-6f; - using Vec2f = Eigen::Matrix; + using Vec6f = Eigen::Matrix; using Vec4f = Eigen::Matrix; - using Mat24f = Eigen::Matrix; + using Mat64f = Eigen::Matrix; using Mat44f = Eigen::Matrix; - using DualVec2 = Eigen::Matrix; + using DualVec6 = Eigen::Matrix; using DualVec4 = Eigen::Matrix; + + // print the normal_matrix + const Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]"); } // namespace template @@ -53,6 +60,7 @@ template static const float SQRT_W_FX = std::sqrt(W_FX); static const float SQRT_W_MZ = std::sqrt(W_MZ); + static const float SQRT_W_R = std::sqrt(W_R); // const wheel_set blended_des_f_x{ // .fl = low_speed_blend * des_f_x.fl, @@ -85,7 +93,7 @@ template // stays allocation-free and predictable on embedded targets. const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); const auto [alpha_fl, alpha_fr, alpha_rl, alpha_rr] = state.alphas(); - const auto residualVector = [&](const DualVec4 &kappa) -> DualVec2 + const auto residualVector = [&](const DualVec4 &kappa) -> DualVec6 { const wheel_set> predicted_f{ { @@ -107,13 +115,15 @@ template }; const autodiff::dual sum_fx = predicted_f.fl.x + predicted_f.fr.x + predicted_f.rl.x + predicted_f.rr.x; const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); - return DualVec2{ - SQRT_W_FX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint), - SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), - }; + return DualVec6{ SQRT_W_FX * sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint, + SQRT_W_MZ * predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint, + SQRT_W_R * kappa[0], + SQRT_W_R * kappa[1], + SQRT_W_R * kappa[2], + SQRT_W_R * kappa[3] }; }; - Vec4f opt_slip{ 0, 0, 0, 0 }; // output variable + Vec4f opt_slip{ 0.1, 0.1, 0.1, 0.1 }; // output variable float previous_cost = std::numeric_limits::infinity(); @@ -126,43 +136,58 @@ template autodiff::dual(opt_slip(3)), }; // evaluate and calculate jacobian at kappa - DualVec2 residual_at_kappa; - Mat24f jacobian_residual_at_kappa; + DualVec6 residual_at_kappa; + Mat64f jacobian_residual_at_kappa; autodiff::jacobian( residualVector, autodiff::wrt(kappa), autodiff::at(kappa), residual_at_kappa, jacobian_residual_at_kappa); - - // Gauss-Newton solves: - // (J^T J) * delta = -J^T r - // where r is the residual vector and J is dr/dkappa at the current trial slip. - // - // J comes directly from autodiff::jacobian(...), so we do not hand-derive per-wheel slopes - // or yaw-moment sensitivities. The optimizer stays readable: define residuals first, then - // let autodiff provide the linearization used by Gauss-Newton. - Mat44f normal_matrix = jacobian_residual_at_kappa.transpose() * jacobian_residual_at_kappa; - normal_matrix.diagonal().array() += NORMAL_MATRIX_EPS; - - const Vec2f residuals_at_kappa_primal{ + const Vec6f residuals_at_kappa_primal{ static_cast(autodiff::val(residual_at_kappa(0))), static_cast(autodiff::val(residual_at_kappa(1))), + static_cast(autodiff::val(residual_at_kappa(2))), + static_cast(autodiff::val(residual_at_kappa(3))), + static_cast(autodiff::val(residual_at_kappa(4))), + static_cast(autodiff::val(residual_at_kappa(5))), }; + // print residual_at_kapp and jacobian_residual_at_kappa + std::cout << "==============Iteration " << iter + << "===============\n" + "Kappa: \n" + << opt_slip.format(CleanFmt) + << "\n\n" + "Residual at kappa:\n" + << residuals_at_kappa_primal.format(CleanFmt) + << "\n\n" + "Jacobian of residual at kappa:\n" + << jacobian_residual_at_kappa.format(CleanFmt) << std::endl; + + Mat44f normal_matrix = jacobian_residual_at_kappa.transpose() * jacobian_residual_at_kappa; + normal_matrix.diagonal().array() += NORMAL_MATRIX_EPS; // condition problem lmao const Vec4f rhs = -jacobian_residual_at_kappa.transpose() * residuals_at_kappa_primal; const Eigen::LDLT ldlt(normal_matrix); - if (ldlt.info() != Eigen::Success) - { - std::printf("optimizer :( 1\n"); - std::fflush(stdout); - break; - } + { + std::cout << "Normal Matrix:\n" << normal_matrix.format(CleanFmt) << std::endl; + // eigenvalues + const Eigen::EigenSolver es(normal_matrix); + std::cout << "Eigenvalues:\n" << es.eigenvalues() << std::endl; + // condition number + const Eigen::JacobiSVD svd(normal_matrix); + double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); + std::cout << "Condition number: " << cond << std::endl; + std::printf("optimizer :( 1, %d\n", ldlt.info()); + std::fflush(stdout); + break; + } const Vec4f delta = ldlt.solve(rhs); if (!delta.allFinite()) - { - std::printf("optimizer :( 2\n"); - std::fflush(stdout); - break; - } + { + std::printf("optimizer :( 2\n"); + std::fflush(stdout); + break; + } + std::cout << "Delta: \n" << delta.format(CleanFmt) << std::endl; opt_slip += delta; // I would recommend not doing this // consider the following: a large step is required which leaves the clamp space @@ -181,9 +206,6 @@ template previous_cost = cost; } - std::printf("optimized :)\n"); - std::fflush(stdout); - return { .fl = opt_slip(0), .fr = opt_slip(1), diff --git a/firmware/shared/srcpp/util/util_utils.hpp b/firmware/shared/srcpp/util/util_utils.hpp index 93792c59cf..b8618cedc5 100644 --- a/firmware/shared/srcpp/util/util_utils.hpp +++ b/firmware/shared/srcpp/util/util_utils.hpp @@ -18,6 +18,7 @@ #define IS_IN_RANGE(min, max, val) (((val) > (min)) && ((val) < (max))) /* @brief Extract the basename from a file path */ +#ifdef _MSC_VER constexpr const char *filename_only(const char *path) { const char *last_slash = path; @@ -31,6 +32,9 @@ constexpr const char *filename_only(const char *path) return last_slash; } #define __BASENAME__(x) filename_only(x) +#else +#define __BASENAME__(path) (__builtin_strrchr(path, '/') ? __builtin_strrchr(path, '/') + 1 : path) +#endif // Extra guard because HAL defines the same macro #ifndef UNUSED From 68b41ff4277813b3bed69fbab060c79336e98b90 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 16 Apr 2026 00:57:28 -0700 Subject: [PATCH 25/35] WORKS?S??????? --- firmware/hexray/VC/plot_tests.cpp | 8 +- .../controllers/torque_allocator.cpp | 88 +++++++++++-------- 2 files changed, 58 insertions(+), 38 deletions(-) diff --git a/firmware/hexray/VC/plot_tests.cpp b/firmware/hexray/VC/plot_tests.cpp index fed2969248..8041d68b06 100644 --- a/firmware/hexray/VC/plot_tests.cpp +++ b/firmware/hexray/VC/plot_tests.cpp @@ -49,7 +49,13 @@ static void plot_combined_fx() int main() { constexpr app::tv::shared_datatypes::VehicleState state{ - .v_x_mps = 0, .v_y_mps = 0, .yaw_rate_radps = 0, .a_x_mps2 = 1, .a_y_mps2 = 0, .apps = 0.2, .delta = 0 + .v_x_mps = 0, + .v_y_mps = 0, + .yaw_rate_radps = 0, + .a_x_mps2 = 1, + .a_y_mps2 = 0, + .apps = 0.2, + .delta = 0, }; const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0, 0.0); std::cout << fl << " " << fr << " " << rl << " " << rr << std::endl; diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index f3ecff2a26..2896a7596b 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -20,11 +20,9 @@ namespace app::tv::controllers::allocator namespace { // ---- Optimizer tuning ---- - constexpr float W_FX = 0.5f; - constexpr float W_MZ = 0.0f; - constexpr float W_R = 0.5f; // regularization weight on slip sum to encourage convergence in edge cases where the - // problem is underdetermined - static_assert(W_FX + W_MZ + W_R == 1.0f, "Weights must sum to 1"); + constexpr double W_FX = 0.5f; + constexpr double W_MZ = 0.0f; + constexpr double W_R = 15.5f; constexpr int MAX_ITER = 8; [[maybe_unused]] constexpr float SLIP_CLAMP = 0.3f; @@ -32,15 +30,27 @@ namespace constexpr float STEP_TOLERANCE = 1e-5f; constexpr float COST_TOLERANCE = 1e-6f; - using Vec6f = Eigen::Matrix; - using Vec4f = Eigen::Matrix; - using Mat64f = Eigen::Matrix; - using Mat44f = Eigen::Matrix; + using Vec6f = Eigen::Matrix; + using Vec4f = Eigen::Matrix; + using Mat64f = Eigen::Matrix; + using Mat44f = Eigen::Matrix; using DualVec6 = Eigen::Matrix; using DualVec4 = Eigen::Matrix; // print the normal_matrix - const Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]"); + const Eigen::IOFormat CleanFmt(5, 0, ", ", "\n", "[", "]"); + + void debug(const Mat44f &normal_matrix) + { + std::cout << "Normal Matrix:\n" << normal_matrix.format(CleanFmt) << std::endl; + // eigenvalues + const Eigen::EigenSolver es(normal_matrix); + std::cout << "Eigenvalues:\n" << es.eigenvalues() << std::endl; + // condition number + const Eigen::JacobiSVD svd(normal_matrix); + const double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); + std::cout << "Condition number: " << cond << std::endl; + } } // namespace template @@ -58,6 +68,8 @@ template // return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; // } + std::cout << ax_setpoint << " " << omegadot_setpoint << std::endl; + static const float SQRT_W_FX = std::sqrt(W_FX); static const float SQRT_W_MZ = std::sqrt(W_MZ); static const float SQRT_W_R = std::sqrt(W_R); @@ -97,37 +109,47 @@ template { const wheel_set> predicted_f{ { - estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)), + estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)), estimation::tire_model.computeCombinedFy_N(fz_fl, alpha_fl, kappa(0)), }, { - estimation::tire_model.computeCombinedFx_N(fz_fr, alpha_fr, kappa(1)), + estimation::tire_model.computeCombinedFx_N(fz_fr, alpha_fr, kappa(1)), estimation::tire_model.computeCombinedFy_N(fz_fr, alpha_fr, kappa(1)), }, { - estimation::tire_model.computeCombinedFx_N(fz_rl, alpha_rl, kappa(2)), + estimation::tire_model.computeCombinedFx_N(fz_rl, alpha_rl, kappa(2)), estimation::tire_model.computeCombinedFy_N(fz_rl, alpha_rl, kappa(2)), }, { - estimation::tire_model.computeCombinedFx_N(fz_rr, alpha_rr, kappa(3)), + estimation::tire_model.computeCombinedFx_N(fz_rr, alpha_rr, kappa(3)), estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)), }, }; const autodiff::dual sum_fx = predicted_f.fl.x + predicted_f.fr.x + predicted_f.rl.x + predicted_f.rr.x; const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); - return DualVec6{ SQRT_W_FX * sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint, - SQRT_W_MZ * predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint, + // print out all forces and their gradients + // std::cout << "Predicted forces at kappa:\n" + // << "FL: " << predicted_f.fl.x << " N, dFL/dkappa: " << autodiff::derivative(predicted_f.fl.x) + // << "\nFR: " << predicted_f.fr.x << " N, dFR/dkappa: " << autodiff::derivative(predicted_f.fr.x) + // << "\nRL: " << predicted_f.rl.x << " N, dRL/dkappa: " << autodiff::derivative(predicted_f.rl.x) + // << "\nRR: " << predicted_f.rr.x << " N, dRR/dkappa: " << autodiff::derivative(predicted_f.rr.x) + // << std::endl; + // print out sum_fx and its gradient + // std::cout << "Sum Fx: " << sum_fx << " N, d(Sum Fx)/dkappa: " << autodiff::derivative(sum_fx) << std::endl; + return DualVec6{ SQRT_W_FX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint), + SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), SQRT_W_R * kappa[0], SQRT_W_R * kappa[1], SQRT_W_R * kappa[2], SQRT_W_R * kappa[3] }; }; - Vec4f opt_slip{ 0.1, 0.1, 0.1, 0.1 }; // output variable + Vec4f opt_slip{ 0, 0, 0, 0 }; // output variable float previous_cost = std::numeric_limits::infinity(); - for (uint32_t iter = 0; iter < MAX_ITER; ++iter) + uint32_t iter; + for (iter = 0; iter < MAX_ITER; ++iter) { DualVec4 kappa{ autodiff::dual(opt_slip(0)), @@ -141,12 +163,9 @@ template autodiff::jacobian( residualVector, autodiff::wrt(kappa), autodiff::at(kappa), residual_at_kappa, jacobian_residual_at_kappa); const Vec6f residuals_at_kappa_primal{ - static_cast(autodiff::val(residual_at_kappa(0))), - static_cast(autodiff::val(residual_at_kappa(1))), - static_cast(autodiff::val(residual_at_kappa(2))), - static_cast(autodiff::val(residual_at_kappa(3))), - static_cast(autodiff::val(residual_at_kappa(4))), - static_cast(autodiff::val(residual_at_kappa(5))), + autodiff::val(residual_at_kappa(0)), autodiff::val(residual_at_kappa(1)), + autodiff::val(residual_at_kappa(2)), autodiff::val(residual_at_kappa(3)), + autodiff::val(residual_at_kappa(4)), autodiff::val(residual_at_kappa(5)), }; // print residual_at_kapp and jacobian_residual_at_kappa std::cout << "==============Iteration " << iter @@ -162,23 +181,14 @@ template Mat44f normal_matrix = jacobian_residual_at_kappa.transpose() * jacobian_residual_at_kappa; normal_matrix.diagonal().array() += NORMAL_MATRIX_EPS; // condition problem lmao - const Vec4f rhs = -jacobian_residual_at_kappa.transpose() * residuals_at_kappa_primal; const Eigen::LDLT ldlt(normal_matrix); if (ldlt.info() != Eigen::Success) { - std::cout << "Normal Matrix:\n" << normal_matrix.format(CleanFmt) << std::endl; - // eigenvalues - const Eigen::EigenSolver es(normal_matrix); - std::cout << "Eigenvalues:\n" << es.eigenvalues() << std::endl; - // condition number - const Eigen::JacobiSVD svd(normal_matrix); - double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); - std::cout << "Condition number: " << cond << std::endl; - std::printf("optimizer :( 1, %d\n", ldlt.info()); - std::fflush(stdout); + std::cout << "optimizer :( 1\n" << ldlt.info() << std::endl; + debug(normal_matrix); break; } - + const Vec4f rhs = -jacobian_residual_at_kappa.transpose() * residuals_at_kappa_primal; const Vec4f delta = ldlt.solve(rhs); if (!delta.allFinite()) { @@ -205,6 +215,10 @@ template previous_cost = cost; } + if (iter == MAX_ITER) + { + std::cout << "Reached max iterations without convergence.\n" << std::endl; + } return { .fl = opt_slip(0), @@ -214,6 +228,6 @@ template }; } -template wheel_set optimize(const VehicleState &state, float ax_setpoint, float omegadot_setpoint); +// template wheel_set optimize(const VehicleState &state, float ax_setpoint, float omegadot_setpoint); template wheel_set optimize(const VehicleState &state, double ax_setpoint, double omegadot_setpoint); } // namespace app::tv::controllers::allocator From 6c91f74182fdda8a69638e9dd60efdb448c57894 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 16 Apr 2026 01:32:05 -0700 Subject: [PATCH 26/35] working optimizer --- firmware/hexray/VC/plot_tests.cpp | 2 +- .../controllers/torque_allocator.cpp | 44 ++++++------------- 2 files changed, 15 insertions(+), 31 deletions(-) diff --git a/firmware/hexray/VC/plot_tests.cpp b/firmware/hexray/VC/plot_tests.cpp index 8041d68b06..d522b4243b 100644 --- a/firmware/hexray/VC/plot_tests.cpp +++ b/firmware/hexray/VC/plot_tests.cpp @@ -58,5 +58,5 @@ int main() .delta = 0, }; const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0, 0.0); - std::cout << fl << " " << fr << " " << rl << " " << rr << std::endl; + std::cout << "Optimal slip found: " << fl << " " << fr << " " << rl << " " << rr << std::endl; } \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index 2896a7596b..f690f8d58f 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -24,11 +24,11 @@ namespace constexpr double W_MZ = 0.0f; constexpr double W_R = 15.5f; - constexpr int MAX_ITER = 8; - [[maybe_unused]] constexpr float SLIP_CLAMP = 0.3f; - constexpr float NORMAL_MATRIX_EPS = 1e-6f; - constexpr float STEP_TOLERANCE = 1e-5f; - constexpr float COST_TOLERANCE = 1e-6f; + constexpr int MAX_ITER = 8; + [[maybe_unused]] constexpr double SLIP_CLAMP = 0.3; + constexpr float NORMAL_MATRIX_EPS = 1e-6f; + constexpr float STEP_TOLERANCE = 1e-5f; + constexpr float COST_TOLERANCE = 1e-6f; using Vec6f = Eigen::Matrix; using Vec4f = Eigen::Matrix; @@ -38,7 +38,7 @@ namespace using DualVec4 = Eigen::Matrix; // print the normal_matrix - const Eigen::IOFormat CleanFmt(5, 0, ", ", "\n", "[", "]"); + const Eigen::IOFormat CleanFmt(3, 0, ", ", "\n", "[", "]"); void debug(const Mat44f &normal_matrix) { @@ -167,18 +167,6 @@ template autodiff::val(residual_at_kappa(2)), autodiff::val(residual_at_kappa(3)), autodiff::val(residual_at_kappa(4)), autodiff::val(residual_at_kappa(5)), }; - // print residual_at_kapp and jacobian_residual_at_kappa - std::cout << "==============Iteration " << iter - << "===============\n" - "Kappa: \n" - << opt_slip.format(CleanFmt) - << "\n\n" - "Residual at kappa:\n" - << residuals_at_kappa_primal.format(CleanFmt) - << "\n\n" - "Jacobian of residual at kappa:\n" - << jacobian_residual_at_kappa.format(CleanFmt) << std::endl; - Mat44f normal_matrix = jacobian_residual_at_kappa.transpose() * jacobian_residual_at_kappa; normal_matrix.diagonal().array() += NORMAL_MATRIX_EPS; // condition problem lmao const Eigen::LDLT ldlt(normal_matrix); @@ -197,23 +185,19 @@ template break; } - std::cout << "Delta: \n" << delta.format(CleanFmt) << std::endl; - opt_slip += delta; - // I would recommend not doing this - // consider the following: a large step is required which leaves the clamp space - // this would cause the loop to never converge as the step size at each iteration would be sufficiently large to - // continue - // for (int i = 0; i < 4; ++i) - // opt_slip(i) = std::clamp(opt_slip(i), -SLIP_CLAMP, SLIP_CLAMP); - - // Least-squares cost: - // J = 0.5 * r^T r + Vec4f next_opt_slip = opt_slip + delta; + for (int i = 0; i < 4; ++i) + next_opt_slip(i) = std::clamp(next_opt_slip(i), -SLIP_CLAMP, SLIP_CLAMP); + // Least-squares cost: |r|_2^2 // This is used only for convergence monitoring; the actual update is driven by J^T J and J^T r above. - const float cost = 0.5f * residuals_at_kappa_primal.squaredNorm(); + const float cost = residuals_at_kappa_primal.squaredNorm(); + std::cout << "Iter " << iter << ": cost = " << cost << ", opt_slip = [" << opt_slip.transpose() + << "], delta = [" << delta.transpose() << "]\n"; if (delta.norm() < STEP_TOLERANCE || std::fabs(previous_cost - cost) < COST_TOLERANCE) break; previous_cost = cost; + opt_slip = next_opt_slip; } if (iter == MAX_ITER) { From b95da940ddfbae8e3ceedd63aa7639a025b992dd Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 16 Apr 2026 01:37:55 -0700 Subject: [PATCH 27/35] torque allocator W --- .../controllers/torque_allocator.cpp | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index f690f8d58f..39c2fc529d 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -68,8 +68,6 @@ template // return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; // } - std::cout << ax_setpoint << " " << omegadot_setpoint << std::endl; - static const float SQRT_W_FX = std::sqrt(W_FX); static const float SQRT_W_MZ = std::sqrt(W_MZ); static const float SQRT_W_R = std::sqrt(W_R); @@ -127,15 +125,6 @@ template }; const autodiff::dual sum_fx = predicted_f.fl.x + predicted_f.fr.x + predicted_f.rl.x + predicted_f.rr.x; const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); - // print out all forces and their gradients - // std::cout << "Predicted forces at kappa:\n" - // << "FL: " << predicted_f.fl.x << " N, dFL/dkappa: " << autodiff::derivative(predicted_f.fl.x) - // << "\nFR: " << predicted_f.fr.x << " N, dFR/dkappa: " << autodiff::derivative(predicted_f.fr.x) - // << "\nRL: " << predicted_f.rl.x << " N, dRL/dkappa: " << autodiff::derivative(predicted_f.rl.x) - // << "\nRR: " << predicted_f.rr.x << " N, dRR/dkappa: " << autodiff::derivative(predicted_f.rr.x) - // << std::endl; - // print out sum_fx and its gradient - // std::cout << "Sum Fx: " << sum_fx << " N, d(Sum Fx)/dkappa: " << autodiff::derivative(sum_fx) << std::endl; return DualVec6{ SQRT_W_FX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint), SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), SQRT_W_R * kappa[0], @@ -144,10 +133,8 @@ template SQRT_W_R * kappa[3] }; }; - Vec4f opt_slip{ 0, 0, 0, 0 }; // output variable - - float previous_cost = std::numeric_limits::infinity(); - + Vec4f opt_slip{ 0, 0, 0, 0 }; // output variable + float previous_cost = std::numeric_limits::infinity(); uint32_t iter; for (iter = 0; iter < MAX_ITER; ++iter) { @@ -193,7 +180,7 @@ template const float cost = residuals_at_kappa_primal.squaredNorm(); std::cout << "Iter " << iter << ": cost = " << cost << ", opt_slip = [" << opt_slip.transpose() << "], delta = [" << delta.transpose() << "]\n"; - if (delta.norm() < STEP_TOLERANCE || std::fabs(previous_cost - cost) < COST_TOLERANCE) + if ((next_opt_slip - opt_slip).norm() < STEP_TOLERANCE || std::fabs(previous_cost - cost) < COST_TOLERANCE) break; previous_cost = cost; From 14cfe3f6a041d631eb9f548797c63c9706bc517e Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 16 Apr 2026 10:31:44 -0700 Subject: [PATCH 28/35] working for floats as well now --- firmware/hexray/VC/plot_tests.cpp | 4 +- .../controllers/torque_allocator.cpp | 72 +++++++++---------- .../shared_datatypes/decimal_dual.hpp | 8 ++- 3 files changed, 44 insertions(+), 40 deletions(-) diff --git a/firmware/hexray/VC/plot_tests.cpp b/firmware/hexray/VC/plot_tests.cpp index d522b4243b..a7bc695a14 100644 --- a/firmware/hexray/VC/plot_tests.cpp +++ b/firmware/hexray/VC/plot_tests.cpp @@ -48,7 +48,7 @@ static void plot_combined_fx() int main() { - constexpr app::tv::shared_datatypes::VehicleState state{ + constexpr app::tv::shared_datatypes::VehicleState state{ .v_x_mps = 0, .v_y_mps = 0, .yaw_rate_radps = 0, @@ -57,6 +57,6 @@ int main() .apps = 0.2, .delta = 0, }; - const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0, 0.0); + const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0f, 0.0f); std::cout << "Optimal slip found: " << fl << " " << fr << " " << rl << " " << rr << std::endl; } \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index 39c2fc529d..bec0e71476 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -30,24 +30,24 @@ namespace constexpr float STEP_TOLERANCE = 1e-5f; constexpr float COST_TOLERANCE = 1e-6f; - using Vec6f = Eigen::Matrix; - using Vec4f = Eigen::Matrix; - using Mat64f = Eigen::Matrix; - using Mat44f = Eigen::Matrix; - using DualVec6 = Eigen::Matrix; - using DualVec4 = Eigen::Matrix; + template using Vec6 = Eigen::Matrix; + template using Vec4 = Eigen::Matrix; + template using Mat64 = Eigen::Matrix; + template using Mat44 = Eigen::Matrix; + template using DualVec6 = Eigen::Matrix, 6, 1>; + template using DualVec4 = Eigen::Matrix, 4, 1>; // print the normal_matrix const Eigen::IOFormat CleanFmt(3, 0, ", ", "\n", "[", "]"); - void debug(const Mat44f &normal_matrix) + template void debug(const Mat44 &normal_matrix) { std::cout << "Normal Matrix:\n" << normal_matrix.format(CleanFmt) << std::endl; // eigenvalues - const Eigen::EigenSolver es(normal_matrix); + const Eigen::EigenSolver> es(normal_matrix); std::cout << "Eigenvalues:\n" << es.eigenvalues() << std::endl; // condition number - const Eigen::JacobiSVD svd(normal_matrix); + const Eigen::JacobiSVD> svd(normal_matrix); const double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); std::cout << "Condition number: " << cond << std::endl; } @@ -103,68 +103,68 @@ template // stays allocation-free and predictable on embedded targets. const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); const auto [alpha_fl, alpha_fr, alpha_rl, alpha_rr] = state.alphas(); - const auto residualVector = [&](const DualVec4 &kappa) -> DualVec6 + const auto residualVector = [&](const DualVec4 &kappa) -> DualVec6 { const wheel_set> predicted_f{ { - estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)), - estimation::tire_model.computeCombinedFy_N(fz_fl, alpha_fl, kappa(0)), + estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)) / 1000, + estimation::tire_model.computeCombinedFy_N(fz_fl, alpha_fl, kappa(0)) / 1000, }, { - estimation::tire_model.computeCombinedFx_N(fz_fr, alpha_fr, kappa(1)), - estimation::tire_model.computeCombinedFy_N(fz_fr, alpha_fr, kappa(1)), + estimation::tire_model.computeCombinedFx_N(fz_fr, alpha_fr, kappa(1)) / 1000, + estimation::tire_model.computeCombinedFy_N(fz_fr, alpha_fr, kappa(1)) / 1000, }, { - estimation::tire_model.computeCombinedFx_N(fz_rl, alpha_rl, kappa(2)), - estimation::tire_model.computeCombinedFy_N(fz_rl, alpha_rl, kappa(2)), + estimation::tire_model.computeCombinedFx_N(fz_rl, alpha_rl, kappa(2)) / 1000, + estimation::tire_model.computeCombinedFy_N(fz_rl, alpha_rl, kappa(2)) / 1000, }, { - estimation::tire_model.computeCombinedFx_N(fz_rr, alpha_rr, kappa(3)), - estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)), + estimation::tire_model.computeCombinedFx_N(fz_rr, alpha_rr, kappa(3)) / 1000, + estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)) / 1000, }, }; const autodiff::dual sum_fx = predicted_f.fl.x + predicted_f.fr.x + predicted_f.rl.x + predicted_f.rr.x; const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); - return DualVec6{ SQRT_W_FX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint), - SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), - SQRT_W_R * kappa[0], - SQRT_W_R * kappa[1], - SQRT_W_R * kappa[2], - SQRT_W_R * kappa[3] }; + return DualVec6{ SQRT_W_FX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint / 1000), + SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), + SQRT_W_R * kappa[0], + SQRT_W_R * kappa[1], + SQRT_W_R * kappa[2], + SQRT_W_R * kappa[3] }; }; - Vec4f opt_slip{ 0, 0, 0, 0 }; // output variable + Vec4 opt_slip{ 0, 0, 0, 0 }; // output variable float previous_cost = std::numeric_limits::infinity(); uint32_t iter; for (iter = 0; iter < MAX_ITER; ++iter) { - DualVec4 kappa{ + DualVec4 kappa{ autodiff::dual(opt_slip(0)), autodiff::dual(opt_slip(1)), autodiff::dual(opt_slip(2)), autodiff::dual(opt_slip(3)), }; // evaluate and calculate jacobian at kappa - DualVec6 residual_at_kappa; - Mat64f jacobian_residual_at_kappa; + DualVec6 residual_at_kappa; + Mat64 jacobian_residual_at_kappa; autodiff::jacobian( residualVector, autodiff::wrt(kappa), autodiff::at(kappa), residual_at_kappa, jacobian_residual_at_kappa); - const Vec6f residuals_at_kappa_primal{ + const Vec6 residuals_at_kappa_primal{ autodiff::val(residual_at_kappa(0)), autodiff::val(residual_at_kappa(1)), autodiff::val(residual_at_kappa(2)), autodiff::val(residual_at_kappa(3)), autodiff::val(residual_at_kappa(4)), autodiff::val(residual_at_kappa(5)), }; - Mat44f normal_matrix = jacobian_residual_at_kappa.transpose() * jacobian_residual_at_kappa; + Mat44 normal_matrix = jacobian_residual_at_kappa.transpose() * jacobian_residual_at_kappa; normal_matrix.diagonal().array() += NORMAL_MATRIX_EPS; // condition problem lmao - const Eigen::LDLT ldlt(normal_matrix); + const Eigen::LDLT> ldlt(normal_matrix); if (ldlt.info() != Eigen::Success) { std::cout << "optimizer :( 1\n" << ldlt.info() << std::endl; debug(normal_matrix); break; } - const Vec4f rhs = -jacobian_residual_at_kappa.transpose() * residuals_at_kappa_primal; - const Vec4f delta = ldlt.solve(rhs); + const Vec4 rhs = -jacobian_residual_at_kappa.transpose() * residuals_at_kappa_primal; + const Vec4 delta = ldlt.solve(rhs); if (!delta.allFinite()) { std::printf("optimizer :( 2\n"); @@ -172,9 +172,9 @@ template break; } - Vec4f next_opt_slip = opt_slip + delta; + Vec4 next_opt_slip = opt_slip + delta; for (int i = 0; i < 4; ++i) - next_opt_slip(i) = std::clamp(next_opt_slip(i), -SLIP_CLAMP, SLIP_CLAMP); + next_opt_slip(i) = std::clamp(next_opt_slip(i), -static_cast(SLIP_CLAMP), static_cast(SLIP_CLAMP)); // Least-squares cost: |r|_2^2 // This is used only for convergence monitoring; the actual update is driven by J^T J and J^T r above. const float cost = residuals_at_kappa_primal.squaredNorm(); @@ -199,6 +199,6 @@ template }; } -// template wheel_set optimize(const VehicleState &state, float ax_setpoint, float omegadot_setpoint); +template wheel_set optimize(const VehicleState &state, float ax_setpoint, float omegadot_setpoint); template wheel_set optimize(const VehicleState &state, double ax_setpoint, double omegadot_setpoint); } // namespace app::tv::controllers::allocator diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/decimal_dual.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/decimal_dual.hpp index f33bfcfa06..c4d9f60a8a 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/decimal_dual.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/decimal_dual.hpp @@ -1,8 +1,12 @@ #pragma once #include "dual.hpp" -template -concept DecimalOrDual = std::same_as || std::same_as || std::same_as; template concept Decimal = std::same_as || std::same_as; + +template using DecimalDual = autodiff::HigherOrderDual<1, T>; + +template +concept DecimalOrDual = std::same_as || std::same_as || std::same_as> || + std::same_as>; From ba6f1a58bd794e22638714df9124a41bb9317787 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 16 Apr 2026 10:33:36 -0700 Subject: [PATCH 29/35] better ig --- .../controllers/torque_allocator.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index bec0e71476..52d1a159d7 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -107,23 +107,24 @@ template { const wheel_set> predicted_f{ { - estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)) / 1000, - estimation::tire_model.computeCombinedFy_N(fz_fl, alpha_fl, kappa(0)) / 1000, + estimation::tire_model.computeCombinedFx_N(fz_fl, alpha_fl, kappa(0)), + estimation::tire_model.computeCombinedFy_N(fz_fl, alpha_fl, kappa(0)), }, { - estimation::tire_model.computeCombinedFx_N(fz_fr, alpha_fr, kappa(1)) / 1000, - estimation::tire_model.computeCombinedFy_N(fz_fr, alpha_fr, kappa(1)) / 1000, + estimation::tire_model.computeCombinedFx_N(fz_fr, alpha_fr, kappa(1)), + estimation::tire_model.computeCombinedFy_N(fz_fr, alpha_fr, kappa(1)), }, { - estimation::tire_model.computeCombinedFx_N(fz_rl, alpha_rl, kappa(2)) / 1000, - estimation::tire_model.computeCombinedFy_N(fz_rl, alpha_rl, kappa(2)) / 1000, + estimation::tire_model.computeCombinedFx_N(fz_rl, alpha_rl, kappa(2)), + estimation::tire_model.computeCombinedFy_N(fz_rl, alpha_rl, kappa(2)), }, { - estimation::tire_model.computeCombinedFx_N(fz_rr, alpha_rr, kappa(3)) / 1000, - estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)) / 1000, + estimation::tire_model.computeCombinedFx_N(fz_rr, alpha_rr, kappa(3)), + estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)), }, }; - const autodiff::dual sum_fx = predicted_f.fl.x + predicted_f.fr.x + predicted_f.rl.x + predicted_f.rr.x; + const autodiff::dual sum_fx = + predicted_f.fl.x / 1000 + predicted_f.fr.x / 1000 + predicted_f.rl.x / 1000 + predicted_f.rr.x / 1000; const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); return DualVec6{ SQRT_W_FX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint / 1000), SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), From 6a3cf829f0f0e0b4b71bb2d05c858f9169168b89 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 16 Apr 2026 13:57:37 -0700 Subject: [PATCH 30/35] turning tests --- firmware/hexray/VC/plot_tests.cpp | 6 +++--- .../app/torque_vectoring/controllers/torque_allocator.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/firmware/hexray/VC/plot_tests.cpp b/firmware/hexray/VC/plot_tests.cpp index a7bc695a14..8a735541cb 100644 --- a/firmware/hexray/VC/plot_tests.cpp +++ b/firmware/hexray/VC/plot_tests.cpp @@ -48,8 +48,8 @@ static void plot_combined_fx() int main() { - constexpr app::tv::shared_datatypes::VehicleState state{ - .v_x_mps = 0, + constexpr app::tv::shared_datatypes::VehicleState state{ + .v_x_mps = 10, .v_y_mps = 0, .yaw_rate_radps = 0, .a_x_mps2 = 1, @@ -57,6 +57,6 @@ int main() .apps = 0.2, .delta = 0, }; - const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0f, 0.0f); + const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0, 5.0); std::cout << "Optimal slip found: " << fl << " " << fr << " " << rl << " " << rr << std::endl; } \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index 52d1a159d7..d7492b42ab 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -21,10 +21,10 @@ namespace { // ---- Optimizer tuning ---- constexpr double W_FX = 0.5f; - constexpr double W_MZ = 0.0f; + constexpr double W_MZ = 0.5f; constexpr double W_R = 15.5f; - constexpr int MAX_ITER = 8; + constexpr int MAX_ITER = 20; [[maybe_unused]] constexpr double SLIP_CLAMP = 0.3; constexpr float NORMAL_MATRIX_EPS = 1e-6f; constexpr float STEP_TOLERANCE = 1e-5f; @@ -179,8 +179,8 @@ template // Least-squares cost: |r|_2^2 // This is used only for convergence monitoring; the actual update is driven by J^T J and J^T r above. const float cost = residuals_at_kappa_primal.squaredNorm(); - std::cout << "Iter " << iter << ": cost = " << cost << ", opt_slip = [" << opt_slip.transpose() - << "], delta = [" << delta.transpose() << "]\n"; + std::cout << "Iter " << iter << ": cost = " << cost << ", delta = " << delta.transpose().format(CleanFmt) + << "\n"; if ((next_opt_slip - opt_slip).norm() < STEP_TOLERANCE || std::fabs(previous_cost - cost) < COST_TOLERANCE) break; From 3b10a21c4d696b5bc308a384af07ba86a4b9a157 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 16 Apr 2026 22:00:50 -0700 Subject: [PATCH 31/35] consistent optimizer --- firmware/hexray/VC/plot_tests.cpp | 25 ++++++------ .../controllers/torque_allocator.cpp | 38 +++++++++++++------ .../shared_datatypes/constants.hpp | 4 +- .../app/torque_vectoring/torque_vectoring.cpp | 14 +++---- .../app/torque_vectoring/torque_vectoring.hpp | 12 ++++-- .../torque_vectoring_matlab.h | 2 +- 6 files changed, 58 insertions(+), 37 deletions(-) diff --git a/firmware/hexray/VC/plot_tests.cpp b/firmware/hexray/VC/plot_tests.cpp index 8a735541cb..869d1bc216 100644 --- a/firmware/hexray/VC/plot_tests.cpp +++ b/firmware/hexray/VC/plot_tests.cpp @@ -12,7 +12,7 @@ namespace plt = matplotlibcpp; static void plot_combined_fx() { constexpr int n_points = 301; // includes endpoints - constexpr float fz_N = 1000.0f; + constexpr float fz_N = 700.0f; std::vector kappas; kappas.reserve(n_points); @@ -25,18 +25,19 @@ static void plot_combined_fx() kappas.push_back(kappa); } - for (const float alpha_rad : { 0.3f, 0.25f, 0.2f, 0.15f, 0.1f, 0.05f, 0.0f }) + // for (const float alpha_rad : { 0.3f, 0.25f, 0.2f, 0.15f, 0.1f, 0.05f, 0.0f }) + // {] + const float alpha_rad = 0; + std::vector fxs; + fxs.reserve(kappas.size()); + for (const double kappa : kappas) { - std::vector fxs; - fxs.reserve(kappas.size()); - for (const double kappa : kappas) - { - fxs.push_back(app::tv::estimation::tire_model.computeCombinedFx_N(fz_N, alpha_rad, kappa)); - } - std::stringstream ss; - ss << "\\alpha = " << std::setprecision(2) << alpha_rad; - plt::named_plot(ss.str(), kappas, fxs); + fxs.push_back(app::tv::estimation::tire_model.computeCombinedFx_N(fz_N, alpha_rad, kappa)); } + std::stringstream ss; + ss << "\\alpha = " << std::setprecision(2) << alpha_rad; + plt::named_plot(ss.str(), kappas, fxs); + // } plt::title("F_x as a function of \\kappa and \\alpha"); plt::xlabel("\\kappa"); @@ -59,4 +60,6 @@ int main() }; const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0, 5.0); std::cout << "Optimal slip found: " << fl << " " << fr << " " << rl << " " << rr << std::endl; + + plot_combined_fx(); } \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index d7492b42ab..1d3b8a272b 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -68,9 +68,9 @@ template // return { .fl = 0.0f, .fr = 0.0f, .rl = 0.0f, .rr = 0.0f }; // } - static const float SQRT_W_FX = std::sqrt(W_FX); - static const float SQRT_W_MZ = std::sqrt(W_MZ); - static const float SQRT_W_R = std::sqrt(W_R); + static const T SQRT_W_FX = std::sqrt(W_FX); + static const T SQRT_W_MZ = std::sqrt(W_MZ); + static const T SQRT_W_R = std::sqrt(W_R); // const wheel_set blended_des_f_x{ // .fl = low_speed_blend * des_f_x.fl, @@ -103,7 +103,7 @@ template // stays allocation-free and predictable on embedded targets. const auto [fz_fl, fz_fr, fz_rl, fz_rr] = state.est_Fz_N(); const auto [alpha_fl, alpha_fr, alpha_rl, alpha_rr] = state.alphas(); - const auto residualVector = [&](const DualVec4 &kappa) -> DualVec6 + const auto residualVector = [&](const DualVec4 &kappa, const bool debug = false) -> DualVec6 { const wheel_set> predicted_f{ { @@ -123,15 +123,22 @@ template estimation::tire_model.computeCombinedFy_N(fz_rr, alpha_rr, kappa(3)), }, }; - const autodiff::dual sum_fx = + const autodiff::dual sum_fx_over_1k = predicted_f.fl.x / 1000 + predicted_f.fr.x / 1000 + predicted_f.rl.x / 1000 + predicted_f.rr.x / 1000; const autodiff::dual predicted_mz = state.est_Mz_N(predicted_f); - return DualVec6{ SQRT_W_FX * (sum_fx - CAR_MASS_AT_CG_KG * ax_setpoint / 1000), - SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), - SQRT_W_R * kappa[0], - SQRT_W_R * kappa[1], - SQRT_W_R * kappa[2], - SQRT_W_R * kappa[3] }; + + if (debug) + { + std::cout << "f_x/1000=" << sum_fx_over_1k << " mz=" << predicted_mz << std::endl; + } + return DualVec6{ + SQRT_W_FX * (sum_fx_over_1k - CAR_MASS_AT_CG_KG * ax_setpoint / 1000), + SQRT_W_MZ * (predicted_mz - CAR_YAW_MOMENT_INERTIA_KGM2 * omegadot_setpoint), + SQRT_W_R * kappa[0], + SQRT_W_R * kappa[1], + SQRT_W_R * kappa[2], + SQRT_W_R * kappa[3], + }; }; Vec4 opt_slip{ 0, 0, 0, 0 }; // output variable @@ -192,6 +199,15 @@ template std::cout << "Reached max iterations without convergence.\n" << std::endl; } + const auto r = residualVector( + { + autodiff::dual(opt_slip(0)), + autodiff::dual(opt_slip(1)), + autodiff::dual(opt_slip(2)), + autodiff::dual(opt_slip(3)), + }, + true); + return { .fl = opt_slip(0), .fr = opt_slip(1), diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index f9cac0de5f..adc2152ee3 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -34,7 +34,7 @@ inline constexpr float WHEEL_RADIUS_M = WHEEL_DIAMETER_IN * IN_TO_M / 2.0f; // VEHICLE MASS & CENTER OF GRAVITY // ============================================================================= -inline constexpr float CAR_MASS_AT_CG_KG = 300.0f; // Mass with driver (verified with suspension team) +inline constexpr double CAR_MASS_AT_CG_KG = 300.0; // Mass with driver (verified with suspension team) // Estimated yaw moment of inertia about CG (TODO: Update with suspension team) inline constexpr float CAR_YAW_MOMENT_INERTIA_KGM2 = 150.0f; @@ -94,7 +94,7 @@ inline constexpr float SLIP_RATIO_IDEAL = 0.05f; // Ideal slip ratio for maximum // TODO: Replace with reverse/anti-Ackermann model inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = 0.3f; // Note: Underestimate for wheel angles > 40° (see Confluence/Steering System) -inline constexpr float MAX_AX_MPS2 = 10; // TODO idk this number bruh +inline constexpr float MAX_AX_MPS2 = 30; // TODO idk this number bruh // ============================================================================= // UTILITY FUNCTIONS & CONVERSION HELPERS diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index d9e20d4358..f804b4deb1 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -32,9 +32,7 @@ template ControlOutput update(const VehicleState &state) //------------------------------------- POWER LIMITER -----------------------------------// // TODO: slip_ratio_opt -> slipRatioToWheelAngularVelocity() -> power limiter -> torque request - return { { kappa_opt.fl, kappa_opt.fr, kappa_opt.rl, kappa_opt.rr }, - { 5, 5, 5, 5 }, - { -5, -5, -5, -5 } }; + return { { kappa_opt.fl, kappa_opt.fr, kappa_opt.rl, kappa_opt.rr }, { 21, 21, 21, 21 }, { -15, -15, -15, -15 } }; } template ControlOutput update(const VehicleState &state); template ControlOutput update(const VehicleState &state); @@ -91,12 +89,12 @@ template ControlOutputAutonomous update_autonomous(const VehicleS template ControlOutputAutonomous update_autonomous(const VehicleState &state); template ControlOutputAutonomous update_autonomous(const VehicleState &state); -void kappa_update_matlab(double kappas[4], const double v_x, double out[4]) +void kappa_update_matlab(double kappas[4], const double v_x, double oemgas[4]) { const auto [fl, fr, rl, rr] = kappa_update({ .fl = kappas[0], .fr = kappas[1], .rl = kappas[2], .rr = kappas[3] }, v_x); - out[0] = fl; - out[1] = fr; - out[2] = rl; - out[3] = rr; + oemgas[0] = fl; + oemgas[1] = fr; + oemgas[2] = rl; + oemgas[3] = rr; } diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp index e39bb141da..5b1734fab6 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -51,9 +51,13 @@ app::tv::shared_datatypes::wheel_set T v_x_mps_capped = std::max(v_x_mps, static_cast(1)); return { - .fl = GEAR_RATIO * (static_cast(1) + kappas.fl) * (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), - .fr = GEAR_RATIO * (static_cast(1) + kappas.fr) * (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), - .rl = GEAR_RATIO * (static_cast(1) + kappas.rl) * (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), - .rr = GEAR_RATIO * (static_cast(1) + kappas.rr) * (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), + .fl = GEAR_RATIO * (static_cast(1) + kappas.fl) * + (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), + .fr = GEAR_RATIO * (static_cast(1) + kappas.fr) * + (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), + .rl = GEAR_RATIO * (static_cast(1) + kappas.rl) * + (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), + .rr = GEAR_RATIO * (static_cast(1) + kappas.rr) * + (v_x_mps_capped / static_cast(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)), }; } diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h index 746b676140..000b31942f 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h @@ -21,5 +21,5 @@ extern "C" /** * Matlab wrapper for kappa_update */ - void kappa_update_matlab(double kappas[4], double v_x, double out[4]); + void kappa_update_matlab(double kappas[4], double v_x, double oemgas[4]); } \ No newline at end of file From 2945824d6b440c749395a1adc9053cb7ec14e760 Mon Sep 17 00:00:00 2001 From: Aditya-Dhiman4 <87353212+Aditya-Dhiman4@users.noreply.github.com> Date: Mon, 20 Apr 2026 11:28:40 -0700 Subject: [PATCH 32/35] Steering Model (#1949) ### Changelist ### Testing Done ### Resolved Tickets --- firmware/hexray/FSM/src/io/io_apps.cpp | 7 ++- .../estimation/steering_model.cpp | 63 +++++++++++++++++++ .../estimation/steering_model.hpp | 15 +++++ .../shared_datatypes/constants.hpp | 18 ++++-- .../hexray/VC/test/test_steeringModel.cpp | 44 +++++++++++++ firmware/shared/srcpp/util/util_utils.hpp | 36 ++++++----- 6 files changed, 161 insertions(+), 22 deletions(-) create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.cpp create mode 100644 firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.hpp create mode 100644 firmware/hexray/VC/test/test_steeringModel.cpp diff --git a/firmware/hexray/FSM/src/io/io_apps.cpp b/firmware/hexray/FSM/src/io/io_apps.cpp index 75a8b46d04..791e9722ff 100644 --- a/firmware/hexray/FSM/src/io/io_apps.cpp +++ b/firmware/hexray/FSM/src/io/io_apps.cpp @@ -2,6 +2,7 @@ #include "hw_adcs.hpp" #include "util_utils.hpp" #include +#include //===== // Geometry and ADC Constants for Pedal Sensors -> PAPPS = primary apps, SAPPS = secondary apps (same as liam's @@ -71,7 +72,7 @@ static float calcAppsAngle(const float cos_law_coefficient, const float pot_len, // Calculate the cosine law expression: (a^2 + b^2 - c^2) / (2ab) const float value = cos_law_coefficient - pot_len * pot_len / cos_law_denominator; const float acos_input = - CLAMP(value, -1.0f, 1.0f); // where c is represented indirectly via the measured length (pot_len) + std::clamp(value, -1.0f, 1.0f); // where c is represented indirectly via the measured length (pot_len) return acos(acos_input); } @@ -110,7 +111,7 @@ float getPrimary(void) // Scale the percentage to account for the dead zone. const float pedal_percentage = (100.0f / (100.0f - DEAD_ZONE_PERCENT)) * (pedal_percentage_raw - DEAD_ZONE_PERCENT); - return CLAMP(pedal_percentage, 0.0f, 100.0f); + return std::clamp(pedal_percentage, 0.0f, 100.0f); } bool isPrimaryOCSC(void) @@ -139,7 +140,7 @@ float getSecondary(void) // Scale the percentage to account for the dead zone. const float pedal_percentage = (100.0f / (100.0f - DEAD_ZONE_PERCENT)) * (pedal_percentage_raw - DEAD_ZONE_PERCENT); - return CLAMP(pedal_percentage, 0.0f, 100.0f); + return std::clamp(pedal_percentage, 0.0f, 100.0f); } bool isSecondaryOCSC(void) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.cpp new file mode 100644 index 0000000000..391f9b3126 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.cpp @@ -0,0 +1,63 @@ +#include "torque_vectoring/estimation/steering_model.hpp" + +#include +#include +#include +#include + +#include "torque_vectoring/shared_datatypes/constants.hpp" +#include "util_utils.hpp" + +namespace +{ +// Linear interpolation of inner wheel with respect to steering angle +// Note: 0.09803348769 Degrees of error at full lock +template inline float inner_wheel_ang_rad(const T steer_ang_rad) +{ + return (static_cast(0.2651718671) * steer_ang_rad); +} + +// Linear interpolation of outer wheel with respect to steering angle +// Note: 0.3468418335 Degrees of error at full lock +template inline float outer_wheel_ang_rad(const T steer_ang_rad) +{ + return (static_cast(0.274579971) * steer_ang_rad); +} +} // namespace + +using namespace app::tv::shared_datatypes; + +namespace app::tv::estimators::steering +{ +template [[nodiscard]] wheel_set wheel_steer_angles(const T steer_ang_rad) +{ + const T steer_ang_clamped_rad = std::clamp( + steer_ang_rad, -static_cast(vd_constants::STEER_WHEEL_RANGE_rad), + static_cast(vd_constants::STEER_WHEEL_RANGE_rad)); + /** + * This model is based off of a table that maps steering wheel angle to wheel angle. + * Given the steering wheel angle, we scale it into an index, and then linearly interpolate + * across its upper and lower elements to determine the actual wheel angle + * + * Suspension Spreadsheet: + * https://docs.google.com/spreadsheets/d/1gB3h8JgjsrMDLsJRusXe3zQUWp_cwrUp/edit?gid=2114943012#gid=2114943012 + */ + + app::tv::shared_datatypes::wheel_set wheel_ang_rad{}; + + // TODO: Verify axis and steering angle sensor values + // Positive steer_ang_rad = right turn + if (steer_ang_clamped_rad >= static_cast(0.0)) + { + wheel_ang_rad.fr = inner_wheel_ang_rad(steer_ang_clamped_rad); + wheel_ang_rad.fl = outer_wheel_ang_rad(steer_ang_clamped_rad); + } + else + { + wheel_ang_rad.fr = outer_wheel_ang_rad(steer_ang_clamped_rad); + wheel_ang_rad.fl = inner_wheel_ang_rad(steer_ang_clamped_rad); + } + + return wheel_ang_rad; +} +} // namespace app::tv::estimators::steering diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.hpp new file mode 100644 index 0000000000..a82a55977e --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include "torque_vectoring/shared_datatypes/wheel_set.hpp" + +namespace app::tv::estimators::steering +{ +/** + * @brief Line fit mapping steering wheel angles to wheel angles + * + * @param steer_ang_rad: steering wheel angle in readiants + * + * @return WheelSteerAngles influenced by steering wheel input, rear wheels always 0 here + */ +template [[nodiscard]] app::tv::shared_datatypes::wheel_set wheel_steer_angles(const T steer_ang_rad); +} // namespace app::tv::estimators::steering diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp index adc2152ee3..a9b6b2e6be 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp @@ -64,6 +64,7 @@ inline constexpr uint16_t POWER_TO_TORQUE_CONVERSION_FACTOR = 9550; // 60/(2*pi) // POWER & THERMAL LIMITS // ============================================================================= +// TODO: Verify all of these // Power Limits inline constexpr float RULES_BASED_POWER_LIMIT_KW = 80.0f; // FSAE maximum allowed power inline constexpr float POWER_LIMIT_CAR_kW = 40.0f; // TODO: Update with hexray constants or remove @@ -87,14 +88,21 @@ inline constexpr float PID_POWER_FACTOR_MIN = -0.9f; // TODO: May need adjustm inline constexpr float PID_POWER_FACTOR_MAX = 0.1f; // TODO: May need adjustment // ============================================================================= -// TIRE & TRACTION PARAMETERS +// WHEEL AND STEERING PARAMETERS // ============================================================================= inline constexpr float SLIP_RATIO_IDEAL = 0.05f; // Ideal slip ratio for maximum traction -// TODO: Replace with reverse/anti-Ackermann model -inline constexpr float APPROX_STEERING_TO_WHEEL_ANGLE = 0.3f; -// Note: Underestimate for wheel angles > 40° (see Confluence/Steering System) -inline constexpr float MAX_AX_MPS2 = 30; // TODO idk this number bruh +inline constexpr float MAX_AX_MPS2 = 30; // TODO idk this number bruh + +inline constexpr float STEER_WHEEL_RANGE_rad = 1.48632f; +inline constexpr float STEER_WHEEL_RANGE_deg = RAD_TO_DEG(1.48632f); + +// Note: Bump camber is the amount the camber changes in degrees due to compression +inline constexpr float STATIC_CAMBER_FRONT_deg = -1.0f; +inline constexpr float FRONT_BUMP_CAMBER_deg_mm = 0.02f; + +inline constexpr float STATIC_CAMBER_REAR_deg = -0.75f; +inline constexpr float REAR_BUMP_CAMBER_deg_mm = 0.06f; // ============================================================================= // UTILITY FUNCTIONS & CONVERSION HELPERS diff --git a/firmware/hexray/VC/test/test_steeringModel.cpp b/firmware/hexray/VC/test/test_steeringModel.cpp new file mode 100644 index 0000000000..7ac6a8ae77 --- /dev/null +++ b/firmware/hexray/VC/test/test_steeringModel.cpp @@ -0,0 +1,44 @@ +#include +#include "test/test_VCBase.hpp" + +#include "vc_fakes.hpp" +#include "torque_vectoring/estimation/steering_model.hpp" +#include "torque_vectoring/datatypes/datatypes_vd_constants.hpp" +#include "util_utils.hpp" + +class TVSteeringModelTest : public VCBaseTest +{ +}; + +using namespace app::tv::estimators; +using namespace app::tv::datatypes; + +TEST_F(TVSteeringModelTest, ZeroSteeringAngle) +{ + steering::WheelSteerAngles ws_ang_rad = steering::wheel_steer_angles(0.0f); + + ASSERT_FLOAT_EQ(ws_ang_rad.rr_rad, 0.0f); + ASSERT_FLOAT_EQ(ws_ang_rad.rl_rad, 0.0f); + ASSERT_FLOAT_EQ(ws_ang_rad.fr_rad, 0.0f); + ASSERT_FLOAT_EQ(ws_ang_rad.fl_rad, 0.0f); +} + +TEST_F(TVSteeringModelTest, MaxSteeringAngleRightTurn) +{ + steering::WheelSteerAngles ws_ang_rad = steering::wheel_steer_angles(vd_constants::STEER_WHEEL_RANGE_rad); + + ASSERT_FLOAT_EQ(ws_ang_rad.rr_rad, 0.0f); + ASSERT_FLOAT_EQ(ws_ang_rad.rl_rad, 0.0f); + EXPECT_NEAR(0.395840674f, ws_ang_rad.fr_rad, 0.002f); + EXPECT_NEAR(0.414166631f, ws_ang_rad.fl_rad, 0.0065f); +} + +TEST_F(TVSteeringModelTest, MaxSteeringAngleLeftTurn) +{ + steering::WheelSteerAngles ws_ang_rad = steering::wheel_steer_angles(-vd_constants::STEER_WHEEL_RANGE_rad); + + ASSERT_FLOAT_EQ(ws_ang_rad.rr_rad, 0.0f); + ASSERT_FLOAT_EQ(ws_ang_rad.rl_rad, 0.0f); + EXPECT_NEAR(-0.414166631f, ws_ang_rad.fr_rad, 0.0065f); + EXPECT_NEAR(-0.395840674f, ws_ang_rad.fl_rad, 0.002f); +} diff --git a/firmware/shared/srcpp/util/util_utils.hpp b/firmware/shared/srcpp/util/util_utils.hpp index b8618cedc5..f76d2b3127 100644 --- a/firmware/shared/srcpp/util/util_utils.hpp +++ b/firmware/shared/srcpp/util/util_utils.hpp @@ -2,20 +2,30 @@ #define NUM_ELEMENTS_IN_ARRAY(array_pointer) sizeof(array_pointer) / sizeof(array_pointer[0]) -#ifndef MIN -#define MIN(a, b) (((a) < (b)) ? (a) : (b)) -#endif +#ifdef __cplusplus +#include +template [[nodiscard]] inline constexpr T MIN_OF(const T x, const U... y) +{ + T result = x; + ((result = std::min(result, y)), ...); + return result; +} -#ifndef MAX -#define MAX(a, b) (((a) > (b)) ? (a) : (b)) -#endif +template [[nodiscard]] inline constexpr T SQUARE(const T x) +{ + return x * x; +} -#define MIN3(x, y, z) (MIN(MIN((x), (y)), (z))) -#define MIN4(w, x, y, z) (MIN(MIN(MIN((w), (x)), (y)), (z))) -#define CLAMP(x, min, max) (MAX(MIN(x, max), min)) -#define CLAMP_TO_ONE(x) (((x) <= 0) ? 1 : ((x) > 1 ? 1 : (x))) // initialize to 1 if value is <=0 -#define SQUARE(x) ((x) * (x)) -#define IS_IN_RANGE(min, max, val) (((val) > (min)) && ((val) < (max))) +template [[nodiscard]] inline constexpr bool IS_IN_RANGE(const T min, const T max, const T x) +{ + return (x > min) && (x < max); +} + +template [[nodiscard]] inline constexpr bool SIGN(const T x) +{ + return (x > 0) ? 1 : ((x < 0) ? -1 : 0); +} +#endif /* @brief Extract the basename from a file path */ #ifdef _MSC_VER @@ -47,8 +57,6 @@ constexpr const char *filename_only(const char *path) unsigned char _unused; \ } name; -#define NUM_ELEMENTS_IN_ARRAY(array_pointer) sizeof(array_pointer) / sizeof(array_pointer[0]) - #ifdef __cplusplus #define CFUNC extern "C" #define NORET [[noreturn]] From da931f52f39fe9167581ceab25711265e34d29de Mon Sep 17 00:00:00 2001 From: shayana18 Date: Sat, 25 Apr 2026 17:21:49 -0700 Subject: [PATCH 33/35] 6.2 implemented --- .../estimation/tire_model.cpp | 208 +++++++++------ .../estimation/tire_model.hpp | 252 +++++++++++++----- 2 files changed, 307 insertions(+), 153 deletions(-) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index a9c5a9b27e..ffca1d4028 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -11,10 +11,6 @@ namespace app::tv::estimation // static namespace :) namespace { - // [[nodiscard]] float safeMagnitude(const float value) - // { - // return std::fmax(std::fabs(value), SMALL_EPSILON); - // } [[nodiscard]] float safeTemplateDenominator(const float value) { if (std::fabs(value) >= SMALL_EPSILON) @@ -24,6 +20,17 @@ namespace return value < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; } + [[nodiscard]] double safeTemplateDenominator(const double value) + { + const double small_epsilon = static_cast(SMALL_EPSILON); + + if (std::fabs(value) >= small_epsilon) + { + return value; + } + + return value < 0.0 ? -small_epsilon : small_epsilon; + } [[nodiscard]] autodiff::dual safeTemplateDenominator(const autodiff::dual &value) { const double primal = autodiff::val(value); @@ -31,13 +38,13 @@ namespace return value; return primal < 0.0 ? -SMALL_EPSILON : SMALL_EPSILON; } - float safeSignedDenominator(const float value) + [[nodiscard]] float safeSignedDenominator(const float value) { if (std::fabs(value) >= SMALL_EPSILON) return value; return value < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; } - constexpr float sign(const float value) + [[nodiscard]] constexpr float sign(const float value) { if (value > 0.0f) return 1.0f; @@ -45,7 +52,15 @@ namespace return -1.0f; return 0.0f; } - autodiff::dual sign(const autodiff::dual &value) + [[nodiscard]] constexpr double sign(const double value) + { + if (value > 0.0) + return 1.0; + if (value < 0.0) + return -1.0; + return 0.0; + } + [[nodiscard]] autodiff::dual sign(const autodiff::dual &value) { if (value > 0.0f) return 1.0f; @@ -53,6 +68,12 @@ namespace return -1.0f; return 0.0f; } + template [[nodiscard]] T clampLessEqualOne(const T &value) + { + using std::abs; + + return T(0.5f) * (value + T(1.0f) - abs(value - T(1.0f))); + } } // namespace template @@ -60,13 +81,8 @@ template { const T pure_fx_0 = computePureFx_N(fz_N, kappa); const auto coefficients = combinedFxMagicFormulaCoefficients(fz_N, alpha_rad, kappa); - // Low-speed safeguard: - // Below a small vehicle-speed threshold the tire model can predict unrealistically large - // forces because the slip calculation becomes ill-conditioned while the fitted Pacejka - // peak terms remain large. Blend the final combined-slip force output to zero so the model - // fades out smoothly instead of producing unstable low-speed force demands. - // - // Pacejka Page 181 (4.E50): F_x = G_xa * F_x0 + + // Combined-slip longitudinal force: F_x = G_xa * F_x0. return coefficients.g_xa * pure_fx_0; } template float TireModel::computeCombinedFx_N(float normal_load_N, float slip_angle_rad, const float &slip_ratio) const; @@ -83,10 +99,7 @@ template const float pure_fy_0 = computePureFy_N(fz_N, alpha_rad); const auto coefficients = combinedFyMagicFormulaCoefficients(fz_N, alpha_rad, kappa); - // Apply the same low-speed force-availability blend used for Fx so the optimizer sees a - // self-consistent pair of tire forces as the vehicle approaches a stop. - // - // Pacejka Pages 181-182 (4.E58): F_y = G_yk * F_y0 + S_vyk + // Combined-slip lateral force: F_y = G_yk * F_y0 + S_vyk. return coefficients.g_yk * T(pure_fy_0) + coefficients.s_vyk; } template float @@ -126,13 +139,13 @@ template autodiff::dual TireModel::computeCombinedFy_N( // return (wheel_surface_speed_mps - effective_wheel_speed_mps) / safeMagnitude(effective_wheel_speed_mps); // } -//------------------------------------------ Pacejka 5.2 ----------------------------------// +//------------------------------------------ Pacejka MF 6.2 ----------------------------------// template TireModel::PureFxMagicFormulaCoefficients TireModel::pureFxMagicFormulaCoefficients(const float normal_load_N, const T &slip_ratio) const { - // Assumes lambda terms are 1.0, gamma = 0, and pressure effects are captured by the fixed 12_PSI fitted row. + // Reduced MF 6.2 assumptions: gamma = 0, dpi = 0, and zeta_0..zeta_4 = 1. const float clamped_normal_load_N = std::fmax(normal_load_N, 0.0f); const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); const float s_hx = pureFx_Sh(normalized_load_delta); @@ -158,7 +171,7 @@ TireModel::PureFxMagicFormulaCoefficients TireModel::PureFyMagicFormulaCoefficients TireModel::pureFyMagicFormulaCoefficients(const float fz, const float alpha) const { - // Assumes gamma = 0, lambda terms are 1.0, and pressure effects are captured by the fixed 12_PSI fitted row. + // Reduced MF 6.2 assumptions: gamma = 0, dpi = 0, and the K_ygamma / S_Hyy / S_Vyy branch collapses to zero. const float clamped_normal_load_N = std::fmax(fz, 0.0f); const float normalized_load_delta = normalizedLoadDelta(clamped_normal_load_N); const float s_hy = pureFy_Sh(normalized_load_delta); @@ -195,7 +208,7 @@ float TireModel::computePureFy_N(const float fz_N, const float alpha) const { const auto coefficients = pureFyMagicFormulaCoefficients(fz_N, alpha); - // Pacejka Page 180 (4.E19): F_y0 + // Pure-slip lateral force: F_y0. const float b_y_alpha_y = coefficients.b_y * coefficients.alpha_y; return coefficients.d_y * std::sin( @@ -206,25 +219,26 @@ float TireModel::computePureFy_N(const float fz_N, const float alpha) const constexpr float TireModel::combinedFx_SHxa() const { - // Pacejka Page 181 (4.E57): S_Hxa + // MF 6.2 combined-slip horizontal shift S_Hxalpha. return fit_comb_fx_.rHx1; } constexpr float TireModel::combinedFx_Alpha_s(const float alpha) const { - // Pacejka Page 181 (4.E53): alpha_s = alpha* + S_Hxa - return alpha + combinedFx_SHxa(); + // The torque-vectoring stack provides slip angle with the opposite sign to the ISO-W/Pacejka convention. + // Convert once at the tire-model boundary so the public API stays unchanged and Fy is not mirrored. + return pacejkaSlipAngle(alpha) + combinedFx_SHxa(); } constexpr float TireModel::combinedFx_Cxa() const { - // Pacejka Page 181 (4.E55): C_xa + // MF 6.2 combined-slip shape factor C_xalpha. return fit_comb_fx_.rCx1; } float TireModel::combinedFx_Exa(const float normalized_load_delta) const { - // Pacejka Page 181 (4.E56): E_xa with reduced-model assumptions and E_xa <= 1. + // MF 6.2 combined-slip curvature E_xalpha with the reduced gamma = 0 form and E <= 1 clamp. return std::fmin(fit_comb_fx_.rEx1 + fit_comb_fx_.rEx2 * normalized_load_delta, 1.0f); } @@ -233,8 +247,8 @@ template T TireModel::combinedFx_Bxa(const T &kappa) const using std::atan; using std::cos; - // Pacejka Page 181 (4.E54): B_xa with gamma* = 0 and lambda_xa = 1. - return T(fit_comb_fx_.rBx1) * cos(atan(T(fit_comb_fx_.rBx2) * kappa)); + // MF 6.2 combined-slip stiffness B_xalpha with gamma = 0. + return T(fit_comb_fx_.rBx1) * cos(atan(T(fit_comb_fx_.rBx2) * kappa)) * T(scaling_factors_.LXAL); } template T TireModel::combinedFx_Gxao(const CombinedFxMagicFormulaCoefficients &coefficients) const @@ -242,7 +256,7 @@ template T TireModel::combinedFx_Gxao(const CombinedFxMagicFor using std::atan; using std::cos; - // Pacejka Page 181 (4.E52): G_xao + // MF 6.2 reference combined-slip reduction G_xalpha0. const T u = coefficients.b_xa * coefficients.s_hxa; const T phi = u - coefficients.e_xa * (u - atan(u)); return cos(coefficients.c_xa * atan(phi)); @@ -253,7 +267,7 @@ template T TireModel::combinedFx_Gxa(const CombinedFxMagicForm using std::atan; using std::cos; - // Pacejka Page 181 (4.E51): G_xa with G_xao > 0 protected by a signed epsilon denominator. + // MF 6.2 combined-slip reduction G_xalpha with G_xalpha0 protected by a signed epsilon denominator. const T u = coefficients.b_xa * coefficients.alpha_s; const T phi = u - coefficients.e_xa * (u - atan(u)); const T numerator = cos(coefficients.c_xa * atan(phi)); @@ -287,32 +301,35 @@ TireModel::CombinedFxMagicFormulaCoefficients TireModel::combinedFxMagicFormu float TireModel::combinedFy_SHyk(const float normalized_load_delta) const { - // Pacejka Page 182 (4.E65): S_Hyk + // MF 6.2 combined-slip horizontal shift S_Hykappa. return fit_comb_fy_.rHy1 + fit_comb_fy_.rHy2 * normalized_load_delta; } template T TireModel::combinedFy_Kappa_s(const float normalized_load_delta, const T &slip_ratio) const { - // Pacejka Page 181 (4.E61): kappa_s = kappa + S_Hyk + // MF 6.2 shifted slip ratio kappa_s = kappa + S_Hykappa. return slip_ratio + T(combinedFy_SHyk(normalized_load_delta)); } constexpr float TireModel::combinedFy_Cyk() const { - // Pacejka Page 182 (4.E63): C_yk + // MF 6.2 combined-slip shape factor C_ykappa. return fit_comb_fy_.rCy1; } float TireModel::combinedFy_Eyk(const float normalized_load_delta) const { - // Pacejka Page 182 (4.E64): E_yk with reduced-model assumptions and E_yk <= 1. + // MF 6.2 combined-slip curvature E_ykappa with the reduced gamma = 0 form and E <= 1 clamp. return std::fmin(fit_comb_fy_.rEy1 + fit_comb_fy_.rEy2 * normalized_load_delta, 1.0f); } float TireModel::combinedFy_Byk(const float slip_angle_rad) const { - // Pacejka Page 182 (4.E62): B_yk with gamma* = 0 and lambda_yk = 1. - return fit_comb_fy_.rBy1 * std::cos(std::atan(fit_comb_fy_.rBy2 * (slip_angle_rad - fit_comb_fy_.rBy3))); + // MF 6.2 combined-slip stiffness B_ykappa with gamma = 0. + const float pacejka_alpha_rad = pacejkaSlipAngle(slip_angle_rad); + + return fit_comb_fy_.rBy1 * std::cos(std::atan(fit_comb_fy_.rBy2 * (pacejka_alpha_rad - fit_comb_fy_.rBy3))) * + scaling_factors_.LYKA; } float TireModel::combinedFy_Dvyk( @@ -320,23 +337,27 @@ float TireModel::combinedFy_Dvyk( const float normalized_load_delta, const float slip_angle_rad) const { - // Pacejka Page 182 (4.E67): D_vyk with gamma* = 0 and zeta_2 = 1. + // MF 6.2 combined-slip lateral offset amplitude D_vyk with gamma = 0 and zeta_2 = 1. + const float pacejka_alpha_rad = pacejkaSlipAngle(slip_angle_rad); + return pureFy_mu(normalized_load_delta) * normal_load_N * (fit_comb_fy_.rVy1 + fit_comb_fy_.rVy2 * normalized_load_delta) * - std::cos(std::atan(fit_comb_fy_.rVy4 * slip_angle_rad)); + std::cos(std::atan(fit_comb_fy_.rVy4 * pacejka_alpha_rad)); } template T TireModel::combinedFy_Svyk(const T &d_vyk, const T &slip_ratio) const { using std::sin, std::atan; - // Pacejka Page 182 (4.E66): S_vyk with lambda_vyk = 1. - return d_vyk * sin(T(fit_comb_fy_.rVy5) * atan(T(fit_comb_fy_.rVy6) * slip_ratio)); + // MF 6.2 combined-slip lateral offset S_vykappa. + return d_vyk * sin(T(fit_comb_fy_.rVy5) * atan(T(fit_comb_fy_.rVy6) * slip_ratio)) * + T(scaling_factors_.LVYKA); } template T TireModel::combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients &coefficients) const { using std::cos, std::atan; - // Pacejka Page 181 (4.E60): G_yko + + // MF 6.2 reference combined-slip reduction G_ykappa0. const T u = coefficients.b_yk * coefficients.s_hyk; const T phi = u - coefficients.e_yk * (u - atan(u)); return cos(coefficients.c_yk * atan(phi)); @@ -346,7 +367,7 @@ template T TireModel::combinedFy_Gyk(const CombinedFyMagicForm { using std::cos, std::atan; - // Pacejka Page 181 (4.E59): G_yk with G_yko > 0 protected by a signed epsilon denominator. + // MF 6.2 combined-slip reduction G_ykappa with G_ykappa0 protected by a signed epsilon denominator. const T u = coefficients.b_yk * coefficients.kappa_s; const T phi = u - coefficients.e_yk * (u - atan(u)); const T numerator = cos(coefficients.c_yk * atan(phi)); @@ -381,124 +402,149 @@ TireModel::CombinedFyMagicFormulaCoefficients TireModel::combinedFyMagicFormu return coefficients; } -//-------------------------------------------------------------------- MJ Pure Coefficents 5.2 +//-------------------------------------------------------------------- MF 6.2 Pure Coefficients //----------------------------------------------------------------------// -float TireModel::normalizedLoadDelta(const float normal_load_N) +float TireModel::referenceNormalLoad_N() const { - return (normal_load_N - NOMINAL_FZ_N) / safeSignedDenominator(NOMINAL_FZ_N); + return FZ0 * scaling_factors_.LFZ0; +} + +constexpr float TireModel::pacejkaSlipAngle(const float slip_angle_rad) +{ + return -slip_angle_rad; +} + +float TireModel::normalizedLoadDelta(const float normal_load_N) const +{ + const float reference_normal_load_N = referenceNormalLoad_N(); + return (normal_load_N - reference_normal_load_N) / safeSignedDenominator(reference_normal_load_N); } constexpr float TireModel::pureFx_Sh(const float normalized_load_delta) const { - // Pacejka Page 179 (4.E17): S_Hx - return fit_pure_fx_.hx_1 + fit_pure_fx_.hx_2 * normalized_load_delta; + // MF 6.2 pure-slip horizontal shift S_Hx. + return (fit_pure_fx_.p_Hx1 + fit_pure_fx_.p_Hx2 * normalized_load_delta) * scaling_factors_.LHX; } template T TireModel::pureFx_Kappa(const float normalized_load_delta, const T &slip_ratio) const { - // Pacejka Page 179 (4.E10): kappa_x = kappa + S_Hx + // MF 6.2 shifted slip ratio kappa_x = kappa + S_Hx. return slip_ratio + T(pureFx_Sh(normalized_load_delta)); } constexpr float TireModel::pureFx_C() const { - // Pacejka Page 179 (4.E11): C_x - return fit_pure_fx_.cx_1; + // MF 6.2 pure-slip shape factor C_x. + return fit_pure_fx_.p_Cx1 * scaling_factors_.LCX; } constexpr float TireModel::pureFx_mu(const float normalized_load_delta) const { - // Pacejka Page 179 (4.E13): mu_x with pressure/camber terms reduced to the current assumptions. - return fit_pure_fx_.dx_1 + fit_pure_fx_.dx_2 * normalized_load_delta; + // MF 6.2 pure-slip friction mu_x with gamma = 0 and dpi = 0, so p_Dx3 is inactive here. + return (fit_pure_fx_.p_Dx1 + fit_pure_fx_.p_Dx2 * normalized_load_delta) * scaling_factors_.LMUX; } constexpr float TireModel::pureFx_D(const float normal_load_N, const float normalized_load_delta) const { - // Pacejka Page 179 (4.E12): D_x = mu_x * F_z + // MF 6.2 pure-slip peak factor D_x = mu_x * F_z. return pureFx_mu(normalized_load_delta) * normal_load_N; } template T TireModel::pureFx_E(const float normalized_load_delta, const T &kappa_x) const { - // Pacejka Page 179 (4.E14): E_x + // MF 6.2 pure-slip curvature E_x with the reduced gamma = 0, dpi = 0 form and E <= 1 clamp. const float normalized_load_delta_squared = normalized_load_delta * normalized_load_delta; - const float base_e_x = fit_pure_fx_.ex_1 + fit_pure_fx_.ex_2 * normalized_load_delta + - fit_pure_fx_.ex_3 * normalized_load_delta_squared; - return base_e_x * (1.0f - fit_pure_fx_.ex_4 * sign(kappa_x)); + const T e_x = (T(fit_pure_fx_.p_Ex1 + fit_pure_fx_.p_Ex2 * normalized_load_delta + + fit_pure_fx_.p_Ex3 * normalized_load_delta_squared) * + (T(1.0f) - T(fit_pure_fx_.p_Ex4) * sign(kappa_x))) * + T(scaling_factors_.LEX); + + return clampLessEqualOne(e_x); } float TireModel::pureFx_K(const float normal_load_N, const float normalized_load_delta) const { - // Pacejka Page 179 (4.E15): K_xk - return normal_load_N * (fit_pure_fx_.kx_1 + fit_pure_fx_.kx_2 * normalized_load_delta) * - std::exp(fit_pure_fx_.kx_3 * normalized_load_delta); + // MF 6.2 pure-slip longitudinal stiffness K_xkappa with dpi = 0. + return normal_load_N * (fit_pure_fx_.p_Kx1 + fit_pure_fx_.p_Kx2 * normalized_load_delta) * + std::exp(fit_pure_fx_.p_Kx3 * normalized_load_delta) * scaling_factors_.LKX; } float TireModel::pureFx_B(const float slip_stiffness, const float shape_factor, const float peak_factor) { - // Pacejka Page 179 (4.E16): B_x = K_xk / (C_x * D_x + epsilon_x) + // MF 6.2 pure-slip stiffness factor B_x = K_xkappa / (C_x * D_x + epsilon_x). return slip_stiffness / safeSignedDenominator(shape_factor * peak_factor); } constexpr float TireModel::pureFx_Sv(const float normal_load_N, const float normalized_load_delta) const { - // Pacejka Page 179 (4.E18): S_Vx - return normal_load_N * (fit_pure_fx_.vx_1 + fit_pure_fx_.vx_2 * normalized_load_delta); + // MF 6.2 pure-slip vertical shift S_Vx. With dpi = 0, p_Vx pressure terms remain inactive. + return normal_load_N * (fit_pure_fx_.p_Vx1 + fit_pure_fx_.p_Vx2 * normalized_load_delta) * scaling_factors_.LVX * + scaling_factors_.LMUX; } constexpr float TireModel::pureFy_Sh(const float normalized_load_delta) const { - // Pacejka Page 180 (4.E27): S_Hy with gamma terms reduced to zero. - return fit_pure_fy_.hy_1 + fit_pure_fy_.hy_2 * normalized_load_delta; + // MF 6.2 pure-slip horizontal shift S_Hy. With gamma = 0, S_Hyy collapses to zero so only S_Hy0 remains. + return (fit_pure_fy_.p_Hy1 + fit_pure_fy_.p_Hy2 * normalized_load_delta) * scaling_factors_.LHY; } constexpr float TireModel::pureFy_Alpha(const float normalized_load_delta, const float slip_angle_rad) const { - // Pacejka Page 180 (4.E20): alpha_y = alpha* + S_Hy - return slip_angle_rad + pureFy_Sh(normalized_load_delta); + // Convert the external slip-angle convention into Pacejka's ISO-W sign before evaluating Fy. + return pacejkaSlipAngle(slip_angle_rad) + pureFy_Sh(normalized_load_delta); } constexpr float TireModel::pureFy_C() const { - // Pacejka Page 180 (4.E21): C_y - return fit_pure_fy_.cy_1; + // MF 6.2 pure-slip shape factor C_y. + return fit_pure_fy_.p_Cy1 * scaling_factors_.LCY; } constexpr float TireModel::pureFy_mu(const float normalized_load_delta) const { - // Pacejka Page 180 (4.E23): mu_y with pressure/camber terms reduced to the current assumptions. - return fit_pure_fy_.dy_1 + fit_pure_fy_.dy_2 * normalized_load_delta; + // MF 6.2 pure-slip friction mu_y with gamma = 0 and dpi = 0, so p_Dy3 is inactive here. + return (fit_pure_fy_.p_Dy1 + fit_pure_fy_.p_Dy2 * normalized_load_delta) * scaling_factors_.LMUY; } constexpr float TireModel::pureFy_D(const float normal_load_N, const float normalized_load_delta) const { - // Pacejka Page 180 (4.E22): D_y = mu_y * F_z + // MF 6.2 pure-slip peak factor D_y = mu_y * F_z. return pureFy_mu(normalized_load_delta) * normal_load_N; } constexpr float TireModel::pureFy_E(const float normalized_load_delta, const float alpha_y) const { - // Pacejka Page 180 (4.E24): E_y with gamma terms reduced to zero. - return (fit_pure_fy_.ey_1 + fit_pure_fy_.ey_2 * normalized_load_delta) * (1.0f - fit_pure_fy_.ey_3 * sign(alpha_y)); + // MF 6.2 pure-slip curvature E_y with gamma = 0 and E <= 1 clamp. + const float e_y = (fit_pure_fy_.p_Ey1 + fit_pure_fy_.p_Ey2 * normalized_load_delta) * + (1.0f - fit_pure_fy_.p_Ey3 * sign(alpha_y)) * scaling_factors_.LEY; + + return std::fmin(e_y, 1.0f); } float TireModel::pureFy_K(const float normal_load_N) const { - // Pacejka Page 180 (4.E25): K_yalpha with gamma/pressure terms reduced to zero. - const float denominator = safeSignedDenominator(fit_pure_fy_.ky_2 * NOMINAL_FZ_N); - return fit_pure_fy_.ky_1 * NOMINAL_FZ_N * std::sin(2.0f * std::atan(normal_load_N / denominator)); + using std::atan; + using std::sin; + + // MF 6.2 pure-slip cornering stiffness K_yalpha. gamma- and pressure-only terms are inactive at gamma = 0, dpi = 0. + const float reference_normal_load_N = referenceNormalLoad_N(); + const float denominator = safeSignedDenominator(fit_pure_fy_.p_Ky2 * reference_normal_load_N); + + return fit_pure_fy_.p_Ky1 * reference_normal_load_N * + sin(fit_pure_fy_.p_Ky4 * atan(normal_load_N / denominator)) * scaling_factors_.LKY; } float TireModel::pureFy_B(const float cornering_stiffness, const float shape_factor, const float peak_factor) { - // Pacejka Page 180 (4.E26): B_y = K_yalpha / (C_y * D_y + epsilon_y) + // MF 6.2 pure-slip stiffness factor B_y = K_yalpha / (C_y * D_y + epsilon_y). return cornering_stiffness / safeSignedDenominator(shape_factor * peak_factor); } constexpr float TireModel::pureFy_Sv(const float normal_load_N, const float normalized_load_delta) const { - // Pacejka Page 180 (4.E29): S_Vy with gamma terms reduced to zero. - return normal_load_N * (fit_pure_fy_.vy_1 + fit_pure_fy_.vy_2 * normalized_load_delta); + // MF 6.2 pure-slip vertical shift S_Vy. With gamma = 0, S_Vyy collapses to zero so only S_Vy0 remains. + return normal_load_N * (fit_pure_fy_.p_Vy1 + fit_pure_fy_.p_Vy2 * normalized_load_delta) * scaling_factors_.LVY * + scaling_factors_.LMUY; } } // namespace app::tv::estimation diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index 7f231a35cb..c21a48aa92 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -18,49 +18,85 @@ class TireModel Rear }; - // note that these only exist for float, dual + // note that these only exist for float, double, dual template [[nodiscard]] T computeCombinedFx_N(float fz_N, float alpha_rad, const T &kappa) const; template [[nodiscard]] T computeCombinedFy_N(float fz_N, float alpha_rad, const T &kappa) const; struct TireFitPureParamFy { - float cy_1; - float dy_1; - float dy_2; - float dy_3; - float ey_1; - float ey_2; - float ey_3; - float ey_4; - float ky_1; - float ky_2; - float ky_3; - float hy_1; - float hy_2; - float hy_3; - float vy_1; - float vy_2; - float vy_3; - float vy_4; + float p_Cy1; + float p_Dy1; + float p_Dy2; + float p_Dy3; + float p_Ey1; + float p_Ey2; + float p_Ey3; + float p_Ey4; + float p_Ey5; + float p_Ky1; + float p_Ky2; + float p_Ky3; + float p_Ky4; + float p_Ky5; + float p_Ky6; + float p_Ky7; + float p_Hy1; + float p_Hy2; + float p_Vy1; + float p_Vy2; + float p_Vy3; + float p_Vy4; + float p_py1; + float p_py2; + float p_py3; + float p_py4; + float p_py5; }; struct TireFitPureParamFx { - float cx_1; - float dx_1; - float dx_2; - float dx_3; - float ex_1; - float ex_2; - float ex_3; - float ex_4; - float kx_1; - float kx_2; - float kx_3; - float hx_1; - float hx_2; - float vx_1; - float vx_2; + float p_Cx1; + float p_Dx1; + float p_Dx2; + float p_Dx3; + float p_Ex1; + float p_Ex2; + float p_Ex3; + float p_Ex4; + float p_Kx1; + float p_Kx2; + float p_Kx3; + float p_Hx1; + float p_Hx2; + float p_Vx1; + float p_Vx2; + float p_px1; + float p_px2; + float p_px3; + float p_px4; + }; + + struct TireScalingFactors + { + float LFZ0; + float LCX; + float LMUX; + float LEX; + float LKX; + float LHX; + float LVX; + float LGAX; + float LXAL; + float LCY; + float LMUY; + float LEY; + float LKY; + float LKYG; + float LHY; + float LVY; + float LGAY; + float LYKA; + float LVYKA; }; struct TireFitCombParamFx @@ -100,8 +136,13 @@ class TireModel const TireFitPureParamFx &fit_pure_fx, const TireFitPureParamFy &fit_pure_fy, const TireFitCombParamFx &fit_comb_fx, - const TireFitCombParamFy &fit_comb_fy) - : fit_pure_fx_(fit_pure_fx), fit_pure_fy_(fit_pure_fy), fit_comb_fx_(fit_comb_fx), fit_comb_fy_(fit_comb_fy) + const TireFitCombParamFy &fit_comb_fy, + const TireScalingFactors &scaling_factors) + : fit_pure_fx_(fit_pure_fx), + fit_pure_fy_(fit_pure_fy), + fit_comb_fx_(fit_comb_fx), + fit_comb_fy_(fit_comb_fy), + scaling_factors_(scaling_factors) { } @@ -152,15 +193,18 @@ class TireModel T g_yk = T(0.0f); }; - static constexpr float NOMINAL_FZ_N = 750.0f; + static constexpr float FZ0 = 890.0f; + static constexpr float R0 = 0.230885999f; + static constexpr float P0 = 82737.12f; //-------------------------------------------------------------------- Class Helpers //----------------------------------------------------------------------// - [[nodiscard]] static float normalizedLoadDelta(float normal_load_N); - // Reduced-model assumptions for combined slip in this pass: - // gamma* = 0, lambda_xa = 1, lambda_yk = 1, lambda_vyk = 1, zeta_2 = 1. - // Pressure dependence is captured by the fixed 12_PSI fitted parameter row. - //-------------------------------------------------------------------- Pure Pacejka 5.2 Helpers + [[nodiscard]] float normalizedLoadDelta(float normal_load_N) const; + [[nodiscard]] float referenceNormalLoad_N() const; + // Reduced MF 6.2 assumptions in this implementation: + // gamma = 0, dpi = 0, zeta_0..zeta_4 = 1, and the scaling factors below come from the fixed Hoosier fit row. + //-------------------------------------------------------------------- Pure Pacejka MF 6.2 Helpers //----------------------------------------------------------------------// + [[nodiscard]] static constexpr float pacejkaSlipAngle(float slip_angle_rad); [[nodiscard]] constexpr float pureFx_Sh(float normalized_load_delta) const; template [[nodiscard]] T pureFx_Kappa(float normalized_load_delta, const T &slip_ratio) const; [[nodiscard]] constexpr float pureFx_C() const; @@ -183,7 +227,7 @@ class TireModel [[nodiscard]] PureFxMagicFormulaCoefficients pureFxMagicFormulaCoefficients(float normal_load_N, const T &slip_ratio) const; [[nodiscard]] PureFyMagicFormulaCoefficients pureFyMagicFormulaCoefficients(float fz, float alpha) const; - //-------------------------------------------------------------------- Combined Pacejka 5.2 Helpers + //-------------------------------------------------------------------- Combined Pacejka MF 6.2 Helpers //----------------------------------------------------------------------// [[nodiscard]] constexpr float combinedFx_SHxa() const; [[nodiscard]] constexpr float combinedFx_Alpha_s(float alpha) const; @@ -220,6 +264,7 @@ class TireModel const TireFitPureParamFy &fit_pure_fy_; const TireFitCombParamFx &fit_comb_fx_; const TireFitCombParamFy &fit_comb_fy_; + const TireScalingFactors &scaling_factors_; }; class HoosierTireModel : public TireModel @@ -229,43 +274,105 @@ class HoosierTireModel : public TireModel If pressure becomes a runtime input later, add pressure interpolation or a refit against the raw tire data. */ static constexpr TireFitPureParamFx HOOSIER_FIT_PURE_FX_12_PSI = { - 0.008656294312f, 366.1925838f, -57.2826791f, 8.208916842f, 1.134744942f, - 0.04728092792f, -0.02191230313f, -0.01799018414f, 62.46585932f, 1.390484252f, - -0.3909753489f, 0.0005429909716f, -0.0007019708842f, 0.01286770906f, 0.004135955934f, + .p_Cx1 = 1.4779238369371919f, + .p_Dx1 = 2.181045396988853f, + .p_Dx2 = 0.07001659216024461f, + .p_Dx3 = 4.097157624686553f, + .p_Ex1 = 0.24999999999999986f, + .p_Ex2 = -0.4935998299083556f, + .p_Ex3 = 0.062308987383295614f, + .p_Ex4 = 0.49999999999999994f, + .p_Kx1 = 44.40869215364958f, + .p_Kx2 = -0.11467697610242364f, + .p_Kx3 = 0.005123300455879327f, + .p_Hx1 = 0.00028611316090635194f, + .p_Hx2 = -0.001728319335508632f, + .p_Vx1 = 0.004999999999999999f, + .p_Vx2 = 0.00477148979863604f, + .p_px1 = 5.985592437722123e-20f, + .p_px2 = -1.4999999999999998f, + .p_px3 = -0.45857497477578263f, + .p_px4 = -0.43069292241848617f, }; static constexpr TireFitPureParamFy HOOSIER_FIT_PURE_FY_12_PSI = { - 0.90721342f, -3.824114982f, -0.4280518359f, 15.56383821f, 1.218066219f, 0.3097318297f, - -0.02930740966f, 0.2068388689f, -54.60640233f, 2.122730258f, 1.671281037f, -0.004171875264f, - -0.00360720284f, -0.0787609381f, -0.1057253032f, -0.02043816084f, 0.83972231f, 1.494231303f, + .p_Cy1 = 1.1204027009158741f, + .p_Dy1 = 2.5264849720779265f, + .p_Dy2 = -0.32531957059268557f, + .p_Dy3 = 8.961207772979192f, + .p_Ey1 = -0.3535685826026044f, + .p_Ey2 = 0.49999999999999994f, + .p_Ey3 = 0.49999999999999994f, + .p_Ey4 = -4.3346742910317335e-12f, + .p_Ey5 = -9.999999999996396f, + .p_Ky1 = -58.87561425961986f, + .p_Ky2 = 2.6000000000000005f, + .p_Ky3 = 0.8244703642222122f, + .p_Ky4 = 1.7352802106825729f, + .p_Ky5 = 0.24677855458172623f, + .p_Ky6 = -4.063776528205966f, + .p_Ky7 = -1.9999999999999838f, + .p_Hy1 = 0.0018254580526529563f, + .p_Hy2 = 0.000636192957410061f, + .p_Vy1 = 0.049999999999999996f, + .p_Vy2 = -0.0036551964003752914f, + .p_Vy3 = 0.09833213814504116f, + .p_Vy4 = 0.046585547800409376f, + .p_py1 = 0.5693120879649871f, + .p_py2 = 0.9999999999962573f, + .p_py3 = -0.15182658503935315f, + .p_py4 = -1.6963735361954737e-22f, + .p_py5 = -0.9999999999931753f, }; static constexpr TireFitCombParamFx HOOSIER_FIT_COMB_FX_12_PSI = { - .rBx1 = 13.046f, - .rBx2 = 9.718f, - .rBx3 = 0.0f, - .rCx1 = 0.9995f, - .rEx1 = -0.4403f, - .rEx2 = -0.4663f, - .rHx1 = -0.0001f, + .rBx1 = 7.480599472060266f, + .rBx2 = 10.358691627632123f, + .rBx3 = 29.999999999999996f, + .rCx1 = 1.399999999995603f, + .rEx1 = -1.1134570691694545f, + .rEx2 = -0.5558333477828554f, + .rHx1 = -9.999999999999997e-07f, }; static constexpr TireFitCombParamFy HOOSIER_FIT_COMB_FY_12_PSI = { - .rBy1 = 10.622f, - .rBy2 = 7.82f, - .rBy3 = 0.00204f, - .rBy4 = 0.0f, - .rCy1 = 1.0587f, - .rEy1 = 0.3148f, - .rEy2 = 0.00487f, - .rHy1 = 0.00947f, - .rHy2 = 0.00975f, - .rVy1 = 0.05187f, - .rVy2 = 0.00049f, - .rVy3 = 0.0f, - .rVy4 = 94.63f, - .rVy5 = 1.8914f, - .rVy6 = 23.8f, + .rBy1 = 20.05591682759158f, + .rBy2 = 19.99972340129322f, + .rBy3 = 9.403590493232056e-07f, + .rBy4 = -10.995639456729068f, + .rCy1 = 0.9048173729141128f, + .rEy1 = -0.7096982445643287f, + .rEy2 = 0.55848931432607f, + .rHy1 = 9.999999999999997e-07f, + .rHy2 = 9.999999999999784e-07f, + .rVy1 = 0.0009999999999999998f, + .rVy2 = 0.001999999999999954f, + .rVy3 = 0.049999999999999996f, + .rVy4 = -1.9999999999999998f, + .rVy5 = 2.4544048158091005f, + .rVy6 = 5.900331527230952f, + }; + + static constexpr TireScalingFactors HOOSIER_SCALING_FACTORS = { + .LFZ0 = 1.0f, + .LCX = 1.0f, + .LMUX = 0.65f, + .LEX = 1.0f, + .LKX = 1.0f, + .LHX = 1.0f, + .LVX = 1.0f, + .LGAX = 1.0f, + .LXAL = 1.0f, + .LCY = 1.0f, + .LMUY = 0.65f, + .LEY = 1.0f, + .LKY = 1.0f, + .LKYG = 1.0f, + .LHY = 1.0f, + .LVY = 1.0f, + .LGAY = 1.0f, + .LYKA = 1.0f, + .LVYKA = 1.0f, }; public: @@ -274,7 +381,8 @@ class HoosierTireModel : public TireModel HOOSIER_FIT_PURE_FX_12_PSI, HOOSIER_FIT_PURE_FY_12_PSI, HOOSIER_FIT_COMB_FX_12_PSI, - HOOSIER_FIT_COMB_FY_12_PSI) + HOOSIER_FIT_COMB_FY_12_PSI, + HOOSIER_SCALING_FACTORS) { } }; From 1328739118c18f185b343067973115b7ba6afc78 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 27 Apr 2026 10:38:43 -0700 Subject: [PATCH 34/35] tire model :) --- firmware/hexray/VC/plot_tests.cpp | 83 ++++++++++++++----- .../estimation/tire_model.cpp | 13 ++- .../estimation/tire_model.hpp | 2 +- 3 files changed, 67 insertions(+), 31 deletions(-) diff --git a/firmware/hexray/VC/plot_tests.cpp b/firmware/hexray/VC/plot_tests.cpp index 869d1bc216..f616904a5b 100644 --- a/firmware/hexray/VC/plot_tests.cpp +++ b/firmware/hexray/VC/plot_tests.cpp @@ -25,19 +25,18 @@ static void plot_combined_fx() kappas.push_back(kappa); } - // for (const float alpha_rad : { 0.3f, 0.25f, 0.2f, 0.15f, 0.1f, 0.05f, 0.0f }) - // {] - const float alpha_rad = 0; - std::vector fxs; - fxs.reserve(kappas.size()); - for (const double kappa : kappas) + for (const float alpha_rad : { 0.15f, 0.125f, 0.1f, 0.075f, 0.05f, 0.025f, 0.0f }) { - fxs.push_back(app::tv::estimation::tire_model.computeCombinedFx_N(fz_N, alpha_rad, kappa)); + std::vector fxs; + fxs.reserve(kappas.size()); + for (const double kappa : kappas) + { + fxs.push_back(app::tv::estimation::tire_model.computeCombinedFx_N(fz_N, alpha_rad, kappa)); + } + std::stringstream ss; + ss << "\\alpha = " << std::setprecision(2) << alpha_rad; + plt::named_plot(ss.str(), kappas, fxs); } - std::stringstream ss; - ss << "\\alpha = " << std::setprecision(2) << alpha_rad; - plt::named_plot(ss.str(), kappas, fxs); - // } plt::title("F_x as a function of \\kappa and \\alpha"); plt::xlabel("\\kappa"); @@ -47,19 +46,57 @@ static void plot_combined_fx() plt::show(); } +static void plot_combined_fy() +{ + constexpr int n_points = 301; // includes endpoints + constexpr float fz_N = 700.0f; + + std::vector alphas; + alphas.reserve(n_points); + for (int i = 0; i < n_points; ++i) + { + constexpr float alpha_max = 0.6f; + constexpr float alpha_min = -0.6f; + const float t = static_cast(i) / static_cast(n_points - 1); + const float alpha = alpha_min + t * (alpha_max - alpha_min); + alphas.push_back(alpha); + } + + for (const double kappa : { 0.3, 0.25, 0.2, 0.15, 0.1, 0.05, 0.0 }) + { + std::vector fys; + fys.reserve(alphas.size()); + for (const float alpha : alphas) + { + fys.push_back(app::tv::estimation::tire_model.computeCombinedFy_N(fz_N, alpha, kappa)); + } + std::stringstream ss; + ss << "\\kappa = " << std::setprecision(2) << kappa; + plt::named_plot(ss.str(), alphas, fys); + } + + plt::title("F_y as a function of \\alpha and \\kappa"); + plt::xlabel("\\alpha"); + plt::ylabel("F_y (N)"); + plt::grid(true); + plt::legend(); + plt::show(); +} + int main() { - constexpr app::tv::shared_datatypes::VehicleState state{ - .v_x_mps = 10, - .v_y_mps = 0, - .yaw_rate_radps = 0, - .a_x_mps2 = 1, - .a_y_mps2 = 0, - .apps = 0.2, - .delta = 0, - }; - const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0, 5.0); - std::cout << "Optimal slip found: " << fl << " " << fr << " " << rl << " " << rr << std::endl; + // constexpr app::tv::shared_datatypes::VehicleState state{ + // .v_x_mps = 10, + // .v_y_mps = 0, + // .yaw_rate_radps = 0, + // .a_x_mps2 = 1, + // .a_y_mps2 = 0, + // .apps = 0.2, + // .delta = 0, + // }; + // const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0, 5.0); + // std::cout << "Optimal slip found: " << fl << " " << fr << " " << rl << " " << rr << std::endl; - plot_combined_fx(); + // plot_combined_fx(); + plot_combined_fy(); } \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp index ffca1d4028..bba223ab33 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -349,8 +349,7 @@ template T TireModel::combinedFy_Svyk(const T &d_vyk, const T { using std::sin, std::atan; // MF 6.2 combined-slip lateral offset S_vykappa. - return d_vyk * sin(T(fit_comb_fy_.rVy5) * atan(T(fit_comb_fy_.rVy6) * slip_ratio)) * - T(scaling_factors_.LVYKA); + return d_vyk * sin(T(fit_comb_fy_.rVy5) * atan(T(fit_comb_fy_.rVy6) * slip_ratio)) * T(scaling_factors_.LVYKA); } template T TireModel::combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients &coefficients) const @@ -455,8 +454,8 @@ template T TireModel::pureFx_E(const float normalized_load_del { // MF 6.2 pure-slip curvature E_x with the reduced gamma = 0, dpi = 0 form and E <= 1 clamp. const float normalized_load_delta_squared = normalized_load_delta * normalized_load_delta; - const T e_x = (T(fit_pure_fx_.p_Ex1 + fit_pure_fx_.p_Ex2 * normalized_load_delta + - fit_pure_fx_.p_Ex3 * normalized_load_delta_squared) * + const T e_x = (T(fit_pure_fx_.p_Ex1 + fit_pure_fx_.p_Ex2 * normalized_load_delta + + fit_pure_fx_.p_Ex3 * normalized_load_delta_squared) * (T(1.0f) - T(fit_pure_fx_.p_Ex4) * sign(kappa_x))) * T(scaling_factors_.LEX); @@ -513,7 +512,7 @@ constexpr float TireModel::pureFy_D(const float normal_load_N, const float norma return pureFy_mu(normalized_load_delta) * normal_load_N; } -constexpr float TireModel::pureFy_E(const float normalized_load_delta, const float alpha_y) const +float TireModel::pureFy_E(const float normalized_load_delta, const float alpha_y) const { // MF 6.2 pure-slip curvature E_y with gamma = 0 and E <= 1 clamp. const float e_y = (fit_pure_fy_.p_Ey1 + fit_pure_fy_.p_Ey2 * normalized_load_delta) * @@ -531,8 +530,8 @@ float TireModel::pureFy_K(const float normal_load_N) const const float reference_normal_load_N = referenceNormalLoad_N(); const float denominator = safeSignedDenominator(fit_pure_fy_.p_Ky2 * reference_normal_load_N); - return fit_pure_fy_.p_Ky1 * reference_normal_load_N * - sin(fit_pure_fy_.p_Ky4 * atan(normal_load_N / denominator)) * scaling_factors_.LKY; + return fit_pure_fy_.p_Ky1 * reference_normal_load_N * sin(fit_pure_fy_.p_Ky4 * atan(normal_load_N / denominator)) * + scaling_factors_.LKY; } float TireModel::pureFy_B(const float cornering_stiffness, const float shape_factor, const float peak_factor) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp index c21a48aa92..03cf53e0b4 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -219,7 +219,7 @@ class TireModel [[nodiscard]] constexpr float pureFy_C() const; [[nodiscard]] constexpr float pureFy_mu(float normalized_load_delta) const; [[nodiscard]] constexpr float pureFy_D(float normal_load_N, float normalized_load_delta) const; - [[nodiscard]] constexpr float pureFy_E(float normalized_load_delta, float alpha_y) const; + [[nodiscard]] float pureFy_E(float normalized_load_delta, float alpha_y) const; [[nodiscard]] float pureFy_K(float normal_load_N) const; [[nodiscard]] static float pureFy_B(float cornering_stiffness, float shape_factor, float peak_factor); [[nodiscard]] constexpr float pureFy_Sv(float normal_load_N, float normalized_load_delta) const; From 79d12c4e627ad7f16db7fed441438e7610338d49 Mon Sep 17 00:00:00 2001 From: Aditya-Dhiman4 Date: Sun, 7 Jun 2026 13:43:22 -0700 Subject: [PATCH 35/35] Optimizer converges, Fz temporarily removed --- firmware/hexray/VC/CMakeLists.txt | 2 +- .../controllers/torque_allocator.cpp | 56 ++++++++++++--- .../vehicle_state_estimator.hpp | 72 ++++++++++--------- .../app/torque_vectoring/torque_vectoring.cpp | 9 ++- .../torque_vectoring_matlab.h | 5 +- 5 files changed, 99 insertions(+), 45 deletions(-) diff --git a/firmware/hexray/VC/CMakeLists.txt b/firmware/hexray/VC/CMakeLists.txt index e853f5f232..7451566827 100644 --- a/firmware/hexray/VC/CMakeLists.txt +++ b/firmware/hexray/VC/CMakeLists.txt @@ -178,7 +178,7 @@ elseif ("${TARGET}" STREQUAL "test") add_library("hexray_VC_torque_vectoring" STATIC ${TV_SRCS}) target_include_directories("hexray_VC_torque_vectoring" PUBLIC "${INCLUDE_DIRS}") target_link_libraries("hexray_VC_torque_vectoring" PUBLIC "autodiff_interface" "eigen_interface" "m") - target_compile_options("hexray_VC_torque_vectoring" PRIVATE /MT) + target_compile_options("hexray_VC_torque_vectoring" PRIVATE /MT /bigobj) # MATPLOTLIB TESTING diff --git a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp index 1d3b8a272b..6492deece6 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -20,6 +20,7 @@ namespace app::tv::controllers::allocator namespace { // ---- Optimizer tuning ---- + // TODO: Should we gain schedule these based on speed? as we go faster prioritize lateral stability? constexpr double W_FX = 0.5f; constexpr double W_MZ = 0.5f; constexpr double W_R = 15.5f; @@ -29,6 +30,8 @@ namespace constexpr float NORMAL_MATRIX_EPS = 1e-6f; constexpr float STEP_TOLERANCE = 1e-5f; constexpr float COST_TOLERANCE = 1e-6f; + constexpr int MAX_LINE_SEARCH_ITER = 6; + constexpr float LINE_SEARCH_SHRINK = 0.5f; template using Vec6 = Eigen::Matrix; template using Vec4 = Eigen::Matrix; @@ -140,9 +143,23 @@ template SQRT_W_R * kappa[3], }; }; + const auto costAt = [&](const Vec4 &slip) -> float + { + const DualVec4 kappa{ + autodiff::dual(slip(0)), + autodiff::dual(slip(1)), + autodiff::dual(slip(2)), + autodiff::dual(slip(3)), + }; + const auto residual = residualVector(kappa); + const Vec6 residual_primal{ + autodiff::val(residual(0)), autodiff::val(residual(1)), autodiff::val(residual(2)), + autodiff::val(residual(3)), autodiff::val(residual(4)), autodiff::val(residual(5)), + }; + return static_cast(residual_primal.squaredNorm()); + }; Vec4 opt_slip{ 0, 0, 0, 0 }; // output variable - float previous_cost = std::numeric_limits::infinity(); uint32_t iter; for (iter = 0; iter < MAX_ITER; ++iter) { @@ -180,19 +197,40 @@ template break; } - Vec4 next_opt_slip = opt_slip + delta; - for (int i = 0; i < 4; ++i) - next_opt_slip(i) = std::clamp(next_opt_slip(i), -static_cast(SLIP_CLAMP), static_cast(SLIP_CLAMP)); // Least-squares cost: |r|_2^2 // This is used only for convergence monitoring; the actual update is driven by J^T J and J^T r above. const float cost = residuals_at_kappa_primal.squaredNorm(); - std::cout << "Iter " << iter << ": cost = " << cost << ", delta = " << delta.transpose().format(CleanFmt) - << "\n"; - if ((next_opt_slip - opt_slip).norm() < STEP_TOLERANCE || std::fabs(previous_cost - cost) < COST_TOLERANCE) + Vec4 next_opt_slip = opt_slip; + float next_cost = cost; + T alpha = 1.0f; + bool accepted_step = false; + for (int line_search_iter = 0; line_search_iter < MAX_LINE_SEARCH_ITER; ++line_search_iter) + { + Vec4 trial_slip = opt_slip + alpha * delta; + for (int i = 0; i < 4; ++i) + trial_slip(i) = std::clamp(trial_slip(i), -static_cast(SLIP_CLAMP), static_cast(SLIP_CLAMP)); + + const float trial_cost = costAt(trial_slip); + if (trial_cost < cost) + { + next_opt_slip = trial_slip; + next_cost = trial_cost; + accepted_step = true; + break; + } + + alpha *= LINE_SEARCH_SHRINK; + } + + std::cout << "Iter " << iter << ": cost = " << cost << ", next_cost = " << next_cost + << ", alpha = " << alpha << ", delta = " << delta.transpose().format(CleanFmt) << "\n"; + if (!accepted_step) break; - previous_cost = cost; - opt_slip = next_opt_slip; + const T step_norm = (next_opt_slip - opt_slip).norm(); + opt_slip = next_opt_slip; + if (step_norm < STEP_TOLERANCE || std::fabs(cost - next_cost) < COST_TOLERANCE) + break; } if (iter == MAX_ITER) { diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp index e515da0366..314683d081 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp @@ -46,12 +46,11 @@ template struct VehicleState [[nodiscard]] wheel_set alphas() const { const auto [fl_v, fr_v, rl_v, rr_v] = v_in_tire_frame(); - const auto [fl_rot, fr_rot, rl_rot, rr_rot] = delta; return { - std::atan2(fl_v.y, safe_vx(fl_v.x)) - fl_rot, - std::atan2(fr_v.y, safe_vx(fr_v.x)) - fr_rot, - std::atan2(rl_v.y, safe_vx(rl_v.x)) - rl_rot, - std::atan2(rr_v.y, safe_vx(rr_v.x)) - rr_rot, + std::atan2(fl_v.y, safe_vx(fl_v.x)), + std::atan2(fr_v.y, safe_vx(fr_v.x)), + std::atan2(rl_v.y, safe_vx(rl_v.x)), + std::atan2(rr_v.y, safe_vx(rr_v.x)), }; } @@ -98,38 +97,47 @@ template struct VehicleState (2.0f * vd_constants::TRACK_WIDTH_m); } + // TODO: NORMAL FORCE CAUSING SIGNIFICANT OCCSILATION IN THE SYSTEM [[nodiscard]] wheel_set est_Fz_N() const { - // static loads - static constexpr T STATIC_FRONT_AXLE_LOAD_N = - vd_constants::CAR_WEIGHT * (vd_constants::DIST_REAR_AXLE_CG_m / vd_constants::WHEELBASE_m); - static constexpr T STATIC_REAR_AXLE_LOAD_N = - vd_constants::CAR_WEIGHT * (vd_constants::DIST_FRONT_AXLE_CG_m / vd_constants::WHEELBASE_m); - static constexpr T STATIC_FRONT_WHEEL_LOAD_N = 0.5f * STATIC_FRONT_AXLE_LOAD_N; - static constexpr T STATIC_REAR_WHEEL_LOAD_N = 0.5f * STATIC_REAR_AXLE_LOAD_N; + // // static loads + // static constexpr T STATIC_FRONT_AXLE_LOAD_N = + // vd_constants::CAR_WEIGHT * (vd_constants::DIST_REAR_AXLE_CG_m / vd_constants::WHEELBASE_m); + // static constexpr T STATIC_REAR_AXLE_LOAD_N = + // vd_constants::CAR_WEIGHT * (vd_constants::DIST_FRONT_AXLE_CG_m / vd_constants::WHEELBASE_m); + // static constexpr T STATIC_FRONT_WHEEL_LOAD_N = 0.5f * STATIC_FRONT_AXLE_LOAD_N; + // static constexpr T STATIC_REAR_WHEEL_LOAD_N = 0.5f * STATIC_REAR_AXLE_LOAD_N; - // load transfer - const T long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(); - const T lat_load_tf = LAT_ACCEL_TERM_VERTICAL_FORCE(); + // // load transfer + // const T long_load_tf = LONG_ACCEL_TERM_VERTICAL_FORCE(); + // const T lat_load_tf = LAT_ACCEL_TERM_VERTICAL_FORCE(); - // downforce and cop components - const T down_force_n = est_downforceFz_N(); - // Assuming COP is a fraction of the total downforce - const T front_cop = dynamicCOPFront(), rear_cop = 1.0f - front_cop, right_cop = dynamicCOPRight(), - left_cop = 1.0f - right_cop; + // // downforce and cop components + // const T down_force_n = est_downforceFz_N(); + // // Assuming COP is a fraction of the total downforce + // const T front_cop = dynamicCOPFront(), rear_cop = 1.0f - front_cop, right_cop = dynamicCOPRight(), + // left_cop = 1.0f - right_cop; + // return { + // .fl = std::fmax( + // 0.0f, + // STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) - lat_load_tf + down_force_n * front_cop * left_cop), + // .fr = std::fmax( + // 0.0f, + // STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) + lat_load_tf + down_force_n * front_cop * right_cop), + // .rl = std::fmax( + // 0.0f, + // STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) - lat_load_tf + down_force_n * rear_cop * left_cop), + // .rr = std::fmax( + // 0.0f, + // STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) + lat_load_tf + down_force_n * rear_cop * right_cop), + // }; + // Temporarily pin normal load for allocator debugging. + static constexpr T CONSTANT_WHEEL_LOAD_N = vd_constants::CAR_WEIGHT / 4.0f; return { - .fl = std::fmax( - 0.0f, - STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) - lat_load_tf + down_force_n * front_cop * left_cop), - .fr = std::fmax( - 0.0f, - STATIC_FRONT_WHEEL_LOAD_N - (0.5f * long_load_tf) + lat_load_tf + down_force_n * front_cop * right_cop), - .rl = std::fmax( - 0.0f, - STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) - lat_load_tf + down_force_n * rear_cop * left_cop), - .rr = std::fmax( - 0.0f, - STATIC_REAR_WHEEL_LOAD_N + (0.5f * long_load_tf) + lat_load_tf + down_force_n * rear_cop * right_cop), + .fl = CONSTANT_WHEEL_LOAD_N, + .fr = CONSTANT_WHEEL_LOAD_N, + .rl = CONSTANT_WHEEL_LOAD_N, + .rr = CONSTANT_WHEEL_LOAD_N, }; } diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp index f804b4deb1..17c034c6c6 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -48,7 +48,8 @@ extern "C" void update_matlab( const double delta_fr, double kappas[4], double torque_max[4], - double torque_min[4]) + double torque_min[4], + double alphas[4]) { const VehicleState state = { .v_x_mps = v_x, .v_y_mps = v_y, @@ -78,6 +79,12 @@ extern "C" void update_matlab( torque_min[1] = k_torque_min.fr; torque_min[2] = k_torque_min.rl; torque_min[3] = k_torque_min.rr; + + const auto [alpha_fl, alpha_fr, alpha_rl, alpha_rr] = state.alphas(); + alphas[0] = alpha_fl; + alphas[1] = alpha_fr; + alphas[2] = alpha_rl; + alphas[3] = alpha_rr; } template ControlOutputAutonomous update_autonomous(const VehicleState &state) diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h index 000b31942f..62a5783cba 100644 --- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h @@ -16,10 +16,11 @@ extern "C" double delta_fr, double kappas[4], double torque_max[4], - double torque_min[4]); + double torque_min[4], + double alphas[4]); /** * Matlab wrapper for kappa_update */ void kappa_update_matlab(double kappas[4], double v_x, double oemgas[4]); -} \ No newline at end of file +}