Feat/db in control

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/rel48/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/rel48/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)
 

drivebrain_app/include/drivebrain_app.hpp

@@ -9,6 +9,7 @@
 
 #include "hytech_msgs.pb.h"
 #include <EstimatorManager.hpp>
+#include <MatlabModelAddHelper.hpp>
 
 class DrivebrainApp {
 public:
@@ -57,7 +58,8 @@ class DrivebrainApp {
   std::unique_ptr<comms::ETHRecvComms<hytech_msgs::VCFData_s>> _vcf_eth_driver; 
 
   /* Controllers */
-  std::shared_ptr<control::LoadCellTorqueController> _controller1; 
+  std::shared_ptr<control::LoadCellTorqueController> _mode1;
+  std::vector<std::shared_ptr<MatlabModel>> _gend_controllers;
 
   /* Vectornav */
   std::unique_ptr<comms::VNDriver> _vn_driver; 

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 <fmt/chrono.h>
 #include <spdlog/spdlog.h>
 #include <filesystem>
+#include <stdexcept>
 
 std::atomic<bool> 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<comms::CANComms>(core::FoxgloveServer::instance().get_param<std::string>("telem_can_device").value(), _dbc_path);
   _aux_can = std::make_unique<comms::CANComms>(core::FoxgloveServer::instance().get_param<std::string>("aux_can_device").value(), _dbc_path);
+  spdlog::info("Initialized CAN drivers");
 
   // Initialize controllers
-  _controller1 = std::make_shared<control::LoadCellTorqueController>(); 
-  if (!_controller1->init()) {
-    spdlog::error("Failed to initialize controller");
+  const size_t num_controllers = 1 + matlab_model_gen::num_controllers;
+    _mode1 = std::make_shared<control::LoadCellTorqueController>(); 
+  if (!_mode1->init()) {
+    spdlog::error("Failed to initialize mode 1");
   }
 
+  std::array<std::shared_ptr<control::Controller<core::ControllerOutput, core::VehicleState>>, 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<control::Controller<ControllerOutput, VehicleState>, num_controllers>::create(controllers);
+  if(!ControllerManager<control::Controller<ControllerOutput, VehicleState>, 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<estimation::EstimatorManager>();
   _estim_manager->handle_inits();
   spdlog::info("Constructed estimator manager");
@@ -109,19 +123,63 @@ 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<hytech::drivebrain_speed_set_input>();
+  auto torque_limit_msg = std::make_shared<hytech::drivebrain_torque_lim_input>();
+  auto desired_torque_msg = std::make_shared<hytech::drivebrain_desired_torque_input>();
+
   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<hytech::drivebrain_speed_set_input> speed_msg = std::make_shared<hytech::drivebrain_speed_set_input>(); 
-    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);
-    // spdlog::info("param {}", core::FoxgloveServer::instance().get_param<float>("estimator_matlabestimmodel/accel_gamma").value());
-    _telem_can->send_message(speed_msg);
+    auto& controller_manager = ControllerManager<control::Controller<ControllerOutput, VehicleState>, 1 + matlab_model_gen::num_controllers>::instance();
+    auto out_struct = controller_manager.step_active_controller(state_and_validity.first);
+
+    std::variant<core::SpeedControlOut, core::TorqueControlOut, std::monostate> 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<core::SpeedControlOut>(&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);
+
+            _aux_can->send_message(desired_rpm_msg);
+            _aux_can->send_message(torque_limit_msg);
+
+        } else if (const core::TorqueControlOut* torqueControl = std::get_if<core::TorqueControlOut>(&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);
+           _aux_can->send_message(desired_torque_msg);
+
+        }
+    }
+
 
     std::tuple<std::string, bool> mcap_status = core::MCAPLogger::instance().status();
     std::string logile_name = std::get<0>(mcap_status);

drivebrain_core/include/ControllerManager.hpp

@@ -17,6 +17,7 @@
 
 namespace core {
 
+
 template <typename ControllerType, size_t NumControllers>
 class ControllerManager {
 public:

drivebrain_core/include/ControllerManager.tpp

@@ -159,10 +159,11 @@ bool core::ControllerManager<ControllerType, NumControllers>::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
     );
 

drivebrain_core/include/DrivebrainControllerInterface.hpp

@@ -49,4 +49,4 @@ class DrivebrainControllerInterface {
         std::string _response; 
 
 };
-}
+}

drivebrain_core/src/DrivebrainControllerInterface.cpp

@@ -1,4 +1,12 @@
+#include "Controller.hpp"
+#include "ControllerManager.hpp"
+#include "StateTracker.hpp"
 #include <DrivebrainControllerInterface.hpp>
+#include <cstddef>
+
+namespace matlab_model_gen {
+inline constexpr std::size_t num_controllers = 1;
+}
 
 /** Singleton Methods */
 void core::DrivebrainControllerInterface::create() {
@@ -65,7 +73,26 @@ 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<control::Controller<ControllerOutput, VehicleState>, 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 bool switched = controller_manager.swap_active_controller(static_cast<size_t>(controller_index), state_and_validity.first);
+
+    if (!switched) {
+        spdlog::warn("Controller switch rejected for index {}", controller_index);
+    }
 }