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/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/CMakeLists.txt b/firmware/hexray/VC/CMakeLists.txt index e56fd58401..7451566827 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/" ) @@ -128,7 +130,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") @@ -143,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" @@ -163,6 +166,33 @@ 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) + + + # 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" + ) + 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 /bigobj) + + + # 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" + "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/controllers/torque_allocator.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..f616904a5b --- /dev/null +++ b/firmware/hexray/VC/plot_tests.cpp @@ -0,0 +1,102 @@ +// ReSharper disable CppTooWideScope +#include "matplotlibcpp.h" +#include "torque_vectoring/estimation/tire_model.hpp" +#include "torque_vectoring/controllers/torque_allocator.hpp" + +#include +#include +#include + +namespace plt = matplotlibcpp; + +static void plot_combined_fx() +{ + constexpr int n_points = 301; // includes endpoints + constexpr float fz_N = 700.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.15f, 0.125f, 0.1f, 0.075f, 0.05f, 0.025f, 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(); +} + +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; + + // plot_combined_fx(); + plot_combined_fy(); +} \ No newline at end of file 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/controllers_dyrc.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_dyrc.cpp new file mode 100644 index 0000000000..736a3ee146 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_dyrc.cpp @@ -0,0 +1,40 @@ +#include "controllers_dyrc.hpp" +#include "torque_vectoring/controllers/controllers_config.hpp" +#include "torque_vectoring/shared_datatypes/constants.hpp" + +using namespace app::tv::shared_datatypes::vd_constants; + +namespace app::tv::controllers::dyrc +{ + +// Configuration +static float ku = DYRC_ku; // understeer gradient + +// Control Scheme +static PID pid(PID_DYRC_config); + +// purely for debugging purposes +static float yaw_moment_Nm = 0.0f; +static float r_ref_rad = 0.0f; + +[[nodiscard]] float computeRefYawRate(const float steer_ang_rad, const float body_velx_mps) +{ + 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) +{ + 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() +{ + return yaw_moment_Nm; +} + +[[nodiscard]] float getRefYawRate() +{ + return r_ref_rad; +} +} // 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/controllers_dyrc.hpp similarity index 86% 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 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/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/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/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.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp new file mode 100644 index 0000000000..6492deece6 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.cpp @@ -0,0 +1,259 @@ +#include "torque_allocator.hpp" + +#include +#include + +#include +#include +#include +#include + +#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; + +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; + + 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; + 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; + 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", "[", "]"); + + 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); + 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 +[[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 + // 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 }; + // } + + 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, + // .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; + + // 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, 2 residuals) so the optimizer + // 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, const bool debug = false) -> 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_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 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); + + 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], + }; + }; + 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 + uint32_t iter; + for (iter = 0; iter < MAX_ITER; ++iter) + { + 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; + Mat64 jacobian_residual_at_kappa; + autodiff::jacobian( + residualVector, autodiff::wrt(kappa), autodiff::at(kappa), residual_at_kappa, jacobian_residual_at_kappa); + 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)), + }; + 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); + if (ldlt.info() != Eigen::Success) + { + std::cout << "optimizer :( 1\n" << ldlt.info() << std::endl; + debug(normal_matrix); + break; + } + 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"); + std::fflush(stdout); + break; + } + + // 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(); + 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; + + 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) + { + 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), + .rl = opt_slip(2), + .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 new file mode 100644 index 0000000000..3890758918 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/controllers/torque_allocator.hpp @@ -0,0 +1,27 @@ +#pragma once + +#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. + * + * 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 using the caller-provided per-wheel tire models. + */ +namespace allocator +{ + /** + * Run Gauss-Newton iterations to find optimal slip ratios. + * @return Per-wheel optimal slip ratios + */ + 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/controllers/yaw_rate_control/controllers_dyrc.cpp b/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp deleted file mode 100644 index 993dc7c7a2..0000000000 --- a/firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include "controllers_dyrc.hpp" -#include "torque_vectoring/controllers/controllers_config.hpp" -#include "torque_vectoring/datatypes/datatypes_vd_constants.hpp" - -using namespace app::tv::datatypes::vd_constants; - -namespace app::tv::controllers::dyrc -{ - -// Configuration -static float ku = DYRC_ku; // understeer gradient - -// Control Scheme -static PID pid(PID_DYRC_config); - -// purely for debugging purposes -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) -{ - 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) -{ - yaw_moment_Nm = pid.compute(computeRefYawRate(steer_ang_rad, body_velx_mps), r_actual_rad, 0.0f); - return yaw_moment_Nm; -} - -[[nodiscard]] float getYawMoment() -{ - return yaw_moment_Nm; -} - -[[nodiscard]] float getRefYawRate() -{ - return r_ref_rad; -} -} // namespace app::tv::controllers::dyrc \ No newline at end of file diff --git a/firmware/hexray/VC/src/app/torque_vectoring/datatypes/datatypes.hpp b/firmware/hexray/VC/src/app/torque_vectoring/datatypes/datatypes.hpp deleted file mode 100644 index e69de29bb2..0000000000 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/estimation/tire_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp new file mode 100644 index 0000000000..bba223ab33 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp @@ -0,0 +1,549 @@ +#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 safeTemplateDenominator(const float value) + { + if (std::fabs(value) >= SMALL_EPSILON) + { + return value; + } + + 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); + if (std::fabs(static_cast(primal)) >= SMALL_EPSILON) + return value; + return primal < 0.0 ? -SMALL_EPSILON : SMALL_EPSILON; + } + [[nodiscard]] float safeSignedDenominator(const float value) + { + if (std::fabs(value) >= SMALL_EPSILON) + return value; + return value < 0.0f ? -SMALL_EPSILON : SMALL_EPSILON; + } + [[nodiscard]] constexpr float sign(const float value) + { + if (value > 0.0f) + return 1.0f; + if (value < 0.0f) + return -1.0f; + return 0.0f; + } + [[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; + if (value < 0.0f) + 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 +[[nodiscard]] T TireModel::computeCombinedFx_N(const float fz_N, const float alpha_rad, const T &kappa) const +{ + const T pure_fx_0 = computePureFx_N(fz_N, kappa); + const auto coefficients = combinedFxMagicFormulaCoefficients(fz_N, alpha_rad, kappa); + + // 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; +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, + const autodiff::dual &slip_ratio) const; + +template +[[nodiscard]] T TireModel::computeCombinedFy_N(const float fz_N, const float alpha_rad, const T &kappa) const +{ + const float pure_fy_0 = computePureFy_N(fz_N, alpha_rad); + const auto coefficients = combinedFyMagicFormulaCoefficients(fz_N, alpha_rad, kappa); + + // 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 + 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, + const autodiff::dual &slip_ratio) 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 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::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 MF 6.2 ----------------------------------// + +template +TireModel::PureFxMagicFormulaCoefficients + TireModel::pureFxMagicFormulaCoefficients(const float normal_load_N, const T &slip_ratio) const +{ + // 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); + 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, + .b_x = T(b_x), + .c_x = T(c_x), + .d_x = T(d_x), + .e_x = e_x, + .s_vx = T(s_vx), + }; +} + +TireModel::PureFyMagicFormulaCoefficients + TireModel::pureFyMagicFormulaCoefficients(const float fz, const float alpha) const +{ + // 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); + 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); + 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, + .e_y = e_y, + .s_vy = s_vy, + }; +} + +template T TireModel::computePureFx_N(const float fz, const T &kappa) const +{ + using std::sin, std::atan; + + 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 fz_N, const float alpha) const +{ + const auto coefficients = pureFyMagicFormulaCoefficients(fz_N, alpha); + + // Pure-slip lateral force: 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)))) + + coefficients.s_vy; +} + +constexpr float TireModel::combinedFx_SHxa() const +{ + // MF 6.2 combined-slip horizontal shift S_Hxalpha. + return fit_comb_fx_.rHx1; +} + +constexpr float TireModel::combinedFx_Alpha_s(const float alpha) const +{ + // 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 +{ + // MF 6.2 combined-slip shape factor C_xalpha. + return fit_comb_fx_.rCx1; +} + +float TireModel::combinedFx_Exa(const float normalized_load_delta) const +{ + // 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); +} + +template T TireModel::combinedFx_Bxa(const T &kappa) const +{ + using std::atan; + using std::cos; + + // 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 +{ + using std::atan; + using std::cos; + + // 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)); +} + +template T TireModel::combinedFx_Gxa(const CombinedFxMagicFormulaCoefficients &coefficients) const +{ + using std::atan; + using std::cos; + + // 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)); + + 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), + }; + + coefficients.g_xao = combinedFx_Gxao(coefficients); + coefficients.g_xa = combinedFx_Gxa(coefficients); + return coefficients; +} + +float TireModel::combinedFy_SHyk(const float normalized_load_delta) const +{ + // 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 +{ + // 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 +{ + // MF 6.2 combined-slip shape factor C_ykappa. + return fit_comb_fy_.rCy1; +} + +float TireModel::combinedFy_Eyk(const float normalized_load_delta) const +{ + // 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 +{ + // 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( + const float normal_load_N, + const float normalized_load_delta, + const float slip_angle_rad) const +{ + // 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 * pacejka_alpha_rad)); +} + +template T TireModel::combinedFy_Svyk(const T &d_vyk, const T &slip_ratio) const +{ + 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); +} + +template T TireModel::combinedFy_Gyko(const CombinedFyMagicFormulaCoefficients &coefficients) const +{ + using std::cos, std::atan; + + // 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)); +} + +template T TireModel::combinedFy_Gyk(const CombinedFyMagicFormulaCoefficients &coefficients) const +{ + using std::cos, std::atan; + + // 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)); + + 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; +} + +//-------------------------------------------------------------------- MF 6.2 Pure Coefficients +//----------------------------------------------------------------------// + +float TireModel::referenceNormalLoad_N() const +{ + 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 +{ + // 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 +{ + // 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 +{ + // 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 +{ + // 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 +{ + // 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 +{ + // 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) * + (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 +{ + // 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) +{ + // 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 +{ + // 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 +{ + // 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 +{ + // 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 +{ + // 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 +{ + // 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 +{ + // MF 6.2 pure-slip peak factor D_y = mu_y * F_z. + return pureFy_mu(normalized_load_delta) * normal_load_N; +} + +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) * + (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 +{ + 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) +{ + // 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 +{ + // 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 new file mode 100644 index 0000000000..03cf53e0b4 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.hpp @@ -0,0 +1,392 @@ +#pragma once +#include "torque_vectoring/shared_datatypes/decimal_dual.hpp" + +namespace app::tv::estimation +{ +class TireModel +{ + public: + enum class WheelSide + { + Left, + Right + }; + + enum class WheelAxle + { + Front, + Rear + }; + + // 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 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 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 + { + 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; + }; + + 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, + 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) + { + } + + private: + template struct PureFxMagicFormulaCoefficients + { + 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 e_y = 0.0f; + float s_vy = 0.0f; + }; + + 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 float FZ0 = 890.0f; + static constexpr float R0 = 0.230885999f; + static constexpr float P0 = 82737.12f; + //-------------------------------------------------------------------- Class 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; + [[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]] 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; + [[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]] 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; + template + [[nodiscard]] PureFxMagicFormulaCoefficients + pureFxMagicFormulaCoefficients(float normal_load_N, const T &slip_ratio) const; + [[nodiscard]] PureFyMagicFormulaCoefficients pureFyMagicFormulaCoefficients(float fz, float alpha) const; + //-------------------------------------------------------------------- Combined Pacejka MF 6.2 Helpers + //----------------------------------------------------------------------// + [[nodiscard]] constexpr float combinedFx_SHxa() 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 &kappa) 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 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_; + const TireFitCombParamFx &fit_comb_fx_; + const TireFitCombParamFy &fit_comb_fy_; + const TireScalingFactors &scaling_factors_; +}; + +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 HOOSIER_FIT_PURE_FX_12_PSI = { + .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 = { + .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 = 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 = 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: + 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, + HOOSIER_SCALING_FACTORS) + { + } +}; + +inline constexpr HoosierTireModel tire_model{}; + +} // 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 new file mode 100644 index 0000000000..f8aa56c21a --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp @@ -0,0 +1,239 @@ +#include "vehicle_state_estimator.hpp" + +#include +#include + +#include "torque_vectoring/shared_datatypes/constants.hpp" + +using namespace app::tv::shared_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; + + constexpr std::size_t VX = 0; + constexpr std::size_t VY = 1; + constexpr std::size_t R = 2; + constexpr std::size_t MZ = 3; + + constexpr std::size_t AX = 0; + constexpr std::size_t AY = 1; + + 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]] autodiff::dual measurementYawRate(const State &x) + { + return x(static_cast(R)); + } + + [[nodiscard]] autodiff::dual measurementYawMoment(const State &x) + { + return x(static_cast(MZ)); + } + + [[nodiscard]] consteval std::array createStateFunctions() + { + return { { + stateTransitionVx, + stateTransitionVy, + stateTransitionYawRate, + stateTransitionYawMoment, + } }; + } + + [[nodiscard]] consteval std::array createMeasurementFunctions() + { + return { { + measurementVx, + measurementVy, + measurementYawRate, + measurementYawMoment, + } }; + } + + [[nodiscard]] 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]] 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; + } + + [[nodiscard]] StateVector initialState() + { + return StateVector::Zero(); + } + + [[nodiscard]] 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; + } + + [[nodiscard]] Measurement pseudoMeasurementFromWheelSpeeds( + 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); + 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; + } + + [[nodiscard]] Filter createFilter() + { + return Filter( + createStateFunctions(), createMeasurementFunctions(), processNoise(), measurementNoise(), initialState(), + initialCovariance()); + } +} // namespace + +namespace VehicleStateEstimator +{ + Filter filter_ = createFilter(); + void reset_filter() + { + filter_ = createFilter(); + } + + [[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 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); + + 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)), + .a_x_mps2 = u(static_cast(AX)), + .a_y_mps2 = u(static_cast(AY)), + .delta = { measured_steering_angle, measured_steering_angle, 0, 0 }, + }; + } + + 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 new file mode 100644 index 0000000000..289d88124d --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include "state_estimation/app_kalman_filter.hpp" +#include "torque_vectoring/torque_vectoring.hpp" +#include "torque_vectoring/shared_datatypes/wheel_set.hpp" + +namespace app::tv::estimation +{ +struct Measurements +{ + // sensor measurements + const float ax; + const float ay; + const float yaw_rate; + + // driver controls + const float delta; + const float apps; + + const shared_datatypes::wheel_set omegas; +}; + +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(); + // 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/datatypes/datatypes_vd_constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp similarity index 57% 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 index 507b85baf2..a9b6b2e6be 100644 --- 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 @@ -1,16 +1,22 @@ #pragma once #include "util_units.hpp" +#include -namespace app::tv::datatypes::vd_constants +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 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,12 +27,16 @@ 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; // ============================================================================= // 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; 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 = @@ -34,6 +44,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; @@ -53,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 @@ -76,13 +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 -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) +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 @@ -96,100 +116,28 @@ 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); + return static_cast(torque / NOMINAL_TORQUE_REQUEST_NM * 1000.0f); } /** * 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); + return torque * (rpm / GEAR_RATIO) / static_cast(POWER_TO_TORQUE_CONVERSION_FACTOR); } /** * 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]] 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); } -// ============================================================================= -// 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) // ============================================================================= @@ -198,4 +146,4 @@ inline constexpr float F = (TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * 2.54f) // 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 \ No newline at end of file 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..c4d9f60a8a --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/decimal_dual.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include "dual.hpp" + +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>; 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..18fc4372e0 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/low_speed_blend.hpp @@ -0,0 +1,28 @@ +#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/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/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp new file mode 100644 index 0000000000..314683d081 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp @@ -0,0 +1,198 @@ +#pragma once +#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 VehicleState +{ + // state variables + 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 + */ + [[nodiscard]] wheel_set> v_in_tire_frame() const + { + 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(delta); + return v; + } + + [[nodiscard]] wheel_set alphas() const + { + const auto [fl_v, fr_v, rl_v, rr_v] = v_in_tire_frame(); + return { + 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)), + }; + } + + // AERODYNAMIC EFFECTS + // these might use internal state later?? + // ReSharper disable once CppMemberFunctionMayBeStatic + [[nodiscard]] constexpr T dynamicCOPFront() const { return 1.0f - vd_constants::COP_REAR; } + // ReSharper disable once CppMemberFunctionMayBeStatic + [[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 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; + } + + // ============================================================================= + // 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 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; + } + /** + * Lateral load transfer component (page 21) + * Transfers load to outside wheels during cornering + * @return Load transfer force per side (N) + */ + [[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); + } + + // 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; + + // // 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; + // 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 = CONSTANT_WHEEL_LOAD_N, + .fr = CONSTANT_WHEEL_LOAD_N, + .rl = CONSTANT_WHEEL_LOAD_N, + .rr = CONSTANT_WHEEL_LOAD_N, + }; + } + + /** + * Get body slip + * @return + */ + [[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]] F est_Mz_N(wheel_set> tires_F_N) const + { + // TODO aligning moment contributions to the yaw moment equation + 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 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 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 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; + } + + /** + * Yaw moment distribution factor Kmz (page 57) + * Accounts for load transfer effect on yaw moment generation capacity + * @return Effective moment arm (m) + */ + [[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 T KMZ() const + { + 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); + } + /** + * Moment scaling factor F (page 58) + * Relates torque differential to yaw moment through track width and effective radius + */ + static constexpr T F = (vd_constants::TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * IN_TO_M)) * GEAR_RATIO; + + 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 \ 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_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..17c034c6c6 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp @@ -0,0 +1,107 @@ +#include "torque_vectoring.hpp" +#include "torque_vectoring_matlab.h" // this is just for matlab interface syncing + +#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" + +using namespace app::tv::shared_datatypes; +using namespace vd_constants; + +template ControlOutput update(const VehicleState &state) +{ + //------------------------------------- HIGH LEVEL CONTROLLER ----------------------------// + 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); + + //------------------------------------- LOW LEVEL CONTROLLER -----------------------------// + + // 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); + + // ReSharper disable once CppUseStructuredBinding + 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 { { 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); + +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 kappas[4], + double torque_max[4], + double torque_min[4], + double alphas[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, + } }; + // 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; + 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; + + 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) +{ + (void)state; + // TODO inshallah one day + return {}; +} +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 oemgas[4]) +{ + const auto [fl, fr, rl, rr] = + kappa_update({ .fl = kappas[0], .fr = kappas[1], .rl = kappas[2], .rr = kappas[3] }, v_x); + 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 new file mode 100644 index 0000000000..5b1734fab6 --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp @@ -0,0 +1,63 @@ +#pragma once +#include "shared_datatypes/constants.hpp" +#include "shared_datatypes/vehicle_state_estimator.hpp" +#include "shared_datatypes/wheel_set.hpp" +// #include + +template struct ControlOutput +{ + app::tv::shared_datatypes::wheel_set kappas; + 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_max; + app::tv::shared_datatypes::wheel_set torque_min; + const T delta = 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 + */ +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 + */ +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) +{ + // 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 = 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 new file mode 100644 index 0000000000..62a5783cba --- /dev/null +++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h @@ -0,0 +1,26 @@ +#pragma once + +extern "C" +{ + /** + * Matlab Wrapper for update + */ + 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 kappas[4], + double torque_max[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]); +} 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/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(); 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 +#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 diff --git a/firmware/shared/srcpp/util/util_utils.hpp b/firmware/shared/srcpp/util/util_utils.hpp index 5634841192..f76d2b3127 100644 --- a/firmware/shared/srcpp/util/util_utils.hpp +++ b/firmware/shared/srcpp/util/util_utils.hpp @@ -2,23 +2,49 @@ #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 +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) +#else #define __BASENAME__(path) (__builtin_strrchr(path, '/') ? __builtin_strrchr(path, '/') + 1 : path) +#endif // Extra guard because HAL defines the same macro #ifndef UNUSED @@ -31,8 +57,6 @@ 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]]