From 42b22530604ac5441bc1dba1ead0dcdbd0347be6 Mon Sep 17 00:00:00 2001 From: nebudev14 Date: Mon, 13 Apr 2026 03:04:45 -0400 Subject: [PATCH 1/4] create matlab controllers and controller manager --- drivebrain_app/include/drivebrain_app.hpp | 4 ++- drivebrain_app/src/drivebrain_app.cpp | 28 ++++++++++++++----- drivebrain_core/include/ControllerManager.hpp | 1 + 3 files changed, 25 insertions(+), 8 deletions(-) diff --git a/drivebrain_app/include/drivebrain_app.hpp b/drivebrain_app/include/drivebrain_app.hpp index b367467..9cb8ff3 100644 --- a/drivebrain_app/include/drivebrain_app.hpp +++ b/drivebrain_app/include/drivebrain_app.hpp @@ -9,6 +9,7 @@ #include "hytech_msgs.pb.h" #include +#include class DrivebrainApp { public: @@ -57,7 +58,8 @@ class DrivebrainApp { std::unique_ptr> _vcf_eth_driver; /* Controllers */ - std::shared_ptr _controller1; + std::shared_ptr _mode1; + std::vector> _gend_controllers; /* Vectornav */ std::unique_ptr _vn_driver; diff --git a/drivebrain_app/src/drivebrain_app.cpp b/drivebrain_app/src/drivebrain_app.cpp index 0d81ac3..2e01154 100644 --- a/drivebrain_app/src/drivebrain_app.cpp +++ b/drivebrain_app/src/drivebrain_app.cpp @@ -2,6 +2,7 @@ #include "ETHRecvComms.hpp" #include "FoxgloveServer.hpp" #include "MCAPLogger.hpp" +#include "MatlabModelAddHelper.hpp" #include "hytech_msgs.pb.h" #include "Telemetry.hpp" #include "ControllerManager.hpp" @@ -14,6 +15,7 @@ #include #include #include +#include std::atomic running{true}; @@ -63,23 +65,35 @@ void DrivebrainApp::run() { spdlog::error("Failed to initialize vectornav driver"); } - spdlog::info("Initialized ethernet drivers"); // CAN device names are defined in the drivebrain JSON config _telem_can = std::make_unique(core::FoxgloveServer::instance().get_param("telem_can_device").value(), _dbc_path); _aux_can = std::make_unique(core::FoxgloveServer::instance().get_param("aux_can_device").value(), _dbc_path); + spdlog::info("Initialized CAN drivers"); // Initialize controllers - _controller1 = std::make_shared(); - if (!_controller1->init()) { - spdlog::error("Failed to initialize controller"); + const size_t num_controllers = 1 + matlab_model_gen::num_controllers; + _mode1 = std::make_shared(); + if (!_mode1->init()) { + spdlog::error("Failed to initialize mode 1"); } + std::array>, num_controllers> controllers{_mode1}; + auto _gend_controllers = matlab_model_gen::create_controllers(_estim_manager); + if (_gend_controllers.size() + 1 != controllers.size()) { + throw std::runtime_error("Failed to initialize matlab generated controllers! Wrong vector size!"); + } + std::copy(_gend_controllers.begin(), _gend_controllers.end(), controllers.begin() + 1); + + // Create controller manager instance + ControllerManager, num_controllers>::create(controllers); + if(!ControllerManager, num_controllers>::instance().init()) { + throw std::runtime_error("Failed to initialize controller manager"); + } - - spdlog::info("Initialized CAN drivers"); - + spdlog::info("Constructed controller manager"); + _estim_manager = std::make_shared(); _estim_manager->handle_inits(); spdlog::info("Constructed estimator manager"); diff --git a/drivebrain_core/include/ControllerManager.hpp b/drivebrain_core/include/ControllerManager.hpp index c9a1d86..1a93f2d 100644 --- a/drivebrain_core/include/ControllerManager.hpp +++ b/drivebrain_core/include/ControllerManager.hpp @@ -16,6 +16,7 @@ namespace core { +using DefaultControllerType = ; template class ControllerManager { From 9ba20a46b11bf16d93d49634803e6e8ac43bdf9e Mon Sep 17 00:00:00 2001 From: nebudev14 Date: Mon, 13 Apr 2026 19:33:51 -0400 Subject: [PATCH 2/4] poggers --- config/drivebrain_config.json | 4 +- drivebrain_app/src/drivebrain_app.cpp | 52 ++++++++++++++++--- drivebrain_core/include/ControllerManager.hpp | 2 +- .../include/DrivebrainControllerInterface.hpp | 2 +- .../src/DrivebrainControllerInterface.cpp | 22 +++++++- 5 files changed, 71 insertions(+), 11 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 491614b..f81ece6 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -150,8 +150,8 @@ "use_min_cell_derate": false }, "DrivebrainControllerInterface": { - "should_log": true, - "controller_index": 1 + "should_log": true, + "controller_index": 0 }, "use_vectornav": false, "use_fake_vn": false, diff --git a/drivebrain_app/src/drivebrain_app.cpp b/drivebrain_app/src/drivebrain_app.cpp index 2e01154..639ca25 100644 --- a/drivebrain_app/src/drivebrain_app.cpp +++ b/drivebrain_app/src/drivebrain_app.cpp @@ -123,18 +123,58 @@ void DrivebrainApp::_loop() { std::chrono::microseconds loop_time_ms((int) (loop_time * 1000000.0f)); auto next_tick = std::chrono::steady_clock::now(); + auto desired_rpm_msg = std::make_shared(); + auto torque_limit_msg = std::make_shared(); + auto desired_torque_msg = std::make_shared(); + while(running) { next_tick += loop_time_ms; auto state_and_validity = core::StateTracker::instance().get_latest_state_and_validity(); _estim_manager->evaluate_all_estimators(state_and_validity.first); - std::shared_ptr speed_msg = std::make_shared(); - speed_msg->set_drivebrain_set_rpm_fl(1.0); - speed_msg->set_drivebrain_set_rpm_fr(2.0); - speed_msg->set_drivebrain_set_rpm_rl(4.0); - speed_msg->set_drivebrain_set_rpm_rr(8.0); - // _telem_can->send_message(speed_msg); + auto& controller_manager = ControllerManager, 1 + matlab_model_gen::num_controllers>::instance(); + auto out_struct = controller_manager.step_active_controller(state_and_validity.first); + + std::variant cmd_out = out_struct.out; + core::StateTracker::instance().set_previous_control_output(out_struct); + + bool state_is_valid = state_and_validity.second; + if(state_is_valid) + { + + if (const core::SpeedControlOut* speedControl = std::get_if(&cmd_out)) { // speed controller, set RPM + + desired_rpm_msg->set_drivebrain_set_rpm_fl(speedControl->desired_rpms.FL); + desired_rpm_msg->set_drivebrain_set_rpm_fr(speedControl->desired_rpms.FR); + desired_rpm_msg->set_drivebrain_set_rpm_rl(speedControl->desired_rpms.RL); + desired_rpm_msg->set_drivebrain_set_rpm_rr(speedControl->desired_rpms.RR); + + core::log(desired_rpm_msg); + core::log(torque_limit_msg); + + // same with torque limits + torque_limit_msg->set_drivebrain_torque_fl(::abs(speedControl->torque_lim_nm.FL)); + torque_limit_msg->set_drivebrain_torque_fr(::abs(speedControl->torque_lim_nm.FR)); + torque_limit_msg->set_drivebrain_torque_rl(::abs(speedControl->torque_lim_nm.RL)); + torque_limit_msg->set_drivebrain_torque_rr(::abs(speedControl->torque_lim_nm.RR)); + + _telem_can->send_message(desired_rpm_msg); + _telem_can->send_message(torque_limit_msg); + + } else if (const core::TorqueControlOut* torqueControl = std::get_if(&cmd_out)){ // if it is a torque controller: + // set desired torque + + core::log(desired_torque_msg); + desired_torque_msg->set_drivebrain_torque_fl(torqueControl->desired_torques_nm.FL); + desired_torque_msg->set_drivebrain_torque_fr(torqueControl->desired_torques_nm.FR); + desired_torque_msg->set_drivebrain_torque_rl(torqueControl->desired_torques_nm.RL); + desired_torque_msg->set_drivebrain_torque_rr(torqueControl->desired_torques_nm.RR); + + _telem_can->send_message(desired_torque_msg); + } + } + std::tuple mcap_status = core::MCAPLogger::instance().status(); std::string logile_name = std::get<0>(mcap_status); diff --git a/drivebrain_core/include/ControllerManager.hpp b/drivebrain_core/include/ControllerManager.hpp index 1a93f2d..bf16e94 100644 --- a/drivebrain_core/include/ControllerManager.hpp +++ b/drivebrain_core/include/ControllerManager.hpp @@ -16,7 +16,7 @@ namespace core { -using DefaultControllerType = ; + template class ControllerManager { diff --git a/drivebrain_core/include/DrivebrainControllerInterface.hpp b/drivebrain_core/include/DrivebrainControllerInterface.hpp index a451442..39db346 100644 --- a/drivebrain_core/include/DrivebrainControllerInterface.hpp +++ b/drivebrain_core/include/DrivebrainControllerInterface.hpp @@ -49,4 +49,4 @@ class DrivebrainControllerInterface { std::string _response; }; -} \ No newline at end of file +} diff --git a/drivebrain_core/src/DrivebrainControllerInterface.cpp b/drivebrain_core/src/DrivebrainControllerInterface.cpp index 2a3aba2..89c8625 100644 --- a/drivebrain_core/src/DrivebrainControllerInterface.cpp +++ b/drivebrain_core/src/DrivebrainControllerInterface.cpp @@ -1,4 +1,12 @@ +#include "Controller.hpp" +#include "ControllerManager.hpp" +#include "StateTracker.hpp" #include +#include + +namespace matlab_model_gen { +inline constexpr std::size_t num_controllers = 1; +} /** Singleton Methods */ void core::DrivebrainControllerInterface::create() { @@ -65,7 +73,19 @@ void core::DrivebrainControllerInterface::_request_stop_logging() { } void core::DrivebrainControllerInterface::_request_controller_change(int controller_index) { - spdlog::info("Controller change requested"); + spdlog::info("Controller change requested: {}", controller_index); + if (controller_index < 0) { + spdlog::warn("Ignoring negative controller index request: {}", controller_index); + return; + } + const auto state_and_validity = core::StateTracker::instance().get_latest_state_and_validity(); + const size_t num_controllers = 1 + matlab_model_gen::num_controllers; + auto& controller_manager = ControllerManager, num_controllers>::instance(); + const bool switched = controller_manager.swap_active_controller(static_cast(controller_index), state_and_validity.first); + + if (!switched) { + spdlog::warn("Controller switch rejected for index {}", controller_index); + } } From 3f35c76e55052fab32c8c49c3e113782ca6107e4 Mon Sep 17 00:00:00 2001 From: nebudev14 Date: Fri, 17 Apr 2026 21:11:06 -0400 Subject: [PATCH 3/4] send msgs to aux can + fix ctr manage --- drivebrain_app/src/drivebrain_app.cpp | 7 ++++++- drivebrain_core/include/ControllerManager.tpp | 7 ++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/drivebrain_app/src/drivebrain_app.cpp b/drivebrain_app/src/drivebrain_app.cpp index 639ca25..b1c2926 100644 --- a/drivebrain_app/src/drivebrain_app.cpp +++ b/drivebrain_app/src/drivebrain_app.cpp @@ -162,6 +162,9 @@ void DrivebrainApp::_loop() { _telem_can->send_message(desired_rpm_msg); _telem_can->send_message(torque_limit_msg); + _aux_can->send_message(desired_rpm_msg); + _aux_can->send_message(torque_limit_msg); + } else if (const core::TorqueControlOut* torqueControl = std::get_if(&cmd_out)){ // if it is a torque controller: // set desired torque @@ -170,8 +173,10 @@ void DrivebrainApp::_loop() { desired_torque_msg->set_drivebrain_torque_fr(torqueControl->desired_torques_nm.FR); desired_torque_msg->set_drivebrain_torque_rl(torqueControl->desired_torques_nm.RL); desired_torque_msg->set_drivebrain_torque_rr(torqueControl->desired_torques_nm.RR); + + _telem_can->send_message(desired_torque_msg); + _aux_can->send_message(desired_torque_msg); - _telem_can->send_message(desired_torque_msg); } } diff --git a/drivebrain_core/include/ControllerManager.tpp b/drivebrain_core/include/ControllerManager.tpp index 2448228..bc1f26b 100644 --- a/drivebrain_core/include/ControllerManager.tpp +++ b/drivebrain_core/include/ControllerManager.tpp @@ -159,10 +159,11 @@ bool core::ControllerManager::swap_active_contro } core::ControllerOutput next_output = _controllers[new_controller_index]->step_controller(input); - - status_type can_switch_controller = _can_switch_controler( + + status_type can_switch_controller = _can_switch_controller( input, - {_controllers[_current_controller_index]->step_controller(input).current_controller_output}, + // {_controllers[_current_controller_index]->step_controller(input).current_controller_output}, + _current_ctr_manager_state.current_controller_output, next_output ); From bac1d79cf120875ca8f4b7c301e7b66b9f4954bc Mon Sep 17 00:00:00 2001 From: Krish Kittur Date: Sun, 19 Apr 2026 16:40:43 -0400 Subject: [PATCH 4/4] udpate controller update prints --- CMakeLists.txt | 4 ++-- .../src/DrivebrainControllerInterface.cpp | 13 ++++++++++--- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9a270e9..3ecbd5d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -48,14 +48,14 @@ FetchContent_MakeAvailable(HT_CAN) FetchContent_Declare( simulink_automation_msgs_cpp - URL "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel38/proto_outputs.tar.gz" + URL "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel50/proto_outputs.tar.gz" ) FetchContent_MakeAvailable(simulink_automation_msgs_cpp) FetchContent_Declare( drivebrain_simulink_models - URL "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel38/codegen_outputs.tar.gz" + URL "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel50/codegen_outputs.tar.gz" ) FetchContent_MakeAvailable(drivebrain_simulink_models) diff --git a/drivebrain_core/src/DrivebrainControllerInterface.cpp b/drivebrain_core/src/DrivebrainControllerInterface.cpp index 89c8625..b3c63b7 100644 --- a/drivebrain_core/src/DrivebrainControllerInterface.cpp +++ b/drivebrain_core/src/DrivebrainControllerInterface.cpp @@ -73,14 +73,21 @@ void core::DrivebrainControllerInterface::_request_stop_logging() { } void core::DrivebrainControllerInterface::_request_controller_change(int controller_index) { - spdlog::info("Controller change requested: {}", controller_index); + const size_t num_controllers = 1 + matlab_model_gen::num_controllers; + auto& controller_manager = ControllerManager, num_controllers>::instance(); + + if (controller_index == controller_manager.get_active_controller_index()) { + return; + } + + spdlog::info("Controller swap requested to controller: {}", controller_index); + if (controller_index < 0) { spdlog::warn("Ignoring negative controller index request: {}", controller_index); return; } + const auto state_and_validity = core::StateTracker::instance().get_latest_state_and_validity(); - const size_t num_controllers = 1 + matlab_model_gen::num_controllers; - auto& controller_manager = ControllerManager, num_controllers>::instance(); const bool switched = controller_manager.swap_active_controller(static_cast(controller_index), state_and_validity.first); if (!switched) {