diff --git a/.gitignore b/.gitignore
index bdb44409..ea51db76 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,4 +9,7 @@ core.*
 result-man/
 result-man
 .cache/
-*.dbc
\ No newline at end of file
+*.dbc
+
+compile_commands.json
+*.log
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8c093d5a..a696dc88 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -27,6 +27,9 @@ configure_file(
     @ONLY
 )
 
+## simulink codegend estimators and controllers
+find_package(codegen CONFIG REQUIRED)
+
 ### this one is generated from nix-proto but using foxglove's 
 # find_package(foxglove-schemas_proto_cpp CONFIG REQUIRED) 
 
@@ -56,9 +59,9 @@ find_package(drivebrain_core REQUIRED)
 
 include(create_package)
 
-#####################
-# created libraries #
-#####################
+###################################
+# created drivebrain sw libraries #
+###################################
 
 # utils
 add_library(drivebrain_common_utils SHARED 
@@ -82,8 +85,11 @@ add_library(drivebrain_comms SHARED
     drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp
     drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp
     drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp
-    drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp
     drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp
+    drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp
+    drivebrain_core_impl/drivebrain_comms/src/SurreyAeroComms.cpp
+    drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp
+    drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp
 )
 
 target_compile_features(drivebrain_comms PUBLIC cxx_std_17)
@@ -100,8 +106,10 @@ target_include_directories(drivebrain_comms PUBLIC
 )
 
 target_link_libraries(drivebrain_comms PUBLIC 
+    codegen::matlab_model
     drivebrain_core::drivebrain_core
     drivebrain_common_utils
+    drivebrain_control
     gRPC::grpc++_reflection
     foxglove_websocket::foxglove_websocket
     protobuf::libprotobuf
@@ -117,7 +125,13 @@ target_link_libraries(drivebrain_comms PUBLIC
 make_cmake_package(drivebrain_comms drivebrain)
 
 
-add_library(drivebrain_control SHARED drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp)
+add_library(drivebrain_control SHARED 
+    drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp
+    drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp
+    drivebrain_core_impl/drivebrain_control/src/SimplePowerLimiter.cpp
+    drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp
+)
+
 # drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp
 target_include_directories(drivebrain_control PUBLIC
     $<INSTALL_INTERFACE:drivebrain_core_impl/drivebrain_control/include>
@@ -128,18 +142,20 @@ target_link_libraries(drivebrain_control PUBLIC
     drivebrain_core::drivebrain_core
     hytech_np_proto_cpp::hytech_np_proto_cpp
     protobuf::libprotobuf
+    drivebrain_common_utils
 )
 
 make_cmake_package(drivebrain_control drivebrain)
 
-add_library(drivebrain_mcap_logger SHARED drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp)
+add_library(drivebrain_mcap_logger SHARED drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp)
 target_include_directories(drivebrain_mcap_logger PUBLIC
-    $<INSTALL_INTERFACE:drivebrain_core_impl/drivebrain_mcap_logger/include>
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/drivebrain_core_impl/drivebrain_mcap_logger/include>    
+    $<INSTALL_INTERFACE:drivebrain_core_impl/drivebrain_logging/include>
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/drivebrain_core_impl/drivebrain_logging/include>    
 )
 
 target_link_libraries(drivebrain_mcap_logger PUBLIC
     drivebrain_core::drivebrain_core 
+    codegen::matlab_model
     drivebrain_common_utils
     protobuf::libprotobuf
     mcap::mcap
@@ -160,10 +176,19 @@ target_include_directories(drivebrain_estimation PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/drivebrain_core_impl/drivebrain_estimation/include>    
 )
 
+target_link_libraries(drivebrain_estimation PUBLIC
+    drivebrain_core::drivebrain_core 
+    drivebrain_common_utils
+    hytech_np_proto_cpp::hytech_np_proto_cpp
+    drivebrain_core_msgs_proto_cpp::drivebrain_core_msgs_proto_cpp
+    protobuf::libprotobuf
+)
+
 make_cmake_package(drivebrain_estimation drivebrain)
 
 add_library(drivebrain_app SHARED
     drivebrain_app/src/DriveBrainApp.cpp
+    drivebrain_app/src/arg_parse.cpp
 )
 
 target_include_directories(drivebrain_app PUBLIC
@@ -173,21 +198,38 @@ target_include_directories(drivebrain_app PUBLIC
 
 target_link_libraries(drivebrain_app PUBLIC
     drivebrain_core::drivebrain_core
+    codegen::matlab_model
     drivebrain_control
     drivebrain_comms
     drivebrain_mcap_logger
     Boost::program_options
 )
 
-target_link_libraries(drivebrain_estimation PUBLIC
-    drivebrain_core::drivebrain_core 
-    drivebrain_common_utils
-    hytech_np_proto_cpp::hytech_np_proto_cpp
-    drivebrain_core_msgs_proto_cpp::drivebrain_core_msgs_proto_cpp
+make_cmake_package(drivebrain_app drivebrain)
+
+#####################################
+# drivebrain util / debug libraries #
+#####################################
+
+add_library(drivebrain_utils SHARED
+    drivebrain_utils/src/MCAPReplay.cpp
+)
+
+target_include_directories(drivebrain_utils PUBLIC
+    $<INSTALL_INTERFACE:drivebrain_utils/include>
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/drivebrain_utils/include>
+)
+
+target_link_libraries(drivebrain_utils PUBLIC
     protobuf::libprotobuf
+    mcap::mcap
+    Boost::program_options
+    spdlog::spdlog
+    drivebrain_comms # CANDriver
 )
 
-make_cmake_package(drivebrain_app drivebrain)
+
+make_cmake_package(drivebrain_utils drivebrain)
 
 ###############
 # executables #
@@ -203,6 +245,11 @@ make_cmake_package(drivebrain_app drivebrain)
 #     Boost::boost
 # )
 
+
+add_executable(replay_app drivebrain_utils/src/replay_app.cpp)
+
+target_link_libraries(replay_app PUBLIC drivebrain_utils)
+
 add_executable(test_param_server test/test_param_server.cpp)
 
 target_link_libraries(test_param_server PUBLIC 
@@ -211,6 +258,26 @@ target_link_libraries(test_param_server PUBLIC
     zstd::libzstd_shared
 )
 
+add_executable(test_surrey_aero_comms test/test_surrey_aero_comms.cpp)
+
+target_link_libraries(test_surrey_aero_comms PUBLIC 
+    drivebrain_comms
+)
+
+add_executable(test_speedtech_comms test/test_speedtech_comms.cpp)
+
+target_link_libraries(test_speedtech_comms PUBLIC 
+    drivebrain_comms
+)
+
+
+
+add_executable(test_scale_comms test/test_scale_comms.cpp)
+
+target_link_libraries(test_scale_comms PUBLIC 
+    drivebrain_comms
+)
+
 # add_executable(test_dbcan test/test_dbcan.cpp)
 
 # target_link_libraries(test_dbcan PUBLIC
@@ -237,6 +304,14 @@ target_link_libraries(test_db_grpc PUBLIC
     gRPC::grpc++_reflection
 )
 
+add_executable(test_config_schema test/test_configurable.cpp)
+
+target_link_libraries(test_config_schema PRIVATE
+    drivebrain_core::drivebrain_core
+    drivebrain_mcap_logger
+    drivebrain_comms
+)
+
 
 add_executable(test_mcap test/test_mcap/test_mcap_logging.cpp test/test_mcap/BuildFileDescriptorSet.cpp)
 
@@ -260,21 +335,25 @@ target_compile_features(test_vn PUBLIC cxx_std_11)
 ### system executable ###
 ###                   ###
 
-add_executable(alpha_build 
+add_executable(drivebrain_exe
     drivebrain_app/main.cpp
 )
 
 # Add include directory for DriveBrainApp header
-target_include_directories(alpha_build PRIVATE
+target_include_directories(drivebrain_exe PRIVATE
     ${CMAKE_CURRENT_SOURCE_DIR}/drivebrain_app
 )
-target_link_libraries(alpha_build PUBLIC
+
+enable_testing()
+
+target_link_libraries(drivebrain_exe PUBLIC
     drivebrain_app
     drivebrain_core::drivebrain_core
     drivebrain_control
     drivebrain_comms
     drivebrain_mcap_logger
     Boost::program_options
+    codegen::matlab_model
 )
 
 add_executable(test_build 
@@ -291,15 +370,15 @@ target_link_libraries(test_build PUBLIC
 
 enable_testing()
 
-add_executable(alpha_test 
+add_executable(drivebrain_test 
     unit_test/main.cpp
-    unit_test/SimpleControllerTest.cpp
+    unit_test/SimpleSpeedControllerTest.cpp
+    unit_test/SimpleTorqueControllerTest.cpp
+    unit_test/test_controller_manager.cpp
+    unit_test/ControllerTesting.cpp
 )
 
-
-
-
-target_link_libraries(alpha_test PUBLIC 
+target_link_libraries(drivebrain_test PUBLIC 
     drivebrain_core::drivebrain_core
     drivebrain_control
     drivebrain_comms
@@ -307,7 +386,7 @@ target_link_libraries(alpha_test PUBLIC
     gtest
 )
 
-add_test(NAME MyTest COMMAND alpha_test)
+add_test(NAME Drivebrain_Tests COMMAND drivebrain_test)
 
 target_link_libraries(mcu_standin PUBLIC
     drivebrain_core_msgs_proto_cpp::drivebrain_core_msgs_proto_cpp
@@ -317,8 +396,21 @@ target_link_libraries(mcu_standin PUBLIC
 
 include(GNUInstallDirs)
 install(TARGETS 
-        alpha_build
+        drivebrain_exe
         test_param_server
         test_build
     RUNTIME 
         DESTINATION ${CMAKE_INSTALL_BINDIR})
+
+
+add_executable(TCMUX_integration_test test/controller_manager_integration.cpp)
+
+target_link_libraries(
+    TCMUX_integration_test PUBLIC
+    hytech_np_proto_cpp::hytech_np_proto_cpp
+    drivebrain_core_msgs_proto_cpp::drivebrain_core_msgs_proto_cpp
+    drivebrain_core::drivebrain_core
+    drivebrain_control
+    Boost::boost
+    drivebrain_app
+)
\ No newline at end of file
diff --git a/README.md b/README.md
index d7b87801..4d067b0a 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,39 @@
+# execution
+
+```bash
+./build/drivebrain_exe -h
+Allowed options:
+  -h [ --help ]           produce help message
+  -p [ --param-path ] arg Path to the parameter JSON file
+  -d [ --dbc-path ] arg   Path to the DBC file (optional)
+  -c [ --2-can ] arg      use secondary CAN
+```
+
+```bash
+ ./build/replay_app -h
+Allowed options:
+  -h [ --help ]         produce help message
+  -m [ --mcap ] arg     Path to mcap file
+  -c [ --config ] arg   Path to config file
+  -d [ --dbc ] arg      Path to dbc file
+```
+- drivebrain:
+```
+./build/drivebrain_exe -p ./config/drivebrain_config.json -d ./config/hytech.dbc -c false
+```
+
+- replay app:
+
+```bash
+./build/replay_app -m <path_to_car_recorded_mcap_file> -c ./config/drivebrain_config.json -d ./config/hytech.dbc
+```
+
+## `grpcui` connection for start/stop recording and switching controllers in devshell
+
+within the devshell:
+```bash
+grpcui -plaintext localhost:6969
+```
 # About
 
 This repo contains the main executable that runs on the Drivebrain embedded computer on HyTech Racing's cars. This code is deployed as a `systemd` service onto the car within HyTech's [raspberry pi nixos description](https://github.com/hytech-racing/hytech_nixos). This service handles, among other things:
@@ -115,4 +151,24 @@ example `c_cpp_properties.json` file to be placed in your `.vscode/`:
     ],
     "version": 4
 }
+```
+
+## debugging example `launch.json`
+
+```json
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "Debug",
+      "type": "cppdbg", // use "cppdbg" with gdb on Linux
+      "request": "launch",
+      "program": "${workspaceFolder}/build/drivebrain_exe",
+      "args": ["-p", "./config/drivebrain_config.json", "-d", "./config/hytech.dbc", "-c", "false"],
+      "cwd": "${workspaceFolder}",
+      "stopAtEntry": false,
+      "internalConsoleOptions": "openOnSessionStart"
+    }
+  ]
+}
 ```
\ No newline at end of file
diff --git a/cmake/drivebrain_utilsConfig.cmake.in b/cmake/drivebrain_utilsConfig.cmake.in
new file mode 100644
index 00000000..993932ee
--- /dev/null
+++ b/cmake/drivebrain_utilsConfig.cmake.in
@@ -0,0 +1,11 @@
+@PACKAGE_INIT@
+
+include(CMakeFindDependencyMacro)
+
+find_dependency(mcap)
+find_dependency(fmt)
+find_dependency(spdlog)
+find_dependency(protobuf)
+find_dependency(Boost COMPONENTS program_options)
+
+include("${CMAKE_CURRENT_LIST_DIR}/drivebrain_utilsTargets.cmake")
diff --git a/config/config_test.json b/config/config_test.json
new file mode 100644
index 00000000..f25ada5d
--- /dev/null
+++ b/config/config_test.json
@@ -0,0 +1,14 @@
+{
+    "ConfigureableTest" :
+    {
+        "test_float": 1.0,
+        "test_bool": false,
+        "test_double": 2.0,
+        "test_string": "asdf",
+        "test_int": 2
+    },
+    "asdf2":
+    {
+        "test_val": 3.0 
+    }
+}
\ No newline at end of file
diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json
index 63a0c1ab..e36c3892 100644
--- a/config/drivebrain_config.json
+++ b/config/drivebrain_config.json
@@ -1,60 +1,156 @@
 {
-    "CANDriver": {
+    "CANDriverPrimary": {
         "canbus_device": "vcan0",
-        "path_to_dbc": "/home/ben/drivebrain_software/config/hytech.dbc"
+        "path_to_dbc": "./config/hytech.dbc"
     },
-    "SimpleController": {
-        "max_torque": 21,
+    "CANDriverSecondary": {
+        "canbus_device": "vcan1",
+        "path_to_dbc": "./config/hytech.dbc"
+    },
+    "SimpleSpeedController": {
+        "max_torque": 21.0,
         "max_regen_torque": 10.0,
         "rear_torque_scale": 1.0,
         "regen_torque_scale": 0.6,
-        "positive_speed_set" : 3
-    }, 
+        "positive_speed_set" : 45,
+        "max_power_kw": 63.0,
+        "dt_rate_hz": 1000
+    },
+    "test_mode0_MatlabModel": {
+        "max_torq": 21.0,
+        "use_regen": true
+    },
+    "test_pid_tv_MatlabModel": {
+        "p": 1.0,
+        "i": 0.0,
+        "d": 0.0,
+        "use_fake_vx": false,
+        "fake_vx": 0.0,
+        "use_fake_wz": false,
+        "fake_wz": 0.0,
+        "motor_rpmlimit": 2000,
+        "steer_sensor_offset": 33.0
+    },
+    "LoadCellVectoringTorqueController": {
+        "max_torque": 21.0,
+        "max_regen_torque": 10.0,
+        "rear_torque_scale": 1.0,
+        "regen_torque_scale": 0.6,
+        "positive_speed_set" : 45,
+        "max_power_kw": 63.0,
+        "dt_rate_hz": 1000,
+        "apply_vectoring_in_regen": true
+    },
     "VNDriver": {
-        "device_name": "/dev/ttyUSB0",
+        "device_name": "/dev/ttyUSB1",
         "baud_rate": 921600, 
         "port": 1,
         "freq_divisor": 1
     },
-    "Tire_Model_Codegen_MatlabModel":
-    {
-        "LMUXFL": 1,
-        "LMUYFL": 1,
-        "LMUXFR": 1,
-        "LMUYFR": 1,
-        "LMUXRL": 1,
-        "LMUYRL": 1,
-        "LMUXRR": 1,
-        "LMUYRR": 1,
-        "Interp_x1_FL": 604,
-        "Interp_x2_FL": 1073,
-        "Interp_x3_FL": 1320,
-        "interp_y1_FL": 385,
-        "interp_y2_FL": 598,
-        "interp_y3_FL": 718,
-        "Interp_x1_FR": 761,
-        "Interp_x2_FR": 1276,
-        "Interp_x3_FR": 1570,
-        "interp_y1_FR": 440,
-        "interp_y2_FR": 654,
-        "interp_y3_FR": 684,
-        "Interp_x1_RL": 906,
-        "Interp_x2_RL": 1160,
-        "Interp_x3_RL": 1496,
-        "interp_y1_RL": 517,
-        "interp_y2_RL": 637,
-        "interp_y3_RL": 819,
-        "Interp_x1_RR": 638,
-        "Interp_x2_RR": 855,
-        "Interp_x3_RR": 1165,
-        "interp_y1_RR": 451,
-        "interp_y2_RR": 565,
-        "interp_y3_RR": 734,
-        "useFakeData": false,
-        "Fake_Vx": 3.0,
-        "DriveBiasFront": 1.0,
-        "BrakeBiasFront": 1.0,
-        "fake_psi_dot": 0.2,
-        "integral_gain": 1414
-    }
+    "ControllerManager": {
+        "max_controller_switch_speed_ms": 5.0, 
+        "max_requested_rpm": 20000.0,
+        "max_torque_switch_nm": 10.0,
+        "max_accel_switch_float": 0.1
+    },
+    "StateEstimator": {
+        "fl_sus_pot_min": 1.0,
+        "fl_sus_pot_min_mm": 0.0,
+        "fl_sus_pot_max": 2000.0,
+        "fl_sus_pot_max_mm": 1.0,
+        "fr_sus_pot_min": 1.0,
+        "fr_sus_pot_min_mm": 0.0,
+        "fr_sus_pot_max": 2000.0,
+        "fr_sus_pot_max_mm": 1.0,
+        "rl_sus_pot_min": 1.0,
+        "rl_sus_pot_min_mm": 0.0,
+        "rl_sus_pot_max": 2000.0,
+        "rl_sus_pot_max_mm": 1.0,
+        "rr_sus_pot_min": 1.0,
+        "rr_sus_pot_min_mm": 0.0,
+        "rr_sus_pot_max": 2000.0,
+        "rr_sus_pot_max_mm": 1.0,
+        "fl_load_cell_offset": 0.0,
+        "fl_load_cell_scale": 1.0,
+        "fr_load_cell_offset": 0.0,
+        "fr_load_cell_scale": 1.0,
+        "rl_load_cell_offset": 0.0,
+        "rl_load_cell_scale": 1.0,
+        "rr_load_cell_offset": 0.0,
+        "rr_load_cell_scale": 1.0
+    },
+    "SurreyAeroComms": {
+        "port_name": "/dev/ttyACM0"
+    },
+    "ScaleComms": {
+        "baud_rate": 115200,
+        "port_name": "/dev/ttyUSB0"
+    },
+    "SpeedTechComms" :{
+        "baud_rate": 9600,
+        "port_name": "/dev/ttyUSB0"
+    },
+    "gain_MatlabEstimModel": {
+        "in":2.0
+    },
+    "estimator_MatlabEstimModel": {
+        "steer_sensor_offset": 0.0,
+        "static_fz_N_fl": 0.0,
+        "static_fz_N_fr": 0.0,
+        "static_fz_N_rl": 0.0,
+        "static_fz_N_rr": 0.0,
+        "cgz_m": 0.0,
+        "fz_Q": 0.0,
+        "fz_R": 0.0,
+        "amk_eff_modifier": 0.0,
+        "vy_Q11": 0.0,
+        "wz_Q22": 0.0,
+        "vy_R11": 0.0,
+        "wz_R22": 0.0,
+        "process_noise_vx": 0.0,
+        "process_noise_yaw_rate": 0.0,
+        "process_noise_ax": 0.0,
+        "percent_wheel_diff_noise_gain": 0.0,
+        "accel_noise_coef": 0.0,
+        "yaw_rate_noise_coef": 0.0,
+        "ay_cap_m_s2": 0.0,
+        "des_Mz_gain": 0.0,
+        "drive_motor_torq_lim": 0.0,
+        "regen_motor_torq_lim": 0.0,
+        "elec_p_lim_kW": 0.0,
+        "motor_rpm_lim": 0.0,
+        "brake_deadzone": 0.0,
+        "accel_deadzone": 0.0,
+        "vel_saturation": 0.0,
+        "per_motor_elec_p_lim_kW": 0.0,
+        "use_steer_deadzone": false,
+        "use_intent_w_derate": false,
+        "amk_eff_modifier_fl":0.0,
+        "amk_eff_modifier_fr": 0.0,
+        "amk_eff_modifier_rl": 0.0,
+        "amk_eff_modifier_rr": 0.0,
+        "global_eff_modifier": 0.0
+    },
+    "qp_torq_allocator_MatlabModel": {
+        "mux": 0.0,
+        "high_axle_alpha": 0.0,
+        "high_axle_beta": 0.0,
+        "high_axle_lambda": 0.0,
+        "low_axle_alpha": 0.0,
+        "low_axle_beta": 0.0,
+        "low_axle_lambda": 0.0,
+        "k_opt": 0.0,
+        "torq_side_delta": 0.0,
+        "torq_long_delta": 0.0,
+        "w_starting": 0.0,
+        "w_ending": 0.0,
+        "coast_brake_torq": 0.0,
+        "estimator_intent_motor_rpm_lim": 0.0
+    }, 
+    "use_vectornav": false,
+    "use_fake_vn": true,
+    "use_surrey_aero": false,
+    "use_scale_comms": false,
+    "use_laptimer": true
+
 }
\ No newline at end of file
diff --git a/config/hytech.dbc b/config/hytech.dbc
index dcdff98b..95beb6d3 100644
--- a/config/hytech.dbc
+++ b/config/hytech.dbc
@@ -36,6 +36,22 @@ BS_:
 BU_: 
 
 
+BO_ 1220 IZZE_IR_TEMPS: 8 Peripherals
+ SG_ izze_brake_IR_temp_4 : 55|16@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ izze_brake_IR_temp_3 : 39|16@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ izze_brake_IR_temp_2 : 23|16@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ izze_brake_IR_temp_1 : 7|16@0+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 1025 ACU_SHUNT_MEASUREMENTS: 6 ECU
+ SG_ ts_out_filtered_read : 32|16@1+ (0.169047463702246,0) [0|0] "volts" Vector__XXX
+ SG_ pack_filtered_read : 16|16@1+ (0.16869351172632,0) [0|0] "volts" Vector__XXX
+ SG_ current_shunt_read : 0|16@1+ (0.291373985492,-666) [0|0] "amps" Vector__XXX
+
+BO_ 5 Attitude: 6 ECU
+ SG_ Roll_angle : 32|9@1- (0.25,0) [0|0] "deg" Vector__XXX
+ SG_ Pitch_angle : 16|9@1- (0.25,0) [0|0] "deg" Vector__XXX
+ SG_ Height : 0|12@1- (0.035,0) [0|71.645] "cm" Vector__XXX
+
 BO_ 222 BMS_BALANCING_STATUS: 8 ECU
  SG_ cell_60_balancing_status : 63|1@1+ (1,0) [0|0] "" Vector__XXX
  SG_ cell_59_balancing_status : 62|1@1+ (1,0) [0|0] "" Vector__XXX
@@ -153,6 +169,9 @@ BO_ 215 BMS_VOLTAGES: 8 ECU
  SG_ low_voltage : 16|16@1+ (0.0001,0) [0|0] "" Vector__XXX
  SG_ average_voltage : 0|16@1+ (0.0001,0) [0|0] "V" Vector__XXX
 
+BO_ 203 BRAKE_PRESSURE_SENSOR: 2 ECU
+ SG_ brake_sensor_analog_read : 0|16@1+ (3.0517578125,-312.5) [0|0] "psi" Vector__XXX
+
 BO_ 221 CCU_STATUS: 1 ECU
  SG_ charger_enabled : 0|1@1+ (1,0) [0|0] "" Vector__XXX
 
@@ -172,211 +191,210 @@ BO_ 2566869221 CHARGER_DATA: 7 ECU
  SG_ output_dc_voltage_low : 8|8@1+ (1,0) [0|0] "" Vector__XXX
  SG_ output_dc_voltage_high : 0|8@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 256 EM_MEASUREMENT: 8 ECU
- SG_ em_voltage : 39|32@0- (1.52590219e-05,0) [0|0] "Volts" Vector__XXX
- SG_ em_current : 7|32@0- (1.52590219e-05,0) [0|0] "Coulombs" Vector__XXX
+BO_ 2028 CONTROLLER_BOOLEAN: 8 ECU
+ SG_ controller_use_nl_tcs_slipschedu : 13|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_rpm_tcs_gain_sche : 12|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_nl_tcs_gain_sche : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_torque_bias : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_no_regen_5kph : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_discontin_brakes : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_dec_yaw_pid_brake : 7|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_tcs_lim_yaw_pid : 6|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_tcs : 5|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_power_limit : 4|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_pid_power_limit : 3|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_normal_force : 2|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_pid_tv : 1|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_use_launch : 0|1@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 1024 EM_STATUS: 2 ECU
- SG_ logging : 10|1@1+ (1,0) [0|0] "none" Vector__XXX
- SG_ overpower_error : 9|1@1+ (1,0) [0|0] "none" Vector__XXX
- SG_ overvoltage_error : 8|1@1+ (1,0) [0|0] "none" Vector__XXX
- SG_ current_gain : 4|4@1+ (1,0) [0|0] "none" Vector__XXX
- SG_ voltage_gain : 0|4@1+ (1,0) [0|0] "none" Vector__XXX
+BO_ 2011 CONTROLLER_NORMAL_DIST: 8 ECU
+ SG_ controller_normal_percent_rr : 48|16@1- (0.0001,0) [0|0] "" Vector__XXX
+ SG_ controller_normal_percent_rl : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX
+ SG_ controller_normal_percent_fr : 16|16@1- (0.0001,0) [0|0] "" Vector__XXX
+ SG_ controller_normal_percent_fl : 0|16@1- (0.0001,0) [0|0] "" Vector__XXX
 
-BO_ 184 MC1_ENERGY: 8 ECU
- SG_ feedback_torque : 48|16@1- (1,0) [0|0] "Mn" Vector__XXX
- SG_ motor_power : 16|32@1+ (1,0) [0|0] "" Vector__XXX
- SG_ dc_bus_voltage : 0|16@1+ (1,0) [0|0] "V" Vector__XXX
+BO_ 2012 CONTROLLER_NORMAL_TORQUE: 8 ECU
+ SG_ controller_normal_torque_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_normal_torque_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_normal_torque_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_normal_torque_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
 
-BO_ 160 MC1_SETPOINTS_COMMAND: 8 ECU
- SG_ negative_torque_limit : 48|16@1- (0.01,0) [0|0] "Mn" Vector__XXX
- SG_ positive_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX
- SG_ speed_setpoint_rpm : 16|16@1- (1,0) [0|0] "rpm" Vector__XXX
- SG_ remove_error : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ driver_enable : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ hv_enable : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_enable : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 1263 CONTROLLER_PID_TV_DATA: 8 ECU
+ SG_ controller_output : 20|20@1- (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_input : 0|20@1- (0.01,0) [0|0] "" Vector__XXX
 
-BO_ 176 MC1_STATUS: 8 ECU
- SG_ magnetizing_current : 48|16@1- (1,0) [0|0] "A" Vector__XXX
- SG_ torque_current : 32|16@1- (0.1,0) [0|0] "A" Vector__XXX
- SG_ speed_rpm : 16|16@1- (1,0) [0|0] "" Vector__XXX
- SG_ derating_on : 15|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_on : 14|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ dc_on : 12|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ quit_dc_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ warning : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ error : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ system_ready : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 1264 CONTROLLER_PID_TV_DELTA_DATA: 8 ECU
+ SG_ pid_tv_rr_delta : 30|10@1- (0.1,0) [0|0] "" Vector__XXX
+ SG_ pid_tv_rl_delta : 20|10@1- (0.1,0) [0|0] "" Vector__XXX
+ SG_ pid_tv_fr_delta : 10|10@1- (0.1,0) [0|0] "" Vector__XXX
+ SG_ pid_tv_fl_delta : 0|10@1- (0.1,0) [0|0] "" Vector__XXX
 
-BO_ 180 MC1_TEMPS: 8 ECU
- SG_ igbt_temp : 48|16@1- (0.1,0) [0|0] "C" Vector__XXX
- SG_ diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_temp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX
- SG_ motor_temp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX
+BO_ 1997 CONTROLLER_PID_YAW: 8 ECU
+ SG_ controller_yaw_pid_output : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_yaw_rate_error : 16|16@1- (0.0001,0) [0|0] "rad/s" Vector__XXX
+ SG_ vehm_kin_desired_yaw_rate_rad_s : 0|16@1- (0.0001,0) [0|0] "rad/s" Vector__XXX
 
-BO_ 164 MC1_TORQUE_COMMAND: 2 ECU
- SG_ torque_command : 0|16@1- (0.1,0) [0|0] "% Mn" Vector__XXX
+BO_ 2010 CONTROLLER_PID_YAW_TORQUE: 8 ECU
+ SG_ controller_yaw_torque_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_yaw_torque_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_yaw_torque_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_yaw_torque_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
 
-BO_ 185 MC2_ENERGY: 8 ECU
- SG_ feedback_torque : 48|16@1- (1,0) [0|0] "Mn" Vector__XXX
- SG_ motor_power : 16|32@1+ (1,0) [0|0] "" Vector__XXX
- SG_ dc_bus_voltage : 0|16@1+ (1,0) [0|0] "V" Vector__XXX
+BO_ 2014 CONTROLLER_POWER_LIM: 8 ECU
+ SG_ controller_power_lim_torque_adj : 20|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_power_lim_error : 4|16@1- (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_power_lim_status : 0|4@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 161 MC2_SETPOINTS_COMMAND: 8 ECU
- SG_ negative_torque_limit : 48|16@1- (0.01,0) [0|0] "Mn" Vector__XXX
- SG_ positive_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX
- SG_ speed_setpoint_rpm : 16|16@1- (1,0) [0|0] "rpm" Vector__XXX
- SG_ remove_error : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ driver_enable : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ hv_enable : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_enable : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 2026 CONTROLLER_POWER_LIM_CORNER_POW: 8 ECU
+ SG_ controller_power_lim_cornerp_rr : 48|16@1- (0.01,0) [0|0] "kW" Vector__XXX
+ SG_ controller_power_lim_cornerp_rl : 32|16@1- (0.01,0) [0|0] "kW" Vector__XXX
+ SG_ controller_power_lim_cornerp_fr : 16|16@1- (0.01,0) [0|0] "kW" Vector__XXX
+ SG_ controller_power_lim_cornerp_fl : 0|16@1- (0.01,0) [0|0] "kW" Vector__XXX
 
-BO_ 177 MC2_STATUS: 8 ECU
- SG_ magnetizing_current : 48|16@1- (1,0) [0|0] "A" Vector__XXX
- SG_ torque_current : 32|16@1- (0.1,0) [0|0] "A" Vector__XXX
- SG_ speed_rpm : 16|16@1- (1,0) [0|0] "" Vector__XXX
- SG_ derating_on : 15|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_on : 14|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ dc_on : 12|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ quit_dc_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ warning : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ error : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ system_ready : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 2027 CONTROLLER_POWER_LIM_TORQUE: 8 ECU
+ SG_ controller_power_lim_torque_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_power_lim_torque_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_power_lim_torque_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_power_lim_torque_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
 
-BO_ 181 MC2_TEMPS: 8 ECU
- SG_ igbt_temp : 48|16@1- (0.1,0) [0|0] "C" Vector__XXX
- SG_ diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_temp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX
- SG_ motor_temp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX
+BO_ 1966 CONTROLLER_REGEN_5KPH_STATUS: 8 ECU
+ SG_ controller_regen_5kph_status_rr : 3|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_regen_5kph_status_rl : 2|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_regen_5kph_status_fr : 1|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_regen_5kph_status_fl : 0|1@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 165 MC2_TORQUE_COMMAND: 2 ECU
- SG_ torque_command : 0|16@1- (0.1,0) [0|0] "% Mn" Vector__XXX
+BO_ 1965 CONTROLLER_REGEN_5KPH_TORQUE: 8 ECU
+ SG_ controller_regen_5kph_torq_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_regen_5kph_torq_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_regen_5kph_torq_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_regen_5kph_torq_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
 
-BO_ 186 MC3_ENERGY: 8 ECU
- SG_ feedback_torque : 48|16@1- (1,0) [0|0] "Mn" Vector__XXX
- SG_ motor_power : 16|32@1+ (1,0) [0|0] "" Vector__XXX
- SG_ dc_bus_voltage : 0|16@1+ (1,0) [0|0] "V" Vector__XXX
+BO_ 2043 CONTROLLER_TCS_CONFIG: 8 ECU
+ SG_ controller_tcs_sl_nlperc_star_rr : 46|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_sl_nlperc_end_rr : 38|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_sl_nlperc_end_fr : 30|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_sl_nlperc_star_fr : 22|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_vel_thresh : 14|8@1+ (0.01,0) [0|0] "m/s" Vector__XXX
+ SG_ controller_tcs_launch_vel_thresh : 8|6@1+ (0.01,0) [0|0] "m/s" Vector__XXX
+ SG_ controller_tcs_launch_dead_zone : 0|8@1+ (0.1,0) [0|0] "Nm" Vector__XXX
 
-BO_ 162 MC3_SETPOINTS_COMMAND: 8 ECU
- SG_ negative_torque_limit : 48|16@1- (0.01,0) [0|0] "Mn" Vector__XXX
- SG_ positive_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX
- SG_ speed_setpoint_rpm : 16|16@1- (1,0) [0|0] "rpm" Vector__XXX
- SG_ remove_error : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ driver_enable : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ hv_enable : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_enable : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 1980 CONTROLLER_TCS_CONFIG_CONT: 8 ECU
+ SG_ controller_tcs_SL_end_rear : 56|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_SL_start_rear : 48|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_SL_end_front : 40|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_SL_start_front : 32|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_lauSL_end_rear : 24|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_lauSL_start_rear : 16|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_lauSL_end_front : 8|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_lauSL_start_front : 0|8@1+ (0.01,0) [0|0] "" Vector__XXX
 
-BO_ 178 MC3_STATUS: 8 ECU
- SG_ magnetizing_current : 48|16@1- (1,0) [0|0] "A" Vector__XXX
- SG_ torque_current : 32|16@1- (0.1,0) [0|0] "A" Vector__XXX
- SG_ speed_rpm : 16|16@1- (1,0) [0|0] "" Vector__XXX
- SG_ derating_on : 15|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_on : 14|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ dc_on : 12|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ quit_dc_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ warning : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ error : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ system_ready : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 1981 CONTROLLER_TCS_DIFF_CONFIG: 8 ECU
+ SG_ controller_tcs_launch_LRdiff : 51|13@1+ (0.01,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_w_steer_upper_b : 39|12@1+ (0.01,0) [0|0] "deg" Vector__XXX
+ SG_ controller_tcs_w_steer_lower_b : 27|12@1+ (0.01,0) [0|0] "deg" Vector__XXX
+ SG_ controller_tcs_gen_LRdiff_upperB : 13|14@1+ (0.01,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_gen_LRdiff_lowerB : 0|13@1+ (0.01,0) [0|0] "Nm" Vector__XXX
 
-BO_ 182 MC3_TEMPS: 8 ECU
- SG_ igbt_temp : 48|16@1- (0.1,0) [0|0] "C" Vector__XXX
- SG_ diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_temp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX
- SG_ motor_temp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX
+BO_ 1996 CONTROLLER_TCS_NL_SCHE_CONFIG: 8 ECU
+ SG_ controller_tcs_startper_nl_rear : 24|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_startper_nl_front : 16|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_endper_nl_rear : 8|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_endper_nl_front : 0|8@1+ (0.01,0) [0|0] "" Vector__XXX
 
-BO_ 166 MC3_TORQUE_COMMAND: 2 ECU
- SG_ torque_command : 0|16@1- (0.1,0) [0|0] "% Mn" Vector__XXX
+BO_ 1967 CONTROLLER_TCS_PID_CONFIG: 8 ECU
+ SG_ controller_tcs_pid_p_rr : 48|16@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_pid_p_rl : 32|16@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_pid_p_fr : 16|16@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_pid_p_fl : 0|16@1+ (0.001,0) [0|0] "" Vector__XXX
 
-BO_ 187 MC4_ENERGY: 8 ECU
- SG_ feedback_torque : 48|16@1- (1,0) [0|0] "Mn" Vector__XXX
- SG_ motor_power : 16|32@1+ (1,0) [0|0] "" Vector__XXX
- SG_ dc_bus_voltage : 0|16@1+ (1,0) [0|0] "V" Vector__XXX
+BO_ 2046 CONTROLLER_TCS_PID_INPUT: 8 ECU
+ SG_ controller_tcs_pid_input_rr : 48|16@1- (0.0001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_pid_input_rl : 32|16@1- (0.0001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_pid_input_fr : 16|16@1- (0.0001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_pid_input_fl : 0|16@1- (0.0001,0) [0|0] "Nm" Vector__XXX
 
-BO_ 163 MC4_SETPOINTS_COMMAND: 8 ECU
- SG_ negative_torque_limit : 48|16@1- (0.01,0) [0|0] "Mn" Vector__XXX
- SG_ positive_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX
- SG_ speed_setpoint_rpm : 16|16@1- (1,0) [0|0] "rpm" Vector__XXX
- SG_ remove_error : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ driver_enable : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ hv_enable : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_enable : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 1962 CONTROLLER_TCS_PID_OUTPUT: 8 ECU
+ SG_ controller_tcs_pid_output_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_pid_output_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_pid_output_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_pid_output_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
 
-BO_ 179 MC4_STATUS: 8 ECU
- SG_ magnetizing_current : 48|16@1- (1,0) [0|0] "A" Vector__XXX
- SG_ torque_current : 32|16@1- (0.1,0) [0|0] "A" Vector__XXX
- SG_ speed_rpm : 16|16@1- (1,0) [0|0] "" Vector__XXX
- SG_ derating_on : 15|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_on : 14|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ dc_on : 12|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ quit_dc_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ warning : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ error : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ system_ready : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 1982 CONTROLLER_TCS_RPM_SCHE_CONFIG: 8 ECU
+ SG_ controller_tcs_upper_rpm_rear : 45|15@1+ (1,0) [0|0] "RPM" Vector__XXX
+ SG_ controller_tcs_upper_rpm_front : 30|15@1+ (1,0) [0|0] "RPM" Vector__XXX
+ SG_ controller_tcs_lower_rpm_rear : 15|15@1+ (1,0) [0|0] "RPM" Vector__XXX
+ SG_ controller_tcs_lower_rpm_front : 0|15@1+ (1,0) [0|0] "RPM" Vector__XXX
 
-BO_ 183 MC4_TEMPS: 8 ECU
- SG_ igbt_temp : 48|16@1- (0.1,0) [0|0] "C" Vector__XXX
- SG_ diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_temp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX
- SG_ motor_temp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX
+BO_ 1979 CONTROLLER_TCS_SATURATION_CONFIG: 8 ECU
+ SG_ controller_tcs_saturation_rear : 12|12@1+ (0.01,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_saturation_front : 0|12@1+ (0.01,0) [0|0] "Nm" Vector__XXX
 
-BO_ 167 MC4_TORQUE_COMMAND: 2 ECU
- SG_ torque_command : 0|16@1- (0.1,0) [0|0] "% Mn" Vector__XXX
+BO_ 1983 CONTROLLER_TCS_SLIP_TARGETS: 8 ECU
+ SG_ controller_tcs_slip_target_rear : 12|12@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_slip_target_front : 0|12@1+ (0.001,0) [0|0] "" Vector__XXX
 
-BO_ 204 MCU_ANALOG_READINGS: 8 ECU
- SG_ glv_battery_voltage : 48|16@1+ (1,0) [0|0] "" Vector__XXX
- SG_ hall_effect_current : 32|16@1- (1,0) [0|0] "N/A" Vector__XXX
- SG_ steering_2 : 16|16@1+ (1,0) [0|0] "" Vector__XXX
- SG_ steering_1 : 0|16@1- (1,0) [0|0] "" Vector__XXX
+BO_ 2045 CONTROLLER_TCS_STATUS: 8 ECU
+ SG_ controller_tcs_status_rr : 6|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_status_rl : 4|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_status_fr : 2|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ controller_tcs_status_fl : 0|2@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 198 MCU_FRONT_POTS: 4 ECU
- SG_ potentiometer_fr : 16|16@1+ (1,0) [0|0] "lbs" Vector__XXX
- SG_ potentiometer_fl : 0|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+BO_ 2044 CONTROLLER_TCS_TORQUE: 8 ECU
+ SG_ controller_tcs_torque_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_torque_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_torque_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_tcs_torque_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
 
-BO_ 197 MCU_LOAD_CELLS: 4 ECU
- SG_ load_cell_fr : 16|16@1+ (1,0) [0|0] "lbs" Vector__XXX
- SG_ load_cell_fl : 0|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+BO_ 1964 CONTROLLER_TORQUE_BIAS: 8 ECU
+ SG_ controller_bias_torq_avg_rear : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_bias_torq_avg_front : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
 
-BO_ 196 MCU_PEDAL_READINGS: 8 ECU
- SG_ mechanical_brake_percent_float : 16|8@1+ (0.39215686275,0) [0|0] "percent" Vector__XXX
- SG_ brake_percent_float : 8|8@1+ (0.39215686275,0) [0|0] "percent" Vector__XXX
- SG_ accel_percent_float : 0|8@1+ (0.39215686275,0) [0|0] "percent" Vector__XXX
+BO_ 2042 CONTROLLER_TORQUE_SETUP: 8 ECU
+ SG_ controller_max_nl_brake_sc_front : 56|8@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ controller_max_yaw_nl_accel_perc : 52|4@1+ (0.1,0) [0|0] "" Vector__XXX
+ SG_ controller_torque_mode : 44|8@1+ (0.1,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_regen_limit : 32|12@1- (0.01,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_constrained_torq_req : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
+ SG_ controller_initial_torque_req : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
 
-BO_ 199 MCU_REAR_POTS: 4 ECU
- SG_ potentiometer_rr : 16|16@1+ (1,0) [0|0] "lbs" Vector__XXX
- SG_ potentiometer_rl : 0|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+BO_ 1978 CONTROLLER_YAW_PID_CONFIG: 8 ECU
+ SG_ controller_yaw_pid_brakes_d : 54|10@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_yaw_pid_brakes_i : 44|10@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_yaw_pid_brakes_p : 33|11@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_yaw_pid_d : 23|10@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_yaw_pid_i : 13|10@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ controller_yaw_pid_p : 0|13@1+ (0.001,0) [0|0] "" Vector__XXX
 
-BO_ 195 MCU_STATUS: 8 ECU
- SG_ distance_traveled_m : 48|16@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ torque_mode : 40|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ max_torque : 32|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ drive_mode : 28|4@1+ (1,0) [0|0] "" Vector__XXX
- SG_ pack_charge_critical : 24|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ launch_control_active : 23|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ software_ok : 22|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ energy_meter_present : 20|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inverter_error : 19|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ ecu_state : 16|3@1+ (1,0) [0|0] "" Vector__XXX
- SG_ drive_buzzer : 15|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ no_accel_or_brake_implausibility : 14|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ bspd_brake_high : 13|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ bspd_current_high : 12|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ brake_pedal_active : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ no_brake_implausibility : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ no_accel_implausibility : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mechanical_brake_active : 8|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ shutdown_e_above_threshold : 7|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ software_ok_high : 6|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ shutdown_d_above_threshold : 5|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ bspd_ok_high : 4|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ shutdown_c_above_threshold : 3|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ bms_ok_high : 2|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ shutdown_b_above_threshold : 1|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ imd_ok_high : 0|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 6 Configuration: 6 ECU
+ SG_ Velocity_Filtering : 40|3@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ CAN_Termination_State_Bus_2 : 39|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ CAN_Termination_State_Bus_1 : 38|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ IMU_Filtering : 35|3@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ Attitude_Filtering : 32|3@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ Sensor_X_Location : 20|12@1- (0.01,0) [0|0] "m" Vector__XXX
+ SG_ Sensor_Y_Location : 8|12@1- (0.01,0) [0|0] "m" Vector__XXX
+ SG_ Height_Offset : 0|8@1- (0.5,0) [0|0] "cm" Vector__XXX
+
+BO_ 2033 DASHBOARD_BUZZER_CONTROL: 2 ECU
+ SG_ torque_limit_enum_value : 8|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ in_pedal_calibration_state : 1|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ dash_buzzer_flag : 0|1@1+ (1,0) [0|1] "" Vector__XXX
+
+BO_ 235 DASHBOARD_MCU_STATE: 6 ECU
+ SG_ dial_state : 40|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ pack_charge_led : 32|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ glv_led : 24|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ drive_buzzer : 20|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ ams_led : 18|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ imd_led : 16|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ motor_controller_error_led : 14|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ start_status_led : 12|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inertia_status_led : 10|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ cockpit_brb_led : 8|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mechanical_brake_led : 6|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mode_led : 4|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ launch_control_led : 2|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ bots_led : 0|2@1+ (1,0) [0|0] "" Vector__XXX
 
 BO_ 236 DASHBOARD_STATE: 3 ECU
  SG_ dial_state : 16|8@1+ (1,0) [0|0] "" Vector__XXX
@@ -394,32 +412,319 @@ BO_ 236 DASHBOARD_STATE: 3 ECU
  SG_ mark_button : 1|1@1+ (1,0) [0|0] "" Vector__XXX
  SG_ start_button : 0|1@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 235 DASHBOARD_MCU_STATE: 6 ECU
- SG_ dial_state : 40|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ pack_charge_led : 32|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ glv_led : 24|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ drive_buzzer : 20|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ ams_led : 18|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ imd_led : 16|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ motor_controller_error_led : 14|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ start_status_led : 12|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ inertia_status_led : 10|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ cockpit_brb_led : 8|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mechanical_brake_led : 6|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mode_led : 4|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ launch_control_led : 2|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ bots_led : 0|2@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 768 DASH_INPUT: 2 ECU
+ SG_ dash_dial_mode : 8|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ right_shifter_button : 7|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ left_shifter_button : 6|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ data_button_is_pressed : 5|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ start_button : 4|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mode_button : 3|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ motor_controller_cycle_button : 2|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ preset_button : 1|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ led_dimmer_button : 0|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 243 DRIVEBRAIN_DESIRED_TORQUE_INPUT: 8 ECU
+ SG_ drivebrain_torque_rr : 48|16@1- (0.01,0) [0|0] "" Vector__XXX
+ SG_ drivebrain_torque_rl : 32|16@1- (0.01,0) [0|0] "" Vector__XXX
+ SG_ drivebrain_torque_fr : 16|16@1- (0.01,0) [0|0] "" Vector__XXX
+ SG_ drivebrain_torque_fl : 0|16@1- (0.01,0) [0|0] "" Vector__XXX
+
+BO_ 242 DRIVEBRAIN_SPEED_SET_INPUT: 8 ECU
+ SG_ drivebrain_set_rpm_rr : 48|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ drivebrain_set_rpm_rl : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ drivebrain_set_rpm_fr : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ drivebrain_set_rpm_fl : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 241 DRIVEBRAIN_TORQUE_LIM_INPUT: 8 ECU
+ SG_ drivebrain_torque_rr : 48|16@1- (0.01,0) [0|0] "" Vector__XXX
+ SG_ drivebrain_torque_rl : 32|16@1- (0.01,0) [0|0] "" Vector__XXX
+ SG_ drivebrain_torque_fr : 16|16@1- (0.01,0) [0|0] "" Vector__XXX
+ SG_ drivebrain_torque_fl : 0|16@1- (0.01,0) [0|0] "" Vector__XXX
 
-BO_ 149 TCU_LAP_TIMES: 8 ECU
- SG_ lap_clock_state : 48|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ prev_lap_time : 24|24@1+ (1,0) [0|0] "ms" Vector__XXX
- SG_ best_lap_time : 0|24@1+ (1,0) [0|0] "ms" Vector__XXX
+BO_ 303 DRIVETRAIN_COMMAND: 8 ECU
+ SG_ drivetrain_traj_torque_lim_rr : 48|16@1- (0.001,0) [0|0] "" Vector__XXX
+ SG_ drivetrain_traj_torque_lim_rl : 32|16@1- (0.001,0) [0|0] "" Vector__XXX
+ SG_ drivetrain_traj_torque_lim_fr : 16|16@1- (0.001,0) [0|0] "" Vector__XXX
+ SG_ drivetrain_traj_torque_lim_fl : 0|16@1- (0.001,0) [0|0] "" Vector__XXX
 
-BO_ 148 TCU_DRIVER_MSG: 8 ECU
- SG_ target_lap_time : 40|24@1+ (1,0) [0|0] "ms" Vector__XXX
- SG_ driver_msg_var_2 : 24|16@1+ (1,0) [0|1] "" Vector__XXX
- SG_ driver_msg_var_1 : 8|16@1+ (1,0) [0|1] "" Vector__XXX
- SG_ driver_message : 0|8@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 513 DRIVETRAIN_ERR_STATUS_TELEM: 8 ECU
+ SG_ mc4_diagnostic_number : 48|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc3_diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc2_diagnostic_number : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc1_diagnostic_number : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 516 DRIVETRAIN_FILTER_OUT_TORQUE_TEL: 8 ECU
+ SG_ rr_motor_torque : 48|16@1- (1,0) [0|0] "nm" Vector__XXX
+ SG_ rl_motor_torque : 32|16@1- (1,0) [0|0] "nm" Vector__XXX
+ SG_ fr_motor_torque : 16|16@1- (1,0) [0|0] "nm" Vector__XXX
+ SG_ fl_motor_torque : 0|16@1- (1,0) [0|0] "nm" Vector__XXX
+
+BO_ 512 DRIVETRAIN_RPMS_TELEM: 8 ECU
+ SG_ rl_motor_rpm : 48|16@1- (1,0) [0|0] "" Vector__XXX
+ SG_ rr_motor_rpm : 32|16@1- (1,0) [0|0] "" Vector__XXX
+ SG_ fl_motor_rpm : 16|16@1- (1,0) [0|0] "" Vector__XXX
+ SG_ fr_motor_rpm : 0|16@1- (1,0) [0|0] "" Vector__XXX
+
+BO_ 514 DRIVETRAIN_STATUS_TELEM: 8 ECU
+ SG_ accel_percent : 48|8@1+ (1,0) [0|100] "" Vector__XXX
+ SG_ brake_percent : 40|8@1+ (1,0) [0|100] "" Vector__XXX
+ SG_ brake_implausible : 33|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ accel_implausible : 32|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ mc4_warning : 31|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc4_system_ready : 30|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc4_quit_inverter_on : 29|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc4_quit_dc : 28|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc4_inverter_on : 27|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc4_error : 26|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc4_derating_on : 25|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc4_dc_on : 24|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc3_warning : 23|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc3_system_ready : 22|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc3_quit_inverter_on : 21|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc3_quit_dc : 20|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc3_inverter_on : 19|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc3_error : 18|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc3_derating_on : 17|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc3_dc_on : 16|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc2_warning : 15|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc2_system_ready : 14|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc2_quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc2_quit_dc : 12|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc2_inverter_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc2_error : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc2_derating_on : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc2_dc_on : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc1_warning : 7|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc1_system_ready : 6|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc1_quit_inverter_on : 5|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc1_quit_dc : 4|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc1_inverter_on : 3|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc1_error : 2|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc1_derating_on : 1|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mc1_dc_on : 0|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 515 DRIVETRAIN_TORQUE_TELEM: 8 ECU
+ SG_ rr_motor_torque : 48|16@1- (1,0) [0|0] "nm" Vector__XXX
+ SG_ rl_motor_torque : 32|16@1- (1,0) [0|0] "nm" Vector__XXX
+ SG_ fr_motor_torque : 16|16@1- (1,0) [0|0] "nm" Vector__XXX
+ SG_ fl_motor_torque : 0|16@1- (1,0) [0|0] "nm" Vector__XXX
+
+BO_ 256 EM_MEASUREMENT: 8 ECU
+ SG_ em_voltage : 39|32@0- (1.52590219e-05,0) [0|0] "Volts" Vector__XXX
+ SG_ em_current : 7|32@0- (1.52590219e-05,0) [0|0] "Coulombs" Vector__XXX
+
+BO_ 1024 EM_STATUS: 2 ECU
+ SG_ logging : 10|1@1+ (1,0) [0|0] "none" Vector__XXX
+ SG_ overpower_error : 9|1@1+ (1,0) [0|0] "none" Vector__XXX
+ SG_ overvoltage_error : 8|1@1+ (1,0) [0|0] "none" Vector__XXX
+ SG_ current_gain : 4|4@1+ (1,0) [0|0] "none" Vector__XXX
+ SG_ voltage_gain : 0|4@1+ (1,0) [0|0] "none" Vector__XXX
+
+BO_ 237 FRONT_SUSPENSION: 8 ECU
+ SG_ fr_shock_pot : 48|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ fr_load_cell : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ fl_shock_pot : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ fl_load_cell : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 245 FRONT_THERMISTORS: 8 ECU
+ SG_ thermistor_motor_fr : 16|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX
+ SG_ thermistor_motor_fl : 0|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX
+
+BO_ 1 IMU: 8 ECU
+ SG_ Pitch_Rate : 52|11@1- (0.01,0) [0|0] "rad/s" Vector__XXX
+ SG_ Yaw_Rate : 41|11@1- (0.01,0) [0|0] "rad/s" Vector__XXX
+ SG_ Roll_Rate : 30|11@1- (0.01,0) [0|0] "rad/s" Vector__XXX
+ SG_ Accel_Z : 20|10@1- (0.057,0) [0|29.127] "m/s2" Vector__XXX
+ SG_ Accel_Y : 10|10@1- (0.057,0) [0|29.127] "m/s2" Vector__XXX
+ SG_ Accel_X : 0|10@1- (0.057,0) [0|29.127] "m/s2" Vector__XXX
+
+BO_ 151 INV1_CONTROL_INPUT: 6 ECU
+ SG_ negative_torque_limit : 32|16@1- (0.0097999999999549,0) [0|0] "Mn" Vector__XXX
+ SG_ positive_torque_limit : 16|16@1- (0.0097999999999549,0) [0|0] "Mn" Vector__XXX
+ SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" Vector__XXX
+
+BO_ 259 INV1_CONTROL_PARAMETER: 8 ECU
+ SG_ speed_control_kd : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_ki : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_kp : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 144 INV1_CONTROL_WORD: 2 ECU
+ SG_ remove_error : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ driver_enable : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ hv_enable : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inverter_enable : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 114 INV1_DYNAMICS: 8 ECU
+ SG_ actual_speed_rpm : 48|16@1- (1,0) [0|0] "" Vector__XXX
+ SG_ actual_torque_nm : 32|16@1- (0.0097999999999549,0) [0|0] "" Vector__XXX
+ SG_ actual_power_w : 0|32@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 116 INV1_FEEDBACK: 8 ECU
+ SG_ speed_control_kd : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_ki : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_kp : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 115 INV1_POWER: 8 ECU
+ SG_ reactive_power_var : 32|32@1- (1,0) [0|0] "" Vector__XXX
+ SG_ active_power_w : 0|32@1- (1,0) [0|0] "" Vector__XXX
+
+BO_ 112 INV1_STATUS: 8 ECU
+ SG_ diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dc_bus_voltage : 16|16@1+ (1,0) [0|0] "V" Vector__XXX
+ SG_ derating_on : 15|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inverter_on : 14|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dc_on : 12|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ quit_dc_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ warning : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ error : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ system_ready : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 113 INV1_TEMPS: 6 ECU
+ SG_ igbt_temp : 32|16@1- (0.1,0) [0|0] "C" Vector__XXX
+ SG_ inverter_temp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX
+ SG_ motor_temp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX
+
+BO_ 152 INV2_CONTROL_INPUT: 6 ECU
+ SG_ negative_torque_limit : 32|16@1- (0.0097999999999549,0) [0|0] "Mn" Vector__XXX
+ SG_ positive_torque_limit : 16|16@1- (0.0097999999999549,0) [0|0] "Mn" Vector__XXX
+ SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" Vector__XXX
+
+BO_ 260 INV2_CONTROL_PARAMETER: 8 ECU
+ SG_ speed_control_kd : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_ki : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_kp : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 145 INV2_CONTROL_WORD: 2 ECU
+ SG_ remove_error : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ driver_enable : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ hv_enable : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inverter_enable : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 119 INV2_DYNAMICS: 8 ECU
+ SG_ actual_speed_rpm : 48|16@1- (1,0) [0|0] "" Vector__XXX
+ SG_ actual_torque_nm : 32|16@1- (0.0097999999999549,0) [0|0] "" Vector__XXX
+ SG_ actual_power_w : 0|32@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 121 INV2_FEEDBACK: 8 ECU
+ SG_ speed_control_kd : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_ki : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_kp : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 120 INV2_POWER: 8 ECU
+ SG_ reactive_power_var : 32|32@1- (1,0) [0|0] "" Vector__XXX
+ SG_ active_power_w : 0|32@1- (1,0) [0|0] "" Vector__XXX
+
+BO_ 117 INV2_STATUS: 8 ECU
+ SG_ diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dc_bus_voltage : 16|16@1+ (1,0) [0|0] "V" Vector__XXX
+ SG_ derating_on : 15|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inverter_on : 14|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dc_on : 12|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ quit_dc_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ warning : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ error : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ system_ready : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 118 INV2_TEMPS: 6 ECU
+ SG_ igbt_temp : 32|16@1- (0.1,0) [0|0] "C" Vector__XXX
+ SG_ inverter_temp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX
+ SG_ motor_temp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX
+
+BO_ 153 INV3_CONTROL_INPUT: 6 ECU
+ SG_ negative_torque_limit : 32|16@1- (0.0097999999999549,0) [0|0] "Mn" Vector__XXX
+ SG_ positive_torque_limit : 16|16@1- (0.0097999999999549,0) [0|0] "Mn" Vector__XXX
+ SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" Vector__XXX
+
+BO_ 261 INV3_CONTROL_PARAMETER: 8 ECU
+ SG_ speed_control_kd : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_ki : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_kp : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 146 INV3_CONTROL_WORD: 2 ECU
+ SG_ remove_error : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ driver_enable : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ hv_enable : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inverter_enable : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 130 INV3_DYNAMICS: 8 ECU
+ SG_ actual_speed_rpm : 48|16@1- (1,0) [0|0] "" Vector__XXX
+ SG_ actual_torque_nm : 32|16@1- (0.0097999999999549,0) [0|0] "" Vector__XXX
+ SG_ actual_power_w : 0|32@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 132 INV3_FEEDBACK: 8 ECU
+ SG_ speed_control_kd : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_ki : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_kp : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 131 INV3_POWER: 8 ECU
+ SG_ reactive_power_var : 32|32@1- (1,0) [0|0] "" Vector__XXX
+ SG_ active_power_w : 0|32@1- (1,0) [0|0] "" Vector__XXX
+
+BO_ 128 INV3_STATUS: 8 ECU
+ SG_ diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dc_bus_voltage : 16|16@1+ (1,0) [0|0] "V" Vector__XXX
+ SG_ derating_on : 15|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inverter_on : 14|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dc_on : 12|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ quit_dc_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ warning : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ error : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ system_ready : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 129 INV3_TEMPS: 6 ECU
+ SG_ igbt_temp : 32|16@1- (0.1,0) [0|0] "C" Vector__XXX
+ SG_ inverter_temp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX
+ SG_ motor_temp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX
+
+BO_ 258 INV4_CONTROL_INPUT: 6 ECU
+ SG_ negative_torque_limit : 32|16@1- (0.0097999999999549,0) [0|0] "Mn" Vector__XXX
+ SG_ positive_torque_limit : 16|16@1- (0.0097999999999549,0) [0|0] "Mn" Vector__XXX
+ SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" Vector__XXX
+
+BO_ 262 INV4_CONTROL_PARAMETER: 8 ECU
+ SG_ speed_control_kd : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_ki : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_kp : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 147 INV4_CONTROL_WORD: 2 ECU
+ SG_ remove_error : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ driver_enable : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ hv_enable : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inverter_enable : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 135 INV4_DYNAMICS: 8 ECU
+ SG_ actual_speed_rpm : 48|16@1- (1,0) [0|0] "" Vector__XXX
+ SG_ actual_torque_nm : 32|16@1- (0.0097999999999549,0) [0|0] "" Vector__XXX
+ SG_ actual_power_w : 0|32@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 137 INV4_FEEDBACK: 8 ECU
+ SG_ speed_control_kd : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_ki : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_control_kp : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 136 INV4_POWER: 8 ECU
+ SG_ reactive_power_var : 32|32@1- (1,0) [0|0] "" Vector__XXX
+ SG_ active_power_w : 0|32@1- (1,0) [0|0] "" Vector__XXX
+
+BO_ 133 INV4_STATUS: 8 ECU
+ SG_ diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dc_bus_voltage : 16|16@1+ (1,0) [0|0] "V" Vector__XXX
+ SG_ derating_on : 15|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inverter_on : 14|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dc_on : 12|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ quit_dc_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ warning : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ error : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ system_ready : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 134 INV4_TEMPS: 6 ECU
+ SG_ igbt_temp : 32|16@1- (0.1,0) [0|0] "C" Vector__XXX
+ SG_ inverter_temp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX
+ SG_ motor_temp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX
 
 BO_ 1060 LF_TTPMS_1: 8 ECU
  SG_ LF_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX
@@ -493,29 +798,112 @@ BO_ 1071 LR_TTPMS_6: 8 ECU
  SG_ LR_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX
  SG_ LR_TTPMS_TC : 7|16@0+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 1072 RF_TTPMS_1: 8 ECU
- SG_ RF_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX
- SG_ RF_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX
- SG_ RF_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX
- SG_ RF_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" Vector__XXX
+BO_ 184 MC1_ENERGY: 8 ECU
+ SG_ feedback_torque : 48|16@1- (1,0) [0|0] "Mn" Vector__XXX
+ SG_ motor_power : 16|32@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dc_bus_voltage : 0|16@1+ (1,0) [0|0] "V" Vector__XXX
 
-BO_ 1073 RF_TTPMS_2: 8 ECU
- SG_ RF_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
- SG_ RF_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
- SG_ RF_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
- SG_ RF_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+BO_ 204 MCU_ANALOG_READINGS: 8 ECU
+ SG_ glv_battery_voltage : 48|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ hall_effect_current : 32|16@1- (1,0) [0|0] "N/A" Vector__XXX
+ SG_ steering_2 : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ steering_1 : 0|16@1- (1,0) [0|0] "" Vector__XXX
 
-BO_ 1074 RF_TTPMS_3: 8 ECU
- SG_ RF_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
- SG_ RF_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
- SG_ RF_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
- SG_ RF_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+BO_ 257 MCU_ERROR_STATES: 8 ECU
+ SG_ torque_controller_mux_status : 0|3@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 1075 RF_TTPMS_4: 8 ECU
- SG_ RF_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
- SG_ RF_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
- SG_ RF_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
- SG_ RF_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+BO_ 198 MCU_FRONT_POTS: 4 ECU
+ SG_ potentiometer_fr : 16|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+ SG_ potentiometer_fl : 0|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+
+BO_ 197 MCU_LOAD_CELLS: 4 ECU
+ SG_ load_cell_fr : 16|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+ SG_ load_cell_fl : 0|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+
+BO_ 205 MCU_PEDAL_RAW: 8 ECU
+ SG_ brake_2_raw : 36|12@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ brake_1_raw : 24|12@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ accel_2_raw : 12|12@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ accel_1_raw : 0|12@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 195 MCU_STATUS: 8 ECU
+ SG_ distance_traveled_m : 48|16@1+ (0.01,0) [0|0] "" Vector__XXX
+ SG_ torque_mode : 40|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ max_torque : 32|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ drive_mode : 28|4@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ pack_charge_critical : 24|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ launch_control_active : 23|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ software_ok : 22|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ energy_meter_present : 20|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ inverter_error : 19|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ ecu_state : 16|3@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ drive_buzzer : 15|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ no_accel_or_brake_implausibility : 14|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ bspd_brake_high : 13|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ bspd_current_high : 12|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ brake_pedal_active : 11|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ no_brake_implausibility : 10|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ no_accel_implausibility : 9|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mechanical_brake_active : 8|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ shutdown_e_above_threshold : 7|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ software_ok_high : 6|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ shutdown_d_above_threshold : 5|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ bspd_ok_high : 4|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ shutdown_c_above_threshold : 3|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ bms_ok_high : 2|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ shutdown_b_above_threshold : 1|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ imd_ok_high : 0|1@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 200 MCU_SUSPENSION: 8 ECU
+ SG_ load_cell_fr : 48|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+ SG_ load_cell_fl : 32|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+ SG_ potentiometer_fr : 16|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+ SG_ potentiometer_fl : 0|16@1+ (1,0) [0|0] "lbs" Vector__XXX
+
+BO_ 192 PEDALS_SYSTEM_DATA: 5 ECU
+ SG_ brake_pedal : 24|16@1+ (1.5259e-05,0) [0|0] "" Vector__XXX
+ SG_ accel_pedal : 8|16@1+ (1.5259e-05,0) [0|0] "" Vector__XXX
+ SG_ implaus_exceeded_max_duration : 6|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ brake_accel_implausibility : 5|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ mechanical_brake_active : 4|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ accel_pedal_active : 3|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ brake_pedal_active : 2|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ brake_implausible : 1|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ accel_implausible : 0|1@1+ (1,0) [0|1] "" Vector__XXX
+
+BO_ 223 PENTHOUSE_ACCUM_MSG: 8 ECU
+ SG_ hall_curr_signal : 16|16@1+ (1,0) [0|0] "mV" Vector__XXX
+ SG_ hall_curr_ref : 0|16@1+ (1,0) [0|0] "mV" Vector__XXX
+
+BO_ 228 REAR_SUSPENSION: 8 ECU
+ SG_ rr_shock_pot : 48|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ rl_shock_pot : 32|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ rr_load_cell : 16|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ rl_load_cell : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 1072 RF_TTPMS_1: 8 ECU
+ SG_ RF_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX
+ SG_ RF_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX
+ SG_ RF_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX
+ SG_ RF_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 1073 RF_TTPMS_2: 8 ECU
+ SG_ RF_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+ SG_ RF_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+ SG_ RF_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+ SG_ RF_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+
+BO_ 1074 RF_TTPMS_3: 8 ECU
+ SG_ RF_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+ SG_ RF_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+ SG_ RF_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+ SG_ RF_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+
+BO_ 1075 RF_TTPMS_4: 8 ECU
+ SG_ RF_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+ SG_ RF_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+ SG_ RF_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
+ SG_ RF_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
 
 BO_ 1076 RF_TTPMS_5: 8 ECU
  SG_ RF_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX
@@ -565,70 +953,6 @@ BO_ 1083 RR_TTPMS_6: 8 ECU
  SG_ RR_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX
  SG_ RR_TTPMS_TC : 7|16@0+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 1025 ACU_SHUNT_MEASUREMENTS: 6 ECU
- SG_ ts_out_filtered_read : 32|16@1+ (0.169047463702246,0) [0|0] "volts" Vector__XXX
- SG_ pack_filtered_read : 16|16@1+ (0.16869351172632,0) [0|0] "volts" Vector__XXX
- SG_ current_shunt_read : 0|16@1+ (0.291373985492,-666) [0|0] "amps" Vector__XXX
-
-BO_ 1054 STATE_OF_CHARGE: 8 ECU
- SG_ charge_coulombs : 39|32@0+ (0.0001,0) [0|0] "Coulombs" Vector__XXX
- SG_ min_cell_voltage_est : 16|16@1+ (0.0001,0) [0|0] "volts" Vector__XXX
- SG_ charge_percentage : 7|16@0+ (0.01,0) [0|0] "percent" Vector__XXX
-
-BO_ 223 PENTHOUSE_ACCUM_MSG: 8 ECU
- SG_ hall_curr_signal : 16|16@1+ (1,0) [0|0] "mV" Vector__XXX
- SG_ hall_curr_ref : 0|16@1+ (1,0) [0|0] "mV" Vector__XXX
-
-BO_ 512 DRIVETRAIN_RPMS_TELEM: 8 ECU
- SG_ rl_motor_rpm : 48|16@1- (1,0) [0|0] "" Vector__XXX
- SG_ rr_motor_rpm : 32|16@1- (1,0) [0|0] "" Vector__XXX
- SG_ fl_motor_rpm : 16|16@1- (1,0) [0|0] "" Vector__XXX
- SG_ fr_motor_rpm : 0|16@1- (1,0) [0|0] "" Vector__XXX
-
-BO_ 513 DRIVETRAIN_ERR_STATUS_TELEM: 8 ECU
- SG_ mc4_diagnostic_number : 48|16@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc3_diagnostic_number : 32|16@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc2_diagnostic_number : 16|16@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc1_diagnostic_number : 0|16@1+ (1,0) [0|0] "" Vector__XXX
-
-BO_ 514 DRIVETRAIN_STATUS_TELEM: 8 ECU
- SG_ accel_percent : 48|8@1+ (1,0) [0|100] "" Vector__XXX
- SG_ brake_percent : 40|8@1+ (1,0) [0|100] "" Vector__XXX
- SG_ brake_implausible : 33|1@1+ (1,0) [0|1] "" Vector__XXX
- SG_ accel_implausible : 32|1@1+ (1,0) [0|1] "" Vector__XXX
- SG_ mc4_warning : 31|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc4_system_ready : 30|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc4_quit_inverter_on : 29|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc4_quit_dc : 28|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc4_inverter_on : 27|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc4_error : 26|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc4_derating_on : 25|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc4_dc_on : 24|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc3_warning : 23|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc3_system_ready : 22|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc3_quit_inverter_on : 21|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc3_quit_dc : 20|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc3_inverter_on : 19|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc3_error : 18|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc3_derating_on : 17|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc3_dc_on : 16|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc2_warning : 15|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc2_system_ready : 14|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc2_quit_inverter_on : 13|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc2_quit_dc : 12|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc2_inverter_on : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc2_error : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc2_derating_on : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc2_dc_on : 8|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc1_warning : 7|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc1_system_ready : 6|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc1_quit_inverter_on : 5|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc1_quit_dc : 4|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc1_inverter_on : 3|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc1_error : 2|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc1_derating_on : 1|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mc1_dc_on : 0|1@1+ (1,0) [0|0] "" Vector__XXX
-
 BO_ 229 SAB_THERMISTORS_1: 8 ECU
  SG_ thermistor_acc2 : 48|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX
  SG_ thermistor_acc1 : 32|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX
@@ -640,86 +964,60 @@ BO_ 230 SAB_THERMISTORS_2: 6 ECU
  SG_ thermistor_motor_rr : 16|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX
  SG_ thermistor_motor_rl : 0|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX
 
-BO_ 228 SAB_SUSPENSION: 8 ECU
- SG_ load_cell_rr : 48|16@1+ (1,0) [0|0] "lbs" Vector__XXX
- SG_ load_cell_rl : 32|16@1+ (1,0) [0|0] "lbs" Vector__XXX
- SG_ potentiometer_rr : 16|16@1+ (1,0) [0|0] "lbs" Vector__XXX
- SG_ potentiometer_rl : 0|16@1+ (1,0) [0|0] "lbs" Vector__XXX
-
-BO_ 232 TCU_STATUS: 3 ECU
- SG_ tcu_recording_state : 16|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ shutdown_status : 0|16@1+ (1,0) [0|0] "" Vector__XXX
-
-BO_ 208 VN_VEL: 8 ECU
- SG_ vn_vel_uncertainty : 48|16@1+ (0.0005,0) [0|0] "m/s" Vector__XXX
- SG_ vn_body_vel_z : 32|16@1- (0.002,0) [0|0] "m/s" Vector__XXX
- SG_ vn_body_vel_y : 16|16@1- (0.002,0) [0|0] "m/s" Vector__XXX
- SG_ vn_body_vel_x : 0|16@1- (0.002,0) [0|0] "m/s" Vector__XXX
-
-BO_ 209 VN_LINEAR_ACCEL: 8 ECU
- SG_ vn_lin_ins_accel_z : 32|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
- SG_ vn_lin_ins_accel_y : 16|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
- SG_ vn_lin_ins_accel_x : 0|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
-
-BO_ 210 VN_LINEAR_ACCEL_UNCOMP: 8 ECU
- SG_ vn_lin_uncomp_accel_z : 32|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
- SG_ vn_lin_uncomp_accel_y : 16|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
- SG_ vn_lin_uncomp_accel_x : 0|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
-
-BO_ 211 VN_YPR: 8 ECU
- SG_ vn_roll : 32|16@1- (0.01,0) [0|0] "deg" Vector__XXX
- SG_ vn_pitch : 16|16@1- (0.01,0) [0|0] "deg" Vector__XXX
- SG_ vn_yaw : 0|16@1- (0.01,0) [0|0] "deg" Vector__XXX
-
-BO_ 212 VN_LAT_LON: 8 ECU
- SG_ vn_gps_lon : 32|32@1- (1e-06,0) [0|0] "deg" Vector__XXX
- SG_ vn_gps_lat : 0|32@1- (1e-06,0) [0|0] "deg" Vector__XXX
-
-BO_ 224 VN_GPS_TIME_MSG: 8 ECU
- SG_ vn_gps_time : 0|64@1+ (1,0) [0|0] "ns" Vector__XXX
-
-BO_ 225 VN_STATUS: 8 ECU
- SG_ vn_gps_status : 0|16@1+ (1,0) [0|0] "" Vector__XXX
-
-BO_ 515 DRIVETRAIN_TORQUE_TELEM: 8 ECU
- SG_ rr_motor_torque : 48|16@1- (0.01,0) [0|0] "nm" Vector__XXX
- SG_ rl_motor_torque : 32|16@1- (0.01,0) [0|0] "nm" Vector__XXX
- SG_ fr_motor_torque : 16|16@1- (0.01,0) [0|0] "nm" Vector__XXX
- SG_ fl_motor_torque : 0|16@1- (0.01,0) [0|0] "nm" Vector__XXX
-
-BO_ 200 MCU_SUSPENSION: 8 ECU
- SG_ load_cell_fr : 48|16@1+ (1,0) [0|0] "lbs" Vector__XXX
- SG_ load_cell_fl : 32|16@1+ (1,0) [0|0] "lbs" Vector__XXX
- SG_ potentiometer_fr : 16|16@1+ (1,0) [0|0] "lbs" Vector__XXX
- SG_ potentiometer_fl : 0|16@1+ (1,0) [0|0] "lbs" Vector__XXX
-
-BO_ 227 VN_ANGULAR_RATE: 8 ECU
- SG_ angular_rate_z : 32|16@1- (0.001,0) [0|0] "rad/s" Vector__XXX
- SG_ angular_rate_y : 16|16@1- (0.001,0) [0|0] "rad/s" Vector__XXX
- SG_ angular_rate_x : 0|16@1- (0.001,0) [0|0] "rad/s" Vector__XXX
+BO_ 1054 STATE_OF_CHARGE: 8 ECU
+ SG_ charge_coulombs : 39|32@0+ (0.0001,0) [0|0] "Coulombs" Vector__XXX
+ SG_ min_cell_voltage_est : 16|16@1+ (0.0001,0) [0|0] "volts" Vector__XXX
+ SG_ charge_percentage : 7|16@0+ (0.01,0) [0|0] "percent" Vector__XXX
 
-BO_ 231 VN_ECEF_POS_XY: 8 ECU
- SG_ vn_ecef_pos_y : 32|32@1- (0.005,0) [0|0] "m" Vector__XXX
- SG_ vn_ecef_pos_x : 0|32@1- (0.005,0) [0|0] "m" Vector__XXX
+BO_ 1055 STEERING_DATA: 6 ECU
+ SG_ steering_digital_raw : 16|32@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ steering_analog_raw : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 3 Settings: 6 ECU
+ SG_ Velocity_Filtering : 43|3@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ IMU_Filtering : 40|3@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ Attitude_Filtering : 37|3@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ Sensor_Y_Location : 25|12@1- (0.01,0) [0|0] "m" Vector__XXX
+ SG_ Sensor_X_Location : 13|12@1- (0.01,0) [0|0] "m" Vector__XXX
+ SG_ Height_Offset : 5|8@1- (0.5,0) [0|0] "cm" Vector__XXX
+ SG_ CAN_Termination_Setting_Bus_2 : 4|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ CAN_Termination_Setting_Bus_1 : 3|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ Broadcast_rate : 0|3@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 2 State: 5 ECU
+ SG_ SpeedBeam_Health : 32|3@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ sensor_3_Validity : 30|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ sensor_2_Validity : 28|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ sensor_1_Validity : 26|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ sensor_0_Validity : 24|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ Firmware_Version : 8|16@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ Temperature_Internal : 0|8@1- (0.5,20) [0|0] "C" Vector__XXX
 
-BO_ 233 VN_ECEF_POS_Z_MSG: 8 ECU
- SG_ vn_ecef_pos_z : 0|32@1- (0.005,0) [0|0] "m" Vector__XXX
+BO_ 1265 TCMUX_STATUS_REPORT: 8 ECU
+ SG_ torque_limit : 36|16@1+ (0.001,0) [0|0] "" Vector__XXX
+ SG_ torque_mode : 28|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ dash_dial_mode : 20|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mode_actual : 12|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ mode_intended : 4|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ steering_system_has_err : 3|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ tc_not_ready : 2|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ torque_delta_above_thresh : 1|1@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ speed_above_thresh : 0|1@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 205 MCU_PEDAL_RAW: 8 ECU
- SG_ brake_2_raw : 36|12@1+ (1,0) [0|0] "" Vector__XXX
- SG_ brake_1_raw : 24|12@1+ (1,0) [0|0] "" Vector__XXX
- SG_ accel_2_raw : 12|12@1+ (1,0) [0|0] "" Vector__XXX
- SG_ accel_1_raw : 0|12@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 148 TCU_DRIVER_MSG: 8 ECU
+ SG_ target_lap_time : 40|24@1+ (1,0) [0|0] "ms" Vector__XXX
+ SG_ driver_msg_var_2 : 24|16@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ driver_msg_var_1 : 8|16@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ driver_message : 0|8@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 1263 CONTROLLER_PID_TV_DATA: 8 ECU
- SG_ controller_output : 20|20@1- (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_input : 0|20@1- (0.01,0) [0|0] "" Vector__XXX
+BO_ 149 TCU_LAP_TIMES: 8 ECU
+ SG_ lap_clock_state : 48|8@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ prev_lap_time : 24|24@1+ (1,0) [0|0] "ms" Vector__XXX
+ SG_ best_lap_time : 0|24@1+ (1,0) [0|0] "ms" Vector__XXX
 
-BO_ 1264 CONTROLLER_PID_TV_DELTA_DATA: 8 ECU
- SG_ pid_tv_rr_delta : 30|10@1- (0.1,0) [0|0] "" Vector__XXX
- SG_ pid_tv_rl_delta : 20|10@1- (0.1,0) [0|0] "" Vector__XXX
- SG_ pid_tv_fr_delta : 10|10@1- (0.1,0) [0|0] "" Vector__XXX
- SG_ pid_tv_fl_delta : 0|10@1- (0.1,0) [0|0] "" Vector__XXX
+BO_ 232 TCU_STATUS: 3 ECU
+ SG_ tcu_recording_state : 16|2@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ shutdown_status : 0|16@1+ (1,0) [0|0] "" Vector__XXX
 
 BO_ 201 TC_SIMPLE: 8 ECU
  SG_ accel_request_state : 48|1@1+ (1,0) [0|0] "" Vector__XXX
@@ -744,6 +1042,9 @@ BO_ 2047 VEHM_ALPHA: 8 ECU
 BO_ 2031 VEHM_BETA: 8 ECU
  SG_ vehm_beta_deg : 0|16@1- (0.001,0) [0|0] "deg" Vector__XXX
 
+BO_ 1995 VEHM_KIN_DES_YAW_RATE_RAD_S_MSG: 8 ECU
+ SG_ vehm_kin_desired_yaw_rate_rad_s : 0|16@1- (0.0001,0) [0|0] "rad/s" Vector__XXX
+
 BO_ 1998 VEHM_LONG_CORNER_VEL: 8 ECU
  SG_ vehm_long_corner_vel_rr : 48|16@1- (0.01,0) [0|0] "m/s" Vector__XXX
  SG_ vehm_long_corner_vel_rl : 32|16@1- (0.01,0) [0|0] "m/s" Vector__XXX
@@ -756,207 +1057,26 @@ BO_ 1999 VEHM_SL: 8 ECU
  SG_ vehm_sl_fr : 16|16@1- (0.0001,0) [0|0] "" Vector__XXX
  SG_ vehm_sl_fl : 0|16@1- (0.0001,0) [0|0] "" Vector__XXX
 
-BO_ 1994 VEHM_WHEEL_STEER_AVG_DEG_MSG: 8 ECU
- SG_ vehm_wheel_steer_avg_deg : 0|16@1- (0.001,0) [0|0] "deg" Vector__XXX
-
-BO_ 1995 VEHM_KIN_DES_YAW_RATE_RAD_S_MSG: 8 ECU
- SG_ vehm_kin_desired_yaw_rate_rad_s : 0|16@1- (0.0001,0) [0|0] "rad/s" Vector__XXX
-
-BO_ 1997 CONTROLLER_PID_YAW: 8 ECU
- SG_ controller_yaw_pid_output : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_yaw_rate_error : 16|16@1- (0.0001,0) [0|0] "rad/s" Vector__XXX
- SG_ vehm_kin_desired_yaw_rate_rad_s : 0|16@1- (0.0001,0) [0|0] "rad/s" Vector__XXX
-
-BO_ 2010 CONTROLLER_PID_YAW_TORQUE: 8 ECU
- SG_ controller_yaw_torque_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_yaw_torque_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_yaw_torque_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_yaw_torque_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
-
-BO_ 2011 CONTROLLER_NORMAL_DIST: 8 ECU
- SG_ controller_normal_percent_rr : 48|16@1- (0.0001,0) [0|0] "" Vector__XXX
- SG_ controller_normal_percent_rl : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX
- SG_ controller_normal_percent_fr : 16|16@1- (0.0001,0) [0|0] "" Vector__XXX
- SG_ controller_normal_percent_fl : 0|16@1- (0.0001,0) [0|0] "" Vector__XXX
-
-BO_ 2012 CONTROLLER_NORMAL_TORQUE: 8 ECU
- SG_ controller_normal_torque_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_normal_torque_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_normal_torque_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_normal_torque_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
-
-BO_ 2014 CONTROLLER_POWER_LIM: 8 ECU
- SG_ controller_power_lim_torque_adj : 20|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_power_lim_error : 4|16@1- (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_power_lim_status : 0|4@1+ (1,0) [0|0] "" Vector__XXX
-
-BO_ 2026 CONTROLLER_POWER_LIM_CORNER_POW: 8 ECU
- SG_ controller_power_lim_cornerp_rr : 48|16@1- (0.01,0) [0|0] "kW" Vector__XXX
- SG_ controller_power_lim_cornerp_rl : 32|16@1- (0.01,0) [0|0] "kW" Vector__XXX
- SG_ controller_power_lim_cornerp_fr : 16|16@1- (0.01,0) [0|0] "kW" Vector__XXX
- SG_ controller_power_lim_cornerp_fl : 0|16@1- (0.01,0) [0|0] "kW" Vector__XXX
-
-BO_ 2027 CONTROLLER_POWER_LIM_TORQUE: 8 ECU
- SG_ controller_power_lim_torque_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_power_lim_torque_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_power_lim_torque_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_power_lim_torque_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
-
-BO_ 2028 CONTROLLER_BOOLEAN: 8 ECU
- SG_ controller_use_nl_tcs_slipschedu : 13|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_rpm_tcs_gain_sche : 12|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_nl_tcs_gain_sche : 11|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_torque_bias : 10|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_no_regen_5kph : 9|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_discontin_brakes : 8|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_dec_yaw_pid_brake : 7|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_tcs_lim_yaw_pid : 6|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_tcs : 5|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_power_limit : 4|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_pid_power_limit : 3|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_normal_force : 2|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_pid_tv : 1|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_use_launch : 0|1@1+ (1,0) [0|0] "" Vector__XXX
-
-BO_ 2042 CONTROLLER_TORQUE_SETUP: 8 ECU
- SG_ controller_max_nl_brake_sc_front : 56|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_max_yaw_nl_accel_perc : 52|4@1+ (0.1,0) [0|0] "" Vector__XXX
- SG_ controller_torque_mode : 44|8@1+ (0.1,0) [0|0] "Nm" Vector__XXX
- SG_ controller_regen_limit : 32|12@1- (0.01,0) [0|0] "Nm" Vector__XXX
- SG_ controller_constrained_torq_req : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_initial_torque_req : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
-
-BO_ 2043 CONTROLLER_TCS_CONFIG: 8 ECU
- SG_ controller_tcs_sl_nlperc_star_rr : 46|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_sl_nlperc_end_rr : 38|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_sl_nlperc_end_fr : 30|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_sl_nlperc_star_fr : 22|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_vel_thresh : 14|8@1+ (0.01,0) [0|0] "m/s" Vector__XXX
- SG_ controller_tcs_launch_vel_thresh : 8|6@1+ (0.01,0) [0|0] "m/s" Vector__XXX
- SG_ controller_tcs_launch_dead_zone : 0|8@1+ (0.1,0) [0|0] "Nm" Vector__XXX
-
-BO_ 2044 CONTROLLER_TCS_TORQUE: 8 ECU
- SG_ controller_tcs_torque_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_torque_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_torque_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_torque_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
-
-BO_ 2045 CONTROLLER_TCS_STATUS: 8 ECU
- SG_ controller_tcs_status_rr : 6|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_status_rl : 4|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_status_fr : 2|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_status_fl : 0|2@1+ (1,0) [0|0] "" Vector__XXX
-
-BO_ 2046 CONTROLLER_TCS_PID_INPUT: 8 ECU
- SG_ controller_tcs_pid_input_rr : 48|16@1- (0.0001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_pid_input_rl : 32|16@1- (0.0001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_pid_input_fr : 16|16@1- (0.0001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_pid_input_fl : 0|16@1- (0.0001,0) [0|0] "Nm" Vector__XXX
-
-BO_ 1962 CONTROLLER_TCS_PID_OUTPUT: 8 ECU
- SG_ controller_tcs_pid_output_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_pid_output_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_pid_output_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_pid_output_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
-
 BO_ 1963 VEHM_WHEEL_LIN_VEL: 8 ECU
  SG_ vehm_wheel_lin_vel_rr : 48|16@1- (0.01,0) [0|0] "m/s" Vector__XXX
  SG_ vehm_wheel_lin_vel_rl : 32|16@1- (0.01,0) [0|0] "m/s" Vector__XXX
  SG_ vehm_wheel_lin_vel_fr : 16|16@1- (0.01,0) [0|0] "m/s" Vector__XXX
  SG_ vehm_wheel_lin_vel_fl : 0|16@1- (0.01,0) [0|0] "m/s" Vector__XXX
 
-BO_ 1964 CONTROLLER_TORQUE_BIAS: 8 ECU
- SG_ controller_bias_torq_avg_rear : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_bias_torq_avg_front : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
-
-BO_ 1965 CONTROLLER_REGEN_5KPH_TORQUE: 8 ECU
- SG_ controller_regen_5kph_torq_rr : 48|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_regen_5kph_torq_rl : 32|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_regen_5kph_torq_fr : 16|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
- SG_ controller_regen_5kph_torq_fl : 0|16@1- (0.001,0) [0|0] "Nm" Vector__XXX
-
-BO_ 1966 CONTROLLER_REGEN_5KPH_STATUS: 8 ECU
- SG_ controller_regen_5kph_status_rr : 3|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_regen_5kph_status_rl : 2|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_regen_5kph_status_fr : 1|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ controller_regen_5kph_status_fl : 0|1@1+ (1,0) [0|0] "" Vector__XXX
-
-BO_ 1978 CONTROLLER_YAW_PID_CONFIG: 8 ECU
- SG_ controller_yaw_pid_brakes_d : 54|10@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_yaw_pid_brakes_i : 44|10@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_yaw_pid_brakes_p : 33|11@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_yaw_pid_d : 23|10@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_yaw_pid_i : 13|10@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_yaw_pid_p : 0|13@1+ (0.001,0) [0|0] "" Vector__XXX
-
-BO_ 1967 CONTROLLER_TCS_PID_CONFIG: 8 ECU
- SG_ controller_tcs_pid_p_rr : 48|16@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_pid_p_rl : 32|16@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_pid_p_fr : 16|16@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_pid_p_fl : 0|16@1+ (0.001,0) [0|0] "" Vector__XXX
-
-BO_ 245 FRONT_THERMISTORS: 8 ECU
- SG_ thermistor_motor_fr : 16|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX
- SG_ thermistor_motor_fl : 0|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX
-
-BO_ 1979 CONTROLLER_TCS_SATURATION_CONFIG: 8 ECU
- SG_ controller_tcs_saturation_rear : 12|12@1+ (0.01,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_saturation_front : 0|12@1+ (0.01,0) [0|0] "Nm" Vector__XXX
-
-BO_ 1981 CONTROLLER_TCS_DIFF_CONFIG: 8 ECU
- SG_ controller_tcs_launch_LRdiff : 51|13@1+ (0.01,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_w_steer_upper_b : 39|12@1+ (0.01,0) [0|0] "deg" Vector__XXX
- SG_ controller_tcs_w_steer_lower_b : 27|12@1+ (0.01,0) [0|0] "deg" Vector__XXX
- SG_ controller_tcs_gen_LRdiff_upperB : 13|14@1+ (0.01,0) [0|0] "Nm" Vector__XXX
- SG_ controller_tcs_gen_LRdiff_lowerB : 0|13@1+ (0.01,0) [0|0] "Nm" Vector__XXX
-
-BO_ 257 MCU_ERROR_STATES: 8 ECU
- SG_ torque_controller_mux_status : 0|3@1+ (1,0) [0|0] "" Vector__XXX
-
-BO_ 1055 STEERING_SYSTEM_REPORT: 8 ECU
- SG_ steering_analog_status : 52|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ steering_encoder_status : 50|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ steering_system_status : 48|2@1+ (1,0) [0|0] "" Vector__XXX
- SG_ steering_analog_angle : 32|16@1- (0.01,0) [0|0] "" Vector__XXX
- SG_ steering_encoder_angle : 16|16@1- (0.01,0) [0|0] "deg" Vector__XXX
- SG_ steering_system_angle : 0|16@1- (0.01,0) [0|0] "deg" Vector__XXX
-
-BO_ 1982 CONTROLLER_TCS_RPM_SCHE_CONFIG: 8 ECU
- SG_ controller_tcs_upper_rpm_rear : 45|15@1+ (1,0) [0|0] "RPM" Vector__XXX
- SG_ controller_tcs_upper_rpm_front : 30|15@1+ (1,0) [0|0] "RPM" Vector__XXX
- SG_ controller_tcs_lower_rpm_rear : 15|15@1+ (1,0) [0|0] "RPM" Vector__XXX
- SG_ controller_tcs_lower_rpm_front : 0|15@1+ (1,0) [0|0] "RPM" Vector__XXX
-
-BO_ 1996 CONTROLLER_TCS_NL_SCHE_CONFIG: 8 ECU
- SG_ controller_tcs_startper_nl_rear : 24|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_startper_nl_front : 16|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_endper_nl_rear : 8|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_endper_nl_front : 0|8@1+ (0.01,0) [0|0] "" Vector__XXX
-
-BO_ 1980 CONTROLLER_TCS_CONFIG_CONT: 8 ECU
- SG_ controller_tcs_SL_end_rear : 56|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_SL_start_rear : 48|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_SL_end_front : 40|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_SL_start_front : 32|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_lauSL_end_rear : 24|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_lauSL_start_rear : 16|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_lauSL_end_front : 8|8@1+ (0.01,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_lauSL_start_front : 0|8@1+ (0.01,0) [0|0] "" Vector__XXX
-
-BO_ 1983 CONTROLLER_TCS_SLIP_TARGETS: 8 ECU
- SG_ controller_tcs_slip_target_rear : 12|12@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ controller_tcs_slip_target_front : 0|12@1+ (0.001,0) [0|0] "" Vector__XXX
+BO_ 1994 VEHM_WHEEL_STEER_AVG_DEG_MSG: 8 ECU
+ SG_ vehm_wheel_steer_avg_deg : 0|16@1- (0.001,0) [0|0] "deg" Vector__XXX
 
-BO_ 1265 TCMUX_STATUS_REPORT: 8 ECU
- SG_ torque_limit : 36|16@1+ (0.001,0) [0|0] "" Vector__XXX
- SG_ torque_mode : 28|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ dash_dial_mode : 20|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mode_actual : 12|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ mode_intended : 4|8@1+ (1,0) [0|0] "" Vector__XXX
- SG_ steering_system_has_err : 3|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ tc_not_ready : 2|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ torque_delta_above_thresh : 1|1@1+ (1,0) [0|0] "" Vector__XXX
- SG_ speed_above_thresh : 0|1@1+ (1,0) [0|0] "" Vector__XXX
+BO_ 227 VN_ANGULAR_RATE: 8 ECU
+ SG_ angular_rate_z : 32|16@1- (0.001,0) [0|0] "rad/s" Vector__XXX
+ SG_ angular_rate_y : 16|16@1- (0.001,0) [0|0] "rad/s" Vector__XXX
+ SG_ angular_rate_x : 0|16@1- (0.001,0) [0|0] "rad/s" Vector__XXX
+
+BO_ 231 VN_ECEF_POS_XY: 8 ECU
+ SG_ vn_ecef_pos_y : 32|32@1- (0.005,0) [0|0] "m" Vector__XXX
+ SG_ vn_ecef_pos_x : 0|32@1- (0.005,0) [0|0] "m" Vector__XXX
+
+BO_ 233 VN_ECEF_POS_Z_MSG: 8 ECU
+ SG_ vn_ecef_pos_z : 0|32@1- (0.005,0) [0|0] "m" Vector__XXX
 
 BO_ 234 VN_GNSS_COMP_SIG_HEALTH: 8 ECU
  SG_ num_com_sats_rtk : 56|8@1+ (1,0) [0|0] "" Vector__XXX
@@ -968,12 +1088,53 @@ BO_ 234 VN_GNSS_COMP_SIG_HEALTH: 8 ECU
  SG_ num_sats_rtk_1 : 8|8@1+ (1,0) [0|0] "" Vector__XXX
  SG_ num_sats_pvt_1 : 0|8@1+ (1,0) [0|0] "" Vector__XXX
 
-BO_ 303 DRIVETRAIN_COMMAND: 8 ECU
- SG_ drivetrain_traj_torque_lim_rr : 48|16@1- (0.001,0) [0|0] "" Vector__XXX
- SG_ drivetrain_traj_torque_lim_rl : 32|16@1- (0.001,0) [0|0] "" Vector__XXX
- SG_ drivetrain_traj_torque_lim_fr : 16|16@1- (0.001,0) [0|0] "" Vector__XXX
- SG_ drivetrain_traj_torque_lim_fl : 0|16@1- (0.001,0) [0|0] "" Vector__XXX
+BO_ 224 VN_GPS_TIME_MSG: 8 ECU
+ SG_ vn_gps_time : 0|64@1+ (1,0) [0|0] "ns" Vector__XXX
+
+BO_ 212 VN_LAT_LON: 8 ECU
+ SG_ vn_gps_lon : 32|32@1- (1e-06,0) [0|0] "deg" Vector__XXX
+ SG_ vn_gps_lat : 0|32@1- (1e-06,0) [0|0] "deg" Vector__XXX
+
+BO_ 209 VN_LINEAR_ACCEL: 8 ECU
+ SG_ vn_lin_ins_accel_z : 32|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
+ SG_ vn_lin_ins_accel_y : 16|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
+ SG_ vn_lin_ins_accel_x : 0|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
+
+BO_ 210 VN_LINEAR_ACCEL_UNCOMP: 8 ECU
+ SG_ vn_lin_uncomp_accel_z : 32|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
+ SG_ vn_lin_uncomp_accel_y : 16|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
+ SG_ vn_lin_uncomp_accel_x : 0|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX
+
+BO_ 225 VN_STATUS: 8 ECU
+ SG_ vn_gps_status : 0|16@1+ (1,0) [0|0] "" Vector__XXX
+
+BO_ 208 VN_VEL: 8 ECU
+ SG_ vn_vel_uncertainty : 48|16@1+ (0.0005,0) [0|0] "m/s" Vector__XXX
+ SG_ vn_body_vel_z : 32|16@1- (0.002,0) [0|0] "m/s" Vector__XXX
+ SG_ vn_body_vel_y : 16|16@1- (0.002,0) [0|0] "m/s" Vector__XXX
+ SG_ vn_body_vel_x : 0|16@1- (0.002,0) [0|0] "m/s" Vector__XXX
+
+BO_ 211 VN_YPR: 8 ECU
+ SG_ vn_roll : 32|16@1- (0.01,0) [0|0] "deg" Vector__XXX
+ SG_ vn_pitch : 16|16@1- (0.01,0) [0|0] "deg" Vector__XXX
+ SG_ vn_yaw : 0|16@1- (0.01,0) [0|0] "deg" Vector__XXX
+
+BO_ 4 Velocities: 8 ECU
+ SG_ Velocity_z_CGCorrected : 53|9@1- (0.025,0) [0|0] "m/s" Vector__XXX
+ SG_ Velocity_y_CGCorrected : 43|10@1- (0.03,0) [0|0] "m/s" Vector__XXX
+ SG_ Velocity_x_CGCorrected : 31|12@1- (0.02,0) [0|0] "m/s" Vector__XXX
+ SG_ Velocity_z : 22|9@1- (0.025,0) [0|0] "m/s" Vector__XXX
+ SG_ Velocity_y : 12|10@1- (0.03,0) [0|0] "m/s" Vector__XXX
+ SG_ Velocity_x : 0|12@1- (0.02,0) [0|0] "m/s" Vector__XXX
+
+BO_ 0 brake_temps: 8 ECU
 
+BO_ 1280 ACU_OK: 1 ECU
+ SG_ imd_ok : 1|1@1+ (1,0) [0|1] "" Vector__XXX
+ SG_ bms_ok : 0|1@1+ (1,0) [0|1] "" Vector__XXX
+
+BO_TX_BU_ 1025 : ECU,Peripherals;
+BO_TX_BU_ 5 : ECU,Peripherals;
 BO_TX_BU_ 222 : ECU,Peripherals;
 BO_TX_BU_ 226 : ECU,Peripherals;
 BO_TX_BU_ 218 : ECU,Peripherals;
@@ -983,41 +1144,88 @@ BO_TX_BU_ 213 : ECU,Peripherals;
 BO_TX_BU_ 219 : ECU,Peripherals;
 BO_TX_BU_ 217 : ECU,Peripherals;
 BO_TX_BU_ 215 : ECU,Peripherals;
+BO_TX_BU_ 203 : ECU,Peripherals;
 BO_TX_BU_ 221 : ECU,Peripherals;
 BO_TX_BU_ 2550588916 : ECU,Peripherals;
 BO_TX_BU_ 2566869221 : ECU,Peripherals;
+BO_TX_BU_ 2028 : ECU,Peripherals;
+BO_TX_BU_ 2011 : ECU,Peripherals;
+BO_TX_BU_ 2012 : ECU,Peripherals;
+BO_TX_BU_ 1263 : ECU,Peripherals;
+BO_TX_BU_ 1264 : ECU,Peripherals;
+BO_TX_BU_ 1997 : ECU,Peripherals;
+BO_TX_BU_ 2010 : ECU,Peripherals;
+BO_TX_BU_ 2014 : ECU,Peripherals;
+BO_TX_BU_ 2026 : ECU,Peripherals;
+BO_TX_BU_ 2027 : ECU,Peripherals;
+BO_TX_BU_ 1966 : ECU,Peripherals;
+BO_TX_BU_ 1965 : ECU,Peripherals;
+BO_TX_BU_ 2043 : ECU,Peripherals;
+BO_TX_BU_ 1980 : ECU,Peripherals;
+BO_TX_BU_ 1981 : ECU,Peripherals;
+BO_TX_BU_ 1996 : ECU,Peripherals;
+BO_TX_BU_ 1967 : ECU,Peripherals;
+BO_TX_BU_ 2046 : ECU,Peripherals;
+BO_TX_BU_ 1962 : ECU,Peripherals;
+BO_TX_BU_ 1982 : ECU,Peripherals;
+BO_TX_BU_ 1979 : ECU,Peripherals;
+BO_TX_BU_ 1983 : ECU,Peripherals;
+BO_TX_BU_ 2045 : ECU,Peripherals;
+BO_TX_BU_ 2044 : ECU,Peripherals;
+BO_TX_BU_ 1964 : ECU,Peripherals;
+BO_TX_BU_ 2042 : ECU,Peripherals;
+BO_TX_BU_ 1978 : ECU,Peripherals;
+BO_TX_BU_ 6 : ECU,Peripherals;
+BO_TX_BU_ 2033 : ECU,Peripherals;
+BO_TX_BU_ 235 : ECU,Peripherals;
+BO_TX_BU_ 236 : ECU,Peripherals;
+BO_TX_BU_ 768 : ECU,Peripherals;
+BO_TX_BU_ 243 : ECU,Peripherals;
+BO_TX_BU_ 242 : ECU,Peripherals;
+BO_TX_BU_ 241 : ECU,Peripherals;
+BO_TX_BU_ 303 : ECU,Peripherals;
+BO_TX_BU_ 513 : ECU,Peripherals;
+BO_TX_BU_ 516 : ECU,Peripherals;
+BO_TX_BU_ 512 : ECU,Peripherals;
+BO_TX_BU_ 514 : ECU,Peripherals;
+BO_TX_BU_ 515 : ECU,Peripherals;
 BO_TX_BU_ 256 : ECU,Peripherals;
 BO_TX_BU_ 1024 : ECU,Peripherals;
-BO_TX_BU_ 184 : ECU,Peripherals;
-BO_TX_BU_ 160 : ECU,Peripherals;
-BO_TX_BU_ 176 : ECU,Peripherals;
-BO_TX_BU_ 180 : ECU,Peripherals;
-BO_TX_BU_ 164 : ECU,Peripherals;
-BO_TX_BU_ 185 : ECU,Peripherals;
-BO_TX_BU_ 161 : ECU,Peripherals;
-BO_TX_BU_ 177 : ECU,Peripherals;
-BO_TX_BU_ 181 : ECU,Peripherals;
-BO_TX_BU_ 165 : ECU,Peripherals;
-BO_TX_BU_ 186 : ECU,Peripherals;
-BO_TX_BU_ 162 : ECU,Peripherals;
-BO_TX_BU_ 178 : ECU,Peripherals;
-BO_TX_BU_ 182 : ECU,Peripherals;
-BO_TX_BU_ 166 : ECU,Peripherals;
-BO_TX_BU_ 187 : ECU,Peripherals;
-BO_TX_BU_ 163 : ECU,Peripherals;
-BO_TX_BU_ 179 : ECU,Peripherals;
-BO_TX_BU_ 183 : ECU,Peripherals;
-BO_TX_BU_ 167 : ECU,Peripherals;
-BO_TX_BU_ 204 : ECU,Peripherals;
-BO_TX_BU_ 198 : ECU,Peripherals;
-BO_TX_BU_ 197 : ECU,Peripherals;
-BO_TX_BU_ 196 : ECU,Peripherals;
-BO_TX_BU_ 199 : ECU,Peripherals;
-BO_TX_BU_ 195 : ECU,Peripherals;
-BO_TX_BU_ 236 : ECU,Peripherals;
-BO_TX_BU_ 235 : ECU,Peripherals;
-BO_TX_BU_ 149 : ECU,Peripherals;
-BO_TX_BU_ 148 : ECU,Peripherals;
+BO_TX_BU_ 237 : ECU,Peripherals;
+BO_TX_BU_ 245 : ECU,Peripherals;
+BO_TX_BU_ 1 : ECU,Peripherals;
+BO_TX_BU_ 151 : ECU,Peripherals;
+BO_TX_BU_ 259 : ECU,Peripherals;
+BO_TX_BU_ 144 : ECU,Peripherals;
+BO_TX_BU_ 114 : ECU,Peripherals;
+BO_TX_BU_ 116 : ECU,Peripherals;
+BO_TX_BU_ 115 : ECU,Peripherals;
+BO_TX_BU_ 112 : ECU,Peripherals;
+BO_TX_BU_ 113 : ECU,Peripherals;
+BO_TX_BU_ 152 : ECU,Peripherals;
+BO_TX_BU_ 260 : ECU,Peripherals;
+BO_TX_BU_ 145 : ECU,Peripherals;
+BO_TX_BU_ 119 : ECU,Peripherals;
+BO_TX_BU_ 121 : ECU,Peripherals;
+BO_TX_BU_ 120 : ECU,Peripherals;
+BO_TX_BU_ 117 : ECU,Peripherals;
+BO_TX_BU_ 118 : ECU,Peripherals;
+BO_TX_BU_ 153 : ECU,Peripherals;
+BO_TX_BU_ 261 : ECU,Peripherals;
+BO_TX_BU_ 146 : ECU,Peripherals;
+BO_TX_BU_ 130 : ECU,Peripherals;
+BO_TX_BU_ 132 : ECU,Peripherals;
+BO_TX_BU_ 131 : ECU,Peripherals;
+BO_TX_BU_ 128 : ECU,Peripherals;
+BO_TX_BU_ 129 : ECU,Peripherals;
+BO_TX_BU_ 258 : ECU,Peripherals;
+BO_TX_BU_ 262 : ECU,Peripherals;
+BO_TX_BU_ 147 : ECU,Peripherals;
+BO_TX_BU_ 135 : ECU,Peripherals;
+BO_TX_BU_ 137 : ECU,Peripherals;
+BO_TX_BU_ 136 : ECU,Peripherals;
+BO_TX_BU_ 133 : ECU,Peripherals;
+BO_TX_BU_ 134 : ECU,Peripherals;
 BO_TX_BU_ 1060 : ECU,Peripherals;
 BO_TX_BU_ 1061 : ECU,Peripherals;
 BO_TX_BU_ 1062 : ECU,Peripherals;
@@ -1030,6 +1238,17 @@ BO_TX_BU_ 1068 : ECU,Peripherals;
 BO_TX_BU_ 1069 : ECU,Peripherals;
 BO_TX_BU_ 1070 : ECU,Peripherals;
 BO_TX_BU_ 1071 : ECU,Peripherals;
+BO_TX_BU_ 184 : ECU,Peripherals;
+BO_TX_BU_ 204 : ECU,Peripherals;
+BO_TX_BU_ 257 : ECU,Peripherals;
+BO_TX_BU_ 198 : ECU,Peripherals;
+BO_TX_BU_ 197 : ECU,Peripherals;
+BO_TX_BU_ 205 : ECU,Peripherals;
+BO_TX_BU_ 195 : ECU,Peripherals;
+BO_TX_BU_ 200 : ECU,Peripherals;
+BO_TX_BU_ 192 : ECU,Peripherals;
+BO_TX_BU_ 223 : ECU,Peripherals;
+BO_TX_BU_ 228 : ECU,Peripherals;
 BO_TX_BU_ 1072 : ECU,Peripherals;
 BO_TX_BU_ 1073 : ECU,Peripherals;
 BO_TX_BU_ 1074 : ECU,Peripherals;
@@ -1042,73 +1261,47 @@ BO_TX_BU_ 1080 : ECU,Peripherals;
 BO_TX_BU_ 1081 : ECU,Peripherals;
 BO_TX_BU_ 1082 : ECU,Peripherals;
 BO_TX_BU_ 1083 : ECU,Peripherals;
-BO_TX_BU_ 1025 : ECU,Peripherals;
-BO_TX_BU_ 1054 : ECU,Peripherals;
-BO_TX_BU_ 223 : ECU,Peripherals;
-BO_TX_BU_ 512 : ECU,Peripherals;
-BO_TX_BU_ 513 : ECU,Peripherals;
-BO_TX_BU_ 514 : ECU,Peripherals;
 BO_TX_BU_ 229 : ECU,Peripherals;
 BO_TX_BU_ 230 : ECU,Peripherals;
-BO_TX_BU_ 228 : ECU,Peripherals;
+BO_TX_BU_ 1054 : ECU,Peripherals;
+BO_TX_BU_ 1055 : ECU,Peripherals;
+BO_TX_BU_ 3 : ECU,Peripherals;
+BO_TX_BU_ 2 : ECU,Peripherals;
+BO_TX_BU_ 1265 : ECU,Peripherals;
+BO_TX_BU_ 148 : ECU,Peripherals;
+BO_TX_BU_ 149 : ECU,Peripherals;
 BO_TX_BU_ 232 : ECU,Peripherals;
-BO_TX_BU_ 208 : ECU,Peripherals;
-BO_TX_BU_ 209 : ECU,Peripherals;
-BO_TX_BU_ 210 : ECU,Peripherals;
-BO_TX_BU_ 211 : ECU,Peripherals;
-BO_TX_BU_ 212 : ECU,Peripherals;
-BO_TX_BU_ 224 : ECU,Peripherals;
-BO_TX_BU_ 225 : ECU,Peripherals;
-BO_TX_BU_ 515 : ECU,Peripherals;
-BO_TX_BU_ 200 : ECU,Peripherals;
-BO_TX_BU_ 227 : ECU,Peripherals;
-BO_TX_BU_ 231 : ECU,Peripherals;
-BO_TX_BU_ 233 : ECU,Peripherals;
-BO_TX_BU_ 205 : ECU,Peripherals;
-BO_TX_BU_ 1263 : ECU,Peripherals;
-BO_TX_BU_ 1264 : ECU,Peripherals;
 BO_TX_BU_ 201 : ECU,Peripherals;
 BO_TX_BU_ 202 : ECU,Peripherals;
 BO_TX_BU_ 2047 : ECU,Peripherals;
 BO_TX_BU_ 2031 : ECU,Peripherals;
+BO_TX_BU_ 1995 : ECU,Peripherals;
 BO_TX_BU_ 1998 : ECU,Peripherals;
 BO_TX_BU_ 1999 : ECU,Peripherals;
-BO_TX_BU_ 1994 : ECU,Peripherals;
-BO_TX_BU_ 1995 : ECU,Peripherals;
-BO_TX_BU_ 1997 : ECU,Peripherals;
-BO_TX_BU_ 2010 : ECU,Peripherals;
-BO_TX_BU_ 2011 : ECU,Peripherals;
-BO_TX_BU_ 2012 : ECU,Peripherals;
-BO_TX_BU_ 2014 : ECU,Peripherals;
-BO_TX_BU_ 2026 : ECU,Peripherals;
-BO_TX_BU_ 2027 : ECU,Peripherals;
-BO_TX_BU_ 2028 : ECU,Peripherals;
-BO_TX_BU_ 2042 : ECU,Peripherals;
-BO_TX_BU_ 2043 : ECU,Peripherals;
-BO_TX_BU_ 2044 : ECU,Peripherals;
-BO_TX_BU_ 2045 : ECU,Peripherals;
-BO_TX_BU_ 2046 : ECU,Peripherals;
-BO_TX_BU_ 1962 : ECU,Peripherals;
 BO_TX_BU_ 1963 : ECU,Peripherals;
-BO_TX_BU_ 1964 : ECU,Peripherals;
-BO_TX_BU_ 1965 : ECU,Peripherals;
-BO_TX_BU_ 1966 : ECU,Peripherals;
-BO_TX_BU_ 1978 : ECU,Peripherals;
-BO_TX_BU_ 1967 : ECU,Peripherals;
-BO_TX_BU_ 245 : ECU,Peripherals;
-BO_TX_BU_ 1979 : ECU,Peripherals;
-BO_TX_BU_ 1981 : ECU,Peripherals;
-BO_TX_BU_ 257 : ECU,Peripherals;
-BO_TX_BU_ 1055 : ECU,Peripherals;
-BO_TX_BU_ 1982 : ECU,Peripherals;
-BO_TX_BU_ 1996 : ECU,Peripherals;
-BO_TX_BU_ 1980 : ECU,Peripherals;
-BO_TX_BU_ 1983 : ECU,Peripherals;
-BO_TX_BU_ 1265 : ECU,Peripherals;
+BO_TX_BU_ 1994 : ECU,Peripherals;
+BO_TX_BU_ 227 : ECU,Peripherals;
+BO_TX_BU_ 231 : ECU,Peripherals;
+BO_TX_BU_ 233 : ECU,Peripherals;
 BO_TX_BU_ 234 : ECU,Peripherals;
-BO_TX_BU_ 303 : ECU,Peripherals;
+BO_TX_BU_ 224 : ECU,Peripherals;
+BO_TX_BU_ 212 : ECU,Peripherals;
+BO_TX_BU_ 209 : ECU,Peripherals;
+BO_TX_BU_ 210 : ECU,Peripherals;
+BO_TX_BU_ 225 : ECU,Peripherals;
+BO_TX_BU_ 208 : ECU,Peripherals;
+BO_TX_BU_ 211 : ECU,Peripherals;
+BO_TX_BU_ 4 : ECU,Peripherals;
+BO_TX_BU_ 0 : ECU,Peripherals;
+BO_TX_BU_ 1280 : ECU,Peripherals;
 
 
+CM_ SG_ 1025 ts_out_filtered_read "Voltage across tractive system";
+CM_ SG_ 1025 pack_filtered_read "Filtered pack voltage";
+CM_ SG_ 1025 current_shunt_read "The shunt in penthouse; Measures current that goes through the whole tractive system -- used for coulumb counting";
+CM_ SG_ 5 Roll_angle "Sign convention in ISO Standard";
+CM_ SG_ 5 Pitch_angle "Sign convention in ISO Standard";
+CM_ SG_ 5 Height "Height after adjusting for height offset";
 CM_ BO_ 226 "UNUSED message to send data on the BMS current draw.";
 CM_ SG_ 226 total_discharge "UNUSED SIGNAL. See legacy AMS firmware (code-2024) for usage.";
 CM_ SG_ 226 total_charge "UNUSED SIGNAL. See legacy AMS firmware (code-2024) for usage.";
@@ -1117,6 +1310,13 @@ CM_ SG_ 2550588916 max_charging_current_low "Unused for any PCAN stuff only for
 CM_ SG_ 2550588916 max_charging_current_high "Unused for any PCAN stuff only for Elcon Charger";
 CM_ SG_ 2550588916 max_charging_voltage_low "Unused for any PCAN stuff only for Elcon Charger";
 CM_ SG_ 2550588916 max_charging_voltage_high "Unused for any PCAN stuff only for Elcon Charger";
+CM_ SG_ 6 Sensor_X_Location "This is the X position of the center of the sensor, with ISO signedness (positive is forward, negative is rearward) This is used to generate the CG_Corrected velocities";
+CM_ SG_ 6 Sensor_Y_Location "This is the Y position of the center of the sensor, with ISO signedness (positive is left (driver side in US) negative is right (passenger) This is used to generate the CG_Corrected velocities";
+CM_ SG_ 6 Height_Offset "This value will be SUBTRACTED from the height.  So if you mount the sensor at 40cm, and you want that to read as 20cm, set this value to 20cm";
+CM_ SG_ 235 pack_charge_led "pack charge led has a value from 0 to 255 representing a percentage that can be displayed on dashboard as a gradient between two colors";
+CM_ SG_ 235 glv_led "glv led has a value from 0 to 255 representing a percentage that can be displayed on dashboard as a gradient between two colors";
+CM_ SG_ 236 tcu_recording_state "The current state of TCU data recording (off, requested,on,saving)";
+CM_ SG_ 768 dash_dial_mode "Dashboard dial position";
 CM_ BO_ 256 "Contains the voltage and current readings from the Energy Meter. Sent by the AMS.";
 CM_ SG_ 256 em_voltage "The voltage, in Volts, measured by the Energy Meter.";
 CM_ SG_ 256 em_current "The current draw, in amps, measured by the Energy Meter.";
@@ -1126,45 +1326,31 @@ CM_ SG_ 1024 overpower_error "Whether or not the Energy Meter is reading an over
 CM_ SG_ 1024 overvoltage_error "Whether or not the energy meter is reading an over-voltage error.";
 CM_ SG_ 1024 current_gain "This selects the \"gain\" mode for the EM current measurements. The specific multipliers for each gain mode are configured with the EMD Tool.";
 CM_ SG_ 1024 voltage_gain "This selects the \"gain\" mode for the EM voltage measurements. The specific multipliers for each gain mode are configured with the EMD Tool.";
+CM_ SG_ 245 thermistor_motor_fr "Motor cooling loop temperature";
+CM_ SG_ 245 thermistor_motor_fl "Motor cooling loop temperature";
+CM_ SG_ 1 Pitch_Rate "Sign convention in ISO Standard";
+CM_ SG_ 1 Roll_Rate "Sign convention in ISO Standard";
+CM_ SG_ 1 Accel_Z "Follows ISO Standard Sign Convention";
+CM_ SG_ 1 Accel_Y "Follows ISO Standard Sign Convention";
+CM_ SG_ 1 Accel_X "Follows ISO Standard Sign Convention";
+CM_ SG_ 151 negative_torque_limit "AMK made up unit";
+CM_ SG_ 151 positive_torque_limit "Made up units from AMK";
+CM_ SG_ 152 negative_torque_limit "AMK made up unit";
+CM_ SG_ 152 positive_torque_limit "Made up units from AMK";
+CM_ SG_ 153 negative_torque_limit "AMK made up unit";
+CM_ SG_ 153 positive_torque_limit "Made up units from AMK";
+CM_ SG_ 258 negative_torque_limit "AMK made up unit";
+CM_ SG_ 258 positive_torque_limit "Made up units from AMK";
+CM_ SG_ 1066 LR_TTPMS_SN "Serial Number";
 CM_ SG_ 184 feedback_torque "random made up AMK units";
 CM_ SG_ 184 motor_power "Made up units from AMK";
-CM_ SG_ 160 negative_torque_limit "AMK made up unit";
-CM_ SG_ 160 positive_torque_limit "Made up units from AMK";
-CM_ SG_ 164 torque_command "Made up unit by AMK (consult Shayan)";
-CM_ SG_ 185 feedback_torque "random made up AMK units";
-CM_ SG_ 185 motor_power "Made up units from AMK";
-CM_ SG_ 161 negative_torque_limit "AMK made up unit";
-CM_ SG_ 161 positive_torque_limit "Made up units from AMK";
-CM_ SG_ 165 torque_command "Made up unit by AMK (consult Shayan)";
-CM_ SG_ 186 feedback_torque "random made up AMK units";
-CM_ SG_ 186 motor_power "Made up units from AMK";
-CM_ SG_ 162 negative_torque_limit "AMK made up unit";
-CM_ SG_ 162 positive_torque_limit "Made up units from AMK";
-CM_ SG_ 166 torque_command "Made up unit by AMK (consult Shayan)";
-CM_ SG_ 187 feedback_torque "random made up AMK units";
-CM_ SG_ 187 motor_power "Made up units from AMK";
-CM_ SG_ 163 negative_torque_limit "AMK made up unit";
-CM_ SG_ 163 positive_torque_limit "Made up units from AMK";
-CM_ SG_ 167 torque_command "Made up unit by AMK (consult Shayan)";
-CM_ SG_ 196 mechanical_brake_percent_float "The percentage at which mechanical brake activates represented by an unsigned float";
-CM_ SG_ 196 brake_percent_float "The percentage of accel pedal travel represented by an unsigned float";
-CM_ SG_ 196 accel_percent_float "The percentage of accel pedal travel represented by an unsigned float";
+CM_ BO_ 257 "this message will have states internal to the MCU code";
 CM_ SG_ 195 torque_mode "torque mode";
 CM_ SG_ 195 drive_mode "The current drive mode on the ECU irrespective of dial mapping";
-CM_ SG_ 236 tcu_recording_state "The current state of TCU data recording (off, requested,on,saving)";
-CM_ SG_ 235 pack_charge_led "pack charge led has a value from 0 to 255 representing a percentage that can be displayed on dashboard as a gradient between two colors";
-CM_ SG_ 235 glv_led "glv led has a value from 0 to 255 representing a percentage that can be displayed on dashboard as a gradient between two colors";
-CM_ BO_ 149 "The TCU_LAP_TIMES message contains the best and previous lap times to the dashboard as calculated by the TCU or manually input by the pit crew. It also relays the current state of the TCU's clock to the dashboard so that it can run an approximate stopwatc";
-CM_ BO_ 148 "TCU_DRIVER_MSG contains a signal that corresponds to a preset list of messages on the dashboard. It also includes two variables to customize the contents of the message with numerical values. It also includes the target lap time which is determined by the";
-CM_ SG_ 148 target_lap_time "target lap time is the time determined by the pit crew that the driver should aim for. mostly useful for guiding the driver in endurance races.";
-CM_ SG_ 148 driver_message "corresponds to a pre-set list of driver messages on the dashboard that the TCU can request to be displayed in event of comms failure";
-CM_ SG_ 1066 LR_TTPMS_SN "Serial Number";
-CM_ SG_ 1025 ts_out_filtered_read "Voltage across tractive system";
-CM_ SG_ 1025 pack_filtered_read "Filtered pack voltage";
-CM_ SG_ 1025 current_shunt_read "The shunt in penthouse; Measures current that goes through the whole tractive system -- used for coulumb counting";
-CM_ SG_ 1054 charge_coulombs "The charge in the accumulator, in Coulombs";
-CM_ SG_ 1054 min_cell_voltage_est "Lowest-cell-voltage estimate based on the state of charge, according to VOLTAGE_LOOKUP_TABLE. Ex: 87% SoC would display 3.798V.";
-CM_ SG_ 1054 charge_percentage "The charge in the accumulator, as a percentage.";
+CM_ SG_ 192 brake_pedal "the scaled brake pedal value between 0 and 1";
+CM_ SG_ 192 accel_pedal "the scaled accelerator pedal value";
+CM_ SG_ 192 implaus_exceeded_max_duration "a pedal implausibility has been present for longer than allowed";
+CM_ SG_ 192 brake_accel_implausibility "brake and accel are pressed";
 CM_ SG_ 229 thermistor_acc2 "acculum cooling loop temp";
 CM_ SG_ 229 thermistor_acc1 "acculum cooling loop temp";
 CM_ SG_ 229 thermistor_inv2 "invertor cooling loop tmep";
@@ -1172,20 +1358,30 @@ CM_ SG_ 229 thermistor_inv1 "inverter cooling loop temp";
 CM_ SG_ 230 thermistor_pump "temp of the pump";
 CM_ SG_ 230 thermistor_motor_rr "Motor cooling loop temperature";
 CM_ SG_ 230 thermistor_motor_rl "Motor cooling loop temperature";
+CM_ SG_ 1054 charge_coulombs "The charge in the accumulator, in Coulombs";
+CM_ SG_ 1054 min_cell_voltage_est "Lowest-cell-voltage estimate based on the state of charge, according to VOLTAGE_LOOKUP_TABLE. Ex: 87% SoC would display 3.798V.";
+CM_ SG_ 1054 charge_percentage "The charge in the accumulator, as a percentage.";
+CM_ BO_ 1055 "Steering reading; system and sensor status";
+CM_ SG_ 1055 steering_digital_raw "raw measurement by digital steering encoder";
+CM_ SG_ 1055 steering_analog_raw "raw measurement as measured by bottom steering analog sensor";
+CM_ SG_ 3 Sensor_Y_Location "This is the Y position of the center of the sensor, with ISO signedness (positive is left (driver side in US) negative is right (passenger) This is used to generate the CG_Corrected velocities";
+CM_ SG_ 3 Sensor_X_Location "This is the X position of the center of the sensor, with ISO signedness (positive is forward, negative is rearward) This is used to generate the CG_Corrected velocities";
+CM_ SG_ 3 Height_Offset "This value will be SUBTRACTED from the height.  So if you mount the sensor at 40cm, and you want that to read as 20cm, set this value to 20cm";
+CM_ SG_ 2 Temperature_Internal "Measurement of the internal air temperature of the SpeedBeam sensor";
+CM_ SG_ 1265 torque_limit "AMK inverter torque limit in use";
+CM_ SG_ 1265 torque_mode "torque mode";
+CM_ SG_ 1265 dash_dial_mode "Dashboard dial position";
+CM_ SG_ 1265 mode_actual "actual mode in tcmux";
+CM_ SG_ 1265 mode_intended "TC mode selected by driver";
+CM_ SG_ 1265 steering_system_has_err "Steering system data in ERROR state";
+CM_ SG_ 1265 tc_not_ready "Selected TC not in ready state";
+CM_ SG_ 1265 torque_delta_above_thresh "Torque delta between old and new controllers is < 0.5Nm on every wheel";
+CM_ SG_ 1265 speed_above_thresh "Vehicle speed is above 5 m/s, TCMux does not allow driver to switch mode";
+CM_ BO_ 148 "TCU_DRIVER_MSG contains a signal that corresponds to a preset list of messages on the dashboard. It also includes two variables to customize the contents of the message with numerical values. It also includes the target lap time which is determined by the";
+CM_ SG_ 148 target_lap_time "target lap time is the time determined by the pit crew that the driver should aim for. mostly useful for guiding the driver in endurance races.";
+CM_ SG_ 148 driver_message "corresponds to a pre-set list of driver messages on the dashboard that the TCU can request to be displayed in event of comms failure";
+CM_ BO_ 149 "The TCU_LAP_TIMES message contains the best and previous lap times to the dashboard as calculated by the TCU or manually input by the pit crew. It also relays the current state of the TCU's clock to the dashboard so that it can run an approximate stopwatc";
 CM_ SG_ 232 tcu_recording_state "The current state of TCU data recording (off, requested,on,saving)";
-CM_ BO_ 208 "Vehicle body velocity measured by VN-300";
-CM_ SG_ 208 vn_vel_uncertainty "how much error there might be";
-CM_ SG_ 209 vn_lin_ins_accel_z "estimated acceleration of INS";
-CM_ SG_ 209 vn_lin_ins_accel_y "estimated acceleration of INS";
-CM_ SG_ 209 vn_lin_ins_accel_x "estimated acceleration of INS";
-CM_ SG_ 210 vn_lin_uncomp_accel_z "IMU acceleration";
-CM_ SG_ 210 vn_lin_uncomp_accel_y "IMU acceleration";
-CM_ SG_ 210 vn_lin_uncomp_accel_x "IMU acceleration";
-CM_ SG_ 224 vn_gps_time "time since start of epoch 1980";
-CM_ SG_ 225 vn_gps_status "GPS fix status";
-CM_ SG_ 231 vn_ecef_pos_y "earth centered earth fixed";
-CM_ SG_ 231 vn_ecef_pos_x "earth centered earth fixed";
-CM_ SG_ 233 vn_ecef_pos_z "earth centered earth fixed";
 CM_ BO_ 201 "MCU's simple torque controller";
 CM_ SG_ 201 accel_request_state "The current state of the acceleration request (0 = decelerating, 1 = accelerating)";
 CM_ SG_ 201 rear_regen_scale "the front/rear torque scaling";
@@ -1196,25 +1392,9 @@ CM_ SG_ 201 torque_request "The torque request calculated from the accelleration
 CM_ SG_ 202 initial_launch_target "The initial launch speed target requested by the launch controller";
 CM_ SG_ 202 algo_active "State of whether the launch algorithm has taken over control from the initial launch target";
 CM_ SG_ 202 launch_control_state "The current state of the launch controller (LAUNCH_NOT_READY, LAUNCH_READY, LAUNCHING)";
-CM_ SG_ 245 thermistor_motor_fr "Motor cooling loop temperature";
-CM_ SG_ 245 thermistor_motor_fl "Motor cooling loop temperature";
-CM_ BO_ 257 "this message will have states internal to the MCU code";
-CM_ BO_ 1055 "Steering reading; system and sensor status";
-CM_ SG_ 1055 steering_analog_status "0 : good; 1 : clamped";
-CM_ SG_ 1055 steering_encoder_status "0 : nominal; 1 : marginal; 2 : error";
-CM_ SG_ 1055 steering_system_status "0 : nominal; 1 : marginal; 2 : degraded; 3 : error";
-CM_ SG_ 1055 steering_analog_angle "Angle measured by bottom steering analog sensor";
-CM_ SG_ 1055 steering_encoder_angle "Angle measured by upper steering encoder";
-CM_ SG_ 1055 steering_system_angle "Angle reported by steering system";
-CM_ SG_ 1265 torque_limit "AMK inverter torque limit in use";
-CM_ SG_ 1265 torque_mode "torque mode";
-CM_ SG_ 1265 dash_dial_mode "Dashboard dial position";
-CM_ SG_ 1265 mode_actual "actual mode in tcmux";
-CM_ SG_ 1265 mode_intended "TC mode selected by driver";
-CM_ SG_ 1265 steering_system_has_err "Steering system data in ERROR state";
-CM_ SG_ 1265 tc_not_ready "Selected TC not in ready state";
-CM_ SG_ 1265 torque_delta_above_thresh "Torque delta between old and new controllers is < 0.5Nm on every wheel";
-CM_ SG_ 1265 speed_above_thresh "Vehicle speed is above 5 m/s, TCMux does not allow driver to switch mode";
+CM_ SG_ 231 vn_ecef_pos_y "earth centered earth fixed";
+CM_ SG_ 231 vn_ecef_pos_x "earth centered earth fixed";
+CM_ SG_ 233 vn_ecef_pos_z "earth centered earth fixed";
 CM_ SG_ 234 num_com_sats_rtk "num of common satelites that are used in the RTK solution on both receivers";
 CM_ SG_ 234 num_com_sats_pvt "num of common satelites that are used in the PVT solution on both receivers";
 CM_ SG_ 234 highest_cn0_2 "Highest CN0 reported on receiver B";
@@ -1223,18 +1403,46 @@ CM_ SG_ 234 num_sats_pvt_2 "num of common satelites that are used in the PVT sol
 CM_ SG_ 234 highest_cn0_1 "Highest CN0 reported on receiver A";
 CM_ SG_ 234 num_sats_rtk_1 "num of common satelites that are used in the RTK solution on receiver A";
 CM_ SG_ 234 num_sats_pvt_1 "num of common satelites that are used in the PVT solution on receiver A";
+CM_ SG_ 224 vn_gps_time "time since start of epoch 1980";
+CM_ SG_ 209 vn_lin_ins_accel_z "estimated acceleration of INS";
+CM_ SG_ 209 vn_lin_ins_accel_y "estimated acceleration of INS";
+CM_ SG_ 209 vn_lin_ins_accel_x "estimated acceleration of INS";
+CM_ SG_ 210 vn_lin_uncomp_accel_z "IMU acceleration";
+CM_ SG_ 210 vn_lin_uncomp_accel_y "IMU acceleration";
+CM_ SG_ 210 vn_lin_uncomp_accel_x "IMU acceleration";
+CM_ SG_ 225 vn_gps_status "GPS fix status";
+CM_ BO_ 208 "Vehicle body velocity measured by VN-300";
+CM_ SG_ 208 vn_vel_uncertainty "how much error there might be";
+CM_ SG_ 4 Velocity_z_CGCorrected "ISO Standard Sign, Positive is Vehicle Moving Up";
+CM_ SG_ 4 Velocity_y_CGCorrected "ISO Standard Sign, Positive is Vehicle Turning Left";
+CM_ SG_ 4 Velocity_x_CGCorrected "ISO Standard Sign, Positive is Vehicle Moving Forward";
+CM_ SG_ 4 Velocity_z "ISO Standard Sign, Positive is Vehicle Moving Up";
+CM_ SG_ 4 Velocity_y "ISO Standard Sign, Positive is Vehicle Turning Left";
+CM_ SG_ 4 Velocity_x "ISO Standard Sign, Positive is Vehicle Moving Forward";
 
 
 
-VAL_ 256 em_voltage 0 "" 1 "" 2 "" 3 "" ;
-VAL_ 195 ecu_state 0 "STARTUP" 1 "TRACTIVE_SYSTEM_NOT_ACTIVE" 2 "TRACTIVE_SYSTEM_ACTIVE" 3 "ENABLING_INVERTER" 4 "WAITING_READY_TO_DRIVE_SOUND" 5 "READY_TO_DRIVE" ;
-VAL_ 236 dial_state 0 "MODE_0" 1 "MODE_1" 2 "MODE_2" 3 "MODE_3" 4 "MODE_4" 5 "MODE_5" ;
+VAL_ 6 Velocity_Filtering 0 "NO_FILTERING_VEL" 1 "LIGHT_FILTERING_VEL" 2 "MEDIUM_FILTERING_VEL" 3 "HEAVY_FILTERING_VEL" ;
+VAL_ 6 CAN_Termination_State_Bus_2 0 "CAN_BUS_UNTERMINATED_CAN2_STATUS" 1 "CAN_BUS_TERMINATED_CAN2_STATUS" ;
+VAL_ 6 CAN_Termination_State_Bus_1 0 "CAN_BUS_UNTERMINATED_CAN1_STATUS" 1 "CAN_BUS_TERMINATED_CAN1_STATUS" ;
+VAL_ 6 IMU_Filtering 0 "NO_FILTERING_IMU" 1 "LIGHT_FILTERING_IMU" 2 "MEDIUM_FILTERING_IMU" 3 "HEAVY_FILTERING_IMU" ;
+VAL_ 6 Attitude_Filtering 0 "NO_FILTERING_ATT" 1 "LIGHT_FILTERING_ATT" 2 "MEDIUM_FILTERING_ATT" 3 "HEAVY_FILTERING_ATT" ;
 VAL_ 235 dial_state 0 "MODE_0" 1 "MODE_1" 2 "MODE_2" 3 "MODE_3" 4 "MODE_4" 5 "MODE_5" ;
+VAL_ 236 dial_state 0 "MODE_0" 1 "MODE_1" 2 "MODE_2" 3 "MODE_3" 4 "MODE_4" 5 "MODE_5" ;
+VAL_ 257 torque_controller_mux_status 0 "NO_ERROR" 1 "ERROR_SPEED_DIFF_TOO_HIGH" 2 "ERROR_TORQUE_DIFF_TOO_HIGH" 3 "ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS" 4 "ERROR_CONTROLLER_NULL_POINTER" ;
+VAL_ 195 ecu_state 0 "STARTUP" 1 "TRACTIVE_SYSTEM_NOT_ACTIVE" 2 "TRACTIVE_SYSTEM_ACTIVE" 3 "ENABLING_INVERTER" 4 "WAITING_READY_TO_DRIVE_SOUND" 5 "READY_TO_DRIVE" ;
+VAL_ 3 Velocity_Filtering 0 "NO_FILTERING_VEL" 1 "LIGHT_FILTERING_VEL" 2 "MEDIUM_FILTERING_VEL" 3 "HEAVY_FILTERING_VEL" ;
+VAL_ 3 IMU_Filtering 0 "NO_FILTERING_IMU" 1 "LIGHT_FILTERING_IMU" 2 "MEDIUM_FILTERING_IMU" 3 "HEAVY_FILTERING_IMU" ;
+VAL_ 3 Attitude_Filtering 0 "NO_FILTERING_ATT" 1 "LIGHT_FILTERING_ATT" 2 "MEDIUM_FILTERING_ATT" 3 "HEAVY_FILTERING_ATT" ;
+VAL_ 3 CAN_Termination_Setting_Bus_2 0 "CAN_BUS_UNTERMINATED_CAN2_setting" 1 "CAN_BUS_TERMINATED_CAN2_setting" ;
+VAL_ 3 CAN_Termination_Setting_Bus_1 0 "CAN_BUS_UNTERMINATED_CAN1_setting" 1 "CAN_BUS_TERMINATED_CAN1_setting" ;
+VAL_ 3 Broadcast_rate 0 "hz_1" 1 "hz_5" 2 "hz_10" 3 "hz_50" 4 "hz_100" 5 "hz_250" 6 "hz_500" ;
+VAL_ 2 SpeedBeam_Health 0 "SENSOR_HEALTHY" 1 "NON_MEASUREMENT_MODE" 2 "DATA_OPTICAL_QUALITY_POOR" 3 "TEMPERATURE_TOO_HIGH" 4 "SAVING_CALIBRATION" 5 "SENSOR_COMMUNICATION_ERROR" 6 "SPEED_TOO_HIGH" ;
+VAL_ 2 sensor_3_Validity 0 "Sensor_Valid_sense_3" 1 "Response_Timeout_sense_3" 2 "Bad_Optical_Signal_sense_3" 3 "Speed_Too_High_sense_3" ;
+VAL_ 2 sensor_2_Validity 0 "Sensor_Valid_sense_2" 1 "Response_Timeout_sense_2" 2 "Bad_Optical_Signal_sense_2" 3 "Speed_Too_High_sense_2" ;
+VAL_ 2 sensor_1_Validity 0 "Sensor_Valid_sense_1" 1 "Response_Timeout_sense_1" 2 "Bad_Optical_Signal_sense_1" 3 "Speed_Too_High_sense_1" ;
+VAL_ 2 sensor_0_Validity 0 "Sensor_Valid_sense_0" 1 "Response_Timeout_sense_0" 2 "Bad_Optical_Signal_sense_0" 3 "Speed_Too_High_sense_0" ;
 VAL_ 225 vn_gps_status 0 "NO_FIX" 1 "TIME_ONLY" 2 "FIX_2D" 3 "FIX_3D" ;
-VAL_ 257 torque_controller_mux_status 0 "NO_ERROR" 1 "ERROR_SPEED_DIFF_TOO_HIGH" 2 "ERROR_TORQUE_DIFF_TOO_HIGH" ;
-VAL_ 1055 steering_analog_status 0 "" 1 "" ;
-VAL_ 1055 steering_encoder_status 0 "" 1 "" 2 "" ;
-VAL_ 1055 steering_system_status 0 "" 1 "" 2 "" 3 "" ;
-
+SIG_VALTYPE_ 1055 steering_digital_raw : 1;
 
 
diff --git a/config/simple_torque_controller_test.json b/config/simple_torque_controller_test.json
new file mode 100644
index 00000000..b5f2e0d7
--- /dev/null
+++ b/config/simple_torque_controller_test.json
@@ -0,0 +1,8 @@
+{
+    "SimpleTorqueController": {
+        "max_torque": 21.0,
+        "max_regen_torque": 10.0,
+        "rear_torque_scale": 1.0,
+        "regen_torque_scale": 0.6
+    }
+}
\ No newline at end of file
diff --git a/config/test_controller_manager.json b/config/test_controller_manager.json
new file mode 100644
index 00000000..d54d1206
--- /dev/null
+++ b/config/test_controller_manager.json
@@ -0,0 +1,23 @@
+{
+    "SimpleSpeedController": {
+        "max_torque": 21.0,
+        "max_regen_torque": 10.0,
+        "rear_torque_scale": 1.0,
+        "regen_torque_scale": 0.6,
+        "positive_speed_set" : 25,
+        "max_power_kw": 63.0,
+        "dt_rate_hz": 1000
+    },
+    "SimpleTorqueController": {
+        "max_torque": 21.0,
+        "max_regen_torque": 10.0,
+        "rear_torque_scale": 1.0,
+        "regen_torque_scale": 0.6
+    },
+    "ControllerManager": {
+        "max_controller_switch_speed_ms": 5.0, 
+        "max_requested_rpm": 20000.0,
+        "max_torque_switch_nm": 10.0,
+        "max_accel_switch_float": 0.1
+    }
+}
\ No newline at end of file
diff --git a/config/test_scale_comms.json b/config/test_scale_comms.json
new file mode 100644
index 00000000..6032efee
--- /dev/null
+++ b/config/test_scale_comms.json
@@ -0,0 +1,6 @@
+{
+  "ScaleComms": {
+    "baud_rate": 115200,
+    "port_name": "/dev/ttyUSB0"
+  }
+}
\ No newline at end of file
diff --git a/config/test_speedtech_comms.json b/config/test_speedtech_comms.json
new file mode 100644
index 00000000..f9e91583
--- /dev/null
+++ b/config/test_speedtech_comms.json
@@ -0,0 +1,6 @@
+{
+    "SpeedTechComms": {
+        "baud_rate": 9600,
+        "port_name": "/dev/ttyUSB0"
+    }
+}
\ No newline at end of file
diff --git a/config/test_surrey_aero_comms.json b/config/test_surrey_aero_comms.json
new file mode 100644
index 00000000..1e06ad84
--- /dev/null
+++ b/config/test_surrey_aero_comms.json
@@ -0,0 +1,5 @@
+{
+    "SurreyAeroComms": {
+        "port_name": "/dev/ttyACM0"
+    }
+}
\ No newline at end of file
diff --git a/config/test_tcmux_integration.json b/config/test_tcmux_integration.json
new file mode 100644
index 00000000..2c6377d5
--- /dev/null
+++ b/config/test_tcmux_integration.json
@@ -0,0 +1,15 @@
+g{
+    "ControllerManager": {
+        "max_controller_switch_speed_ms": 1.793, 
+        "max_requested_rpm": 20000.0,
+        "max_torque_switch_nm": 10.0,
+        "max_accel_switch_float": 0.1
+    },
+    "SimpleSpeedController": {
+        "max_torque": 21.0,
+        "max_regen_torque": 10.0,
+        "rear_torque_scale": 1.0,
+        "regen_torque_scale": 0.6,
+        "positive_speed_set" : 3.0
+    }
+}
\ No newline at end of file
diff --git a/default.nix b/default.nix
index 4526699f..c6b9502b 100644
--- a/default.nix
+++ b/default.nix
@@ -1,7 +1,7 @@
 { pkgs, stdenv, cmake, boost, pkg-config, lz4 ,zstd, protobuf, nlohmann_json, 
   foxglove-ws-protocol-cpp, cmake_macros, hytech_np_proto_cpp, dbcppp, gtest, 
   drivebrain_core_msgs_proto_cpp, mcap, db_service_grpc_cpp, grpc, vn_lib, 
-  drivebrain_core, spdlog, fmt, ... }:
+  drivebrain_core, spdlog, fmt, drivebrain-simulink-gen-pkg, ... }:
 stdenv.mkDerivation {
   name = "drivebrain_software";
   src = ./.;
@@ -10,7 +10,7 @@ stdenv.mkDerivation {
 
   propagatedBuildInputs = [ protobuf lz4 zstd boost cmake_macros nlohmann_json foxglove-ws-protocol-cpp 
             hytech_np_proto_cpp dbcppp gtest drivebrain_core_msgs_proto_cpp mcap 
-            db_service_grpc_cpp grpc vn_lib drivebrain_core spdlog fmt ]; 
+            db_service_grpc_cpp grpc vn_lib drivebrain_core spdlog fmt drivebrain-simulink-gen-pkg ]; 
   dontStrip = true;
   cmakeFlags = [ "-DCMAKE_FIND_DEBUG_MODE=ON" ];
 }
\ No newline at end of file
diff --git a/drivebrain_app/debug_main.cpp b/drivebrain_app/debug_main.cpp
index ae4509f9..d0f62116 100644
--- a/drivebrain_app/debug_main.cpp
+++ b/drivebrain_app/debug_main.cpp
@@ -1,12 +1,12 @@
 
 #include <JsonFileHandler.hpp>
 #include <CANComms.hpp>
-#include <SimpleController.hpp>
+#include <SimpleSpeedController.hpp>
+#include <ControllerManager.hpp>
 #include <StateEstimator.hpp>
-#include <MCUETHComms.hpp>
 #include <VNComms.hpp>
 #include <MsgLogger.hpp>
-#include <MCAPProtobufLogger.hpp>
+#include <DrivebrainMCAPLogger.hpp>
 #include <mcap/writer.hpp>
 #include <DrivebrainBase.hpp>
 #include <foxglove_server.hpp>
@@ -40,6 +40,8 @@
 
 #include <spdlog/spdlog.h>
 
+#include "arg_parse.hpp"
+
 // TODO first application will have
 
 // - [x] message queue that can send messages between the CAN driver and the controller
@@ -47,50 +49,18 @@
 // - [ ] fix the CAN messages that cant currently be encoded into the protobuf messages
 // - [x] simple controller
 
-std::atomic<bool> stop_signal{false};
-// Signal handler function
-void signal_handler(int signal)
-{
-    spdlog::info("Interrupt signal ({}) received. Cleaning up...", signal);
-    stop_signal.store(true); // Set running to false to exit the main loop or gracefully terminate
-}
-
-
-std::pair<std::string, std::string> parse_arguments(int argc, char* argv[]) {
-    namespace po = boost::program_options;
-    po::options_description desc("Allowed options");
-    std::string param_path = "config/drivebrain_config.json";
-    std::string dbc_path;
-
-    desc.add_options()
-        ("help,h", "produce help message")
-        ("param-path,p", po::value<std::string>(&param_path), "Path to the parameter JSON file")
-        ("dbc-path,d", po::value<std::string>(&dbc_path), "Path to the DBC file (optional)");
-
-    po::variables_map vm;
-    po::store(po::parse_command_line(argc, argv, desc), vm);
-    po::notify(vm);
-
-    if (vm.count("help")) {
-        std::cout << desc << std::endl;
-        std::exit(0);
-    }
-
-    return {param_path, dbc_path};
-}
-
 int main(int argc, char *argv[])
 {
-    
     try {
-
-        auto [param_path, dbc_path] = parse_arguments(argc, argv);
+        
+        auto [param_path, dbc_path, second_can] = parse_arguments(argc, argv);
 
         DriveBrainSettings settings{
             .run_db_service = true,
             .run_io_context = true,
             .run_process_loop = true,
-            .use_vectornav = false
+            .use_vectornav = false,
+            .use_secondary_can = second_can
         };
         
         std::cout <<"creating app" <<std::endl;
diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp
index 79e04f44..de6be710 100644
--- a/drivebrain_app/include/DriveBrainApp.hpp
+++ b/drivebrain_app/include/DriveBrainApp.hpp
@@ -1,18 +1,30 @@
 // DriveBrainApp.hpp
 #pragma once
 
+#include "SurreyAeroComms.hpp"
+
+#include <SpeedTechComms.hpp>
+#include <ScaleComms.hpp>
 #include <JsonFileHandler.hpp>
 #include <CANComms.hpp>
-#include <SimpleController.hpp>
+
+#include <SimpleSpeedController.hpp>
+#include <LoadCellVectoringTorqueController.hpp>
+
+#include <ControllerManager.hpp>
 #include <StateEstimator.hpp>
-#include <MCUETHComms.hpp>
 #include <VNComms.hpp>
 #include <MsgLogger.hpp>
-#include <MCAPProtobufLogger.hpp>
+#include <DrivebrainMCAPLogger.hpp>
+#include <hytech_msgs.pb.h>
 #include <mcap/writer.hpp>
 #include <DrivebrainBase.hpp>
 #include <foxglove_server.hpp>
 #include <DBServiceImpl.hpp>
+#include <ETHRecvComms.hpp>
+
+#include <MatlabModelAddHelper.hpp>
+#include <EstimatorManager.hpp>
 
 #include <thread>
 #include <chrono>
@@ -34,6 +46,7 @@ struct DriveBrainSettings {
     bool run_io_context{true};
     bool run_process_loop{true};
     bool use_vectornav{true};
+    bool use_secondary_can{true};
 };
 
 class DriveBrainApp {
@@ -48,6 +61,8 @@ class DriveBrainApp {
     // Private member functions
     void _process_loop();
     void _signal_handler(int signal);
+    void _setup_loggers(std::vector<std::shared_ptr<core::common::Loggable<std::shared_ptr<google::protobuf::Message>>>> logging_components);
+
 private:
     // Private member variables
     static std::atomic<bool> _stop_signal;
@@ -56,27 +71,56 @@ class DriveBrainApp {
     core::JsonFileHandler _config;
     std::optional<std::string> _dbc_path;
     boost::asio::io_context _io_context;
+    boost::asio::io_context _io_context_secondary_can;
+    boost::asio::io_context _aero_usb_io_context;
+    boost::asio::io_context _io_context_speed_tech_serial;
+    boost::asio::io_context _scale_usb_io_context;
     
     core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> _rx_queue;
-    core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> _can_tx_queue;
+    core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> _primary_can_tx_queue;
+    core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> _secondary_can_tx_queue;
     core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> _eth_tx_queue;
     core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> _live_telem_queue;
 
-    std::vector<core::common::Configurable*> _configurable_components;
-    std::unique_ptr<common::MCAPProtobufLogger> _mcap_logger;
-    std::unique_ptr<control::SimpleController> _controller;
-    // std::unique_ptr<estimation::Tire_Model_Codegen_MatlabModel> _matlab_math;
-    std::unique_ptr<core::FoxgloveWSServer> _foxglove_server;
-    std::shared_ptr<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>> _message_logger;
-    std::unique_ptr<core::StateEstimator> _state_estimator;
-    std::unique_ptr<comms::CANDriver> _driver;
-    std::unique_ptr<comms::MCUETHComms> _eth_driver;
-    std::unique_ptr<comms::VNDriver> _vn_driver;
+    std::vector<std::shared_ptr<core::common::Configurable>> _configurable_components;
+    std::shared_ptr<common::DrivebrainMCAPLogger> _mcap_logger;
+    std::shared_ptr<control::SimpleSpeedController> controller1;
+    std::shared_ptr<control::LoadCellVectoringTorqueController> _mode1;
+
+
+    control::ControllerManager<control::Controller<core::ControllerOutput, core::VehicleState>, 2 + matlab_model_gen::num_controllers> _controllerManager;
+
+    std::shared_ptr<core::FoxgloveWSServer> _foxglove_server;
+    std::shared_ptr<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>> _message_logger = nullptr;
+    std::shared_ptr<core::StateEstimator> _state_estimator;
+    std::shared_ptr<comms::CANDriver> _driver_primary_can;
+    std::shared_ptr<comms::CANDriver> _driver_secondary_can;
+    std::shared_ptr<comms::SurreyAeroComms> _aero_sensor_driver = nullptr;
+    std::shared_ptr<comms::SpeedTechComms> _lap_timer_driver;
+    bool _using_lap_timer = false;
+    std::shared_ptr<comms::ETHRecvComms<hytech_msgs::ACUAllData>> _acu_eth_driver;
+    std::shared_ptr<comms::ETHRecvComms<hytech_msgs::ACUCoreData>> _acu_eth_driver_core;
+    std::shared_ptr<comms::ETHRecvComms<hytech_msgs::VCRData_s>> _vcr_eth_driver;
+    std::shared_ptr<comms::ETHRecvComms<hytech_msgs::VCFData_s>> _vcf_eth_driver;
+    std::shared_ptr<comms::ETHRecvComms<hytech_msgs::VNData>> _fake_vn = nullptr;
+    std::shared_ptr<comms::VNDriver> _vn_driver;
     std::unique_ptr<DBInterfaceImpl> _db_service;
     
+    std::shared_ptr<comms::ScaleComms> _scale_comms = nullptr;
+
+    std::vector<std::shared_ptr<MatlabModel>> _gend_controllers;
+
+    std::shared_ptr<estimation::EstimatorManager> _estim_manager;
     std::thread _process_thread;
     std::thread _io_context_thread;
+    std::thread _io_context_secondary_thread;
     std::thread _db_service_thread;
-
+    std::thread _aero_usb_io_context_thread;
+    std::thread _io_context_speed_tech_serial_thread;
+    std::thread _scale_usb_io_context_thread;
     const DriveBrainSettings _settings;
+
+    std::chrono::steady_clock::time_point _last_send_time = std::chrono::steady_clock::now();
+    const std::chrono::seconds _send_period{1}; // 1 Hz
+
 };
\ No newline at end of file
diff --git a/drivebrain_app/include/arg_parse.hpp b/drivebrain_app/include/arg_parse.hpp
new file mode 100644
index 00000000..05bd7bb7
--- /dev/null
+++ b/drivebrain_app/include/arg_parse.hpp
@@ -0,0 +1,14 @@
+#ifndef __ARG_PARSE_H__
+#define __ARG_PARSE_H__
+
+#include <boost/program_options.hpp>
+
+#include <string>
+#include <utility>
+#include <tuple>
+
+
+std::tuple<std::string, std::string, bool> parse_arguments(int argc, char* argv[]);
+
+
+#endif // __ARG_PARSE_H__
\ No newline at end of file
diff --git a/drivebrain_app/main.cpp b/drivebrain_app/main.cpp
index 6025b2c9..9496f858 100644
--- a/drivebrain_app/main.cpp
+++ b/drivebrain_app/main.cpp
@@ -1,11 +1,11 @@
 #include <JsonFileHandler.hpp>
 #include <CANComms.hpp>
-#include <SimpleController.hpp>
+#include <SimpleSpeedController.hpp>
+#include <ControllerManager.hpp>
 #include <StateEstimator.hpp>
-#include <MCUETHComms.hpp>
 #include <VNComms.hpp>
 #include <MsgLogger.hpp>
-#include <MCAPProtobufLogger.hpp>
+#include <DrivebrainMCAPLogger.hpp>
 #include <mcap/writer.hpp>
 #include <DrivebrainBase.hpp>
 #include <foxglove_server.hpp>
@@ -38,7 +38,7 @@
 #include <sstream>
 
 #include <spdlog/spdlog.h>
-
+#include "arg_parse.hpp"
 // TODO first application will have
 
 // - [x] message queue that can send messages between the CAN driver and the controller
@@ -46,51 +46,19 @@
 // - [ ] fix the CAN messages that cant currently be encoded into the protobuf messages
 // - [x] simple controller
 
-std::atomic<bool> stop_signal{false};
-// Signal handler function
-void signal_handler(int signal)
-{
-    spdlog::info("Interrupt signal ({}) received. Cleaning up...", signal);
-    stop_signal.store(true); // Set running to false to exit the main loop or gracefully terminate
-}
-
-
-std::pair<std::string, std::string> parse_arguments(int argc, char* argv[]) {
-    namespace po = boost::program_options;
-    po::options_description desc("Allowed options");
-    std::string param_path = "config/drivebrain_config.json";
-    std::string dbc_path;
-
-    desc.add_options()
-        ("help,h", "produce help message")
-        ("param-path,p", po::value<std::string>(&param_path), "Path to the parameter JSON file")
-        ("dbc-path,d", po::value<std::string>(&dbc_path), "Path to the DBC file (optional)");
-
-    po::variables_map vm;
-    po::store(po::parse_command_line(argc, argv, desc), vm);
-    po::notify(vm);
-
-    if (vm.count("help")) {
-        std::cout << desc << std::endl;
-        std::exit(0);
-    }
-
-    return {param_path, dbc_path};
-}
-
 int main(int argc, char *argv[])
 {
-    std::signal(SIGINT, signal_handler);
     
     try {
 
-        auto [param_path, dbc_path] = parse_arguments(argc, argv);
+        auto [param_path, dbc_path, use_secondary_can] = parse_arguments(argc, argv);
 
         DriveBrainSettings settings{
             .run_db_service = true,
             .run_io_context = true,
             .run_process_loop = true,
-            .use_vectornav = true
+            .use_vectornav = false,
+            .use_secondary_can = use_secondary_can
         };
         
         std::cout <<"creating app" <<std::endl;
diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp
index 8b323bd1..6067f70b 100644
--- a/drivebrain_app/src/DriveBrainApp.cpp
+++ b/drivebrain_app/src/DriveBrainApp.cpp
@@ -1,11 +1,18 @@
 // DriveBrainApp.cpp
 #include "DriveBrainApp.hpp"
 
+#include "SimpleSpeedController.hpp"
+#include "SurreyAeroComms.hpp"
+#include "SpeedTechComms.hpp"
 #include "hytech.pb.h"
+#include <hytech_msgs.pb.h>
+#include <memory>
 #include <mutex>
+#include <spdlog/common.h>
+#include <spdlog/spdlog.h>
 #include <thread>
 
-std::atomic<bool> DriveBrainApp::_stop_signal{false};
+std::atomic<bool> stop_signal{false};
 
 DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& dbc_path, const DriveBrainSettings& settings)
     : _param_path(param_path)
@@ -13,129 +20,310 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d
     , _logger(core::LogLevel::INFO)
     , _config(_param_path)
     , _settings(settings)
-
+    , controller1(std::make_shared<control::SimpleSpeedController>(_config))
+    , _controllerManager(_config, {controller1})
+    , _estim_manager(std::make_shared<estimation::EstimatorManager>(_config))  // Initialize correctly
 {
+    // spdlog::info("top o");
+    std::vector<std::weak_ptr<core::common::Configurable>> configurable_components;
+    spdlog::set_level(spdlog::level::info);
 
-    spdlog::set_level(spdlog::level::warn);
+    
+    controller1 = std::make_shared<control::SimpleSpeedController>(_config);
+    if (!controller1->init()) {
+        throw std::runtime_error("Failed to initialize controller");
+    }
+    configurable_components.push_back(controller1);
+    spdlog::info("made mode 0 controller");
 
-    _mcap_logger = std::make_unique<common::MCAPProtobufLogger>("temp");
+    _mode1 = std::make_shared<control::LoadCellVectoringTorqueController>(_config);
+    if (!_mode1->init()) {
+        throw std::runtime_error("Failed to mode 1 controller");
+    }
+    configurable_components.push_back(_mode1);
+    spdlog::info("made mode 1 controller");
     
-    _controller = std::make_unique<control::SimpleController>(_logger, _config);
-    _configurable_components.push_back(_controller.get());
+    _estim_manager->handle_inits(configurable_components); // calls throw internally here
+    std::array<std::shared_ptr<control::Controller<core::ControllerOutput, core::VehicleState>>, 2 + matlab_model_gen::num_controllers> controllers{};
     
-    // bool matlab_construction_failed = false;
-    // _matlab_math = std::make_unique<estimation::Tire_Model_Codegen_MatlabModel>(
-    //     _logger, _config, matlab_construction_failed);
+    std::array<std::shared_ptr<control::Controller<core::ControllerOutput, core::VehicleState>>, 2> existing_controllers = {controller1, _mode1};
+   
+    _gend_controllers = matlab_model_gen::create_controllers(_config, configurable_components, _estim_manager);
+    if(_gend_controllers.size()+(existing_controllers.size()) != controllers.size())
+    {
+        throw std::runtime_error("Failed to initialize matlab generated controllers! Wrong vector size!");
+    }
     
-    // _configurable_components.push_back(_matlab_math.get());
+    std::copy(existing_controllers.begin(), existing_controllers.end(), controllers.begin());
+
+    std::copy(_gend_controllers.begin(), _gend_controllers.end(), controllers.begin()+2);
     
-    _foxglove_server = std::make_unique<core::FoxgloveWSServer>(_configurable_components);
     
-    _message_logger = std::make_shared<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>>(
-        ".mcap", true,
-        std::bind(&common::MCAPProtobufLogger::log_msg, std::ref(*_mcap_logger), std::placeholders::_1),
-        std::bind(&common::MCAPProtobufLogger::close_current_mcap, std::ref(*_mcap_logger)),
-        std::bind(&common::MCAPProtobufLogger::open_new_mcap, std::ref(*_mcap_logger), std::placeholders::_1),
-        std::bind(&core::FoxgloveWSServer::send_live_telem_msg, std::ref(*_foxglove_server), std::placeholders::_1));
-    
-    _state_estimator = std::make_unique<core::StateEstimator>(_logger, _message_logger);
+    // TODO make this required for the controller manager and remove use of raii for this shared ptrs to the controllers for construction of cm
+    _controllerManager.update_controllers(controllers);
+    if(!_controllerManager.init()){
+        throw std::runtime_error("Failed to initialize controller manager");
+    }
+
+    _state_estimator = std::make_shared<core::StateEstimator>(_config);
+    if(!_state_estimator->init())
+    {
+        throw std::runtime_error("Failed to initialize state estimator");
+    }
+
+    configurable_components.push_back(_state_estimator);
+    spdlog::info("made state estimator");
     
     bool construction_failed = false;
-    _driver = std::make_unique<comms::CANDriver>(
-        _config, _logger, _message_logger,_can_tx_queue, _io_context, 
-        _dbc_path, construction_failed, *_state_estimator);
+    // this also calls init() in the constructor
+    
+    _driver_primary_can = std::make_shared<comms::CANDriver>(
+        _config, _primary_can_tx_queue, _io_context, 
+        _dbc_path, construction_failed, _state_estimator, "CANDriverPrimary");
     
     if (construction_failed) {
         throw std::runtime_error("Failed to construct CAN driver");
     }
     
-    _configurable_components.push_back(_driver.get());
+    _driver_secondary_can = std::make_shared<comms::CANDriver>(
+        _config, _secondary_can_tx_queue, _io_context_secondary_can, 
+        _dbc_path, construction_failed, _state_estimator, "CANDriverSecondary");
     
-    _eth_driver = std::make_unique<comms::MCUETHComms>(
-        _logger, _eth_tx_queue, _message_logger, *_state_estimator,
-        _io_context, "192.168.1.30", 2001, 2000);
-    if(_settings.use_vectornav)
-    {
-        _vn_driver = std::make_unique<comms::VNDriver>(_config, _logger, _message_logger, *_state_estimator, _io_context);
+    if (construction_failed) {
+        throw std::runtime_error("Failed to construct CAN driver");
     }
+
     
-    if (!_controller->init()) {
-        throw std::runtime_error("Failed to initialize controller");
+    configurable_components.push_back(_driver_primary_can);
+    configurable_components.push_back(_driver_secondary_can);
+    spdlog::info("made CAN driver");
+    _acu_eth_driver_core = std::make_shared<comms::ETHRecvComms<hytech_msgs::ACUCoreData>>(_io_context, 7777);
+    _acu_eth_driver = std::make_shared<comms::ETHRecvComms<hytech_msgs::ACUAllData>>(_io_context, 7766);
+    _vcr_eth_driver = std::make_shared<comms::ETHRecvComms<hytech_msgs::VCRData_s>>(_io_context, 9999, _state_estimator);
+    _vcf_eth_driver = std::make_shared<comms::ETHRecvComms<hytech_msgs::VCFData_s>>(_io_context, 4444);
+    
+    spdlog::info("eth drivers");
+
+    auto switch_modes = 
+    [this](size_t mode) -> bool {
+        return _controllerManager.swap_active_controller(mode, _state_estimator->get_latest_state_and_validity().first);
+    };
+    _db_service = std::make_unique<DBInterfaceImpl>(switch_modes);
+    spdlog::info("made db service");
+
+    nlohmann::json &config_json = _config.get_config();
+    if(config_json.contains("use_vectornav") && config_json["use_vectornav"])
+    {
+        spdlog::info("using vectornav");
+        // on creation calls init()
+        _vn_driver = std::make_shared<comms::VNDriver>(_config, _state_estimator, _io_context, construction_failed);
+        if (construction_failed) {
+           throw std::runtime_error("Failed to construct VN driver");
+        }
+        configurable_components.push_back(_vn_driver);
+    } else if(config_json.contains("use_fake_vn") && config_json["use_fake_vn"])
+    {
+        _fake_vn = std::make_shared<comms::ETHRecvComms<hytech_msgs::VNData>>( _io_context, 13111, _state_estimator);
+    } 
+    
+    if(config_json.contains("use_surrey_aero") && config_json["use_surrey_aero"])
+    {
+        spdlog::info("making surrey aero sensor");
+        _aero_sensor_driver = std::make_shared<comms::SurreyAeroComms>(_config, _aero_usb_io_context);
+        if(!_aero_sensor_driver->init()){
+            throw std::runtime_error("failed to init aero sensor driver");
+
+        }
+        spdlog::info("made surrey aero sensor");
     }
-}
 
-DriveBrainApp::~DriveBrainApp() {
-    _stop_signal.store(true);
+    if(config_json.contains("use_laptimer") && config_json["use_laptimer"])
+    {
+        _lap_timer_driver = std::make_shared<comms::SpeedTechComms>(_config, _io_context_speed_tech_serial);
+        if(!_lap_timer_driver->init()) {
+            throw std::runtime_error("failed to init lap timer driver");
+
+        }
+        _using_lap_timer = true;
+    } else {
+        _using_lap_timer = false;
+    }
     
-    if (_process_thread.joinable()) {
-        _process_thread.join();
+    if(config_json.contains("use_scale_comms") && config_json["use_scale_comms"])
+    {
+        spdlog::info("making scale comms driver");
+        _scale_comms = std::make_shared<comms::ScaleComms>(_config, _scale_usb_io_context);
+        if(!_scale_comms->init()){
+            throw std::runtime_error("failed to init scale comms");
+
+        }
+        spdlog::info("made scale comms driver");
     }
-    spdlog::info("joined main process");
 
-    _io_context.stop();
-    if (_io_context_thread.joinable()) {
-        _io_context_thread.join();
+    
+    // - [x] TODO figure out how im going to get the parameter schemas for each of the configureable components into the mcap logger if 
+    // the mcap logger is needed by the message logger but I wont know the schemas until the components have been created and the each
+    // component is given the message logger on construction. 
+    //   if I just have an initialize method that calls the schema get function and sets a member var to store that schema
+    //   that could work. 
+    
+    // - [x] TODO add in function for getting the current config values periodically of all of the configureable components,
+    //       or, just give the vector of configureable components that gets given to the foxglove webserver instance
+    //       and make the logger also handle the getting of all of the configs of the components (imma do dis way)
+    _mcap_logger = std::make_shared<common::DrivebrainMCAPLogger>("temp", configurable_components);
+    _foxglove_server = std::make_shared<core::FoxgloveWSServer>(configurable_components);
+    
+    spdlog::info("made mcap logger and foxglove server");
+
+    // all things must be initialized before this gets constructed due to logging on init needing the schemas determined by the init 
+    // // functions of the configurable components
+    _message_logger = std::make_shared<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>>(
+        ".mcap", true,
+        std::bind(&common::DrivebrainMCAPLogger::log_msg, _mcap_logger, std::placeholders::_1),
+        std::bind(&common::DrivebrainMCAPLogger::close_current_mcap, _mcap_logger),
+        std::bind(&common::DrivebrainMCAPLogger::open_new_mcap, _mcap_logger, std::placeholders::_1),
+        std::bind(&core::FoxgloveWSServer::send_live_telem_msg, _foxglove_server, std::placeholders::_1),
+        std::bind(&common::DrivebrainMCAPLogger::init_param_schema, _mcap_logger),
+        std::bind(&common::DrivebrainMCAPLogger::log_params, _mcap_logger));
+
+    if(_db_service)
+    {
+        // this one is special because it actually needs the full interface of the message logger
+        _db_service->update_msg_logger(_message_logger);
     }
+
+    using loggertype = std::shared_ptr<core::common::Loggable<std::shared_ptr<google::protobuf::Message>>>;
     
-    if (_db_service) {
-        _db_service->stop_server();
+    std::vector<loggertype> logging_components = {_driver_primary_can, 
+        _driver_secondary_can,
+        _vn_driver,
+        _fake_vn,
+        _state_estimator,
+        _acu_eth_driver_core,
+        _acu_eth_driver,
+        _vcr_eth_driver,
+        _vcf_eth_driver,
+        _aero_sensor_driver, 
+        _scale_comms,
+        _lap_timer_driver
+    };
+    // get the pointers to all of the generated controllers to handle setting of their loggers too
+    logging_components.insert(logging_components.end(), _gend_controllers.begin(), _gend_controllers.end());
+    
+    _setup_loggers(logging_components);
+    
+    // the estimator manager handles the setup of all of the loggers for the generated estimators internally
+    _estim_manager->set_loggers(_message_logger);
+
+    _message_logger->start_logging_params();
+    spdlog::info("constructed app");
+}
+
+
+
+void DriveBrainApp::_setup_loggers(std::vector<std::shared_ptr<core::common::Loggable<std::shared_ptr<google::protobuf::Message>>>> logging_components)
+{
+    if(!_message_logger)
+    {
+        throw std::runtime_error("Failed to set message logger on components, message logger does not exist yet!");
     }
-    if ( _db_service_thread.joinable()) {
-        _db_service_thread.join();
+    for(auto component : logging_components)
+    {
+        if(component)
+        {
+            component->set_msg_logger(_message_logger);
+        } else {
+            spdlog::warn("component does not exist, not setting message logger");
+        }
     }
-    spdlog::info("joined io context");
 }
 
 void DriveBrainApp::_process_loop() {
-    // auto out_msg = std::make_shared<hytech_msgs::MCUCommandData>();
     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 loop_time = _controller->get_dt_sec();
+    auto desired_torque_msg = std::make_shared<hytech::drivebrain_desired_torque_input>();
+    auto loop_time = 0.004;
     auto loop_time_micros = (int)(loop_time * 1000000.0f);
     std::chrono::microseconds loop_chrono_time(loop_time_micros);
 
-    while (!_stop_signal.load()) {
+    while (!stop_signal.load()) {
+        spdlog::debug("looping _process_loop");
         auto start_time = std::chrono::high_resolution_clock::now();
 
         auto state_and_validity = _state_estimator->get_latest_state_and_validity();
-        // TODO handle invalid state. need tc mux
-        auto out_struct = _controller->step_controller(state_and_validity.first);
-        auto temp_desired_torques = state_and_validity.first.matlab_math_temp_out;
-        _state_estimator->set_previous_control_output(out_struct);
-
-        if(temp_desired_torques.res_torque_lim_nm.FL < 0) {
-            desired_rpm_msg->set_drivebrain_set_rpm_fl(0);
-        } else {
-            desired_rpm_msg->set_drivebrain_set_rpm_fl(out_struct.desired_rpms.FL);
-        }
+        
+        _estim_manager->evaluate_all_estimators(state_and_validity.first);
+        auto out_struct = _controllerManager.step_active_controller(state_and_validity.first);
 
-        if(temp_desired_torques.res_torque_lim_nm.FR < 0) {
-            desired_rpm_msg->set_drivebrain_set_rpm_fr(0);
-        } else {
-            desired_rpm_msg->set_drivebrain_set_rpm_fr(out_struct.desired_rpms.FR);
-        }
+        auto now = std::chrono::steady_clock::now();
 
-        if(temp_desired_torques.res_torque_lim_nm.RL < 0) {
-            desired_rpm_msg->set_drivebrain_set_rpm_rl(0);
-        } else {
-            desired_rpm_msg->set_drivebrain_set_rpm_rl(out_struct.desired_rpms.RL);
-        }
+        if (now - _last_send_time >= _send_period) {
+            _last_send_time = now;
 
-        if(temp_desired_torques.res_torque_lim_nm.RR < 0) {
-            desired_rpm_msg->set_drivebrain_set_rpm_rr(0);
-        } else {
-            desired_rpm_msg->set_drivebrain_set_rpm_rr(out_struct.desired_rpms.RR);
+            auto drivebrain_state_data_msg = std::make_shared<hytech::drivebrain_state_data>();
+            hytech::vn_gps_status status = static_cast<hytech::vn_gps_status>(state_and_validity.first.ins_status.status_mode);
+            drivebrain_state_data_msg->set_vn_gps_status(status);
+            
+            {
+                std::unique_lock lk(_primary_can_tx_queue.mtx);
+                _primary_can_tx_queue.deque.push_back(drivebrain_state_data_msg);
+                _primary_can_tx_queue.cv.notify_all();
+            }
         }
+        // state_and_validity.first.ins_status.status_mode
 
-        torque_limit_msg->set_drivebrain_torque_fl(::abs(temp_desired_torques.res_torque_lim_nm.FL));
-        torque_limit_msg->set_drivebrain_torque_fl(::abs(temp_desired_torques.res_torque_lim_nm.FR));
-        torque_limit_msg->set_drivebrain_torque_fl(::abs(temp_desired_torques.res_torque_lim_nm.RL));
-        torque_limit_msg->set_drivebrain_torque_fl(::abs(temp_desired_torques.res_torque_lim_nm.RR));
+        // get current command
+        std::variant<core::SpeedControlOut, core::TorqueControlOut, std::monostate> cmd_out = out_struct.out;
 
+        // push current command for next state estimator call
+        _state_estimator->set_previous_control_output(out_struct);
+        bool state_is_valid = state_and_validity.second;
+        if(state_is_valid)
         {
-            std::unique_lock lk(_can_tx_queue.mtx);
-            _can_tx_queue.deque.push_back(desired_rpm_msg);
-            _can_tx_queue.deque.push_back(torque_limit_msg);
+
+            if (const core::SpeedControlOut* speedControl = std::get_if<core::SpeedControlOut>(&cmd_out)) { // speed controller, set RPM
+
+                // set RPMs in message to the RPMS given from the controller
+            
+                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);
+            
+                if(_message_logger)
+                {
+                    _message_logger->log_msg(static_cast<std::shared_ptr<google::protobuf::Message>>(desired_rpm_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));
+            
+                {
+                    std::unique_lock lk(_primary_can_tx_queue.mtx);
+                    _primary_can_tx_queue.deque.push_back(desired_rpm_msg);
+                    _primary_can_tx_queue.deque.push_back(torque_limit_msg);
+                    _primary_can_tx_queue.cv.notify_all(); // notify the CAN thread to send the messages
+                    // spdlog::info("sent can");
+                }
+
+            } else if (const core::TorqueControlOut* torqueControl = std::get_if<core::TorqueControlOut>(&cmd_out)){ // if it is a torque controller:
+                // set desired torque
+                
+                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);
+                
+                
+                {
+                    std::unique_lock lk(_primary_can_tx_queue.mtx);
+                    _primary_can_tx_queue.deque.push_back(desired_torque_msg); // use new protobuf struct
+                    _primary_can_tx_queue.cv.notify_all(); // notify the CAN thread to send the messages
+                }
+            }
         }
 
         auto end_time = std::chrono::high_resolution_clock::now();
@@ -148,10 +336,10 @@ void DriveBrainApp::_process_loop() {
 }
 
 
-std::atomic<bool> stop_signal{false};
+
 void signal_handler(int signal)
 {
-    spdlog::info("Interrupt signal ({}) received. Cleaning up...", signal);
+    spdlog::info("Interrupt signal db app({}) received. Cleaning up...", signal);
     stop_signal.store(true); // Set running to false to exit the main loop or gracefully terminate
 }
 
@@ -162,10 +350,10 @@ void DriveBrainApp::run() {
         
         if (!_settings.run_db_service) return;
         
-        _db_service = std::make_unique<DBInterfaceImpl>(_message_logger);
         spdlog::info("started db service thread");
         try {
             while (!stop_signal.load()) {
+                spdlog::debug("looping db service thread");
                 _db_service->run_server();
             }
         } catch (const std::exception& e) {
@@ -183,6 +371,51 @@ void DriveBrainApp::run() {
         }
     });
 
+    _io_context_secondary_thread = std::thread([this]() {
+        if (!_settings.run_io_context) return;
+        spdlog::info("Started io context 2 thread");
+        try {
+            _io_context_secondary_can.run();
+        } catch (const std::exception& e) {
+            spdlog::error("Error in io_context 2: {}", e.what());
+        }
+    });
+
+    if(_aero_sensor_driver)
+    {
+        _aero_usb_io_context_thread = std::thread([this]() -> void {
+            spdlog::info("Started _aero_usb_io_context_thread");
+            try {
+                _aero_usb_io_context.run();
+            } catch (const std::exception& e) {
+                spdlog::error("Error in _aero_usb_io_context: {}", e.what());
+            }
+        });
+    }
+    if(_scale_comms)
+    {
+        _scale_usb_io_context_thread = std::thread([this]() -> void {
+            spdlog::info("Started _scale_usb_io_context_thread");
+            try {
+                _scale_usb_io_context.run();
+            } catch (const std::exception& e) {
+                spdlog::error("Error in _scale_usb_io_context: {}", e.what());
+            }
+        });
+    }
+    
+    if(_using_lap_timer)
+    {
+        _io_context_speed_tech_serial_thread = std::thread([this]() -> void {
+            spdlog::info("Started speed tech serial context thread");
+            try {
+                _io_context_speed_tech_serial.run();
+            } catch (const std::exception& e) {
+                spdlog::error("Error in speed tech serial context: {}", e.what());
+            }
+        });
+    }
+
     _process_thread = std::thread([this]() {
         if (!_settings.run_process_loop) return;
         _process_loop();
@@ -190,6 +423,70 @@ void DriveBrainApp::run() {
 
     
     while (!stop_signal.load()) {
+        spdlog::debug("looping DBAPP run");
         std::this_thread::sleep_for(std::chrono::seconds(1));
     }
 }
+
+DriveBrainApp::~DriveBrainApp() {
+    stop_signal.store(true);
+    
+    if(_message_logger)
+    {
+        _message_logger->halt();
+    }
+    
+
+    if (_process_thread.joinable()) {
+        _process_thread.join();
+        spdlog::info("joined main process");
+    }
+    
+    
+    spdlog::info("halted message logger");
+    _io_context.stop();
+    if (_io_context_thread.joinable()) {
+        _io_context_thread.join();
+        spdlog::info("joined io context 1");
+    }
+    
+    _io_context_secondary_can.stop();
+    if (_io_context_secondary_thread.joinable()) {
+        _io_context_secondary_thread.join();
+        spdlog::info("joined io context 2");
+    }
+
+    if(_aero_sensor_driver) {
+        _aero_usb_io_context.stop();
+        if(_aero_usb_io_context_thread.joinable()) {
+            _aero_usb_io_context_thread.join();
+        }
+    }
+    if(_scale_comms)
+    {
+        _scale_usb_io_context.stop();
+        if(_scale_usb_io_context_thread.joinable())
+        {
+            _scale_usb_io_context_thread.join();
+        }
+    }
+    
+    
+    if (_db_service) {
+        _db_service->stop_server();
+    }
+    if ( _db_service_thread.joinable()) {
+        _db_service_thread.join();
+        spdlog::info("joined db service");
+    }
+
+    if(_using_lap_timer)
+    {
+        _io_context_speed_tech_serial.stop();
+        if(_io_context_speed_tech_serial_thread.joinable())
+        {
+            _io_context_speed_tech_serial_thread.join();
+        }
+    }
+    spdlog::info("destructed db app");
+}
\ No newline at end of file
diff --git a/drivebrain_app/src/arg_parse.cpp b/drivebrain_app/src/arg_parse.cpp
new file mode 100644
index 00000000..f633a653
--- /dev/null
+++ b/drivebrain_app/src/arg_parse.cpp
@@ -0,0 +1,30 @@
+#include "arg_parse.hpp"
+
+#include <iostream>
+
+std::tuple<std::string, std::string, bool> parse_arguments(int argc, char* argv[]) {
+    
+    namespace po = boost::program_options;
+    po::options_description desc("Allowed options");
+    std::string param_path = "config/drivebrain_config.json";
+    std::string dbc_path;
+
+    bool use_secondary_can = true;
+
+    desc.add_options()
+        ("help,h", "produce help message")
+        ("param-path,p", po::value<std::string>(&param_path), "Path to the parameter JSON file")
+        ("dbc-path,d", po::value<std::string>(&dbc_path), "Path to the DBC file (optional)")
+        ("2-can,c", po::value<bool>(&use_secondary_can), "use secondary CAN");
+        
+    po::variables_map vm;
+    po::store(po::parse_command_line(argc, argv, desc), vm);
+    po::notify(vm);
+
+    if (vm.count("help")) {
+        std::cout << desc << std::endl;
+        std::exit(0);
+    }
+
+    return {param_path, dbc_path, use_secondary_can};
+}
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp b/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp
new file mode 100644
index 00000000..8807ad83
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp
@@ -0,0 +1,38 @@
+#ifndef __JSONUTILS_H__
+#define __JSONUTILS_H__
+
+#include <vector>
+
+// Ensures a parameter is loaded or the function returns false
+#define LOAD_PARAM_OR_FAIL(param_name, param_type, config_struct)                                  \
+    do {                                                                                           \
+        auto opt_##param_name = get_parameter_value<param_type>(#param_name);                      \
+        if (!opt_##param_name)                                                                     \
+            return false;                                                                          \
+        (config_struct).param_name = *opt_##param_name;                                            \
+    } while (0)
+
+// Similar, but for live parameters
+#define LOAD_LIVE_PARAM_OR_FAIL(param_name, param_type, config_struct)                             \
+    do {                                                                                           \
+        auto opt_##param_name = get_live_parameter<param_type>(#param_name);                       \
+        if (!opt_##param_name)                                                                     \
+            return false;                                                                          \
+        (config_struct).param_name = *opt_##param_name;                                            \
+    } while (0)
+
+// Updates a live parameter safely under a mutex if the type matches
+#define HANDLE_LIVE_PARAM_LOCK_AND_LOAD(param_name, param_type, config_struct, config_mutex,       \
+                                        param_map)                                                 \
+    do {                                                                                           \
+        auto it = (param_map).find(#param_name);                                                   \
+        if (it != (param_map).end()) {                                                             \
+            if (auto pval = std::get_if<param_type>(&it->second)) {                                \
+                std::unique_lock<std::mutex> lk(config_mutex);                                     \
+                (config_struct).param_name = *pval;                                                \
+            }                                                                                      \
+        }                                                                                          \
+    } while (0)
+
+
+#endif // __JSONUTILS_H__
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_common_utils/include/ProtobufUtils.hpp b/drivebrain_core_impl/drivebrain_common_utils/include/ProtobufUtils.hpp
index df3fbe46..8b8f07dd 100644
--- a/drivebrain_core_impl/drivebrain_common_utils/include/ProtobufUtils.hpp
+++ b/drivebrain_core_impl/drivebrain_common_utils/include/ProtobufUtils.hpp
@@ -11,6 +11,7 @@
 
 // TODO figure out if this is the best place to include this or nah
 #include <hytech_msgs.pb.h>
+
 #include <tuple>
 #include <vector>
 #include <string>
diff --git a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp
index fc2cbfaa..8f02a1d6 100644
--- a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp
+++ b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp
@@ -1,8 +1,8 @@
 #pragma once
 // drivebrain includes
 #include <Configurable.hpp>
+#include <Loggable.hpp>
 #include <DriverBus.hpp>
-#include <Logger.hpp>
 #include <MsgLogger.hpp>
 #include <StateEstimator.hpp>
 #include <hytech.pb.h> // generated from CAN description
@@ -55,21 +55,31 @@
 
 namespace comms
 {
-    class CANDriver : public core::common::Configurable
+    class CANDriver : public core::common::Loggable<std::shared_ptr<google::protobuf::Message>>,
+                      public core::common::Configurable
     {
     public:
         using FieldVariant = std::variant<int32_t, int64_t, uint32_t, uint64_t, float, double, bool, std::string, std::monostate>;
         using deqtype = core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>>;
-        using loggertype = core::MsgLogger<std::shared_ptr<google::protobuf::Message>>;
         /// @brief constructur
         /// @param json_file_handler the file handler 
         /// @param in_deq tx queue
         /// @param out_deq receive queue
         /// @param io_context boost asio required context
-        CANDriver(core::JsonFileHandler &json_file_handler, core::Logger& logger, std::shared_ptr<loggertype> message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional<std::string> dbc_path, bool &construction_failed, core::StateEstimator &state_estimator) : 
-            Configurable(logger, json_file_handler, "CANDriver"),
-            _logger(logger),
-            _message_logger(message_logger),
+        CANDriver(core::JsonFileHandler &json_file_handler, deqtype &in_deq, boost::asio::io_context& io_context, std::optional<std::string> dbc_path, bool &construction_failed, std::shared_ptr<core::StateEstimator> state_estimator) : 
+            Configurable(json_file_handler, "CANDriver"),
+            _input_deque_ref(in_deq),
+            _socket(io_context),
+            _dbc_path(dbc_path),
+            _state_estimator(state_estimator)
+        {
+            _running = true;
+            _output_thread = std::thread(&comms::CANDriver::_handle_send_msg_from_queue, this);
+            construction_failed = !init();
+        }
+
+        CANDriver(core::JsonFileHandler &json_file_handler, deqtype &in_deq, boost::asio::io_context& io_context, std::optional<std::string> dbc_path, bool &construction_failed, std::shared_ptr<core::StateEstimator> state_estimator, std::string driver_name) : 
+            Configurable(json_file_handler, driver_name),
             _input_deque_ref(in_deq),
             _socket(io_context),
             _dbc_path(dbc_path),
@@ -81,6 +91,8 @@ namespace comms
         }
         ~CANDriver();
         bool init();
+
+
         void _handle_send_msg_from_queue();
         std::shared_ptr<google::protobuf::Message> pb_msg_recv(const can_frame &in_frame);
         void set_field_values_of_pb_msg(const std::unordered_map<std::string, FieldVariant> &field_values, std::shared_ptr<google::protobuf::Message> message);
@@ -93,6 +105,7 @@ namespace comms
         /// @return variant of types
         FieldVariant get_field_value(std::shared_ptr<google::protobuf::Message> message, const std::string &field_name);
 
+        
         // for exposing to the test framework directly
     protected:
         // socket operations
@@ -109,8 +122,6 @@ namespace comms
         static std::string _to_lowercase(std::string s);
 
     private:
-        core::Logger& _logger;
-        std::shared_ptr<loggertype> _message_logger;
         deqtype &_input_deque_ref;
 
         std::condition_variable _cv;
@@ -125,6 +136,9 @@ namespace comms
         std::unordered_map<std::string, uint64_t> _messages_names_and_ids;
         int _CAN_socket; // can socket bound to
         bool _running = false;
-        core::StateEstimator & _state_estimator;
+        std::shared_ptr<core::StateEstimator> _state_estimator;
+        size_t _recv_process_count = 0;
+        const size_t _recv_process_log_interval = 1000;
+        long _recv_process_max_duration_us = 0;           // Microseconds
     };
 }
diff --git a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp
index 863d6517..e6f7d2d7 100644
--- a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp
+++ b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp
@@ -1,7 +1,9 @@
 #ifndef DBSERVICE_IMPL_HPP
 #define DBSERVICE_IMPL_HPP
 
+#include "VNComms.hpp"
 #include <db_service/v1/service/db_interface.grpc.pb.h>
+#include <google/protobuf/message.h>
 #include <grpcpp/ext/proto_server_reflection_plugin.h>
 #include <iostream>
 #include <memory>
@@ -10,22 +12,26 @@
 #include <grpcpp/grpcpp.h>
 
 #include <MsgLogger.hpp>
+#include <Controllers.hpp>
+#include <ControllerManager.hpp>
 
 class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Service {
 
-    struct InitStruct {
-        std::shared_ptr<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>> logger_inst;
-    };
     grpc::Status RequestStopLogging(grpc::ServerContext* context, const google::protobuf::Empty *rq, db_service::v1::service::LoggerStatus * response) override; 
     grpc::Status RequestStartLogging(grpc::ServerContext* context, const google::protobuf::Empty *rq, db_service::v1::service::LoggerStatus * response) override;
-    grpc::Status RequestCurrentLoggerStatus(grpc::ServerContext* context, const google::protobuf::Empty* request, db_service::v1::service::LoggerStatus* response) override; 
-    
+    grpc::Status RequestCurrentLoggerStatus(grpc::ServerContext* context, const google::protobuf::Empty* rq, db_service::v1::service::LoggerStatus* response) override;
+    grpc::Status RequestControllerChange(grpc::ServerContext* context, const db_service::v1::service::DesiredController* rq, db_service::v1::service::ControllerChangeStatus* response) override; 
+
     public: 
-        DBInterfaceImpl(std::shared_ptr<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>> logger_inst);
+        DBInterfaceImpl(std::function<bool(size_t)> mode_switch);        
         void run_server(); 
         void stop_server();
+        void update_msg_logger(std::shared_ptr<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>> logger_inst) {
+            _logger_inst = logger_inst;
+        }
     private:
         std::shared_ptr<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>> _logger_inst;
+        std::function<bool(size_t)> _mode_switch;
         std::unique_ptr<grpc::Server> _server;  // Store server instance here
 
 };
diff --git a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp
new file mode 100644
index 00000000..1c23d739
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp
@@ -0,0 +1,56 @@
+#ifndef __ETHRECVCOMMS_H__
+#define __ETHRECVCOMMS_H__
+
+#include <memory>
+#include <cstdint>
+
+#include <Loggable.hpp>
+#include <StateEstimator.hpp>
+
+#include <boost/asio.hpp>
+#include <boost/array.hpp>
+#include <boost/bind/bind.hpp>
+
+#include <google/protobuf/message.h>
+#include "hytech_msgs.pb.h"
+
+// - [x] boost asio socket for udp port comms
+// - [x] handle receiving UDP messages on a specific port
+// - [x] handle parsing of UDP message as protobuf message on the port
+// TODO:
+// figure out if we want to keep the queue work flow for sending or if we want to
+// instead use just a direct pointer / ref to a generic driver interface that we
+// can give to the estimation / control thread to handle the sending of the control msgs
+
+namespace comms
+{
+    
+    template<typename ETHMsgType>
+    class ETHRecvComms : public core::common::Loggable<std::shared_ptr<google::protobuf::Message>>
+    {
+    public:
+        
+        ETHRecvComms() = delete;
+        ~ETHRecvComms();
+        ETHRecvComms(boost::asio::io_context &io_context, uint16_t recv_port, std::shared_ptr<core::StateEstimator> state_estim=nullptr);
+        
+        
+    private:
+        void _handle_receive(const boost::system::error_code &error, std::size_t size);
+        void _start_receive();
+        
+    private:
+        std::shared_ptr<core::StateEstimator> _state_estimator = nullptr;
+        std::array<uint8_t, 4096> _recv_buffer;
+        boost::asio::ip::udp::socket _socket;
+        boost::asio::ip::udp::endpoint _remote_endpoint;
+        std::shared_ptr<ETHMsgType> _eth_msg;
+        bool _running = false;
+        std::thread _output_thread;
+    };
+
+}
+
+#include "ETHRecvComms.tpp"
+
+#endif // __ETHRECVCOMMS_H__
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp
new file mode 100644
index 00000000..544113a0
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp
@@ -0,0 +1,59 @@
+#include <ETHRecvComms.hpp>
+#include "hytech_msgs.pb.h"
+#include <spdlog/spdlog.h>
+
+
+using boost::asio::ip::udp;
+namespace comms
+{
+    template <typename ETHMsgType> 
+    ETHRecvComms<ETHMsgType>::ETHRecvComms(boost::asio::io_context &io_context,
+                                           uint16_t port,
+                                           std::shared_ptr<core::StateEstimator> state_estim) 
+                                                : _state_estimator(state_estim),
+                                                  _socket(io_context, udp::endpoint(udp::v4(), port))
+    {
+        _eth_msg = std::make_shared<ETHMsgType>();
+        _start_receive();
+    }
+
+    template <typename ETHMsgType>
+    ETHRecvComms<ETHMsgType>::~ETHRecvComms()
+    {
+        _running = false;
+        spdlog::warn("Destructed ETH COMMS");
+    }
+    
+    template <typename ETHMsgType>
+    void ETHRecvComms<ETHMsgType>::_handle_receive(const boost::system::error_code &error, std::size_t size)
+    {
+
+        if (!error)
+        {
+            _eth_msg->ParseFromArray(_recv_buffer.data(), size);
+            auto out_msg = static_cast<std::shared_ptr<google::protobuf::Message>>(_eth_msg);
+            
+            this->log(out_msg);
+            
+            if(_state_estimator)
+            {
+                
+                _state_estimator->handle_recv_process(out_msg);
+            }
+
+            
+            _start_receive();
+        }
+    }
+
+    template <typename ETHMsgType>
+    void ETHRecvComms<ETHMsgType>::_start_receive()
+    {
+        using namespace boost::placeholders;
+        _socket.async_receive_from(
+            boost::asio::buffer(_recv_buffer), _remote_endpoint,
+            boost::bind(&ETHRecvComms::_handle_receive, this,
+                        boost::asio::placeholders::error,
+                        boost::asio::placeholders::bytes_transferred));
+    }
+}
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/include/ETHSendComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ETHSendComms.hpp
new file mode 100644
index 00000000..e27fddf3
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/include/ETHSendComms.hpp
@@ -0,0 +1,65 @@
+#ifndef __ETHSENDCOMMS_H__
+#define __ETHSENDCOMMS_H__
+
+#include <Logger.hpp>
+#include <StateEstimator.hpp>
+#include <MsgLogger.hpp>
+
+#include <boost/asio.hpp>
+#include <boost/array.hpp>
+#include <boost/bind/bind.hpp>
+
+#include <cstdint>
+#include <google/protobuf/message.h>
+#include "hytech_msgs.pb.h"
+#include <memory>
+
+// - [ ] handle sending UDP messages on a specific port and ip
+// TODO:
+// figure out if we want to keep the queue work flow for sending or if we want to
+// instead use just a direct pointer / ref to a generic driver interface that we
+// can give to the estimation / control thread to handle the sending of the control msgs
+
+    // will make the queue internal and expose a public function for enqueue of a protobuf message to send
+
+
+namespace comms
+{
+    class ETHSendComms : public core::common::Loggable<std::shared_ptr<google::protobuf::Message>>
+    {
+    public:
+        using deqtype = core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>>;
+        using loggertype = core::MsgLogger<std::shared_ptr<google::protobuf::Message>>;
+        
+        ETHSendComms() = delete;
+        ~ETHSendComms();
+        ETHSendComms(boost::asio::io_context &io_context, uint16_t send_port, std::string send_ip, bool bind=true);
+    
+
+        // thread safe call to enqueue a message to the internal dequeue for safe multi-thread access from multiple threads at once
+        void enqueue_msg_to_send(std::shared_ptr<google::protobuf::Message> send_msg);
+        
+    private:
+        // is this required? i think that it is
+        void _send_completition_handler(std::array<uint8_t, 4096> /*message*/,
+                                        const boost::system::error_code & /*error*/,
+                                        std::size_t /*bytes_transferred*/);
+        void _handle_send_msg_from_queue();
+        void _send_message(std::shared_ptr<google::protobuf::Message> msg_out);
+        
+    private:
+
+        boost::asio::ip::udp::endpoint _endp;
+
+        std::array<uint8_t, 4096> _recv_buffer;
+        boost::asio::ip::udp::socket _socket;
+        boost::asio::ip::udp::endpoint _remote_endpoint;
+        bool _running = false;
+        std::thread _output_thread;
+        std::array<uint8_t, 4096> _send_buffer;
+        deqtype _queue;
+    };
+
+}
+
+#endif // __ETHSENDCOMMS_H__
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp
deleted file mode 100644
index 3e97da3d..00000000
--- a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp
+++ /dev/null
@@ -1,71 +0,0 @@
-#ifndef __ETHCOMMS_H__
-#define __ETHCOMMS_H__
-
-#include <Logger.hpp>
-#include <StateEstimator.hpp>
-#include <MsgLogger.hpp>
-
-#include <boost/asio.hpp>
-#include <boost/bind.hpp>
-#include <boost/array.hpp>
-#include <boost/bind/bind.hpp>
-
-#include <google/protobuf/message.h>
-#include "hytech_msgs.pb.h"
-#include <memory>
-
-// - [x] boost asio socket for udp port comms
-// - [x] handle receiving UDP messages on a specific port
-// - [x] handle parsing of UDP message as protobuf message on the port
-// TODO:
-// figure out if we want to keep the queue work flow for sending or if we want to
-// instead use just a direct pointer / ref to a generic driver interface that we
-// can give to the estimation / control thread to handle the sending of the control msgs
-
-namespace comms
-{
-    class MCUETHComms
-    {
-    public:
-        using deqtype = core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>>;
-        using loggertype = core::MsgLogger<std::shared_ptr<google::protobuf::Message>>;
-        MCUETHComms() = delete;
-        ~MCUETHComms();
-        MCUETHComms(core::Logger &logger,
-                    deqtype &in_deq,
-                    std::shared_ptr<loggertype> message_logger,
-                    core::StateEstimator &state_estimator,
-                    boost::asio::io_context &io_context,
-                    const std::string &send_ip,
-                    uint16_t recv_port,
-                    uint16_t send_port);
-
-    private:
-        void _handle_send_msg_from_queue();
-        void _send_message(std::shared_ptr<google::protobuf::Message> msg_out);
-        void _handle_receive(const boost::system::error_code &error, std::size_t size);
-        void _start_receive();
-        void _handle_send(std::array<uint8_t, 2048> /*message*/,
-                          const boost::system::error_code & /*error*/,
-                          std::size_t /*bytes_transferred*/);
-
-    private:
-        core::Logger &_logger;
-        std::shared_ptr<loggertype> _message_logger;
-        core::StateEstimator &_state_estimator;
-        std::array<uint8_t, 2048> _recv_buffer;
-        std::array<uint8_t, 2048> _send_buffer;
-
-        uint16_t _send_port;
-        std::string _send_ip;
-        boost::asio::ip::udp::socket _socket;
-        boost::asio::ip::udp::endpoint _remote_endpoint;
-        std::shared_ptr<hytech_msgs::MCUOutputData> _mcu_msg;
-        deqtype &_input_deque_ref; // "input" = the messages that get input to the ethernet comms driver to send out
-        bool _running = false;
-        std::thread _output_thread;
-    };
-
-}
-
-#endif // __ETHCOMMS_H__
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/include/ScaleComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ScaleComms.hpp
new file mode 100644
index 00000000..fecf142f
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/include/ScaleComms.hpp
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <Configurable.hpp>
+#include <JsonFileHandler.hpp>
+#include <Loggable.hpp>
+#include <MsgLogger.hpp>
+#include <Utils.hpp>
+
+#include <boost/asio.hpp>
+#include <boost/array.hpp>
+
+#include <google/protobuf/message.h>
+
+namespace comms {
+class ScaleComms : public core::common::Loggable<std::shared_ptr<google::protobuf::Message>>,
+                   public core::common::Configurable {
+    struct config {
+        int baud_rate;
+        std::string port_name;
+    } _config;
+    
+    struct ScaleData {
+        veh_vec<float> corner_weights_lbs;
+    };
+  public:
+    
+    ScaleComms(core::JsonFileHandler &json_file_handler, boost::asio::io_context &io);
+    bool init() override final;
+
+  private:
+    void _configure_serial_port(boost::asio::serial_port &serial);
+    private:
+        void _start_receive();
+        std::optional<ScaleData> _parse_buffer(const boost::array<std::uint8_t, 512>& buffer, std::size_t bytes_count);
+        void _log_proto_message(const ScaleData & data);
+    private:
+        boost::asio::serial_port _serial;
+        boost::array<std::uint8_t, 512> _input_buff{};
+};
+} // namespace comms
diff --git a/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp
new file mode 100644
index 00000000..12cb5f2e
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp
@@ -0,0 +1,47 @@
+#pragma once
+
+// drivebrain_core
+#include <Configurable.hpp>
+#include <JsonFileHandler.hpp>
+#include <Loggable.hpp>
+
+#include "hytech_msgs.pb.h"
+
+// protobuf
+#include <google/protobuf/any.pb.h>
+#include <google/protobuf/dynamic_message.h>
+#include <google/protobuf/message.h>
+
+// boost
+#include <boost/array.hpp>
+#include <boost/asio.hpp>
+
+
+// https://wiki.hytechracing.org/books/software/page/speedtech-lap-timing-protocol-description
+
+namespace comms {
+class SpeedTechComms : public core::common::Loggable<std::shared_ptr<google::protobuf::Message>>,
+                       public core::common::Configurable {
+    private:
+        struct config {
+            int baud_rate;
+            std::string port_name;
+        } _config;
+
+    public:
+        SpeedTechComms(
+            core::JsonFileHandler &json_file_handler,
+            boost::asio::io_context &io_context);
+        
+        bool init() override final;
+        
+        void process_buffer(const boost::array<std::uint8_t, 512> &buff, std::size_t length);
+
+    private:
+        void _start_recv();
+    
+    private:
+        boost::array<std::uint8_t, 512> _input_buff;
+        boost::asio::serial_port _serial;
+};
+} // namespace comms
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/include/SurreyAeroComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/SurreyAeroComms.hpp
new file mode 100644
index 00000000..7173e9d9
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/include/SurreyAeroComms.hpp
@@ -0,0 +1,47 @@
+#ifndef __SURREYAEROCOMMS_H__
+#define __SURREYAEROCOMMS_H__
+
+
+#include <Configurable.hpp>
+#include <Loggable.hpp>
+#include <MsgLogger.hpp>
+
+#include <StateEstimator.hpp>
+
+#include <boost/asio.hpp>
+#include <boost/array.hpp>
+
+#include <cstring>
+#include <ratio>
+#include <vector>
+
+#include <google/protobuf/message.h>
+
+
+
+namespace comms {
+
+    class SurreyAeroComms : public core::common::Loggable<std::shared_ptr<google::protobuf::Message>>, public core::common::Configurable {
+        struct config {
+            std::string port_name;
+        } _config;
+    public:
+        SurreyAeroComms(core::JsonFileHandler &json_file_handler, boost::asio::io_context& io);
+        bool init() override final;
+    
+    private:
+        void _start_receive();
+        void _configure_serial_port(boost::asio::serial_port& serial);
+        bool _send_command(const std::string& command);
+        std::optional<std::vector<float>> _extract_sensor_readings(const boost::array<std::uint8_t, 512>& buffer);
+        void _log_proto_message(const std::vector<float>& readings);
+    private:
+        boost::asio::serial_port _serial;
+        std::chrono::steady_clock::time_point _timestamp_of_last_debug_p;
+
+        boost::array<std::uint8_t, 512> _input_buff{};
+    };
+}
+
+#
+#endif // __SURREYAEROCOMMS_H__
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp
index 04902c91..5e9f6700 100644
--- a/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp
+++ b/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp
@@ -1,8 +1,7 @@
 #pragma once
 
 #include <Configurable.hpp>
-#include <Logger.hpp>
-#include <MsgLogger.hpp>
+#include <Loggable.hpp>
 #include <StateEstimator.hpp>
 
 // protobuf
@@ -38,10 +37,14 @@ using SerialPort = boost::asio::serial_port;
 using loggertype = core::MsgLogger<std::shared_ptr<google::protobuf::Message>>;
 
 namespace comms {
-    class VNDriver : public core::common::Configurable
+    class VNDriver : public core::common::Loggable<std::shared_ptr<google::protobuf::Message>>,
+                     public core::common::Configurable
     {
         public:
-            VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr<loggertype> message_logger, ::core::StateEstimator &state_estimator, boost::asio::io_context &io_context);
+            VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr<core::StateEstimator> state_estimator, boost::asio::io_context &io_context, bool &init_successful); 
+            ~VNDriver() {
+                spdlog::info("destructed %s", this->get_name());
+            }
             bool init();
             struct config {
                 int baud_rate;
@@ -49,22 +52,19 @@ namespace comms {
             };
 
         private: 
-            // Private variables
-            core::Logger& _logger;
-            core::StateEstimator &_state_estimator;
+            std::shared_ptr<core::StateEstimator> _state_estimator;
 
 
             vn::protocol::uart::PacketFinder _processor;
             boost::array<std::uint8_t, 512> _output_buff;
             boost::array<std::uint8_t, 512> _input_buff;
             SerialPort _serial;
-            std::shared_ptr<loggertype> _message_logger; 
-            config _config;    
+            config _config;
 
         public: 
             // Public methods
             void log_proto_message(std::shared_ptr<google::protobuf::Message> msg);  
-        
+            
         private:
             // Private methods
             static void _handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts);
diff --git a/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp b/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp
index 33fa7fb6..da3cdf2b 100644
--- a/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp
+++ b/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp
@@ -27,11 +27,13 @@ namespace core
     public:
         FoxgloveWSServer() = delete;
 
-        FoxgloveWSServer(std::vector<core::common::Configurable *> configurable_components);
+        FoxgloveWSServer(std::vector<std::weak_ptr<core::common::Configurable>> configurable_components);
         void send_live_telem_msg(std::shared_ptr<google::protobuf::Message> msg);
         ~FoxgloveWSServer()
         {
+            spdlog::info("stopping foxglove server");
             _server->stop();
+            spdlog::info("stopped foxglove server and destructed foxglove server");
         }
 
     private:
@@ -61,7 +63,7 @@ namespace core
         std::optional<foxglove::Parameter> _convert_foxglove_param(core::common::Configurable::ParamTypes curr_param_val, foxglove::Parameter incoming_param);
         void _handle_foxglove_send();
     private:
-        std::vector<core::common::Configurable *> _components;
+        std::vector<std::weak_ptr<core::common::Configurable>> _components;
         std::unique_ptr<foxglove::ServerInterface<websocketpp::connection_hdl>> _server;
         std::function<void(foxglove::WebSocketLogLevel, char const *)> _log_handler;
         foxglove::ServerOptions _server_options;
diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp
index 088ff636..709e57bb 100644
--- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp
+++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp
@@ -26,9 +26,11 @@ std::string comms::CANDriver::_to_lowercase(std::string s) {
     return s;
 }
 comms::CANDriver::~CANDriver() {
+    spdlog::info("destructing CANDriver %s", this->get_name());
     _running = false;
     _input_deque_ref.cv.notify_all();
     _output_thread.join();
+    spdlog::info("destructed CANDriver %s", this->get_name());
 }
 bool comms::CANDriver::init() {
     auto canbus_device = get_parameter_value<std::string>("canbus_device");
@@ -37,13 +39,10 @@ bool comms::CANDriver::init() {
 
 
     if (!(canbus_device && dbc_file_path)) {
-        _logger.log_string("couldnt get params", core::LogLevel::ERROR);
+        spdlog::error("couldnt get params");
         return false;
     } else if (!std::filesystem::exists(*dbc_file_path)) {
-        std::string msg("params file does not exist! ");
-        msg += " ";
-        msg += (*dbc_file_path);
-        _logger.log_string(msg, core::LogLevel::ERROR);
+        spdlog::error("dbc file {} does not exist!", (*dbc_file_path));
         return false;
     }
     std::shared_ptr<dbcppp::INetwork> net;
@@ -58,22 +57,21 @@ bool comms::CANDriver::init() {
     }
 
     if (!_open_socket(*canbus_device)) {
-        _logger.log_string("couldnt open socket", core::LogLevel::ERROR);
+        spdlog::error("couldnt open socket");
         return false;
     }
 
-    _logger.log_string("inited, started read", core::LogLevel::INFO);
 
+    spdlog::info("inited, started read");
     _do_read();
+    set_configured();
     return true;
 }
 
 bool comms::CANDriver::_open_socket(const std::string &interface_name) {
     int raw_socket = ::socket(PF_CAN, SOCK_RAW, CAN_RAW);
     if (raw_socket < 0) {
-        auto err_str = std::string("Error creating CAN socket: ") + std::string(strerror(errno));
-        _logger.log_string(err_str.c_str(), core::LogLevel::ERROR);
-
+        spdlog::error("Error creating CAN socket: {}", std::string(strerror(errno)));
         return false;
     }
 
@@ -81,14 +79,12 @@ bool comms::CANDriver::_open_socket(const std::string &interface_name) {
     std::strcpy(ifr.ifr_name, interface_name.c_str());
     ioctl(raw_socket, SIOCGIFINDEX, &ifr);
 
-    struct sockaddr_can addr;
+    struct sockaddr_can addr{};
     addr.can_family = AF_CAN;
     addr.can_ifindex = ifr.ifr_ifindex;
 
     if (::bind(raw_socket, reinterpret_cast<sockaddr *>(&addr), sizeof(addr)) < 0) {
-        auto err_str = std::string("Error binding CAN socket: ") + std::string(strerror(errno));
-        _logger.log_string(err_str.c_str(), core::LogLevel::ERROR);
-
+        spdlog::error("Error binding CAN socket: {}", std::string(strerror(errno)));
         ::close(raw_socket);
         return false;
     }
@@ -122,8 +118,28 @@ void comms::CANDriver::_send_message(const struct can_frame &frame) {
 void comms::CANDriver::_handle_recv_CAN_frame(const struct can_frame &frame) {
     auto msg = pb_msg_recv(frame);
     if (msg) {
-        _state_estimator.handle_recv_process(msg);
-        _message_logger->log_msg(msg);
+        if(_state_estimator)
+        {
+            auto start_time = std::chrono::steady_clock::now();
+
+            _state_estimator->handle_recv_process(msg);
+
+            auto end_time = std::chrono::steady_clock::now();
+            auto duration_us = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time).count();
+            // Track maximum duration
+            if (duration_us > _recv_process_max_duration_us)
+            {
+                _recv_process_max_duration_us = duration_us;
+            }
+
+            if (++_recv_process_count % _recv_process_log_interval == 0)
+            {
+                spdlog::info("this CANDriver's handle_recv_process took {} us", duration_us);
+                spdlog::info("handle_recv_process WORST duration in last 1000 calls: {} us", _recv_process_max_duration_us);
+            }
+        }
+        
+        this->log(msg);
     }
 }
 
@@ -314,7 +330,7 @@ comms::CANDriver::_get_CAN_msg(std::shared_ptr<google::protobuf::Message> pb_msg
     can_frame frame{};
     std::string type_url = pb_msg->GetTypeName();
     std::string messageTypeName = type_url.substr(type_url.find_last_of('.') + 1);
-
+    
     if (_messages_names_and_ids.find(messageTypeName) != _messages_names_and_ids.end())
     {
         auto id = _messages_names_and_ids[messageTypeName];
@@ -370,7 +386,7 @@ comms::CANDriver::_get_CAN_msg(std::shared_ptr<google::protobuf::Message> pb_msg
                        field_value);
         }
     } else {
-        spdlog::warn("WARNING: not creating a frame to send due to not finding frame name");
+        spdlog::warn("not creating a frame to send due to not finding frame name");
         return std::nullopt;
     }
 
@@ -380,18 +396,19 @@ comms::CANDriver::_get_CAN_msg(std::shared_ptr<google::protobuf::Message> pb_msg
 void comms::CANDriver::_handle_send_msg_from_queue() {
     // we will assume that this queue only has messages that we want to send
     core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> q;
+
     while (_running) {
         {
+            spdlog::debug("looping _handle_send_msg_from_queue");
             std::unique_lock lk(_input_deque_ref.mtx);
-            // TODO unfuck this, queue management shouldnt live within the queue
-            // itself
+
             _input_deque_ref.cv.wait(
-                lk, [this]() { return !_input_deque_ref.deque.empty() || !_running; });
+                lk, [this]() { return !this->_input_deque_ref.deque.empty() || !this->_running; });
 
             if (_input_deque_ref.deque.empty()) {
+                spdlog::info("Returning, deque empty or not running.");
                 return;
             }
-
             q.deque = _input_deque_ref.deque;
             _input_deque_ref.deque.clear();
         }
@@ -399,10 +416,14 @@ void comms::CANDriver::_handle_send_msg_from_queue() {
         for (const auto &msg : q.deque)
         {
             auto can_msg = _get_CAN_msg(msg);
+            if (!can_msg) {
+                spdlog::error("Failed to generate CAN message from protobuf");
+                continue;
+            }
             if (can_msg)
             {
                 _send_message(*can_msg);
-                _message_logger->log_msg(msg);
+                this->log(msg);
             }
         }
         q.deque.clear();
diff --git a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp
index c9a41bac..e293485d 100644
--- a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp
+++ b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp
@@ -4,7 +4,7 @@
 grpc::Status DBInterfaceImpl::RequestStopLogging(grpc::ServerContext *context, const google::protobuf::Empty *rq, db_service::v1::service::LoggerStatus *response)
 {
     {
-        spdlog::warn("requested stopping of logging");
+        spdlog::info("requested stopping of logging");
         _logger_inst->stop_logging_to_file();
         auto status = _logger_inst->get_logger_status();
 
@@ -35,9 +35,20 @@ grpc::Status DBInterfaceImpl::RequestCurrentLoggerStatus(grpc::ServerContext *co
     }
 }
 
-DBInterfaceImpl::DBInterfaceImpl(std::shared_ptr<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>> logger_inst) : _logger_inst(logger_inst)
+grpc::Status DBInterfaceImpl::RequestControllerChange(grpc::ServerContext *context, const db_service::v1::service::DesiredController* rq, db_service::v1::service::ControllerChangeStatus* response)
 {
+    if(_mode_switch(static_cast<size_t>(rq->requested_controller_index()))){
+        response->set_status("switch");
+    }
+    else{
+        response->set_status("no switch");
+    }
+
+    return grpc::Status::OK;
 }
+
+DBInterfaceImpl::DBInterfaceImpl(std::function<bool(size_t)> mode_switch) : _mode_switch(mode_switch)
+        { }
 void DBInterfaceImpl::stop_server() {
     if (_server) {
         spdlog::warn("Shutting down the server...");
@@ -63,4 +74,4 @@ void DBInterfaceImpl::run_server()
     // Wait for the server to shutdown. Note that some other thread must be
     // responsible for shutting down the server for this call to ever return.
     _server->Wait();
-}
+}
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp
new file mode 100644
index 00000000..85cddb3d
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp
@@ -0,0 +1,84 @@
+#include "ETHSendComms.hpp"
+namespace comms
+{
+using boost::asio::ip::udp;
+ETHSendComms::ETHSendComms(boost::asio::io_context &io_context, uint16_t send_port, std::string send_ip, bool bind) 
+: _endp(boost::asio::ip::make_address(send_ip.c_str()), send_port),
+_socket(io_context)
+{
+    _socket.open(boost::asio::ip::udp::v4());
+
+    if (bind) {
+        _socket.bind(_endp);
+    }
+    _running = true;
+    _output_thread = std::thread(&comms::ETHSendComms::_handle_send_msg_from_queue, this);
+}
+
+ETHSendComms::~ETHSendComms()
+{
+    spdlog::info("destructing ETHSendComms");
+    _running = false;
+    _queue.cv.notify_all();
+    _output_thread.join();
+    spdlog::info("destructed ETHSendComms");
+}
+
+void ETHSendComms::enqueue_msg_to_send(std::shared_ptr<google::protobuf::Message> send_msg)
+{
+    {
+        spdlog::debug("enqueing msg");
+        std::unique_lock lk(_queue.mtx);
+        _queue.deque.push_back(send_msg);
+        _queue.cv.notify_all();
+    }
+}
+
+// ran within the thread
+void ETHSendComms::_handle_send_msg_from_queue()
+{
+    // we will assume that this queue only has messages that we want to send
+    while (_running)
+    {
+        {
+            std::unique_lock lk(_queue.mtx);
+            // TODO unfuck this, queue management shouldnt live within the queue itself
+            _queue.cv.wait(lk, [this]()
+                                        { return !_queue.deque.empty() || !_running; });
+
+            if (_queue.deque.empty())
+            {
+                return;
+            }
+            for (const auto &msg : _queue.deque)
+            {
+                _send_message(msg);
+                this->log(msg);
+            }
+            _queue.deque.clear();
+        }
+    }
+}
+
+// don do nothin
+void ETHSendComms::_send_completition_handler(std::array<uint8_t, 4096> /*message*/,
+                                        const boost::system::error_code & /*error*/,
+                                        std::size_t /*bytes_transferred*/)
+{
+
+}
+
+void ETHSendComms::_send_message(std::shared_ptr<google::protobuf::Message> msg_out)
+{
+    spdlog::info("sending msg over ethernet");
+    msg_out->SerializeToArray(_send_buffer.data(), msg_out->ByteSizeLong());
+    _socket.async_send_to(boost::asio::buffer(_send_buffer, msg_out->ByteSizeLong()),
+                            _endp,
+                            boost::bind(&ETHSendComms::_send_completition_handler,
+                                        this,
+                                        _send_buffer,
+                                        boost::asio::placeholders::error,
+                                        boost::asio::placeholders::bytes_transferred));
+}
+
+}
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp
deleted file mode 100644
index 73272f13..00000000
--- a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-#include <MCUETHComms.hpp>
-#include "hytech_msgs.pb.h"
-#include <spdlog/spdlog.h>
-
-
-using boost::asio::ip::udp;
-namespace comms
-{
-    MCUETHComms::MCUETHComms(core::Logger &logger,
-                             deqtype &in_deq,
-                             std::shared_ptr<loggertype> message_logger,
-                             core::StateEstimator &state_estimator,
-                             boost::asio::io_context &io_context,
-                             const std::string &send_ip,
-                             uint16_t recv_port,
-                             uint16_t send_port) : _logger(logger),
-                                                   _input_deque_ref(in_deq),
-                                                   _message_logger(message_logger),
-                                                   _socket(io_context, udp::endpoint(udp::v4(), recv_port)),
-                                                   _state_estimator(state_estimator),
-                                                   _send_port(send_port),
-                                                   _send_ip(send_ip)
-    {
-        _mcu_msg = std::make_shared<hytech_msgs::MCUOutputData>();
-        _logger.log_string("starting out thread", core::LogLevel::INFO);
-        _running = true;
-        _output_thread = std::thread(&MCUETHComms::_handle_send_msg_from_queue, this);
-        _logger.log_string("starting eth comms recv", core::LogLevel::INFO);
-        _start_receive();
-        _logger.log_string("started eth comms", core::LogLevel::INFO);
-    }
-    MCUETHComms::~MCUETHComms()
-    {
-        _running = false;
-        _input_deque_ref.cv.notify_all();
-        _output_thread.join();
-        spdlog::warn("destructed MCU ETH COMMS");
-    }
-
-    void MCUETHComms::_handle_send_msg_from_queue()
-    {
-        // we will assume that this queue only has messages that we want to send
-        while (_running)
-        {
-            {
-                std::unique_lock lk(_input_deque_ref.mtx);
-                // TODO unfuck this, queue management shouldnt live within the queue itself
-                _input_deque_ref.cv.wait(lk, [this]()
-                                         { return !_input_deque_ref.deque.empty() || !_running; });
-
-                if (_input_deque_ref.deque.empty())
-                {
-                    return;
-                }
-                for (const auto &msg : _input_deque_ref.deque)
-                {
-                    _send_message(msg);
-                    _message_logger->log_msg(msg);
-                }
-                _input_deque_ref.deque.clear();
-            }
-        }
-    }
-    void MCUETHComms::_send_message(std::shared_ptr<google::protobuf::Message> msg_out)
-    {
-        msg_out->SerializeToArray(_send_buffer.data(), msg_out->ByteSizeLong());
-        _socket.async_send_to(boost::asio::buffer(_send_buffer, msg_out->ByteSizeLong()),
-                              udp::endpoint(boost::asio::ip::make_address(_send_ip.c_str()), _send_port),
-                              boost::bind(&MCUETHComms::_handle_send,
-                                          this,
-                                          _send_buffer,
-                                          boost::asio::placeholders::error,
-                                          boost::asio::placeholders::bytes_transferred));
-    }
-
-    void MCUETHComms::_handle_receive(const boost::system::error_code &error, std::size_t size)
-    {
-
-        if (!error)
-        {
-            _mcu_msg->ParseFromArray(_recv_buffer.data(), size);
-            auto out_msg = static_cast<std::shared_ptr<google::protobuf::Message>>(_mcu_msg);
-            _state_estimator.handle_recv_process(out_msg);
-            _message_logger->log_msg(out_msg);
-            _start_receive();
-        }
-    }
-    void MCUETHComms::_handle_send(std::array<uint8_t, 2048> /*message*/,
-                                   const boost::system::error_code & /*error*/,
-                                   std::size_t /*bytes_transferred*/)
-    {
-    }
-
-    void MCUETHComms::_start_receive()
-    {
-        using namespace boost::placeholders;
-        _socket.async_receive_from(
-            boost::asio::buffer(_recv_buffer), _remote_endpoint,
-            boost::bind(&MCUETHComms::_handle_receive, this,
-                        boost::asio::placeholders::error,
-                        boost::asio::placeholders::bytes_transferred));
-    }
-}
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp
new file mode 100644
index 00000000..3f3ad7be
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp
@@ -0,0 +1,89 @@
+#include "ScaleComms.hpp"
+
+
+#include "JSONUtils.hpp"
+#include <hytech_msgs.pb.h>
+
+#include <regex>
+
+namespace comms {
+ScaleComms::ScaleComms(core::JsonFileHandler &json_file_handler, boost::asio::io_context &io)
+    : Configurable(json_file_handler, "ScaleComms"), _serial(io) {}
+
+bool ScaleComms::init()
+{
+    LOAD_PARAM_OR_FAIL(baud_rate, int, _config);
+    LOAD_PARAM_OR_FAIL(port_name, std::string, _config);
+
+    boost::system::error_code ec;
+    (void)_serial.open(_config.port_name, ec);
+    if (ec) {
+        spdlog::error("Error opening serial port for ScaleComms: {}", ec.message());
+        return false;
+    }
+
+    _configure_serial_port(_serial);
+
+    set_configured();
+    _start_receive();
+    return true;
+}
+
+void ScaleComms::_configure_serial_port(boost::asio::serial_port& serial) {
+    serial.set_option(boost::asio::serial_port::baud_rate(static_cast<unsigned int>(_config.baud_rate)));
+    serial.set_option(boost::asio::serial_port::character_size(8));
+    serial.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
+    serial.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));
+    serial.set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none));
+}
+
+std::optional<ScaleComms::ScaleData> ScaleComms::_parse_buffer(const boost::array<std::uint8_t, 512>& buffer, std::size_t bytes_count) {
+    
+    std::string input_data;
+    for (std::size_t i = 0; i < bytes_count; ++i) {
+        
+        if (std::isprint(_input_buff[i])) {
+            input_data += static_cast<char>(_input_buff[i]);
+        }
+    }
+    std::regex pattern(R"(1:\s*([+-]?[0-9]*[.]?[0-9]+)\s*2:\s*([+-]?[0-9]*[.]?[0-9]+)\s*3:\s*([+-]?[0-9]*[.]?[0-9]+)\s*4:\s*([+-]?[0-9]*[.]?[0-9]+))");
+
+    std::smatch match;
+
+    if (std::regex_search(input_data, match, pattern)) {
+        ScaleData data = {};
+        data.corner_weights_lbs.FL = std::stod(match[1]);
+        data.corner_weights_lbs.FR = std::stod(match[2]);
+        data.corner_weights_lbs.RL = std::stod(match[3]);
+        data.corner_weights_lbs.RR = std::stod(match[4]);
+        spdlog::info("scale data fl {} fr {} rl {} rr {}", data.corner_weights_lbs.FL, data.corner_weights_lbs.FR, data.corner_weights_lbs.RL, data.corner_weights_lbs.RR);
+        return data;
+    } else {
+        spdlog::info("erm, no match for:{}", input_data);
+    }
+    return std::nullopt;
+}
+
+void ScaleComms::_log_proto_message(const ScaleData & data)
+{
+    auto msg_out = std::make_shared<hytech_msgs::WeighScaleData>();
+    msg_out->set_weight_lf(data.corner_weights_lbs.FL);
+    msg_out->set_weight_lr(data.corner_weights_lbs.FR);
+    msg_out->set_weight_rf(data.corner_weights_lbs.RL);
+    msg_out->set_weight_rr(data.corner_weights_lbs.RR);
+    this->log(msg_out);
+}
+
+void ScaleComms::_start_receive() {
+    _serial.async_read_some(
+    boost::asio::buffer(_input_buff),
+    [&](const boost::system::error_code &ec, std::size_t bytes_count) {
+        auto scale_data = _parse_buffer(_input_buff, bytes_count);
+        if(scale_data)
+        {
+            _log_proto_message(*scale_data);
+        }
+        _start_receive();
+    });
+}
+} // namespace comms
diff --git a/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp
new file mode 100644
index 00000000..3ad51d32
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp
@@ -0,0 +1,101 @@
+#include <Configurable.hpp>
+
+#include <JSONUtils.hpp>
+#include <SpeedTechComms.hpp>
+#include <cstddef>
+#include <spdlog/spdlog.h>
+
+namespace comms 
+{
+SpeedTechComms::SpeedTechComms(core::JsonFileHandler &json_file_handler,
+                               boost::asio::io_context &io_context)
+                                : core::common::Configurable(json_file_handler, "SpeedTechComms"),
+                                _serial(io_context)
+{
+}
+
+void SpeedTechComms::process_buffer(const boost::array<std::uint8_t, 512> &buff, std::size_t length)
+{
+    if(length == 17)
+    {
+        // modulo 256 checksum
+        std::uint8_t checksum =0;
+        for(size_t i = 2; i < 16; i++)
+        {
+            checksum += buff[i];
+        }
+        if (checksum != buff[16])
+        {
+            spdlog::warn("speedtech data seemingly corrupted, invalid checksum not logging");
+            return;
+        }
+
+        auto lap_count = static_cast<std::size_t>(buff[10]);
+
+        uint32_t lap_time_sec_int = ((buff[11] << 16) | (buff[12] << 8) | buff[13]);
+        uint16_t lap_time_msec_int = ((buff[14] << 8) | buff[15]);
+        float lap_time_seconds = static_cast<float>(lap_time_sec_int);
+        float lap_time_millis = static_cast<float>(lap_time_msec_int);
+        auto lap_time = lap_time_seconds + (lap_time_millis/1000.0f);
+
+        std::shared_ptr<hytech_msgs::SpeedTechLapTime> speed_tech_lap_time_msg = std::make_shared<hytech_msgs::SpeedTechLapTime>();
+        speed_tech_lap_time_msg->set_laptime(lap_time);
+        speed_tech_lap_time_msg->set_lapcount(lap_count);
+
+        spdlog::info("lapcount {} laptime {}", lap_count, lap_time);
+        this->log(speed_tech_lap_time_msg);
+
+    } else {
+        spdlog::warn("speedtech message byte length incorrect");
+    }
+}
+
+bool SpeedTechComms::init()
+{
+    LOAD_PARAM_OR_FAIL(baud_rate, int, _config);
+    LOAD_PARAM_OR_FAIL(port_name, std::string, _config);
+
+    boost::system::error_code ec;
+    auto ec_ret = _serial.open(_config.port_name, ec);
+
+    if (ec)
+    {
+        spdlog::warn("Error: {}", ec.message());
+        spdlog::info("failed to open speedtech serial port");
+        return false;
+    }
+    using SerialPort = boost::asio::serial_port;
+    _serial.set_option(SerialPort::baud_rate(_config.baud_rate));
+    _serial.set_option(SerialPort::character_size(8));
+    _serial.set_option(SerialPort::parity(SerialPort::parity::none));
+    _serial.set_option(SerialPort::stop_bits(SerialPort::stop_bits::one));
+    _serial.set_option(SerialPort::flow_control(SerialPort::flow_control::none));
+    
+    set_configured();
+
+    _start_recv();
+    return true;
+}
+
+void SpeedTechComms::_start_recv()
+{
+    _serial.async_read_some(
+        boost::asio::buffer(_input_buff),
+        [&](const boost::system::error_code &ec, std::size_t bytesCount) -> void
+        {
+            if (ec)
+            {
+                if (ec != boost::asio::error::operation_aborted)
+                {
+                    spdlog::error("speedtech comms ERROR: {}", ec.message());
+                }
+                return;
+            }
+            // _logger.log_string("logging", core::LogLevel::INFO);
+            process_buffer(_input_buff, bytesCount);
+            // Initiate another asynchronous read
+            _start_recv();
+        }
+    );
+}
+}
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_comms/src/SurreyAeroComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/SurreyAeroComms.cpp
new file mode 100644
index 00000000..a66ea44e
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_comms/src/SurreyAeroComms.cpp
@@ -0,0 +1,115 @@
+#include "SurreyAeroComms.hpp"
+
+#include <Configurable.hpp>
+#include <spdlog/spdlog.h>
+
+
+#include "JSONUtils.hpp"
+
+#include "hytech_msgs.pb.h"
+
+namespace comms {
+
+SurreyAeroComms::SurreyAeroComms(core::JsonFileHandler &json_file_handler,
+                                 boost::asio::io_context& io)
+    : Configurable(json_file_handler, "SurreyAeroComms"),
+        _serial(io) {}
+
+void SurreyAeroComms::_configure_serial_port(boost::asio::serial_port& serial) {
+    serial.set_option(boost::asio::serial_port::baud_rate(500000));
+    serial.set_option(boost::asio::serial_port::character_size(8));
+    serial.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
+    serial.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));
+    serial.set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none));
+}
+
+bool SurreyAeroComms::init() {
+
+    LOAD_PARAM_OR_FAIL(port_name, std::string, _config);
+    boost::system::error_code ec;
+    (void)_serial.open(_config.port_name, ec);
+    if (ec) {
+        spdlog::error("Error opening serial port: {}", ec.message());
+        return false;
+    }
+
+    _configure_serial_port(_serial);
+    if(!_send_command("@D"))
+    {
+        return false;
+    }
+
+    spdlog::info("Aero driver initialized, starting receive");
+    set_configured();
+
+    _start_receive();
+    return true;
+}
+
+
+void SurreyAeroComms::_start_receive() {
+    _serial.async_read_some(
+    boost::asio::buffer(_input_buff),
+    [&](const boost::system::error_code &ec, std::size_t bytesCount) {
+        auto opt_readings = _extract_sensor_readings(_input_buff);
+        if(opt_readings)
+        {
+            std::vector<float> sensor_readings = (*opt_readings);
+            _log_proto_message(sensor_readings);
+        }
+        
+        _start_receive();
+    });
+}
+
+std::optional<std::vector<float>> SurreyAeroComms::_extract_sensor_readings(const boost::array<std::uint8_t, 512>& buffer) {
+    std::vector<float> readings;
+    if (buffer[0] != '#') {
+        spdlog::error("invalid aero sensor frame received");
+        return std::nullopt;
+    }
+
+
+    auto now_time = std::chrono::steady_clock::now();
+
+    auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now_time - _timestamp_of_last_debug_p);
+
+    for (size_t i = 0; i < 8; ++i) {
+        float value;
+        std::memcpy(&value, &buffer[1 + i * 4], sizeof(float));
+        
+        readings.push_back(value);
+    }
+    
+    if(elapsed.count() > 100)
+    {
+        spdlog::debug("readings:");
+        for(auto read : readings)
+        {
+            spdlog::debug("{}", read);
+        }
+        _timestamp_of_last_debug_p = now_time;
+    }
+    
+    return readings;
+}
+
+void SurreyAeroComms::_log_proto_message(const std::vector<float>& readings) {
+    auto msg_out = std::make_shared<hytech_msgs::AeroData>();
+    for (float value : readings) {
+        msg_out->add_readings_pa(value);
+    }
+    log(msg_out);
+}
+
+bool SurreyAeroComms::_send_command(const std::string& command) {
+    boost::system::error_code ec;
+    boost::asio::write(_serial, boost::asio::buffer(command), ec);
+
+    if (ec) {
+        spdlog::error("Error sending aero surrey sensor command '{}': with error {}", command, ec.message());
+        return false;
+    }
+    return true;
+}
+}
diff --git a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp
index 7b908710..c1dd75e2 100644
--- a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp
+++ b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp
@@ -20,8 +20,7 @@ namespace comms
     bool VNDriver::init()
     {
         // Try to establish a connection to the driver
-        _logger.log_string("Opening vn driver.", core::LogLevel::INFO);
-
+        spdlog::info("Opening vn driver.");
         auto device_name = get_parameter_value<std::string>("device_name");
         _config.baud_rate = get_parameter_value<int>("baud_rate").value();
         _config.freq_divisor = get_parameter_value<int>("freq_divisor").value();
@@ -31,13 +30,13 @@ namespace comms
         
         boost::system::error_code ec;
 
-        _serial.open(device_name.value(), ec);
+        auto ec_ret = _serial.open(device_name.value(), ec);
 
         if (ec)
         {
             spdlog::warn("Error: {}", ec.message());
-            _logger.log_string("Failed to open vn driver device.", core::LogLevel::INFO);
-            return 1;
+            spdlog::info("failed to open vectornav serial port");
+            return false;
         }
 
         // Set the baud rate of the device along with other configs
@@ -48,32 +47,33 @@ namespace comms
         _serial.set_option(SerialPort::flow_control(SerialPort::flow_control::none));
 
         // Configures the binary outputs for the device
-        _logger.log_string("Configuring binary outputs.", core::LogLevel::INFO);
-
+        spdlog::info("Configuring binary outputs.");
         _configure_binary_outputs();
 
-        return 0;
+        set_configured();
+        return true;
     }
 
-    VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr<loggertype> message_logger, core::StateEstimator &state_estimator, boost::asio::io_context& io)
-        : core::common::Configurable(logger, json_file_handler, "VNDriver"),
-          _logger(logger),
+    VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr<core::StateEstimator> state_estimator, boost::asio::io_context& io, bool &init_not_successful)
+        : core::common::Configurable(json_file_handler, "VNDriver"),
           _state_estimator(state_estimator),
-          _message_logger(message_logger),
           _serial(io)
     {
-        init();
+        init_not_successful = !init();
 
         // Starts read
-        _logger.log_string("Starting vn driver recieve.", core::LogLevel::INFO);
-
-        _start_recieve();
+        if(!init_not_successful)
+        {
+            spdlog::info("Starting vn driver recieve.");
+            _start_recieve();
+        }
+        
     }
 
     void VNDriver::log_proto_message(std::shared_ptr<google::protobuf::Message> msg)
     {
-        _state_estimator.handle_recv_process(static_cast<std::shared_ptr<google::protobuf::Message>>(msg));
-        _message_logger->log_msg(static_cast<std::shared_ptr<google::protobuf::Message>>(msg));
+        _state_estimator->handle_recv_process(static_cast<std::shared_ptr<google::protobuf::Message>>(msg));
+        this->log(msg);
     }
 
     void VNDriver::_configure_binary_outputs()
@@ -90,7 +90,7 @@ namespace comms
             ImuGroup::IMUGROUP_UNCOMPACCEL,
             GpsGroup::GPSGROUP_NONE,
             AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY,
-            (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY),
+            (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU),
             GpsGroup::GPSGROUP_NONE);
 
         boost::asio::async_write(_serial,
@@ -120,7 +120,7 @@ namespace comms
                                      ImuGroup::IMUGROUP_UNCOMPACCEL,
                                      GpsGroup::GPSGROUP_NONE,
                                      AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY,
-                                     (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY),
+                                     (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU),
                                      GpsGroup::GPSGROUP_NONE))
             {
                 spdlog::warn("ERROR: packet is not what we want");
@@ -135,6 +135,7 @@ namespace comms
             uint16_t ins_status = packet.extractUint16();
             auto pos_lla = packet.extractVec3d();
             auto vel_body = packet.extractVec3f();
+            auto vel_uncertainty = packet.extractFloat();
 
             // Create the protobuf message to send
             std::shared_ptr<hytech_msgs::VNData> msg_out = std::make_shared<hytech_msgs::VNData>();
@@ -176,6 +177,8 @@ namespace comms
             vn_ins_msg->set_error_gnss((ins_status >> 6) & 0b1);
             vn_ins_msg->set_gnss_heading_ins((ins_status >> 8) & 0b1); 
             vn_ins_msg->set_gnss_compass((ins_status >> 9) & 0b1); 
+            vn_ins_msg->set_ins_mode_int((ins_status & 0b11));
+            vn_ins_msg->set_ins_vel_u(vel_uncertainty);
 
             this_instance->log_proto_message(static_cast<std::shared_ptr<google::protobuf::Message>>(msg_out));
         }
@@ -189,7 +192,7 @@ namespace comms
     {
         _serial.async_read_some(
             boost::asio::buffer(_input_buff),
-            [&](const boost::system::error_code &ec, std::size_t bytesCount)
+            [&](const boost::system::error_code &ec, std::size_t bytesCount) -> void
             {
                 if (ec)
                 {
diff --git a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp
index 72761dcd..45e2f98a 100644
--- a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp
+++ b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp
@@ -1,4 +1,5 @@
 #include <foxglove_server.hpp>
+#include <memory>
 #include <variant>
 #include <hytech_msgs.pb.h>
 #include <ProtobufUtils.hpp>
@@ -14,7 +15,7 @@
 
 #include <spdlog/spdlog.h>
 
-
+#include <MatlabModelAddHelper.hpp>
 static uint64_t nanosecondsSinceEpoch()
 {
     return uint64_t(std::chrono::duration_cast<std::chrono::nanoseconds>(
@@ -22,7 +23,7 @@ static uint64_t nanosecondsSinceEpoch()
                         .count());
 }
 
-core::FoxgloveWSServer::FoxgloveWSServer(std::vector<core::common::Configurable *> configurable_components) : _components(configurable_components)
+core::FoxgloveWSServer::FoxgloveWSServer(std::vector<std::weak_ptr<core::common::Configurable>> configurable_components) : _components(configurable_components)
 {
     _log_handler = [](foxglove::WebSocketLogLevel, char const *msg)
     {
@@ -70,14 +71,18 @@ core::FoxgloveWSServer::FoxgloveWSServer(std::vector<core::common::Configurable
     };
 
     // TODO make the .proto file name a parameter
+    std::vector<std::string> proto_names = {"hytech_msgs.proto", "hytech.proto"};
+
+    std::vector<std::string> gend_names = matlab_model_gen::get_proto_filenames();
+    proto_names.insert(proto_names.end(), gend_names.begin(), gend_names.end());
 
-    auto potential_id_map = util::generate_name_to_id_map({"hytech_msgs.proto", "hytech.proto"});
+    auto potential_id_map = util::generate_name_to_id_map(proto_names);
     if (potential_id_map)
     {
         _id_name_map = *potential_id_map;
     }
 
-    auto descriptors = util::get_pb_descriptors({"hytech_msgs.proto", "hytech.proto"});
+    auto descriptors = util::get_pb_descriptors(proto_names);
 
     std::vector<foxglove::ChannelWithoutId> channels;
 
@@ -96,6 +101,7 @@ core::FoxgloveWSServer::FoxgloveWSServer(std::vector<core::common::Configurable
         }
     }
 
+    
     auto res_ids = _server->addChannels(channels);
     _server->setHandlers(std::move(hdlrs));
     _server->start("0.0.0.0", 5555);
@@ -103,7 +109,6 @@ core::FoxgloveWSServer::FoxgloveWSServer(std::vector<core::common::Configurable
 
 void core::FoxgloveWSServer::send_live_telem_msg(std::shared_ptr<google::protobuf::Message> msg)
 {
-
     if (_id_name_map.find(msg->GetDescriptor()->name()) != _id_name_map.end())
     {
         auto msg_chan_id = _id_name_map[msg->GetDescriptor()->name()];
@@ -209,20 +214,24 @@ void core::FoxgloveWSServer::_set_db_param(foxglove::Parameter param_update)
     std::string param_name = param_update.getName().substr(split_pos + 1);
     std::string component_name = param_update.getName().substr(0, split_pos);
 
-    for (const auto component : _components)
+    for (const auto component_locked : _components)
     {
-        if (component->get_name() == component_name)
+        if(auto component = component_locked.lock())
         {
-
-            core::common::Configurable::ParamTypes curr_param_val = component->get_cached_param(param_name);
-            std::optional<foxglove::Parameter> converted_type = _convert_foxglove_param(curr_param_val, param_update);
-            if(converted_type)
+            if (component->get_name() == component_name)
             {
-                core::common::Configurable::ParamTypes val = _get_db_param(*converted_type);
-                component->handle_live_param_update(param_name, val);
+
+                core::common::Configurable::ParamTypes curr_param_val = component->get_cached_param(param_name);
+                std::optional<foxglove::Parameter> converted_type = _convert_foxglove_param(curr_param_val, param_update);
+                if(converted_type)
+                {
+                    core::common::Configurable::ParamTypes val = _get_db_param(*converted_type);
+                    component->handle_live_param_update(param_name, val);
+                }
+                return;
             }
-            return;
         }
+        
     }
     spdlog::warn("WARNING: could not find component {}", component_name);
 
@@ -231,17 +240,21 @@ void core::FoxgloveWSServer::_set_db_param(foxglove::Parameter param_update)
 std::vector<foxglove::Parameter> core::FoxgloveWSServer::_get_current_params()
 {
     std::vector<foxglove::Parameter> params;
-    for (const auto component : _components)
+    for (const auto component_locked : _components)
     {
-        std::unordered_map params_map = component->get_params_map();
-        std::string param_parent = component->get_name();
-        std::vector<std::string> param_names = component->get_param_names();
-        for (const auto &component_param_name : param_names)
+        if(auto component = component_locked.lock())
         {
-            std::string foxglove_param_id = param_parent + "/" + component_param_name;
-            foxglove::Parameter fxglove_param = _get_foxglove_param(foxglove_param_id, params_map[component_param_name]);
-            params.push_back(fxglove_param);
+            std::unordered_map params_map = component->get_params_map();
+            std::string param_parent = component->get_name();
+            std::vector<std::string> param_names = component->get_param_names();
+            for (const auto &component_param_name : param_names)
+            {
+                std::string foxglove_param_id = param_parent + "/" + component_param_name;
+                foxglove::Parameter fxglove_param = _get_foxglove_param(foxglove_param_id, params_map[component_param_name]);
+                params.push_back(fxglove_param);
+            }
         }
+        
     }
     return params;
 }
diff --git a/drivebrain_core_impl/drivebrain_control/include/Controllers.hpp b/drivebrain_core_impl/drivebrain_control/include/Controllers.hpp
index ba91a7f4..5290fc3f 100644
--- a/drivebrain_core_impl/drivebrain_control/include/Controllers.hpp
+++ b/drivebrain_core_impl/drivebrain_control/include/Controllers.hpp
@@ -1,2 +1,3 @@
 #pragma once
-#include <SimpleController.hpp>
+#include <SimpleSpeedController.hpp>
+#include <SimpleTorqueController.hpp>
diff --git a/drivebrain_core_impl/drivebrain_control/include/LoadCellVectoringTorqueController.hpp b/drivebrain_core_impl/drivebrain_control/include/LoadCellVectoringTorqueController.hpp
new file mode 100644
index 00000000..0b57f5e8
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_control/include/LoadCellVectoringTorqueController.hpp
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <Controller.hpp>
+#include <Configurable.hpp>
+#include <VehicleDataTypes.hpp>
+
+#include <hytech_msgs.pb.h>
+
+#include <utility>
+#include <mutex>
+
+namespace control
+{
+
+    // TODO make the output CAN message for the drivetrain, rpms telem is just a standin for now
+    class LoadCellVectoringTorqueController : public Controller<core::ControllerOutput, core::VehicleState>, public core::common::Configurable
+    {
+    public:
+        // rear_torque_scale:
+        // 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD
+
+        // regen_torque_scale:
+        // same as rear_torque_scale but applies to regen torque split. 0 = All regen
+        // torque on the front, 1 = 50/50, 2 = all regen torque on the rear
+        
+        struct config {
+            torque_nm max_torque;
+            torque_nm max_regen_torque;
+            float rear_torque_scale;  
+            float regen_torque_scale; 
+            speed_m_s positive_speed_set;
+            float max_power_kw;
+            int dt_rate_hz;
+            bool apply_vectoring_in_regen;
+        };
+        LoadCellVectoringTorqueController(core::JsonFileHandler &json_file_handler) : Configurable(json_file_handler, "LoadCellVectoringTorqueController") {}
+        float get_dt_sec() override { 
+            return (double) 1.0 / _config.dt_rate_hz;
+        }
+        bool init() override final;
+        core::ControllerOutput step_controller(const core::VehicleState &in) override;
+
+    private:
+        void _handle_param_updates(const std::unordered_map<std::string, core::common::Configurable::ParamTypes> &new_param_map);
+    private:
+        std::mutex _config_mutex;
+        config _config{};
+    };
+}
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_control/include/SimplePowerLimiter.hpp b/drivebrain_core_impl/drivebrain_control/include/SimplePowerLimiter.hpp
new file mode 100644
index 00000000..6caceb88
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_control/include/SimplePowerLimiter.hpp
@@ -0,0 +1,9 @@
+#include <VehicleDataTypes.hpp>
+
+namespace control {
+namespace util {
+core::SpeedControlOut apply_power_limit(core::SpeedControlOut current_control,
+                                        veh_vec<float> current_rpms, float max_power_kw);
+}
+
+} // namespace control
diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp
similarity index 57%
rename from drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp
rename to drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp
index ca7a6892..723957dc 100644
--- a/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp
+++ b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp
@@ -1,10 +1,7 @@
 #pragma once
 #include <Controller.hpp>
 #include <Configurable.hpp>
-#include <Logger.hpp>
 #include <hytech_msgs.pb.h>
-#include <Literals.hpp>
-#include <hytech.pb.h>
 #include <VehicleDataTypes.hpp>
 #include <utility>
 #include <mutex>
@@ -16,34 +13,36 @@ namespace control
 {
 
     // TODO make the output CAN message for the drivetrain, rpms telem is just a standin for now
-    class SimpleController : Controller<core::SpeedControlOut, core::VehicleState>, public core::common::Configurable
+    class SimpleSpeedController : public Controller<core::ControllerOutput, core::VehicleState>, public core::common::Configurable
     {
     public:
         // rear_torque_scale:
         // 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD
 
-        // regen_torque_scale:
+        // regen_torque_scale:`
         // same as rear_torque_scale but applies to regen torque split. 0 = All regen
         // torque on the front, 1 = 50/50, 2 = all regen torque on the rear
         
         struct config {
-        torque_nm max_torque;
-        torque_nm max_reg_torque;
-        float rear_torque_scale;  
-        float regen_torque_scale; 
-        speed_m_s positive_speed_set;
-    };
-        SimpleController(core::Logger &logger, core::JsonFileHandler &json_file_handler) : Configurable(logger, json_file_handler, "SimpleController") {}
+            torque_nm max_torque;
+            torque_nm max_reg_torque;
+            float rear_torque_scale;  
+            float regen_torque_scale; 
+            speed_m_s positive_speed_set;
+            float max_power_kw;
+            int dt_rate_hz;
+        };
+        SimpleSpeedController(core::JsonFileHandler &json_file_handler) : Configurable(json_file_handler, "SimpleSpeedController") {}
         float get_dt_sec() override { 
-            return (0.001); 
+            return (double) 1.0 / _config.dt_rate_hz;
         }
         bool init() override;
-        core::SpeedControlOut step_controller(const core::VehicleState &in) override;
+        core::ControllerOutput step_controller(const core::VehicleState &in) override;
 
     private:
         void _handle_param_updates(const std::unordered_map<std::string, core::common::Configurable::ParamTypes> &new_param_map);
     private:
         std::mutex _config_mutex;
-        config _config;
+        config _config{};
     };
 }
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp
new file mode 100644
index 00000000..1791addd
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp
@@ -0,0 +1,46 @@
+#pragma once
+#include <Controller.hpp>
+#include <Configurable.hpp>
+#include <hytech_msgs.pb.h>
+#include <VehicleDataTypes.hpp>
+#include <utility>
+#include <mutex>
+
+// ABOUT: this controller is an implementation of mode 0 but with torque type shit 
+namespace control
+{
+    class SimpleTorqueController : public Controller<core::ControllerOutput, core::VehicleState>, public core::common::Configurable
+    {
+    public:
+        // rear_torque_scale:
+        // 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD
+
+        // regen_torque_scale:
+        // same as rear_torque_scale but applies to regen torque split. 0 = All regen
+        // torque on the front, 1 = 50/50, 2 = all regen torque on the rear
+        
+        struct config {
+            torque_nm max_torque;
+            torque_nm max_reg_torque;
+            float rear_torque_scale;  
+            float regen_torque_scale;
+        };
+        SimpleTorqueController(core::JsonFileHandler &json_file_handler) : Configurable(json_file_handler, "SimpleTorqueController") {}
+        float get_dt_sec() override { 
+            return (0.001); 
+        }
+        bool init() override;
+        core::ControllerOutput step_controller(const core::VehicleState &in) override;
+
+        void get_config() {
+            std::cout << "SimpleTorqueController config: " << std::endl;
+            std::cout << "max_torque: " << _config.max_torque << std::endl;
+            std::cout << "max_reg_torque: " << _config.max_reg_torque << std::endl;
+        }
+    private:
+        void _handle_param_updates(const std::unordered_map<std::string, core::common::Configurable::ParamTypes> &new_param_map);
+    private:
+        std::mutex _config_mutex;
+        config _config;
+    };
+}
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp b/drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp
new file mode 100644
index 00000000..118fd759
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp
@@ -0,0 +1,114 @@
+#include "LoadCellVectoringTorqueController.hpp"
+#include "SimplePowerLimiter.hpp"
+
+#include <variant>
+
+#include <VehicleDataTypes.hpp>
+#include <JSONUtils.hpp>
+
+#include <spdlog/spdlog.h>
+
+void control::LoadCellVectoringTorqueController::_handle_param_updates(const std::unordered_map<std::string, core::common::Configurable::ParamTypes> &new_param_map)
+{
+    HANDLE_LIVE_PARAM_LOCK_AND_LOAD(max_torque, float, _config, _config_mutex, new_param_map);
+    HANDLE_LIVE_PARAM_LOCK_AND_LOAD(max_regen_torque, float, _config, _config_mutex, new_param_map);
+    HANDLE_LIVE_PARAM_LOCK_AND_LOAD(rear_torque_scale, float, _config, _config_mutex, new_param_map);
+    HANDLE_LIVE_PARAM_LOCK_AND_LOAD(regen_torque_scale, float, _config, _config_mutex, new_param_map);
+    HANDLE_LIVE_PARAM_LOCK_AND_LOAD(positive_speed_set, float, _config, _config_mutex, new_param_map);
+    HANDLE_LIVE_PARAM_LOCK_AND_LOAD(max_power_kw, float, _config, _config_mutex, new_param_map);
+    HANDLE_LIVE_PARAM_LOCK_AND_LOAD(dt_rate_hz, int, _config, _config_mutex, new_param_map);
+    HANDLE_LIVE_PARAM_LOCK_AND_LOAD(apply_vectoring_in_regen, bool, _config, _config_mutex, new_param_map);
+}
+
+bool control::LoadCellVectoringTorqueController::init()
+{
+    LOAD_LIVE_PARAM_OR_FAIL(max_torque, float, _config);
+    LOAD_LIVE_PARAM_OR_FAIL(max_regen_torque, float, _config);
+    LOAD_LIVE_PARAM_OR_FAIL(rear_torque_scale, float, _config);
+    LOAD_LIVE_PARAM_OR_FAIL(regen_torque_scale, float, _config);
+    LOAD_LIVE_PARAM_OR_FAIL(positive_speed_set, float, _config);
+    LOAD_LIVE_PARAM_OR_FAIL(max_power_kw, float, _config);
+    LOAD_LIVE_PARAM_OR_FAIL(dt_rate_hz, int, _config);
+    LOAD_LIVE_PARAM_OR_FAIL(apply_vectoring_in_regen, bool, _config);
+    param_update_handler_sig.connect(boost::bind(&control::LoadCellVectoringTorqueController::_handle_param_updates, this, std::placeholders::_1));
+    
+    set_configured();
+    return true;
+}
+
+core::ControllerOutput control::LoadCellVectoringTorqueController::step_controller(const core::VehicleState &in)
+{
+    config cur_config;
+    {
+        std::unique_lock lk(_config_mutex);
+        cur_config = _config;
+    }
+
+    // Both pedals are not pressed and no implausibility has been detected
+    // accelRequest goes between 1.0 and -1.0
+    float accelRequest = (in.input.requested_accel) - (in.input.requested_brake);
+
+    veh_vec<float> current_rpms = in.current_rpms;
+
+    torque_nm torqueRequest = {};
+
+    core::SpeedControlOut type_set = {};
+    core::ControllerOutput cmd_out = {};
+    cmd_out.out = type_set;
+    auto& speed_out = std::get<core::SpeedControlOut>(cmd_out.out);
+    speed_out = {};
+    speed_out.mcu_recv_millis = in.prev_MCU_recv_millis; // heartbeat TODO this isnt needed any more, prob should remove
+    
+    
+    float sum_normal = in.loadcells.FL + in.loadcells.FR + in.loadcells.RL+ in.loadcells.RR;
+    if (accelRequest >= 0.0)
+    {
+        // Positive torque request
+        torqueRequest = ((float)accelRequest) * cur_config.max_torque;
+        
+        float accel_torque_pool = accelRequest * cur_config.max_torque * 4; 
+        
+        
+        auto max_rpm = cur_config.positive_speed_set * constants::METERS_PER_SECOND_TO_RPM;
+        speed_out.desired_rpms.FL = max_rpm;
+        speed_out.desired_rpms.FR = max_rpm;
+        speed_out.desired_rpms.RL = max_rpm;
+        speed_out.desired_rpms.RR = max_rpm;
+
+        speed_out.torque_lim_nm.FL = ((2.0 - cur_config.rear_torque_scale) * accel_torque_pool * (in.loadcells.FL / sum_normal) );
+        speed_out.torque_lim_nm.FR = ((2.0 - cur_config.rear_torque_scale) * accel_torque_pool * (in.loadcells.FR / sum_normal) );
+        speed_out.torque_lim_nm.RL = (cur_config.rear_torque_scale * accel_torque_pool * (in.loadcells.RL / sum_normal) );
+        speed_out.torque_lim_nm.RR = (cur_config.rear_torque_scale * accel_torque_pool * (in.loadcells.RR / sum_normal) );
+        cmd_out.out = control::util::apply_power_limit(speed_out, in.current_rpms, cur_config.max_power_kw);
+    }
+    else
+    {
+        // Negative torque request
+        
+        float regen_torque_pool = accelRequest * cur_config.max_regen_torque * 4 * -1.0; 
+        
+        speed_out.desired_rpms.FL = 0;
+        speed_out.desired_rpms.FR = 0;
+        speed_out.desired_rpms.RL = 0;
+        speed_out.desired_rpms.RR = 0;
+        
+        if(cur_config.apply_vectoring_in_regen)
+        {
+            speed_out.torque_lim_nm.FL = (regen_torque_pool * (in.loadcells.FL / sum_normal) * (2.0 - cur_config.rear_torque_scale));
+            speed_out.torque_lim_nm.FR = (regen_torque_pool * (in.loadcells.FR / sum_normal) * (2.0 - cur_config.rear_torque_scale));
+            speed_out.torque_lim_nm.RL = (regen_torque_pool * (in.loadcells.RL / sum_normal) * cur_config.rear_torque_scale);
+            speed_out.torque_lim_nm.RR = (regen_torque_pool * (in.loadcells.RR / sum_normal) * cur_config.rear_torque_scale);
+            cmd_out.out = speed_out; // no need to apply power limit for regen request
+        } else {
+            float reg_torq = -1.0 * accelRequest * cur_config.max_regen_torque;
+            speed_out.torque_lim_nm.FL = ( reg_torq * ( 2.0 - cur_config.rear_torque_scale) );
+            speed_out.torque_lim_nm.FR = ( reg_torq * ( 2.0 - cur_config.rear_torque_scale) );
+            speed_out.torque_lim_nm.RL = ( reg_torq * cur_config.rear_torque_scale);
+            speed_out.torque_lim_nm.RR = ( reg_torq * cur_config.rear_torque_scale);
+            cmd_out.out = speed_out; // no need to apply power limit for regen request
+        }
+        
+    }
+
+    return cmd_out;
+}
diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp
deleted file mode 100644
index 97647736..00000000
--- a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp
+++ /dev/null
@@ -1,116 +0,0 @@
-#include <SimpleController.hpp>
-#include <variant>
-#include <VehicleDataTypes.hpp>
-#include <spdlog/spdlog.h>
-
-void control::SimpleController::_handle_param_updates(const std::unordered_map<std::string, core::common::Configurable::ParamTypes> &new_param_map)
-{
-    // TODO make this easier to work with, rn variants can shift between any of the param types at runtime in the cache
-    if (auto pval = std::get_if<float>(&new_param_map.at("max_torque")))
-    {   
-        
-        std::unique_lock lk(_config_mutex);
-        _config.max_torque = *pval;
-        spdlog::info("Setting new max torque: {}", _config.max_torque);
-    }
-
-    if (auto pval = std::get_if<float>(&new_param_map.at("max_regen_torque")))
-    {
-        std::unique_lock lk(_config_mutex);
-        _config.max_reg_torque = *pval;
-        spdlog::info("Setting new max regen torque: {}", _config.max_reg_torque);
-    }
-
-    if (auto pval = std::get_if<float>(&new_param_map.at("rear_torque_scale")))
-    {
-        std::unique_lock lk(_config_mutex);
-        _config.rear_torque_scale = *pval;
-        spdlog::info("Setting new rear torque scale: {}", _config.rear_torque_scale);
-    }
-
-    if (auto pval = std::get_if<float>(&new_param_map.at("regen_torque_scale")))
-    {
-        std::unique_lock lk(_config_mutex);
-        _config.regen_torque_scale = *pval;
-        spdlog::info("Setting new regen torque scale: {}", _config.regen_torque_scale);
-    }
-
-    if (auto pval = std::get_if<float>(&new_param_map.at("positive_speed_set")))
-    {
-        std::unique_lock lk(_config_mutex);
-        _config.positive_speed_set = *pval;
-        spdlog::info("Setting new positive speed set: {}", _config.positive_speed_set);
-    }
-}
-
-bool control::SimpleController::init()
-{
-    std::optional max_torque = get_live_parameter<torque_nm>("max_torque");
-    std::optional max_regen_torque = get_live_parameter<torque_nm>("max_regen_torque");
-    std::optional rear_torque_scale = get_live_parameter<float>("rear_torque_scale");
-    std::optional regen_torque_scale = get_live_parameter<float>("regen_torque_scale");
-    std::optional positive_speed_set = get_live_parameter<speed_m_s>("positive_speed_set");
-
-    if (!(max_torque && max_regen_torque && rear_torque_scale && regen_torque_scale && positive_speed_set))
-    {
-        return false;
-    }
-
-    _config = {*max_torque, *max_regen_torque, *rear_torque_scale, *regen_torque_scale, *positive_speed_set};
-
-    param_update_handler_sig.connect(boost::bind(&control::SimpleController::_handle_param_updates, this, std::placeholders::_1));
-
-    return true;
-}
-
-core::SpeedControlOut control::SimpleController::step_controller(const core::VehicleState &in)
-{
-    config cur_config;
-    {
-        std::unique_lock lk(_config_mutex);
-        cur_config = _config;
-    }
-
-    // Both pedals are not pressed and no implausibility has been detected
-    // accelRequest goes between 1.0 and -1.0
-    float accelRequest = (in.input.requested_accel) - (in.input.requested_brake);
-
-    torque_nm torqueRequest;
-
-    // hytech_msgs::MCUCommandData cmd_out;
-    core::SpeedControlOut cmd_out;
-    cmd_out.mcu_recv_millis = in.prev_MCU_recv_millis; // heartbeat
-
-    if (accelRequest >= 0.0)
-    {
-        // Positive torque request
-        torqueRequest = ((float)accelRequest) * cur_config.max_torque;
-
-        auto max_rpm = cur_config.positive_speed_set * constants::METERS_PER_SECOND_TO_RPM;
-        cmd_out.desired_rpms.FL = max_rpm;
-        cmd_out.desired_rpms.FR = max_rpm;
-        cmd_out.desired_rpms.RL = max_rpm;
-        cmd_out.desired_rpms.RR = max_rpm;
-
-        cmd_out.torque_lim_nm.FL = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
-        cmd_out.torque_lim_nm.FR = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
-        cmd_out.torque_lim_nm.RL = (torqueRequest * cur_config.rear_torque_scale);
-        cmd_out.torque_lim_nm.RR = (torqueRequest * cur_config.rear_torque_scale);
-    }
-    else
-    {
-        // Negative torque request
-        torqueRequest = cur_config.max_reg_torque * accelRequest * -1.0;
-        cmd_out.desired_rpms.FL = 0;
-        cmd_out.desired_rpms.FR = 0;
-        cmd_out.desired_rpms.RL = 0;
-        cmd_out.desired_rpms.RR = 0;
-
-        cmd_out.torque_lim_nm.FL = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
-        cmd_out.torque_lim_nm.FR = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
-        cmd_out.torque_lim_nm.RL = (torqueRequest * cur_config.rear_torque_scale);
-        cmd_out.torque_lim_nm.RR = (torqueRequest * cur_config.rear_torque_scale);
-    }
-
-    return cmd_out;
-}
diff --git a/drivebrain_core_impl/drivebrain_control/src/SimplePowerLimiter.cpp b/drivebrain_core_impl/drivebrain_control/src/SimplePowerLimiter.cpp
new file mode 100644
index 00000000..d1517d88
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_control/src/SimplePowerLimiter.cpp
@@ -0,0 +1,81 @@
+#include "SimplePowerLimiter.hpp"
+
+#include <cmath>
+
+namespace control {
+namespace util {
+    core::SpeedControlOut apply_power_limit(core::SpeedControlOut current_control,
+                                             veh_vec<float> current_rpms, float max_power_kw) {
+    auto cmd_out = current_control;
+    // Apply power limit (basically a re-implementation of MCU)
+    float net_torque_mag = 0;
+    float net_power = 0;
+
+    net_torque_mag += ::fabs(cmd_out.torque_lim_nm.FL);
+    net_torque_mag += ::fabs(cmd_out.torque_lim_nm.FR);
+    net_torque_mag += ::fabs(cmd_out.torque_lim_nm.RL);
+    net_torque_mag += ::fabs(cmd_out.torque_lim_nm.RR);
+
+    net_power +=
+        ::fabs(cmd_out.torque_lim_nm.FL) * (current_rpms.FL * constants::RPM_TO_RAD_PER_SECOND);
+    net_power +=
+        ::fabs(cmd_out.torque_lim_nm.FR) * (current_rpms.FR * constants::RPM_TO_RAD_PER_SECOND);
+    net_power +=
+        ::fabs(cmd_out.torque_lim_nm.RL) * (current_rpms.RL * constants::RPM_TO_RAD_PER_SECOND);
+    net_power +=
+        ::fabs(cmd_out.torque_lim_nm.RR) * (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND);
+
+    auto max_power_watt = max_power_kw * 1000.0f;
+    // std::cout << "net power " << net_power <<std::endl;
+    if (net_power > max_power_watt) {
+        // std::cout << "erm why" << max_power_watt <<std::endl;
+        /* FL */
+        // 1. Calculate the torque percent (individual torque/total torque)
+        // 2. Multiply the torque percent by the power limit to ensure that all four powers add up
+        // to power limit
+        float torque_percent_FL = ::fabs(cmd_out.torque_lim_nm.FL / net_torque_mag);
+        float power_per_corner_FL = (torque_percent_FL * max_power_watt);
+
+        // 3. Divide power by rads per seconds to get torque per corner
+        cmd_out.torque_lim_nm.FL =
+            ::fabs(power_per_corner_FL / (current_rpms.FL * constants::RPM_TO_RAD_PER_SECOND));
+
+        /* FR */
+        // 1. Calculate the torque percent (individual torque/total torque)
+        // 2. Multiply the torque percent by the power limit to ensure that all four powers add up
+        // to power limit
+        float torque_percent_FR = ::fabs(cmd_out.torque_lim_nm.FR / net_torque_mag);
+        float power_per_corner_FR = (torque_percent_FR * max_power_watt);
+
+        // 3. Divide power by rads per seconds to get torque per corner
+        cmd_out.torque_lim_nm.FR =
+            ::fabs(power_per_corner_FR / (current_rpms.FR * constants::RPM_TO_RAD_PER_SECOND));
+
+        /* RL */
+        // 1. Calculate the torque percent (individual torque/total torque)
+        // 2. Multiply the torque percent by the power limit to ensure that all four powers add up
+        // to power limit
+        float torque_percent_RL = ::fabs(cmd_out.torque_lim_nm.RL / net_torque_mag);
+        float power_per_corner_RL = (torque_percent_RL * max_power_watt);
+
+        // 3. Divide power by rads per seconds to get torque per corner
+        cmd_out.torque_lim_nm.RL =
+            ::fabs(power_per_corner_RL / (current_rpms.RL * constants::RPM_TO_RAD_PER_SECOND));
+
+        /* RR */
+        // 1. Calculate the torque percent (individual torque/total torque)
+        // 2. Multiply the torque percent by the power limit to ensure that all four powers add up
+        // to power limit
+        float torque_percent_RR = ::fabs(cmd_out.torque_lim_nm.RR / net_torque_mag);
+        float power_per_corner_RR = (torque_percent_RR * max_power_watt);
+
+        // 3. Divide power by rads per seconds to get torque per corner
+        cmd_out.torque_lim_nm.RR =
+            ::fabs(power_per_corner_RR / (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND));
+    }
+
+    // std::cout <<"cmd_out.torque_lim_nm.FL " << cmd_out.torque_lim_nm.FL <<std::endl;
+    return cmd_out;
+}
+} // namespace util
+} // namespace control
diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp
new file mode 100644
index 00000000..c3d65032
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp
@@ -0,0 +1,143 @@
+#include <SimpleSpeedController.hpp>
+#include <variant>
+#include <VehicleDataTypes.hpp>
+#include <spdlog/spdlog.h>
+
+#include "SimplePowerLimiter.hpp"
+
+void control::SimpleSpeedController::_handle_param_updates(const std::unordered_map<std::string, core::common::Configurable::ParamTypes> &new_param_map)
+{
+        // TODO make this easier to work with, rn variants can shift between any of the param types at runtime in the cache
+    if (auto pval = std::get_if<float>(&new_param_map.at("max_torque")))
+    {   
+        
+        std::unique_lock lk(_config_mutex);
+        _config.max_torque = *pval;
+        spdlog::info("Setting new max torque: {}", _config.max_torque);
+    }
+
+    if (auto pval = std::get_if<float>(&new_param_map.at("max_regen_torque")))
+    {
+        std::unique_lock lk(_config_mutex);
+        _config.max_reg_torque = *pval;
+        spdlog::info("Setting new max regen torque: {}", _config.max_reg_torque);
+    }
+
+    if (auto pval = std::get_if<float>(&new_param_map.at("rear_torque_scale")))
+    {
+        std::unique_lock lk(_config_mutex);
+        _config.rear_torque_scale = *pval;
+        spdlog::info("Setting new rear torque scale: {}", _config.rear_torque_scale);
+    }
+
+    if (auto pval = std::get_if<float>(&new_param_map.at("regen_torque_scale")))
+    {
+        std::unique_lock lk(_config_mutex);
+        _config.regen_torque_scale = *pval;
+        spdlog::info("Setting new regen torque scale: {}", _config.regen_torque_scale);
+    }
+
+    if (auto pval = std::get_if<float>(&new_param_map.at("positive_speed_set")))
+    {
+        std::unique_lock lk(_config_mutex);
+        _config.positive_speed_set = *pval;
+        spdlog::info("Setting new positive speed set: {}", _config.positive_speed_set);
+    }
+    if (auto pval = std::get_if<float>(&new_param_map.at("max_power_kw")))
+    {
+        std::unique_lock lk(_config_mutex);
+        _config.max_power_kw = *pval;
+        spdlog::info("Setting new max power limit kw: {}", _config.max_power_kw);
+    }
+
+    if (auto pval = std::get_if<int>(&new_param_map.at("dt_rate_hz")))
+    {
+        std::unique_lock lk(_config_mutex);
+        _config.dt_rate_hz = *pval;
+        spdlog::info("Setting new dt_rate (in hertz) hz: {}", _config.dt_rate_hz);
+    }
+
+}
+
+bool control::SimpleSpeedController::init()
+{
+    std::optional max_torque = get_live_parameter<torque_nm>("max_torque");
+    std::optional max_regen_torque = get_live_parameter<torque_nm>("max_regen_torque");
+    std::optional rear_torque_scale = get_live_parameter<float>("rear_torque_scale");
+    std::optional regen_torque_scale = get_live_parameter<float>("regen_torque_scale");
+    std::optional positive_speed_set = get_live_parameter<speed_m_s>("positive_speed_set");
+    std::optional max_power_kw = get_live_parameter<float>("max_power_kw");
+    std::optional dt_rate_hz = get_live_parameter<int>("dt_rate_hz");
+
+    if (!(max_torque && max_regen_torque && rear_torque_scale && regen_torque_scale && positive_speed_set && max_power_kw && dt_rate_hz))
+    {
+        return false;
+    }
+
+    _config = {*max_torque, *max_regen_torque, *rear_torque_scale, *regen_torque_scale, *positive_speed_set, *max_power_kw, *dt_rate_hz};
+
+    param_update_handler_sig.connect(boost::bind(&control::SimpleSpeedController::_handle_param_updates, this, std::placeholders::_1));
+    // _configured = true;
+    set_configured();
+    return true;
+}
+
+core::ControllerOutput control::SimpleSpeedController::step_controller(const core::VehicleState &in)
+{
+    config cur_config;
+    {
+        std::unique_lock lk(_config_mutex);
+        cur_config = _config;
+    }
+
+    // Both pedals are not pressed and no implausibility has been detected
+    // accelRequest goes between 1.0 and -1.0
+    float accelRequest = (in.input.requested_accel) - (in.input.requested_brake);
+
+    veh_vec<float> current_rpms = in.current_rpms;
+
+    torque_nm torqueRequest = {};
+
+    core::SpeedControlOut type_set = {};
+    core::ControllerOutput cmd_out = {};
+    cmd_out.out = type_set;
+    auto& speed_out = std::get<core::SpeedControlOut>(cmd_out.out);
+    speed_out = {};
+    speed_out.mcu_recv_millis = in.prev_MCU_recv_millis; // heartbeat
+
+    if (accelRequest >= 0.0)
+    {
+        // Positive torque request
+        torqueRequest = ((float)accelRequest) * cur_config.max_torque;
+
+        auto max_rpm = cur_config.positive_speed_set * constants::METERS_PER_SECOND_TO_RPM;
+        speed_out.desired_rpms.FL = max_rpm;
+        speed_out.desired_rpms.FR = max_rpm;
+        speed_out.desired_rpms.RL = max_rpm;
+        speed_out.desired_rpms.RR = max_rpm;
+
+        speed_out.torque_lim_nm.FL = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
+        speed_out.torque_lim_nm.FR = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
+        speed_out.torque_lim_nm.RL = (torqueRequest * cur_config.rear_torque_scale);
+        speed_out.torque_lim_nm.RR = (torqueRequest * cur_config.rear_torque_scale);
+        cmd_out.out = control::util::apply_power_limit(speed_out, in.current_rpms, cur_config.max_power_kw);
+    }
+    else
+    {
+        // Negative torque request
+        torqueRequest = cur_config.max_reg_torque * accelRequest * -1.0;
+        speed_out.desired_rpms.FL = 0;
+        speed_out.desired_rpms.FR = 0;
+        speed_out.desired_rpms.RL = 0;
+        speed_out.desired_rpms.RR = 0;
+
+        speed_out.torque_lim_nm.FL = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
+        speed_out.torque_lim_nm.FR = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
+        speed_out.torque_lim_nm.RL = (torqueRequest * cur_config.rear_torque_scale);
+        speed_out.torque_lim_nm.RR = (torqueRequest * cur_config.rear_torque_scale);
+        cmd_out.out = speed_out; // no need to apply power limit for regen request
+    }
+
+    return cmd_out;
+}
+
diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp
new file mode 100644
index 00000000..1dd98e93
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp
@@ -0,0 +1,95 @@
+#include <SimpleTorqueController.hpp>
+#include <variant>
+#include <VehicleDataTypes.hpp>
+
+void control::SimpleTorqueController::_handle_param_updates(const std::unordered_map<std::string, core::common::Configurable::ParamTypes> &new_param_map)
+{
+    // TODO make this easier to work with, rn variants can shift between any of the param types at runtime in the cache
+    if (auto pval = std::get_if<float>(&new_param_map.at("max_torque")))
+    {   
+        
+        std::unique_lock lk(_config_mutex);
+        _config.max_torque = *pval;
+        std::cout << "setting new max torque " << _config.max_torque << std::endl;
+    }
+
+    if (auto pval = std::get_if<float>(&new_param_map.at("max_regen_torque")))
+    {
+        std::unique_lock lk(_config_mutex);
+        _config.max_reg_torque = *pval;
+        std::cout << "setting new max regen torque " << _config.max_reg_torque << std::endl;
+    }
+    if (auto pval = std::get_if<float>(&new_param_map.at("rear_torque_scale")))
+    {
+        std::unique_lock lk(_config_mutex);
+        _config.rear_torque_scale = *pval;
+        std::cout << "setting new rear_torque_scale " << _config.rear_torque_scale << std::endl;
+    }
+    if (auto pval = std::get_if<float>(&new_param_map.at("regen_torque_scale")))
+    {
+        std::unique_lock lk(_config_mutex);
+        _config.regen_torque_scale = *pval;
+        std::cout << "setting new regen_torque_scale " << _config.regen_torque_scale << std::endl;
+    }
+}
+
+bool control::SimpleTorqueController::init()
+{
+    auto max_torque = get_live_parameter<float>("max_torque");
+    auto max_regen_torque = get_live_parameter<float>("max_regen_torque");
+    auto rear_torque_scale = get_live_parameter<float>("rear_torque_scale");
+    auto regen_torque_scale = get_live_parameter<float>("regen_torque_scale");
+    if (!(max_torque && max_regen_torque && rear_torque_scale && regen_torque_scale))
+    {
+        std::cout << "ERROR: couldn't get params" << std::endl;
+        return false;
+    }
+
+    _config = {*max_torque, *max_regen_torque, *rear_torque_scale, *regen_torque_scale};
+
+    param_update_handler_sig.connect(boost::bind(&control::SimpleTorqueController::_handle_param_updates, this, std::placeholders::_1));
+    set_configured();
+    return true;
+}
+
+core::ControllerOutput control::SimpleTorqueController::step_controller(const core::VehicleState &in)
+{
+    config cur_config;
+    {
+        std::unique_lock lk(_config_mutex);
+        cur_config = _config;
+    }
+    // Both pedals are not pressed and no implausibility has been detected
+    // accelRequest goes between 1.0 and -1.0
+    float accelRequest = (in.input.requested_accel) - (in.input.requested_brake);
+
+    float torqueRequest;
+
+    core::TorqueControlOut _out;
+    core::ControllerOutput cmd_out;
+    cmd_out.out = _out;
+    auto& torque_out = std::get<core::TorqueControlOut>(cmd_out.out);
+
+    if (accelRequest >= 0.0)
+    {
+        // Positive torque request
+        torqueRequest = ((float)accelRequest) * cur_config.max_torque;
+
+        torque_out.desired_torques_nm.FL = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
+        torque_out.desired_torques_nm.FR = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
+        torque_out.desired_torques_nm.RL = (torqueRequest * cur_config.rear_torque_scale);
+        torque_out.desired_torques_nm.RR = (torqueRequest * cur_config.rear_torque_scale);
+    }
+    else
+    {
+        // Negative torque request
+        torqueRequest = cur_config.max_reg_torque * accelRequest * -1.0;
+
+        torque_out.desired_torques_nm.FL = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
+        torque_out.desired_torques_nm.FR = (torqueRequest * (2.0 - cur_config.rear_torque_scale));
+        torque_out.desired_torques_nm.RL = (torqueRequest * cur_config.rear_torque_scale);
+        torque_out.desired_torques_nm.RR = (torqueRequest * cur_config.rear_torque_scale);
+    }
+
+    return cmd_out;
+}
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_estimation/include/HTMath.hpp b/drivebrain_core_impl/drivebrain_estimation/include/HTMath.hpp
new file mode 100644
index 00000000..33b252eb
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_estimation/include/HTMath.hpp
@@ -0,0 +1,28 @@
+#ifndef __HTMATH_H__
+#define __HTMATH_H__
+
+#include <Utils.hpp>
+namespace math
+{
+    template <typename T>
+    float linear_approx(T x_val, T scale, T offset)
+    {
+        return ((static_cast<float>(x_val) * static_cast<float>(scale)) + (static_cast<float>(offset)));
+    }
+    template<typename T>
+    float normalize_linear_scale(T input_value, T min_val, T max_val, float new_min, float new_max)
+    {
+        if (max_val == min_val) return 0; // avoid divide by zero
+        float normalized = (static_cast<float>(input_value) - static_cast<float>(min_val)) / (static_cast<float>(max_val) - static_cast<float>(min_val));
+        float scaled = (new_min + (normalized * (new_max - new_min)));
+        return scaled;
+    }
+    template <typename T>
+    float linear_fit_normal(T input_value, T min_val, T max_val)
+    {
+        return normalize_linear_scale(input_value, min_val, max_val, 0, 1);
+    }
+    
+};
+
+#endif // __HTMATH_H__
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp
index d2c532b3..34196c17 100644
--- a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp
+++ b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp
@@ -11,6 +11,10 @@
 // - if a new message containing state information comes it (that has not arrived before), the state is "appended" to with the new value
 // - if a new message containing state information comes in that has been seen before, the previous state is over-written.
 //   each state update will have with it a timestamp
+#include <Loggable.hpp>
+#include <Configurable.hpp>
+#include <DriverBus.hpp>
+#include <VehicleDataTypes.hpp>
 
 #include <mutex>
 #include <thread>
@@ -26,12 +30,7 @@
 #include <google/protobuf/message.h>
 #include <google/protobuf/dynamic_message.h>
 
-#include <DriverBus.hpp>
-#include <VehicleDataTypes.hpp>
-#include <Logger.hpp>
-#include <MsgLogger.hpp>
 
-#include <Configurable.hpp>
 
 // while we can just have one queue input, if we allowed for multiple queue inputs that each have their own threads
 // that can update pieces of the state that would be optimal.
@@ -52,16 +51,44 @@
 // for now i will just move the state estimator into the estimation impl and call it a day 
 namespace core
 {
-    class StateEstimator
+    class StateEstimator : public core::common::Loggable<std::shared_ptr<google::protobuf::Message>>,
+                           public core::common::Configurable
     {
+        struct config
+        {
+            float fl_sus_pot_min;
+            float fl_sus_pot_min_mm;
+            float fl_sus_pot_max;
+            float fl_sus_pot_max_mm;
+            float fr_sus_pot_min;
+            float fr_sus_pot_min_mm;
+            float fr_sus_pot_max;
+            float fr_sus_pot_max_mm;
+            float rl_sus_pot_min;
+            float rl_sus_pot_min_mm;
+            float rl_sus_pot_max;
+            float rl_sus_pot_max_mm;
+            float rr_sus_pot_min;
+            float rr_sus_pot_min_mm;
+            float rr_sus_pot_max;
+            float rr_sus_pot_max_mm;
+            float fl_load_cell_offset;
+            float fl_load_cell_scale;
+            float fr_load_cell_offset;
+            float fr_load_cell_scale;
+            float rl_load_cell_offset;
+            float rl_load_cell_scale;
+            float rr_load_cell_offset;
+            float rr_load_cell_scale;
+        } _config;
 
     using loggertype = core::MsgLogger<std::shared_ptr<google::protobuf::Message>>;
 
     public:
         using tsq = core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>>;
-        StateEstimator(core::Logger &shared_logger, std::shared_ptr<loggertype> message_logger)
-        : _logger(shared_logger), _message_logger(message_logger)
-        //  _matlab_estimator(matlab_estimator)
+        StateEstimator(core::JsonFileHandler &json_file_handler) : 
+        
+        Configurable(json_file_handler, "StateEstimator")
         {
             _vehicle_state = {}; // initialize to all zeros
             _raw_input_data = {};
@@ -71,11 +98,14 @@ namespace core
             std::chrono::microseconds zero_start_time{0};
             _timestamp_array = {zero_start_time, zero_start_time, zero_start_time, zero_start_time};
         }
-        ~StateEstimator() = default;
+        ~StateEstimator();
 
         void handle_recv_process(std::shared_ptr<google::protobuf::Message> message);
         std::pair<core::VehicleState, bool> get_latest_state_and_validity();
-        void set_previous_control_output(SpeedControlOut prev_control_output);
+        void set_previous_control_output(ControllerOutput prev_control_output);
+        core::VehicleState append_state_variables_from_raw_inputs(core::VehicleState vs, core::RawInputData raw_inputs);
+        bool init() override final;
+
 
     private:
         void _recv_low_level_state(std::shared_ptr<google::protobuf::Message> message);
@@ -84,21 +114,29 @@ namespace core
         template <size_t ind, typename inverter_dynamics_msg>
         void _handle_set_inverter_dynamics(std::shared_ptr<google::protobuf::Message> msg);
 
+        template <size_t ind, typename inverter_temps_msg>
+        void _handle_set_inverter_temps(std::shared_ptr<google::protobuf::Message> msg);
+
         std::shared_ptr<hytech_msgs::VehicleData> _set_ins_state_data(core::VehicleState current_state, std::shared_ptr<hytech_msgs::VehicleData> msg_out);
 
         template <size_t arr_len>
         bool _validate_stamps(const std::array<std::chrono::microseconds, arr_len> &timestamp_arr);
+        std::shared_ptr<hytech_msgs::VehicleData> _set_computed_states(core::VehicleState current_state, std::shared_ptr<hytech_msgs::VehicleData> msg_out);
 
+        void _set_float_veh_vec_message_member(veh_vec<float> from, hytech_msgs::veh_vec_float * to_set);
     private:
-        
-        core::Logger &_logger;
+
         bool _run_recv_threads = false;
         std::mutex _state_mutex;
         core::VehicleState _vehicle_state;
         core::RawInputData _raw_input_data;
         std::array<std::chrono::microseconds, 4> _timestamp_array;
+        
         std::shared_ptr<loggertype> _message_logger;
 
+        std::chrono::seconds _last_debug_veh_state_print;
+        std::chrono::microseconds _debug_maxtime_diff{-1};
+        std::chrono::microseconds _debug_maxjitter_diff{-1};
     };
 }
 
diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp
index 3265154d..3f332226 100644
--- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp
+++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp
@@ -2,43 +2,77 @@
 
 #include <VehicleDataTypes.hpp>
 
-#include <chrono>
 #include <algorithm>
+#include <chrono>
 
 #include <google/protobuf/message.h>
 #include <memory>
 #include <mutex>
 
-#include <base_msgs.pb.h> // from HT_proto
+#include "HTMath.hpp"
+#include "hytech.pb.h"      // from HT_CAN
 #include "hytech_msgs.pb.h" // from HT_proto
-#include "hytech.pb.h" // from HT_CAN
+#include <base_msgs.pb.h>   // from HT_proto
+#include <spdlog/spdlog.h>
+
+#include "JSONUtils.hpp"
 
 using namespace core;
 
-void StateEstimator::handle_recv_process(std::shared_ptr<google::protobuf::Message> message)
+StateEstimator::~StateEstimator()
 {
-    if (message->GetTypeName() == "hytech_msgs.VNData")
-    {
+    spdlog::info("destructed StateEstimator");
+}
+
+// INV1_STATUS INV1_TEMPS INV1_DYNAMICS INV1_POWER INV1_FEEDBACK
+void StateEstimator::_recv_inverter_states(std::shared_ptr<google::protobuf::Message> msg) {
+    auto name = msg->GetTypeName();
+    if (name == "hytech.inv1_dynamics") {
+        _handle_set_inverter_dynamics<0, hytech::inv1_dynamics>(msg);
+    } else if (name == "hytech.inv2_dynamics") {
+        _handle_set_inverter_dynamics<1, hytech::inv2_dynamics>(msg);
+    } else if (name == "hytech.inv3_dynamics") {
+        _handle_set_inverter_dynamics<2, hytech::inv3_dynamics>(msg);
+    } else if (name == "hytech.inv4_dynamics") {
+        _handle_set_inverter_dynamics<3, hytech::inv4_dynamics>(msg);
+    } else if (name == "hytech.inv1_temps") {
+        _handle_set_inverter_temps<0, hytech::inv1_temps>(msg);
+    } else if (name == "hytech.inv2_temps") {
+        _handle_set_inverter_temps<1, hytech::inv2_temps>(msg);
+    } else if (name == "hytech.inv3_temps") {
+        _handle_set_inverter_temps<2, hytech::inv3_temps>(msg);
+    } else if (name == "hytech.inv4_temps") {
+        _handle_set_inverter_temps<3, hytech::inv4_temps>(msg);
+    } else if(name == "hytech.inv1_status") {
+        
+        auto in_msg = std::static_pointer_cast<hytech::inv1_status>(msg);
+        auto voltage = in_msg->dc_bus_voltage();
+        {
+            std::unique_lock lk(_state_mutex);
+            _vehicle_state.acc_data.pack_voltage = static_cast<float>(voltage);
+        }
+    }
+}
+
+void StateEstimator::handle_recv_process(std::shared_ptr<google::protobuf::Message> message) {
+    if (message->GetTypeName() == "hytech_msgs.VNData") {
         auto in_msg = std::static_pointer_cast<hytech_msgs::VNData>(message);
-        xyz_vec<float> body_vel_ms = {
-            (in_msg->vn_vel_m_s().x()),
-            (in_msg->vn_vel_m_s().y()),
-            (in_msg->vn_vel_m_s().z())};
-
-        xyz_vec<float> body_accel_mss = {
-            (in_msg->vn_linear_accel_m_ss().x()),
-            (in_msg->vn_linear_accel_m_ss().y()),
-            (in_msg->vn_linear_accel_m_ss().z())};
-
-        xyz_vec<float> angular_rate_rads = {
-            (in_msg->vn_angular_rate_rad_s().x()),
-            (in_msg->vn_angular_rate_rad_s().y()),
-            (in_msg->vn_angular_rate_rad_s().z())};
-
-        ypr_vec<float> ypr_rad = {
-            (in_msg->vn_ypr_rad().yaw()),
-            (in_msg->vn_ypr_rad().pitch()),
-            (in_msg->vn_ypr_rad().roll())};
+        xyz_vec<float> body_vel_ms = {(in_msg->vn_vel_m_s().x()), (in_msg->vn_vel_m_s().y()),
+                                      (in_msg->vn_vel_m_s().z())};
+
+        xyz_vec<float> body_accel_mss = {(in_msg->vn_linear_accel_m_ss().x()),
+                                         (in_msg->vn_linear_accel_m_ss().y()),
+                                         (in_msg->vn_linear_accel_m_ss().z())};
+
+        xyz_vec<float> angular_rate_rads = {(in_msg->vn_angular_rate_rad_s().x()),
+                                            (in_msg->vn_angular_rate_rad_s().y()),
+                                            (in_msg->vn_angular_rate_rad_s().z())};
+
+        ypr_vec<float> ypr_rad = {(in_msg->vn_ypr_rad().yaw()), (in_msg->vn_ypr_rad().pitch()),
+                                  (in_msg->vn_ypr_rad().roll())};
+
+        auto ins_mode_int = in_msg->status().ins_mode_int();
+        auto vel_u = in_msg->status().ins_vel_u();
 
         {
             std::unique_lock lk(_state_mutex);
@@ -46,6 +80,21 @@ void StateEstimator::handle_recv_process(std::shared_ptr<google::protobuf::Messa
             _vehicle_state.current_body_accel_mss = body_accel_mss;
             _vehicle_state.current_angular_rate_rads = angular_rate_rads;
             _vehicle_state.current_ypr_rad = ypr_rad;
+            _vehicle_state.ins_status.status_mode = ins_mode_int;
+            _vehicle_state.ins_status.vel_uncertainty = vel_u;
+        }
+    } else if (message->GetTypeName() == "hytech_msgs.VCRData_s") {
+        auto in_msg = std::static_pointer_cast<hytech_msgs::VCRData_s>(message);
+        bool is_rtd = (in_msg->status().vehicle_state() == hytech_msgs::VehicleState_e::READY_TO_DRIVE);
+        {
+            std::unique_lock lk(_state_mutex);
+            _vehicle_state.is_ready_to_drive = is_rtd;
+        }
+    } else if (message->GetTypeName() == "hytech_msgs.ACUAllData") {
+        auto in_msg = std::static_pointer_cast<hytech_msgs::ACUAllData>(message);
+        {
+            std::unique_lock lk(_state_mutex);
+            _vehicle_state.acc_data.min_cell_voltage = in_msg->core_data().min_cell_voltage();
         }
     }
     else {
@@ -53,76 +102,62 @@ void StateEstimator::handle_recv_process(std::shared_ptr<google::protobuf::Messa
     }
 }
 
-void StateEstimator::_recv_low_level_state(std::shared_ptr<google::protobuf::Message> message)
-{
+void StateEstimator::_recv_low_level_state(std::shared_ptr<google::protobuf::Message> message) {
     if (message->GetTypeName() == "hytech.rear_suspension") {
-        auto in_msg = std::static_pointer_cast<hytech::rear_suspension>(message);        
+        auto in_msg = std::static_pointer_cast<hytech::rear_suspension>(message);
         {
             std::unique_lock lk(_state_mutex);
-            _timestamp_array[0] = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
+            _timestamp_array[0] = std::chrono::duration_cast<std::chrono::microseconds>(
+                std::chrono::high_resolution_clock::now().time_since_epoch());
             _raw_input_data.raw_load_cell_values.RL = in_msg->rl_load_cell();
             _raw_input_data.raw_load_cell_values.RR = in_msg->rr_load_cell();
             _raw_input_data.raw_shock_pot_values.RL = in_msg->rl_shock_pot();
             _raw_input_data.raw_shock_pot_values.RR = in_msg->rr_shock_pot();
         }
-    } else if(message->GetTypeName() == "hytech.front_suspension")
-    {
+    } else if (message->GetTypeName() == "hytech.front_suspension") {
         auto in_msg = std::static_pointer_cast<hytech::front_suspension>(message);
         {
             std::unique_lock lk(_state_mutex);
-            _timestamp_array[1] = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
+            _timestamp_array[1] = std::chrono::duration_cast<std::chrono::microseconds>(
+                std::chrono::high_resolution_clock::now().time_since_epoch());
             _raw_input_data.raw_load_cell_values.FL = in_msg->fl_load_cell();
             _raw_input_data.raw_load_cell_values.FR = in_msg->fr_load_cell();
             _raw_input_data.raw_shock_pot_values.FL = in_msg->fl_shock_pot();
             _raw_input_data.raw_shock_pot_values.FR = in_msg->fr_shock_pot();
         }
-    } else if(message->GetTypeName() == "hytech.pedals_system_data")
-    {
+    } else if (message->GetTypeName() == "hytech.pedals_system_data") {
         auto in_msg = std::static_pointer_cast<hytech::pedals_system_data>(message);
         core::DriverInput input = {(in_msg->accel_pedal()), (in_msg->brake_pedal())};
         {
             std::unique_lock lk(_state_mutex);
-            _timestamp_array[2] = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
+            _timestamp_array[2] = std::chrono::duration_cast<std::chrono::microseconds>(
+                std::chrono::high_resolution_clock::now().time_since_epoch());
             _vehicle_state.input = input;
         }
-    } else if(message->GetTypeName() == "hytech.steering_data")
-    {
+    } else if (message->GetTypeName() == "hytech.steering_data") {
         auto in_msg = std::static_pointer_cast<hytech::steering_data>(message);
         {
             std::unique_lock lk(_state_mutex);
-            _timestamp_array[3] = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
+            _timestamp_array[3] = std::chrono::duration_cast<std::chrono::microseconds>(
+                std::chrono::high_resolution_clock::now().time_since_epoch());
             _raw_input_data.raw_steering_analog = in_msg->steering_analog_raw();
             _raw_input_data.raw_steering_digital = in_msg->steering_digital_raw();
         }
-    } else {
+    } else if (message->GetTypeName() == "hytech.em_measurement") {
+        auto in_msg = std::static_pointer_cast<hytech::em_measurement>(message);
+        float em_voltage = in_msg->em_voltage();
+        float em_current = in_msg->em_current();
+        {
+            std::unique_lock lk(_state_mutex);
+            _vehicle_state.old_energy_meter_kw = (-1.0f * em_voltage * em_current / 1000.0f); // on ht09 the energy meter is backwards as of right now
+        }
+    } 
+    else {
         _recv_inverter_states(message);
     }
 }
 
-// INV1_STATUS INV1_TEMPS INV1_DYNAMICS INV1_POWER INV1_FEEDBACK
-void StateEstimator::_recv_inverter_states(std::shared_ptr<google::protobuf::Message> msg)
-{
-    auto name = msg->GetTypeName();
-    if( name == "hytech.inv1_status" )
-    {
 
-    } else if(name == "hytech.inv2_status")
-    {
-
-    } else if(name == "hytech.inv1_dynamics")
-    {
-        _handle_set_inverter_dynamics<0, hytech::inv1_dynamics>(msg);
-    } else if(name == "hytech.inv2_dynamics")
-    {
-        _handle_set_inverter_dynamics<1, hytech::inv2_dynamics>(msg);
-    } else if(name == "hytech.inv3_dynamics")
-    {
-        _handle_set_inverter_dynamics<2, hytech::inv3_dynamics>(msg);
-    } else if(name == "hytech.inv4_dynamics")
-    {
-        _handle_set_inverter_dynamics<3, hytech::inv4_dynamics>(msg);
-    }
-}
 
 template <size_t ind, typename inverter_dynamics_msg>
 void StateEstimator::_handle_set_inverter_dynamics(std::shared_ptr<google::protobuf::Message> msg) {
@@ -132,19 +167,37 @@ void StateEstimator::_handle_set_inverter_dynamics(std::shared_ptr<google::proto
         _raw_input_data.raw_inverter_torques.set_from_index<ind>(in_msg->actual_torque_nm());
         _raw_input_data.raw_inverter_power.set_from_index<ind>(in_msg->actual_power_w());
         _vehicle_state.current_rpms.set_from_index<ind>(in_msg->actual_speed_rpm());
+        _vehicle_state.current_torques_nm = _raw_input_data.raw_inverter_torques;
+    }
+}
+
+template <size_t ind, typename inverter_temps_msg>
+void StateEstimator::_handle_set_inverter_temps(std::shared_ptr<google::protobuf::Message> msg) {
+    
+    
+    auto in_msg = std::static_pointer_cast<inverter_temps_msg>(msg);
+    core::DrivetrainData dt_data = {};
+    dt_data.inverter_igbt_temps_c.set_from_index<ind>(in_msg->igbt_temp());
+    dt_data.inverter_temps_c.set_from_index<ind>(in_msg->inverter_temp());
+    dt_data.inverter_motor_temps_c.set_from_index<ind>(in_msg->motor_temp());
+    {
+        std::unique_lock lk(_state_mutex);
+        _vehicle_state.dt_data = dt_data;
     }
 }
 
 // TODO parameterize the timeout threshold
 template <size_t arr_len>
-bool StateEstimator::_validate_stamps(const std::array<std::chrono::microseconds, arr_len> &timestamp_arr)
-{
+bool StateEstimator::_validate_stamps(
+    const std::array<std::chrono::microseconds, arr_len> &timestamp_arr) {
     std::array<std::chrono::microseconds, arr_len> timestamp_array_to_sort;
     {
         std::unique_lock lk(_state_mutex);
         timestamp_array_to_sort = timestamp_arr;
     }
-    const std::chrono::microseconds threshold(30000); // 30 milliseconds in microseconds
+
+    auto debug_copy = timestamp_array_to_sort;
+    const std::chrono::microseconds threshold(500000); // 500 milliseconds in microseconds
 
     // Sort the array
     std::sort(timestamp_array_to_sort.begin(), timestamp_array_to_sort.end());
@@ -154,22 +207,88 @@ bool StateEstimator::_validate_stamps(const std::array<std::chrono::microseconds
     auto max_stamp = timestamp_array_to_sort.back();
 
     bool within_threshold = (max_stamp - min_stamp) <= threshold;
+
+    auto curr_time = std::chrono::duration_cast<std::chrono::microseconds>(
+        std::chrono::high_resolution_clock::now().time_since_epoch());
+
+    constexpr std::chrono::seconds debug_print_period(1);
     
-    auto curr_time = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
     bool all_members_received = min_stamp.count() > 0; // count here is the count in microseconds
-    bool last_update_recent_enough = (std::chrono::duration_cast<std::chrono::microseconds>(curr_time - max_stamp)) < threshold;
+    bool last_update_recent_enough =
+        (std::chrono::duration_cast<std::chrono::microseconds>(curr_time - max_stamp)) < threshold;
+
+    if(std::chrono::duration_cast<std::chrono::microseconds>(curr_time - max_stamp) < _debug_maxtime_diff || (_debug_maxtime_diff.count() < 0))
+    {
+        _debug_maxtime_diff = std::chrono::duration_cast<std::chrono::microseconds>(curr_time - max_stamp);
+    }
+    if(std::chrono::duration_cast<std::chrono::microseconds>(max_stamp - min_stamp) < _debug_maxjitter_diff || (_debug_maxjitter_diff.count() < 0))
+    {
+        _debug_maxjitter_diff = std::chrono::duration_cast<std::chrono::microseconds>(max_stamp - min_stamp);
+    }
+
+    if((curr_time - _last_debug_veh_state_print) > debug_print_period)
+    {
+        _last_debug_veh_state_print = std::chrono::duration_cast<std::chrono::seconds>(curr_time);
+        spdlog::info("_debug_maxtime_diff: {}", _debug_maxtime_diff.count());
+        spdlog::info("_debug_maxjitter_diff: {}", _debug_maxjitter_diff.count());
+    }
 
+    if(!within_threshold)
+    {
+        spdlog::warn("data not recvd within time window theshold: {}", (max_stamp - min_stamp).count());
+    } else if(!all_members_received)
+    {
+        spdlog::warn("not all data recvd yet for state to be valid");
+    } else if(!last_update_recent_enough)
+    {
+        spdlog::warn("max timestamp message has been been recvd in time window {}", (curr_time - max_stamp).count());
+    }
+    
+
+    if(!(within_threshold && all_members_received && last_update_recent_enough))
+    {
+        spdlog::info("vcr suspension val: {}", debug_copy[0].count());
+        spdlog::info("vcf suspension val: {}", debug_copy[1].count());
+        spdlog::info("vcf pedals val: {}", debug_copy[2].count());
+        spdlog::info("steering data val: {}", debug_copy[3].count());
+    }
     return within_threshold && all_members_received && last_update_recent_enough;
 }
 
-void StateEstimator::set_previous_control_output(SpeedControlOut prev_control_output)
-{
+core::VehicleState
+StateEstimator::append_state_variables_from_raw_inputs(core::VehicleState vs,
+                                                       core::RawInputData raw_data) {
+    auto vehicle_state = vs;
+    vehicle_state.suspension_potentiometers_mm.FL = math::normalize_linear_scale(
+        raw_data.raw_shock_pot_values.FL, _config.fl_sus_pot_min, _config.fl_sus_pot_max,
+        _config.fl_sus_pot_min_mm, _config.fl_sus_pot_max_mm);
+    vehicle_state.suspension_potentiometers_mm.FR = math::normalize_linear_scale(
+        raw_data.raw_shock_pot_values.FR, _config.fr_sus_pot_min, _config.fr_sus_pot_max,
+        _config.fr_sus_pot_min_mm, _config.fr_sus_pot_max_mm);
+    vehicle_state.suspension_potentiometers_mm.RL = math::normalize_linear_scale(
+        raw_data.raw_shock_pot_values.RL, _config.rl_sus_pot_min, _config.rl_sus_pot_max,
+        _config.rl_sus_pot_min_mm, _config.rl_sus_pot_max_mm);
+    vehicle_state.suspension_potentiometers_mm.RR = math::normalize_linear_scale(
+        raw_data.raw_shock_pot_values.RR, _config.rr_sus_pot_min, _config.rr_sus_pot_max,
+        _config.rr_sus_pot_min_mm, _config.rr_sus_pot_max_mm);
+    
+    vehicle_state.loadcells.FL = raw_data.raw_load_cell_values.FL;
+    vehicle_state.loadcells.FR = raw_data.raw_load_cell_values.FR;
+    vehicle_state.loadcells.RL = raw_data.raw_load_cell_values.RL;
+    vehicle_state.loadcells.RR = raw_data.raw_load_cell_values.RR;
+
+    vehicle_state.steering_angle_deg = raw_data.raw_steering_analog;
+    return vehicle_state;
+}
+
+void StateEstimator::set_previous_control_output(core::ControllerOutput prev_control_output) {
     std::unique_lock lk(_state_mutex);
     _vehicle_state.prev_controller_output = prev_control_output;
 }
 
-std::shared_ptr<hytech_msgs::VehicleData> StateEstimator::_set_ins_state_data(core::VehicleState current_state, std::shared_ptr<hytech_msgs::VehicleData> msg_out)
-{
+std::shared_ptr<hytech_msgs::VehicleData>
+StateEstimator::_set_ins_state_data(core::VehicleState current_state,
+                                    std::shared_ptr<hytech_msgs::VehicleData> msg_out) {
     hytech_msgs::xyz_vector *current_body_vel_ms = msg_out->mutable_current_body_vel_ms();
     current_body_vel_ms->set_x(current_state.current_body_vel_ms.x);
     current_body_vel_ms->set_y(current_state.current_body_vel_ms.y);
@@ -180,7 +299,8 @@ std::shared_ptr<hytech_msgs::VehicleData> StateEstimator::_set_ins_state_data(co
     current_body_accel_mss->set_y(current_state.current_body_accel_mss.y);
     current_body_accel_mss->set_z(current_state.current_body_accel_mss.z);
 
-    hytech_msgs::xyz_vector *current_angular_rate_rads = msg_out->mutable_current_angular_rate_rads();
+    hytech_msgs::xyz_vector *current_angular_rate_rads =
+        msg_out->mutable_current_angular_rate_rads();
     current_angular_rate_rads->set_x(current_state.current_angular_rate_rads.x);
     current_angular_rate_rads->set_y(current_state.current_angular_rate_rads.y);
     current_angular_rate_rads->set_z(current_state.current_angular_rate_rads.z);
@@ -192,62 +312,141 @@ std::shared_ptr<hytech_msgs::VehicleData> StateEstimator::_set_ins_state_data(co
     return msg_out;
 }
 
-std::pair<core::VehicleState, bool> StateEstimator::get_latest_state_and_validity()
-{
+std::pair<core::VehicleState, bool> StateEstimator::get_latest_state_and_validity() {
     auto state_is_valid = _validate_stamps(_timestamp_array);
     auto state_estim_start = std::chrono::high_resolution_clock::now();
-    core::VehicleState current_state;
-    core::RawInputData current_raw_data;
+    core::VehicleState current_state = {};
+    core::RawInputData current_raw_data = {};
+
     auto state_mutex_start = std::chrono::high_resolution_clock::now();
     {
         std::unique_lock lk(_state_mutex);
+        _vehicle_state = append_state_variables_from_raw_inputs(_vehicle_state, _raw_input_data);
         current_state = _vehicle_state;
         current_raw_data = _raw_input_data;
     }
+
     auto state_mutex_end = std::chrono::high_resolution_clock::now();
 
     // Create the proto message to send
-    std::shared_ptr<hytech_msgs::VehicleData> msg_out = std::make_shared<hytech_msgs::VehicleData>();
-
-    msg_out->set_is_ready_to_drive(true);
+    std::shared_ptr<hytech_msgs::VehicleData> msg_out =
+        std::make_shared<hytech_msgs::VehicleData>();
 
+    msg_out->set_is_ready_to_drive(current_state.is_ready_to_drive);
+    
     hytech_msgs::SpeedControlIn *current_inputs = msg_out->mutable_current_inputs();
     current_inputs->set_accel_percent(current_state.input.requested_accel);
     current_inputs->set_brake_percent(current_state.input.requested_brake);
 
     msg_out = _set_ins_state_data(current_state, msg_out);
-
+    msg_out = _set_computed_states(current_state, msg_out);
     hytech_msgs::veh_vec_float *curr_rpms = msg_out->mutable_current_rpms();
     curr_rpms->set_fl(current_state.current_rpms.FL);
     curr_rpms->set_fr(current_state.current_rpms.FR);
     curr_rpms->set_rl(current_state.current_rpms.RL);
     curr_rpms->set_rr(current_state.current_rpms.RR);
 
+    hytech_msgs::veh_vec_float *curr_torqs = msg_out->mutable_current_torques_nm();
+    curr_torqs->set_fl(current_state.current_torques_nm.FL);
+    curr_torqs->set_fr(current_state.current_torques_nm.FR);
+    curr_torqs->set_rl(current_state.current_torques_nm.RL);
+    curr_torqs->set_rr(current_state.current_torques_nm.RR);
+
+
     msg_out->set_state_is_valid(state_is_valid);
     msg_out->set_steering_angle_deg(current_state.steering_angle_deg);
-
     auto prev_driver_torque_req = msg_out->mutable_driver_torque();
-    prev_driver_torque_req->set_fl(current_state.prev_controller_output.torque_lim_nm.FL);
-    prev_driver_torque_req->set_fr(current_state.prev_controller_output.torque_lim_nm.FR);
-    prev_driver_torque_req->set_rl(current_state.prev_controller_output.torque_lim_nm.RL);
-    prev_driver_torque_req->set_rr(current_state.prev_controller_output.torque_lim_nm.RR);
+
+    if (const core::TorqueControlOut *torqueControl =
+            std::get_if<core::TorqueControlOut>(&current_state.prev_controller_output.out)) {
+        // TODO: REPLACE THIS CODE BLOCK TO PUSH A TORQUE CONTROLLER STRUCT IN THE PROTOBUF MESSAGE,
+        // NOT A SPEED CONTROLLER STRUCT RIGHT NOW IT TREATS DESIRED TORQUE AS A TORQUE LIMIT
+        msg_out->set_is_using_torque_controller(
+            true); // add to message that we are using desired torque commands, not torque limits
+        _set_float_veh_vec_message_member(torqueControl->desired_torques_nm,
+                                          prev_driver_torque_req);
+
+    } else if (const core::SpeedControlOut *speedControl = std::get_if<core::SpeedControlOut>(
+                   &current_state.prev_controller_output
+                        .out)) { // assuming no monostate possible here
+        msg_out->set_is_using_torque_controller(false);
+        _set_float_veh_vec_message_member(speedControl->torque_lim_nm, prev_driver_torque_req);
+    }
+
+    msg_out->set_old_energy_meter_kw(current_state.old_energy_meter_kw);
 
     auto log_start = std::chrono::high_resolution_clock::now();
-    _message_logger->log_msg(static_cast<std::shared_ptr<google::protobuf::Message>>(msg_out));
-    auto log_end = std::chrono::high_resolution_clock::now();
     
+    this->log(msg_out);
+
+    auto log_end = std::chrono::high_resolution_clock::now();
+
     auto state_estim_end = std::chrono::high_resolution_clock::now();
 
-    auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(state_estim_end - state_estim_start);
-    
-    constexpr bool debug = false;
-    if ( (elapsed > std::chrono::microseconds(6000)) && debug ) // 6ms
+    auto elapsed =
+        std::chrono::duration_cast<std::chrono::microseconds>(state_estim_end - state_estim_start);
+
+    constexpr bool debug = true;
+    if ((elapsed > std::chrono::microseconds(4000)) && debug) // 4ms
     {
-        std::cout << "WARNING: timing" << std::endl;
-        std::cout << "total: " << (static_cast<float>(std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count())) << " us\n";
-        std::cout << "state mutex: " << (static_cast<float>(std::chrono::duration_cast<std::chrono::microseconds>(state_mutex_end - state_mutex_start).count())) << " us\n";
-        std::cout << "log time: " << (static_cast<float>(std::chrono::duration_cast<std::chrono::microseconds>(log_end - log_start).count())) << " us\n";
-    }
+        auto total_us = std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count();
+        auto state_mutex_us = std::chrono::duration_cast<std::chrono::microseconds>(state_mutex_end - state_mutex_start).count();
+        auto log_time_us = std::chrono::duration_cast<std::chrono::microseconds>(log_end - log_start).count();
+
+        spdlog::warn("WARNING: timing");
+        spdlog::warn("total: {} us", total_us);
+        spdlog::warn("state mutex: {} us", state_mutex_us);
+        spdlog::warn("log time: {} us", log_time_us);
+    } 
 
     return {current_state, state_is_valid};
+}
+
+bool StateEstimator::init() {
+    // SUS POT PARAMS
+    LOAD_PARAM_OR_FAIL(fl_sus_pot_min, float, _config);
+    LOAD_PARAM_OR_FAIL(fl_sus_pot_min_mm, float, _config);
+    LOAD_PARAM_OR_FAIL(fl_sus_pot_max, float, _config);
+    LOAD_PARAM_OR_FAIL(fl_sus_pot_max_mm, float, _config);
+    LOAD_PARAM_OR_FAIL(fr_sus_pot_min, float, _config);
+    LOAD_PARAM_OR_FAIL(fr_sus_pot_min_mm, float, _config);
+    LOAD_PARAM_OR_FAIL(fr_sus_pot_max, float, _config);
+    LOAD_PARAM_OR_FAIL(fr_sus_pot_max_mm, float, _config);
+    LOAD_PARAM_OR_FAIL(rl_sus_pot_min, float, _config);
+    LOAD_PARAM_OR_FAIL(rl_sus_pot_min_mm, float, _config);
+    LOAD_PARAM_OR_FAIL(rl_sus_pot_max, float, _config);
+    LOAD_PARAM_OR_FAIL(rl_sus_pot_max_mm, float, _config);
+    LOAD_PARAM_OR_FAIL(rr_sus_pot_min, float, _config);
+    LOAD_PARAM_OR_FAIL(rr_sus_pot_min_mm, float, _config);
+    LOAD_PARAM_OR_FAIL(rr_sus_pot_max, float, _config);
+    LOAD_PARAM_OR_FAIL(rr_sus_pot_max_mm, float, _config);
+    
+    // LOAD CELL CONFIGS
+    LOAD_PARAM_OR_FAIL(fl_load_cell_offset, float, _config);
+    LOAD_PARAM_OR_FAIL(fl_load_cell_scale, float, _config);
+    LOAD_PARAM_OR_FAIL(fr_load_cell_offset, float, _config);
+    LOAD_PARAM_OR_FAIL(fr_load_cell_scale, float, _config);
+    LOAD_PARAM_OR_FAIL(rl_load_cell_offset, float, _config);
+    LOAD_PARAM_OR_FAIL(rl_load_cell_scale, float, _config);
+    LOAD_PARAM_OR_FAIL(rr_load_cell_offset, float, _config);
+    LOAD_PARAM_OR_FAIL(rr_load_cell_scale, float, _config);
+    set_configured();
+    return true;
+}
+
+std::shared_ptr<hytech_msgs::VehicleData>
+StateEstimator::_set_computed_states(core::VehicleState current_state,
+                                     std::shared_ptr<hytech_msgs::VehicleData> msg_out) {
+    auto suspension_potentiometers_mm = msg_out->mutable_suspension_potentiometers_mm();
+    _set_float_veh_vec_message_member(current_state.suspension_potentiometers_mm,
+                                      suspension_potentiometers_mm);
+    return msg_out;
+}
+
+void StateEstimator::_set_float_veh_vec_message_member(veh_vec<float> from,
+                                                       hytech_msgs::veh_vec_float *to_set) {
+    to_set->set_fl(from.FL);
+    to_set->set_fr(from.FR);
+    to_set->set_rl(from.RL);
+    to_set->set_rr(from.RR);
 }
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_logging/README.md b/drivebrain_core_impl/drivebrain_logging/README.md
new file mode 100644
index 00000000..9e54258a
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_logging/README.md
@@ -0,0 +1,18 @@
+# overview
+
+the mcap logger handles logging of protobuf and json data. The structred data is logged on call from any other executing thread and on call enqueues a message to be logged by the writer thread. 
+
+the mcap logger is capable of opening and closing mcap files during run-time and upon opening of a new file immediately writes all schemas for all topics / channels that will be logged to. each topic / channel has a specific schema and the schemas are not required to be unique. each schema has an encoding type (json or protobuf) that gets used to tell how to interpret the schema which is required to decode a specific channel's messages.
+
+## protobuf message logging flow
+
+
+
+## parameter logging flow
+
+[example writing json messages / schemas](https://github.com/foxglove/mcap/tree/main/cpp/examples/jsonschema)
+
+the mcap logger's parameter logging function gets called at a fixed rate in the parameter logging thread OR when the thread gets notified of parameter change from the foxglove webserver's thread.
+
+the parameters being logged are a subset of the schema that gets written to the mcap file. the schemas is generated from the json file that gets loaded at runtime.
+
diff --git a/drivebrain_core_impl/drivebrain_logging/include/DrivebrainMCAPLogger.hpp b/drivebrain_core_impl/drivebrain_logging/include/DrivebrainMCAPLogger.hpp
new file mode 100644
index 00000000..265066db
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_logging/include/DrivebrainMCAPLogger.hpp
@@ -0,0 +1,98 @@
+#ifndef __DRIVEBRAINMCAPLOGGER_H__
+#define __DRIVEBRAINMCAPLOGGER_H__
+
+#include <DriverBus.hpp>
+
+#include <nlohmann/json.hpp>
+
+#include <google/protobuf/message.h>
+
+#include <mcap.hpp>
+#include <mcap/writer.hpp>
+#include <mcap/mcap.hpp>
+
+#include <types.hpp>
+#include <unordered_map>
+#include <memory>
+#include <string>
+#include <variant>
+#include <mutex>
+#include <thread>
+#include <optional>
+
+#include "Configurable.hpp" // core::common::Configurable
+#include <functional>
+// - [ ] add functionality for logging parameters json 
+
+
+// USER STORIES
+// - I want to add the ability to log json data for use with logging parameters
+// REQUIREMENTS:
+
+// EXTERNAL REQUIREMENTS:
+    // - assert that all components have been initialized before registering their combined parameter set schema
+// - json message logger function. needed to handle the serialization of the json data (json.dump())
+// - add in schema registration for json message(s): at first there will only need to be one, but we can stay general for now
+// 
+namespace common
+{
+    class DrivebrainMCAPLogger 
+    {
+    public:
+        struct RawMessage
+        {
+            std::string serialized_data;
+            std::string message_name;
+            uint64_t log_time;
+        };
+
+        DrivebrainMCAPLogger(const std::string &base_dir, std::vector<std::weak_ptr<core::common::Configurable>> configurable_components);
+        ~DrivebrainMCAPLogger();
+
+        /// @brief 
+        /// @param out_msg 
+        void log_msg(std::shared_ptr<google::protobuf::Message> out_msg);
+        void log_params();
+        void log_json_struct(const std::string & topic, const nlohmann::json & out_json );
+        
+        void open_new_mcap(const std::string &name);
+        void close_current_mcap();
+
+        bool init_param_schema();
+        
+    private:
+        template <typename... Ts>
+        void _get_params_as_json(const std::string& param_parent, const std::string& name, const core::common::Configurable::ParamTypes& var_val, nlohmann::json & params_all)
+        {
+            // Lambda to check and set parameter value
+            // nlohmann::json & params_all
+            auto set_if_holds = [&](auto type_tag) {
+                using T = decltype(type_tag);
+                if (std::holds_alternative<T>(var_val)) {
+                    params_all[param_parent][name] = std::get<T>(var_val);
+                }
+            };
+
+            (set_if_holds(Ts{}), ...);
+        }
+
+        void _handle_log_to_file();
+        nlohmann::json _get_param_vals();
+        std::optional<nlohmann::json> _get_params_schema();
+        // how this funciton will work is if we have not written the param schema that was determined by the initialization of each of the components
+        // we keep calling this function in the log_params function that is being run into the message logger instance.
+    private:
+        core::common::ThreadSafeDeque<RawMessage> _input_deque;
+        std::thread _log_thread;
+        bool _running = false;
+        bool _param_schema_written = false;
+        mcap::McapWriterOptions _options;
+        mcap::McapWriter _writer;
+        std::mutex _logger_mtx;
+        std::unordered_map<std::string, uint32_t> _msg_name_id_map;
+        
+        std::vector<std::weak_ptr<core::common::Configurable>> _configurable_components;
+    };
+}
+
+#endif // __DRIVEBRAINMCAPLOGGER_H__
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp
new file mode 100644
index 00000000..a9347772
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp
@@ -0,0 +1,273 @@
+#include <MatlabModelAddHelper.hpp>
+#include <nlohmann/json_fwd.hpp>
+#define MCAP_IMPLEMENTATION
+#include <types.hpp>
+#include <DrivebrainMCAPLogger.hpp>
+#include <chrono>
+#include <ProtobufUtils.hpp>
+#include <mcap/writer.hpp>
+#include <mcap.hpp>
+#include <hytech_msgs.pb.h>
+#include <mutex>
+#include <versions.h>
+#include <utility>
+#include <spdlog/spdlog.h>
+
+// #include <MatlabModelAddHelper.hpp>
+
+namespace common
+{
+    DrivebrainMCAPLogger::DrivebrainMCAPLogger(const std::string &base_dir, std::vector<std::weak_ptr<core::common::Configurable>> configurable_components)
+        : _options(mcap::McapWriterOptions("")), _configurable_components(configurable_components)
+    {
+        std::vector<std::string> proto_names = {"hytech_msgs.proto", "hytech.proto"};
+
+        std::vector<std::string> gend_names = matlab_model_gen::get_proto_filenames();
+        proto_names.insert(proto_names.end(), gend_names.begin(), gend_names.end());
+ 
+        auto optional_map = util::generate_name_to_id_map(proto_names);
+        if (optional_map)
+        {
+            spdlog::info("Opened MCAP"); 
+            _msg_name_id_map = *optional_map;
+        }
+        else
+        {
+            spdlog::error("Error: No map generated"); 
+        }
+        {
+            std::unique_lock lk(_input_deque.mtx);
+            _running = true;
+        }
+        _options.noChunking = true;
+        _log_thread = std::thread(&DrivebrainMCAPLogger::_handle_log_to_file, this);
+    }
+
+    DrivebrainMCAPLogger::~DrivebrainMCAPLogger()
+    {
+        spdlog::info("started destruction of mcap logger");
+        {
+            std::unique_lock lk(_input_deque.mtx);
+            _running = false;
+        }
+        spdlog::info("attempting close log thread in mcap logger");
+        _input_deque.cv.notify_all();
+        spdlog::info("notif sent");
+        _log_thread.join();
+        spdlog::info("joined log thread in mcap logger");
+    }
+
+    // is guaranteed to be called before starting the logging to the mcap
+    void DrivebrainMCAPLogger::open_new_mcap(const std::string &name)
+    {
+        spdlog::info("Open MCAP function called"); 
+
+        const auto res = _writer.open(name.c_str(), _options);
+        if (!res.ok())
+        {
+            spdlog::error("Failed to open {} for writing: {}", name, res.message);
+        }
+        // TODO handle message name de-confliction for messages of the same name
+        // message-receiving .protos (non-base .proto files)
+
+        std::vector<std::string> proto_names = {"hytech_msgs.proto", "hytech.proto"};
+
+        std::vector<std::string> gend_names = matlab_model_gen::get_proto_filenames();
+        proto_names.insert(proto_names.end(), gend_names.begin(), gend_names.end());
+        auto receiving_descriptors = util::get_pb_descriptors(proto_names);
+        // auto schema_only_descriptors = util::get_pb_descriptors({"base_msgs.proto"});
+
+        auto add_protobuf_schema_func = [this](const std::vector<const google::protobuf::FileDescriptor *> &descriptors, bool skip_channel)
+        {
+            for (const auto &file_descriptor : descriptors)
+            {
+                for (int i = 0; i < file_descriptor->message_type_count(); ++i)
+                {
+                    const google::protobuf::Descriptor *message_descriptor = file_descriptor->message_type(i);
+                    mcap::Schema schema(message_descriptor->full_name(), "protobuf",
+                                        util::build_file_descriptor_set(message_descriptor).SerializeAsString());
+                    _writer.addSchema(schema);
+                    mcap::Channel channel(message_descriptor->name(), "protobuf", schema.id);
+                    if (!skip_channel)
+                    {
+                        _writer.addChannel(channel);
+                    }
+                }
+            }
+        };
+
+        // add_protobuf_schema_func(schema_only_descriptors, true);
+        add_protobuf_schema_func(receiving_descriptors, false);
+
+        spdlog::info("Added message descriptions to MCAP"); 
+    }
+
+    void DrivebrainMCAPLogger::close_current_mcap()
+    {
+        spdlog::info("Closing MCAP"); 
+        _writer.close();
+    }
+
+    void DrivebrainMCAPLogger::_handle_log_to_file()
+    {
+        core::common::ThreadSafeDeque<RawMessage> q;
+
+        while (true)
+        {
+            {
+                spdlog::debug("looping _handle_log_to_file");
+                std::unique_lock lk(_input_deque.mtx);
+                _input_deque.cv.wait(lk, [this]()
+                                    { return !_input_deque.deque.empty() || !_running; });
+
+                // Ensure exit condition is checked immediately after waking up
+                if (!_running && _input_deque.deque.empty())
+                    break;  // Clean exit instead of return (better readability)
+
+                q.deque = std::move(_input_deque.deque); // Move instead of copy
+            }
+
+            for (auto &msg : q.deque)
+            {
+                mcap::Message msg_to_log;
+                msg_to_log.data = reinterpret_cast<const std::byte *>(msg.serialized_data.data());
+                msg_to_log.dataSize = msg.serialized_data.size();
+
+                msg_to_log.logTime = msg.log_time;
+                msg_to_log.publishTime = msg.log_time;
+
+                {
+                    std::unique_lock lk(_logger_mtx);
+                    msg_to_log.channelId = _msg_name_id_map[msg.message_name];
+                    auto write_res = _writer.write(msg_to_log);
+
+                }
+            }
+
+            q.deque.clear();
+        }
+
+        spdlog::info("Logger thread exiting...");
+
+        // Ensure all resources are cleaned up
+        {
+            std::unique_lock lk(_input_deque.mtx);
+            _input_deque.deque.clear();
+        }
+
+        close_current_mcap();
+    }
+
+    void DrivebrainMCAPLogger::log_msg(std::shared_ptr<google::protobuf::Message> msg_out)
+    {
+        mcap::Timestamp log_time = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+        DrivebrainMCAPLogger::RawMessage msg_to_enque;
+        msg_to_enque.serialized_data = msg_out->SerializeAsString();
+        msg_to_enque.message_name = msg_out->GetDescriptor()->name();
+        msg_to_enque.log_time = log_time;
+
+        {
+            std::unique_lock lk(_input_deque.mtx);
+            _input_deque.deque.push_back(msg_to_enque);
+            _input_deque.cv.notify_all();
+        }
+        
+    }
+
+
+    // NOTE: required to be called after all of the components have been initialized
+    // TODO: make this thread safe by adding a writer mutex as this resource is being worked with in multiple threads:
+    //       1: message logger param log thread (if the param schema cannot be written on construction of the msg logger)
+    //       2: this class's log thread
+    bool DrivebrainMCAPLogger::init_param_schema()
+    {
+        // param schema and channel creation
+        auto params_schema_json = _get_params_schema();
+
+        if(params_schema_json)
+        {
+            mcap::Schema config_schema("drivebrain_configuration", "jsonschema", (*params_schema_json).dump());
+            _writer.addSchema(config_schema);
+            mcap::Channel config_channel("drivebrain_configuration", "json", config_schema.id);
+            _writer.addChannel(config_channel);
+            _param_schema_written = true;
+            // TODO this is a name that cannot be used as a component name. need to assert this in the configureable component
+            _msg_name_id_map["drivebrain_configuration"] = config_channel.id;
+            return true;
+        } else {
+            spdlog::error("error initializing param schema");
+            return false;
+        }
+    }
+    
+    void DrivebrainMCAPLogger::log_params()
+    {
+        if(_param_schema_written)
+        {
+            mcap::Timestamp log_time = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+            // TODO handle getting of all of the current config values from the components here
+            auto contentJson = _get_param_vals();
+            
+            DrivebrainMCAPLogger::RawMessage msg_to_enque;
+            msg_to_enque.serialized_data = contentJson.dump();
+            msg_to_enque.message_name = "drivebrain_configuration";
+            msg_to_enque.log_time = log_time;
+
+            {
+                std::unique_lock lk(_input_deque.mtx);
+                _input_deque.deque.push_back(msg_to_enque);
+                _input_deque.cv.notify_all();
+            }
+        }
+        spdlog::info("attempted param log");
+    }
+    nlohmann::json DrivebrainMCAPLogger::_get_param_vals() {
+    
+        nlohmann::json params_all;
+        for(const auto cc_locked: _configurable_components)
+        {
+            if(auto cc = cc_locked.lock())
+            {
+                std::unordered_map params_map = cc->get_all_params_map();
+                std::string param_parent = cc->get_name();
+                // std::cout << param_parent << std::endl;
+                std::vector<std::string> param_names = cc->get_param_names();
+                for (auto i = params_map.begin(); i != params_map.end(); i++)
+                {
+                    auto name = i->first;
+                    auto var_val = i->second;
+                    
+                    _get_params_as_json<bool, int, float, double, std::string>(param_parent, name, var_val, params_all);
+                }
+            }
+            
+        }
+        return params_all;
+    }
+
+    std::optional<nlohmann::json> DrivebrainMCAPLogger::_get_params_schema()
+    {
+        nlohmann::json top_level_schema;
+        top_level_schema["type"] = "object";
+        size_t component_index = 0;
+        for(const auto component : _configurable_components )
+        {
+            // TODO handle multiple instances of the same component
+            if(auto comp_locked = component.lock())
+            {
+                std::cout << "getting component index: "<<component_index <<std::endl;
+                if(!comp_locked->is_configured())
+                {
+                    return std::nullopt;
+                }
+                component_index++;
+                top_level_schema["properties"][comp_locked->get_name()] = comp_locked->get_schema();
+            }
+        }
+        return top_level_schema;
+    }
+}
+
+
+
diff --git a/drivebrain_core_impl/drivebrain_logging/src/ParamLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/ParamLogger.cpp
new file mode 100644
index 00000000..8b137891
--- /dev/null
+++ b/drivebrain_core_impl/drivebrain_logging/src/ParamLogger.cpp
@@ -0,0 +1 @@
+
diff --git a/drivebrain_core_impl/drivebrain_mcap_logger/include/MCAPProtobufLogger.hpp b/drivebrain_core_impl/drivebrain_mcap_logger/include/MCAPProtobufLogger.hpp
deleted file mode 100644
index 38f34615..00000000
--- a/drivebrain_core_impl/drivebrain_mcap_logger/include/MCAPProtobufLogger.hpp
+++ /dev/null
@@ -1,52 +0,0 @@
-#ifndef __MCAPPROTOBUFLOGGER_H__
-#define __MCAPPROTOBUFLOGGER_H__
-
-#include <google/protobuf/message.h>
-#include <mcap.hpp>
-#include <memory>
-#include <string>
-#include <unordered_map>
-#include <mcap/writer.hpp>
-#include <mcap/mcap.hpp>
-#include <mutex>
-#include <DriverBus.hpp>
-
-#include <thread>
-namespace common
-{
-    class MCAPProtobufLogger 
-    {
-    public:
-        struct ProtobufRawMessage
-        {
-            std::string serialized_data;
-            std::string message_name;
-            uint64_t log_time;
-        };
-
-        
-
-        MCAPProtobufLogger(const std::string &base_dir);
-        ~MCAPProtobufLogger();
-
-        /// @brief 
-        /// @param out_msg 
-        void log_msg(std::shared_ptr<google::protobuf::Message> out_msg);
-        void open_new_mcap(const std::string &name);
-        void close_current_mcap();
-
-    private:
-        void _handle_log_to_file();
-    private:
-        core::common::ThreadSafeDeque<ProtobufRawMessage> _input_deque;
-        std::thread _log_thread;
-        bool _running = false;
-        mcap::McapWriterOptions _options;
-        mcap::McapWriter _writer;
-        std::mutex _logger_mtx;
-        std::unordered_map<std::string, uint32_t> _msg_name_id_map;
-
-    };
-}
-
-#endif // __MCAPPROTOBUFLOGGER_H__
\ No newline at end of file
diff --git a/drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp b/drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp
deleted file mode 100644
index 0dbb9f1a..00000000
--- a/drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp
+++ /dev/null
@@ -1,146 +0,0 @@
-#define MCAP_IMPLEMENTATION
-#include <MCAPProtobufLogger.hpp>
-#include <chrono>
-#include <ProtobufUtils.hpp>
-#include <mcap/writer.hpp>
-#include <mcap.hpp>
-#include <hytech_msgs.pb.h>
-#include <mutex>
-#include <versions.h>
-#include <utility>
-#include <spdlog/spdlog.h> 
-
-namespace common
-{
-    MCAPProtobufLogger::MCAPProtobufLogger(const std::string &base_dir)
-        : _options(mcap::McapWriterOptions(""))
-    {
-        auto optional_map = util::generate_name_to_id_map({"hytech_msgs.proto", "hytech.proto"});
-        if (optional_map)
-        {
-            spdlog::info("Opened MCAP"); 
-            _msg_name_id_map = *optional_map;
-        }
-        else
-        {
-            spdlog::error("Error: No map generated"); 
-        }
-        {
-            std::unique_lock lk(_input_deque.mtx);
-            _running = true;
-        }
-        _options.noChunking = true;
-        _log_thread = std::thread(&MCAPProtobufLogger::_handle_log_to_file, this);
-    }
-    MCAPProtobufLogger::~MCAPProtobufLogger()
-    {
-        {
-            std::unique_lock lk(_input_deque.mtx);
-            _running = false;
-        }
-        _input_deque.cv.notify_all();
-        _log_thread.join();
-    }
-
-    void MCAPProtobufLogger::open_new_mcap(const std::string &name)
-    {
-        spdlog::info("Open MCAP function called"); 
-
-        const auto res = _writer.open(name.c_str(), _options);
-        if (!res.ok())
-        {
-            spdlog::error("Failed to open {} for writing: {}", name, res.message);
-        }
-        // TODO handle message name de-confliction for messages of the same name
-        // message-receiving .protos (non-base .proto files)
-        auto receiving_descriptors = util::get_pb_descriptors({"hytech_msgs.proto", "hytech.proto"});
-        // auto schema_only_descriptors = util::get_pb_descriptors({"base_msgs.proto"});
-
-        auto add_schema_func = [this](const std::vector<const google::protobuf::FileDescriptor *> &descriptors, bool skip_channel)
-        {
-            for (const auto &file_descriptor : descriptors)
-            {
-                for (int i = 0; i < file_descriptor->message_type_count(); ++i)
-                {
-                    const google::protobuf::Descriptor *message_descriptor = file_descriptor->message_type(i);
-                    mcap::Schema schema(message_descriptor->full_name(), "protobuf",
-                                        util::build_file_descriptor_set(message_descriptor).SerializeAsString());
-                    _writer.addSchema(schema);
-                    mcap::Channel channel(message_descriptor->name(), "protobuf", schema.id);
-                    if (!skip_channel)
-                    {
-                        _writer.addChannel(channel);
-                    }
-                }
-            }
-        };
-        // add_schema_func(schema_only_descriptors, true);
-        add_schema_func(receiving_descriptors, false);
-
-        spdlog::info("Added message descriptions to MCAP"); 
-    }
-
-    void MCAPProtobufLogger::close_current_mcap()
-    {
-        spdlog::info("Closing MCAP"); 
-        _writer.close();
-    }
-
-    void MCAPProtobufLogger::_handle_log_to_file()
-    {
-        core::common::ThreadSafeDeque<ProtobufRawMessage> q;
-
-        // this will occasionally take a while (~200ms) to complete a loop iteration so this is in its own thread
-        while (true)
-        {
-            {
-                std::unique_lock lk(_input_deque.mtx);
-                _input_deque.cv.wait(lk, [this]()
-                                    { return !_input_deque.deque.empty() || !_running; });
-                if (!_running)
-                {
-                    _input_deque.deque.clear();
-                    return;
-                }
-                q.deque = _input_deque.deque;
-                _input_deque.deque.clear();
-            }
-            for (auto &msg : q.deque)
-            {
-                mcap::Message msg_to_log;
-                msg_to_log.data = reinterpret_cast<const std::byte *>(msg.serialized_data.data());
-                msg_to_log.dataSize = msg.serialized_data.size();
-                msg_to_log.logTime = msg.log_time;
-                msg_to_log.publishTime = msg.log_time;
-
-                // msg_to_log.sequence = 0; uh, idk https://github.com/foxglove/mcap/blob/main/cpp/mcap/include/mcap/types.hpp#L184
-
-                {
-                    std::unique_lock lk(_logger_mtx);
-                    msg_to_log.channelId = _msg_name_id_map[msg.message_name]; // under the mutex we also lookup in the map
-                    auto write_res = _writer.write(msg_to_log);
-
-                    spdlog::info("Logging message: {}", msg.message_name); 
-                }
-            }
-            q.deque.clear();
-        }
-    }
-
-    void MCAPProtobufLogger::log_msg(std::shared_ptr<google::protobuf::Message> msg_out)
-    {
-        mcap::Timestamp log_time = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
-        MCAPProtobufLogger::ProtobufRawMessage msg_to_enque;
-        msg_to_enque.serialized_data = msg_out->SerializeAsString();
-        msg_to_enque.message_name = msg_out->GetDescriptor()->name();
-        msg_to_enque.log_time = log_time;
-
-        {
-            std::unique_lock lk(_input_deque.mtx);
-            _input_deque.deque.push_back(msg_to_enque);
-            _input_deque.cv.notify_all();
-        }
-
-        
-    }
-}
\ No newline at end of file
diff --git a/drivebrain_utils/include/MCAPReplay.hpp b/drivebrain_utils/include/MCAPReplay.hpp
new file mode 100644
index 00000000..29608807
--- /dev/null
+++ b/drivebrain_utils/include/MCAPReplay.hpp
@@ -0,0 +1,65 @@
+#ifndef __MCAPREPLAY_H__
+#define __MCAPREPLAY_H__
+
+#include <memory>
+#include <string>
+
+#include <google/protobuf/descriptor.pb.h>
+#include <google/protobuf/descriptor_database.h>
+#include <google/protobuf/dynamic_message.h>
+
+
+
+// Drivebrain impl components
+#include <CANComms.hpp>
+#include <ETHSendComms.hpp>
+
+
+#define MCAP_IMPLEMENTATION
+#include "mcap/reader.hpp"
+
+
+// this utility will be able to replay a car MCAP file to 
+// ethernet socket and a virtual CAN bus
+
+// [x] simple reading of mcap file
+// [x] read of mcap at real-time speed
+    // starting with the timestamp of the first message, ensure that we wait until the next message was sent
+    // before we "send" the next message on read of the message.
+// [x] use a CANDriver and send all CAN traffic from MCAP file to vcan
+// [x] use instances of (TODO) ETHSendComms to send the VCF/VCR/ACU data
+    // [x] create ETHSendComms
+// [x] add in fake VN output eth sender 
+    // [x] add fake VN listener into drivebrain software
+
+namespace util
+{
+    class MCAPReplay
+    {
+        public:
+            MCAPReplay(std::string param_file_path, std::string dbc_file_path);
+            
+            void start(std::string filename);
+        private:
+            bool _load_schema(const mcap::SchemaPtr schema, google::protobuf::SimpleDescriptorDatabase* proto_db);
+            std::shared_ptr<google::protobuf::Message> _get_pb_msg(google::protobuf::DescriptorPool &protoPool,
+                                                     google::protobuf::DynamicMessageFactory &protoFactory,
+                                                     const mcap::SchemaPtr schema,
+                                                     google::protobuf::SimpleDescriptorDatabase *protoDbPtr, mcap::Message mcap_msg);
+        private:
+             
+            core::JsonFileHandler _config;
+            core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> _primary_can_tx_queue;
+            boost::asio::io_context _io_context;
+            std::shared_ptr<comms::CANDriver> _driver_primary_can = nullptr;
+            std::shared_ptr<comms::ETHSendComms> _acu_sender = nullptr;
+            std::shared_ptr<comms::ETHSendComms> _vcr_sender = nullptr;
+            std::shared_ptr<comms::ETHSendComms> _acu_core_sender = nullptr;
+            std::shared_ptr<comms::ETHSendComms> _vn_sender = nullptr;
+            mcap::McapReader _mcap_reader;
+        
+            std::thread _io_context_thread;
+    };
+
+}
+#endif // __MCAPREPLAY_H__
\ No newline at end of file
diff --git a/drivebrain_utils/src/MCAPReplay.cpp b/drivebrain_utils/src/MCAPReplay.cpp
new file mode 100644
index 00000000..14705a58
--- /dev/null
+++ b/drivebrain_utils/src/MCAPReplay.cpp
@@ -0,0 +1,168 @@
+#include "MCAPReplay.hpp"
+
+#include <JsonFileHandler.hpp>
+#include <cmath>
+#include <spdlog/spdlog.h>
+
+#include <chrono>
+
+namespace gp = google::protobuf;
+
+namespace util {
+MCAPReplay::MCAPReplay(std::string param_file_path, std::string dbc_file_path)
+    : _config(param_file_path) {
+    std::cout << param_file_path << std::endl;
+    bool cf = false;
+    _driver_primary_can =
+        std::make_shared<comms::CANDriver>(_config, _primary_can_tx_queue, _io_context,
+                                           dbc_file_path, cf, nullptr, "CANDriverPrimary");
+    if(cf)
+    {
+        throw std::runtime_error("Failed to initialize can driver");
+    }
+
+    _acu_sender = std::make_shared<comms::ETHSendComms>(_io_context, 7766, "127.0.0.1", false);
+    // _acu_core_sender = std::make_shared<comms::ETHSendComms>(nullptr, _io_context, 7777, "127.0.0.1");
+    _vcr_sender = std::make_shared<comms::ETHSendComms>(_io_context, 9999, "127.0.0.1", false);
+    _vn_sender = std::make_shared<comms::ETHSendComms>(_io_context, 13111, "127.0.0.1", false); // fake VN 
+    
+
+}
+
+bool MCAPReplay::_load_schema(const mcap::SchemaPtr schema,
+                              google::protobuf::SimpleDescriptorDatabase *protoDb) {
+    gp::FileDescriptorSet fdSet;
+    if (!fdSet.ParseFromArray(schema->data.data(), static_cast<int>(schema->data.size()))) {
+        spdlog::error("failed to parse schema data");
+        return false;
+    }
+    gp::FileDescriptorProto unused;
+    for (int i = 0; i < fdSet.file_size(); ++i) {
+        const auto &file = fdSet.file(i);
+        if (!protoDb->FindFileByName(file.name(), &unused)) {
+            if (!protoDb->Add(file)) {
+                spdlog::error("failed to add def {} to protoDB", file.name());
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+std::shared_ptr<gp::Message> MCAPReplay::_get_pb_msg(gp::DescriptorPool &protoPool,
+                                                     gp::DynamicMessageFactory &protoFactory,
+                                                     const mcap::SchemaPtr schema,
+                                                     google::protobuf::SimpleDescriptorDatabase *protoDbPtr, mcap::Message mcap_msg) {
+    const gp::Descriptor *descriptor = protoPool.FindMessageTypeByName(schema->name);
+
+
+    if (descriptor == nullptr) {
+        if (!_load_schema(schema, protoDbPtr)) {
+            std::cerr << "failed to load schema" << std::endl;
+            return nullptr;
+        }
+        descriptor = protoPool.FindMessageTypeByName(schema->name);
+        if (descriptor == nullptr) {
+            std::cerr << "failed to find descriptor after loading pool" << std::endl;
+            return nullptr;
+        }
+    }
+    auto msg = std::shared_ptr<gp::Message>(protoFactory.GetPrototype(descriptor)->New());
+
+
+    if (!msg->ParseFromArray(mcap_msg.data, static_cast<int>(mcap_msg.dataSize))) {
+      std::cerr << "failed to parse message" << std::endl;
+      return nullptr;
+    }
+
+    return msg;
+}
+
+void MCAPReplay::start(std::string filename) {
+    const auto res = _mcap_reader.open(filename);
+    if (!res.ok()) {
+
+        spdlog::error("Failed to open {}", filename);
+        return;
+    }
+
+    _io_context_thread = std::thread([this]() {
+        try {
+            _io_context.run();
+        } catch (const std::exception &e) {
+            spdlog::error("Error in io_context: {}", e.what());
+        }
+    });
+
+    auto prev_ns = std::chrono::nanoseconds(-1);
+    auto msg_view = _mcap_reader.readMessages();
+    auto prev_start = std::chrono::high_resolution_clock::now();
+    gp::SimpleDescriptorDatabase protoDb;
+    gp::DescriptorPool protoPool(&protoDb);
+    gp::DynamicMessageFactory protoFactory(&protoPool);
+
+    for (auto it = msg_view.begin(); it != msg_view.end(); it++) {
+        // skip any non-protobuf-encoded messages.
+
+        auto loop_start = std::chrono::high_resolution_clock::now();
+        if (it->schema->encoding != "protobuf") {
+            continue;
+        }
+
+        auto next_ns = std::chrono::nanoseconds(it->message.logTime);
+
+        // spdlog::info("time diff in ns: {}", std::chrono::duration_cast<std::chrono::nanoseconds>(
+        // next_ns-prev_ns).count());
+        // TODO add configurable divider for the sleep time
+
+        if (prev_ns.count() > 0) {
+            auto intended_duration = next_ns - prev_ns;
+            auto actual_elapsed = (loop_start - prev_start);
+            if (actual_elapsed < intended_duration) {
+                std::this_thread::sleep_for(intended_duration - actual_elapsed);
+            }
+        }
+
+        prev_ns = next_ns;
+
+        auto msg = _get_pb_msg(protoPool, protoFactory, it->schema, &protoDb, it->message);
+
+        auto msg_name = it->schema->name;
+        // TODO make filter configure-able
+        if ((!msg_name.rfind("hytech.", 0)) && 
+            !(msg_name == "hytech.drivebrain_torque_lim_input") && 
+            !(msg_name == "hytech.drivebrain_speed_set_input") &&
+            !(msg_name == "hytech.drivebrain_desired_torque_input")
+        ) // denotes CAN message that is not a drivebrain output
+        {
+            {
+                std::unique_lock lk(_primary_can_tx_queue.mtx);
+                _primary_can_tx_queue.deque.push_back(msg);
+                _primary_can_tx_queue.cv.notify_all();
+            }
+        } 
+        else if(it->schema->name == "hytech_msgs.ACUAllData")
+        {
+            spdlog::info("sending ACUALLData");
+            _acu_sender->enqueue_msg_to_send(msg);
+        } else if(it->schema->name == "hytech_msgs.VNData")
+        {
+            _vn_sender->enqueue_msg_to_send(msg);
+        } else if(it->schema->name == "hytech_msgs.VCRData_s")
+        {
+            _vcr_sender->enqueue_msg_to_send(msg);
+        }
+
+        prev_start = loop_start;
+    }
+
+    spdlog::info("halted message logger");
+    _io_context.stop();
+    if (_io_context_thread.joinable()) {
+        _io_context_thread.join();
+        spdlog::info("joined io context");
+    }
+
+    _mcap_reader.close();
+}
+} // namespace util
\ No newline at end of file
diff --git a/drivebrain_utils/src/replay_app.cpp b/drivebrain_utils/src/replay_app.cpp
new file mode 100644
index 00000000..e1623469
--- /dev/null
+++ b/drivebrain_utils/src/replay_app.cpp
@@ -0,0 +1,31 @@
+#include "MCAPReplay.hpp"
+#include <iostream>
+#include <boost/program_options.hpp>
+
+int main(int argc, char* argv[])
+{
+    namespace po = boost::program_options;
+    po::options_description desc("Allowed options");
+
+
+    spdlog::set_level(spdlog::level::debug);
+    std::string mcap_filepath, dbc_filepath, config_filepath;
+    desc.add_options()
+        ("help,h", "produce help message")
+        ("mcap,m", po::value<std::string>(&mcap_filepath), "Path to mcap file")
+        ("config,c", po::value<std::string>(&config_filepath), "Path to config file")
+        ("dbc,d", po::value<std::string>(&dbc_filepath), "Path to dbc file");
+    
+    po::variables_map vm;
+    po::store(po::parse_command_line(argc, argv, desc), vm);
+    po::notify(vm);
+
+    if (vm.count("help")) {
+        std::cout << desc<<std::endl;
+        std::exit(0);
+    }
+
+    util::MCAPReplay replay_util(config_filepath, dbc_filepath);
+    replay_util.start(mcap_filepath);
+    return 0;
+}
\ No newline at end of file
diff --git a/flake.lock b/flake.lock
index 2087e112..2d1d3500 100644
--- a/flake.lock
+++ b/flake.lock
@@ -1,18 +1,21 @@
 {
   "nodes": {
     "HT_proto": {
-      "flake": false,
+      "inputs": {
+        "flake-utils": "flake-utils",
+        "nixpkgs": "nixpkgs"
+      },
       "locked": {
-        "lastModified": 1739773905,
-        "narHash": "sha256-VWA9cM2Ra/xWUoWZw+inJ1HesMEXrQmNiquJUuyT2R8=",
+        "lastModified": 1749876080,
+        "narHash": "sha256-ByaYj2GciK9rYQ/t3RzkBbsxGe7iJft2gKkuLszN2ZA=",
         "owner": "hytech-racing",
         "repo": "HT_proto",
-        "rev": "d0399cbef06ecc51afab470db698b6e3d1cf62a4",
+        "rev": "e1cb57ba1cb4634f86d1ae29b805ea73ea90f4ff",
         "type": "github"
       },
       "original": {
         "owner": "hytech-racing",
-        "ref": "2025-02-17T06_32_03",
+        "ref": "2025-06-14T04_41_35",
         "repo": "HT_proto",
         "type": "github"
       }
@@ -52,20 +55,32 @@
     "db-core-src": {
       "flake": false,
       "locked": {
-        "lastModified": 1739899350,
-        "narHash": "sha256-9BYbFPgHMOP756AhWICIzg6qxTdBcT831bO0QhI3WPM=",
+        "lastModified": 1749880473,
+        "narHash": "sha256-OCbbt3Mt7E96bqQ/uyg3yno79ooLr6vXo+bkD9Z+us8=",
         "owner": "hytech-racing",
         "repo": "drivebrain_core",
-        "rev": "a90ba76b5ff105814c1fd64e8112b5eae5f5bae4",
+        "rev": "40f2c209639924b9ecee2bcf3492564d49479939",
         "type": "github"
       },
       "original": {
         "owner": "hytech-racing",
-        "ref": "feature/low_level_inputs",
         "repo": "drivebrain_core",
         "type": "github"
       }
     },
+    "db-simulink-gen-src": {
+      "flake": false,
+      "locked": {
+        "lastModified": 1757551410,
+        "narHash": "sha256-/G9qLyVhUpcuJMQpI3Aval3Gr5Brw59XuwtxXE5+DKc=",
+        "type": "tarball",
+        "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel13/gen_rel.tar"
+      },
+      "original": {
+        "type": "tarball",
+        "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel13/gen_rel.tar"
+      }
+    },
     "dbcppp-src": {
       "flake": false,
       "locked": {
@@ -86,7 +101,7 @@
     },
     "devshell": {
       "inputs": {
-        "nixpkgs": "nixpkgs"
+        "nixpkgs": "nixpkgs_2"
       },
       "locked": {
         "lastModified": 1722113426,
@@ -126,7 +141,7 @@
     },
     "easy_cmake_2": {
       "inputs": {
-        "nixpkgs": "nixpkgs_4",
+        "nixpkgs": "nixpkgs_5",
         "utils": "utils_3"
       },
       "locked": {
@@ -151,11 +166,11 @@
         ]
       },
       "locked": {
-        "lastModified": 1730504689,
-        "narHash": "sha256-hgmguH29K2fvs9szpq2r3pz2/8cJd2LPS+b4tfNFCwE=",
+        "lastModified": 1738453229,
+        "narHash": "sha256-7H9XgNiGLKN1G1CgRh0vUL4AheZSYzPm+zmZ7vxbJdo=",
         "owner": "hercules-ci",
         "repo": "flake-parts",
-        "rev": "506278e768c2a08bec68eb62932193e341f55c90",
+        "rev": "32ea77a06711b758da0ad9bd6a844c5740a87abd",
         "type": "github"
       },
       "original": {
@@ -200,6 +215,24 @@
         "type": "github"
       }
     },
+    "flake-utils": {
+      "inputs": {
+        "systems": "systems"
+      },
+      "locked": {
+        "lastModified": 1710146030,
+        "narHash": "sha256-SZ5L6eA7HJ/nmkzGG7/ISclqe6oZdOZTNoesiInkXPQ=",
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "rev": "b1d9ab70662946ef0850d488da1c9019f3a9752a",
+        "type": "github"
+      },
+      "original": {
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "type": "github"
+      }
+    },
     "flow-ipc-src": {
       "flake": false,
       "locked": {
@@ -235,11 +268,11 @@
     "foxglove-schemas-src": {
       "flake": false,
       "locked": {
-        "lastModified": 1721832154,
-        "narHash": "sha256-Jp47MYWu3Sh60prjNALs1OaFnPZb7tXRNzFbSRZBXU0=",
+        "lastModified": 1738884246,
+        "narHash": "sha256-2o0oVc3GUUFrwktcVzYoshVcLjLam/6unaY6MgrmVAM=",
         "owner": "foxglove",
         "repo": "schemas",
-        "rev": "f028ba1154faa1226a9993cc2223855123dbf817",
+        "rev": "ce1ffa651a01277fdb43b86ac3f42e5b755c6869",
         "type": "github"
       },
       "original": {
@@ -264,6 +297,54 @@
         "type": "github"
       }
     },
+    "glim-ros2-src": {
+      "flake": false,
+      "locked": {
+        "lastModified": 1739507942,
+        "narHash": "sha256-VI8oaJNS265CVD5xqxVAL0ofbM96rkylIMyqZxSp2FY=",
+        "owner": "koide3",
+        "repo": "glim_ros2",
+        "rev": "3d74aaff264c4482f0ee9e152ebcf25299e14d75",
+        "type": "github"
+      },
+      "original": {
+        "owner": "koide3",
+        "repo": "glim_ros2",
+        "type": "github"
+      }
+    },
+    "glim-src": {
+      "flake": false,
+      "locked": {
+        "lastModified": 1740059717,
+        "narHash": "sha256-N/Q1nIwv6y6W0tGjVE9hXmVT1mcm+WRpOH559VGa8Hc=",
+        "owner": "RCMast3r",
+        "repo": "glim",
+        "rev": "37f0e6cf54d880b08f0643cc66e23ab1da76328b",
+        "type": "github"
+      },
+      "original": {
+        "owner": "RCMast3r",
+        "repo": "glim",
+        "type": "github"
+      }
+    },
+    "gtsam-points-src": {
+      "flake": false,
+      "locked": {
+        "lastModified": 1739867676,
+        "narHash": "sha256-f97r/rtwi7l+pf273rjRcfA/GaHs3+Sqbci8Z7+PJ1s=",
+        "owner": "RCMast3r",
+        "repo": "gtsam_points",
+        "rev": "127c2959592e697a38a29139443db14fbf02cdfe",
+        "type": "github"
+      },
+      "original": {
+        "owner": "RCMast3r",
+        "repo": "gtsam_points",
+        "type": "github"
+      }
+    },
     "gtsam-src": {
       "flake": false,
       "locked": {
@@ -292,16 +373,15 @@
         "utils": "utils_2"
       },
       "locked": {
-        "lastModified": 1739624640,
-        "narHash": "sha256-g12QwiJYTQmlNPyCrqetNCMuZVsLL1wzHe5vBVITfvs=",
+        "lastModified": 1749916491,
+        "narHash": "sha256-YKHet4wW71yYoah2AYlHIC5n930HI0lWFkeuxzlSd54=",
         "owner": "hytech-racing",
         "repo": "ht_can",
-        "rev": "0a3f0cd7062cab8d494eb62c6f622de6e913a114",
+        "rev": "ef8559488f5c020ecf83175e1cd12afc84e15167",
         "type": "github"
       },
       "original": {
         "owner": "hytech-racing",
-        "ref": "141",
         "repo": "ht_can",
         "type": "github"
       }
@@ -322,22 +402,6 @@
         "type": "github"
       }
     },
-    "nanopb-proto-api": {
-      "flake": false,
-      "locked": {
-        "lastModified": 1736537572,
-        "narHash": "sha256-+vIvF7t58oB+QrxUBlHoQxLpbkOKv1Oy07ggyXnn4go=",
-        "owner": "nanopb",
-        "repo": "nanopb",
-        "rev": "fc7d53c3fea3ce04af5b98234feaa1c7400f73d6",
-        "type": "github"
-      },
-      "original": {
-        "owner": "nanopb",
-        "repo": "nanopb",
-        "type": "github"
-      }
-    },
     "nebs-packages": {
       "inputs": {
         "commsdsl-src": "commsdsl-src",
@@ -348,6 +412,9 @@
         "flow-ipc-src": "flow-ipc-src",
         "foxglove-mcap-src": "foxglove-mcap-src",
         "foxglove-ws-protocol-src": "foxglove-ws-protocol-src",
+        "glim-ros2-src": "glim-ros2-src",
+        "glim-src": "glim-src",
+        "gtsam-points-src": "gtsam-points-src",
         "gtsam-src": "gtsam-src",
         "libsocketcanpp-src": "libsocketcanpp-src",
         "nix-proto": "nix-proto",
@@ -357,11 +424,11 @@
         "soem-src": "soem-src"
       },
       "locked": {
-        "lastModified": 1739446508,
-        "narHash": "sha256-ARAcuJ1twR7tUQGzqn+UGyFzuoX+wVpyrOvZ4yrgqYo=",
+        "lastModified": 1740060207,
+        "narHash": "sha256-VXZhU9CnTCnQh1SmbituUSIuoT8nnCRxDa2Lps4ii7E=",
         "owner": "RCMast3r",
         "repo": "nebs_packages",
-        "rev": "803448c63e96783ef4b280318a276cc63ba00f09",
+        "rev": "5e6051e167325ee43080051f44880924f55dbb1f",
         "type": "github"
       },
       "original": {
@@ -404,7 +471,7 @@
       "inputs": {
         "nix-filter": "nix-filter",
         "nix-std": "nix-std",
-        "nixpkgs": "nixpkgs_2"
+        "nixpkgs": "nixpkgs_3"
       },
       "locked": {
         "lastModified": 1723991236,
@@ -476,16 +543,16 @@
     },
     "nixpkgs": {
       "locked": {
-        "lastModified": 1722073938,
-        "narHash": "sha256-OpX0StkL8vpXyWOGUD6G+MA26wAXK6SpT94kLJXo6B4=",
+        "lastModified": 1720535198,
+        "narHash": "sha256-zwVvxrdIzralnSbcpghA92tWu2DV2lwv89xZc8MTrbg=",
         "owner": "NixOS",
         "repo": "nixpkgs",
-        "rev": "e36e9f57337d0ff0cf77aceb58af4c805472bfae",
+        "rev": "205fd4226592cc83fd4c0885a3e4c9c400efabb5",
         "type": "github"
       },
       "original": {
         "owner": "NixOS",
-        "ref": "nixpkgs-unstable",
+        "ref": "nixos-23.11",
         "repo": "nixpkgs",
         "type": "github"
       }
@@ -515,6 +582,22 @@
       }
     },
     "nixpkgs_2": {
+      "locked": {
+        "lastModified": 1722073938,
+        "narHash": "sha256-OpX0StkL8vpXyWOGUD6G+MA26wAXK6SpT94kLJXo6B4=",
+        "owner": "NixOS",
+        "repo": "nixpkgs",
+        "rev": "e36e9f57337d0ff0cf77aceb58af4c805472bfae",
+        "type": "github"
+      },
+      "original": {
+        "owner": "NixOS",
+        "ref": "nixpkgs-unstable",
+        "repo": "nixpkgs",
+        "type": "github"
+      }
+    },
+    "nixpkgs_3": {
       "locked": {
         "lastModified": 1719426051,
         "narHash": "sha256-yJL9VYQhaRM7xs0M867ZFxwaONB9T2Q4LnGo1WovuR4=",
@@ -530,7 +613,7 @@
         "type": "github"
       }
     },
-    "nixpkgs_3": {
+    "nixpkgs_4": {
       "locked": {
         "lastModified": 1735563628,
         "narHash": "sha256-OnSAY7XDSx7CtDoqNh8jwVwh4xNL/2HaJxGjryLWzX8=",
@@ -546,7 +629,7 @@
         "type": "github"
       }
     },
-    "nixpkgs_4": {
+    "nixpkgs_5": {
       "locked": {
         "lastModified": 1695825837,
         "narHash": "sha256-4Ne11kNRnQsmSJCRSSNkFRSnHC4Y5gPDBIQGjjPfJiU=",
@@ -562,7 +645,7 @@
         "type": "github"
       }
     },
-    "nixpkgs_5": {
+    "nixpkgs_6": {
       "locked": {
         "lastModified": 1695825837,
         "narHash": "sha256-4Ne11kNRnQsmSJCRSSNkFRSnHC4Y5gPDBIQGjjPfJiU=",
@@ -582,14 +665,14 @@
       "inputs": {
         "HT_proto": "HT_proto",
         "db-core-src": "db-core-src",
+        "db-simulink-gen-src": "db-simulink-gen-src",
         "easy_cmake": "easy_cmake",
         "flake-parts": "flake-parts",
         "foxglove-schemas-src": "foxglove-schemas-src",
         "ht_can": "ht_can",
-        "nanopb-proto-api": "nanopb-proto-api",
         "nebs-packages": "nebs-packages",
         "nix-proto": "nix-proto_2",
-        "nixpkgs": "nixpkgs_3",
+        "nixpkgs": "nixpkgs_4",
         "vn_driver_lib": "vn_driver_lib"
       }
     },
@@ -669,6 +752,21 @@
         "type": "github"
       }
     },
+    "systems_5": {
+      "locked": {
+        "lastModified": 1681028828,
+        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
+        "owner": "nix-systems",
+        "repo": "default",
+        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
+        "type": "github"
+      },
+      "original": {
+        "owner": "nix-systems",
+        "repo": "default",
+        "type": "github"
+      }
+    },
     "treefmt-nix": {
       "inputs": {
         "nixpkgs": [
@@ -692,7 +790,7 @@
     },
     "utils": {
       "inputs": {
-        "systems": "systems"
+        "systems": "systems_2"
       },
       "locked": {
         "lastModified": 1694529238,
@@ -710,7 +808,7 @@
     },
     "utils_2": {
       "inputs": {
-        "systems": "systems_2"
+        "systems": "systems_3"
       },
       "locked": {
         "lastModified": 1726560853,
@@ -728,7 +826,7 @@
     },
     "utils_3": {
       "inputs": {
-        "systems": "systems_3"
+        "systems": "systems_4"
       },
       "locked": {
         "lastModified": 1694529238,
@@ -746,7 +844,7 @@
     },
     "utils_4": {
       "inputs": {
-        "systems": "systems_4"
+        "systems": "systems_5"
       },
       "locked": {
         "lastModified": 1694529238,
@@ -765,7 +863,7 @@
     "vn_driver_lib": {
       "inputs": {
         "easy_cmake": "easy_cmake_2",
-        "nixpkgs": "nixpkgs_5",
+        "nixpkgs": "nixpkgs_6",
         "utils": "utils_4"
       },
       "locked": {
diff --git a/flake.nix b/flake.nix
index 5a0247b1..68cd76ba 100644
--- a/flake.nix
+++ b/flake.nix
@@ -16,58 +16,34 @@
     nix-proto.url = "github:notalltim/nix-proto";
     nix-proto.inputs.nixpkgs.follows = "nixpkgs";
 
-    HT_proto =
-      {
-        type = "github";
-        owner = "hytech-racing";
-        repo = "HT_proto";
-        ref = "2025-02-17T06_32_03";
-        flake = false;
-      };
+    HT_proto.url = "github:hytech-racing/HT_proto/2025-06-14T04_41_35";
 
     foxglove-schemas-src = {
       url = "github:foxglove/schemas";
       flake = false;
     };
 
-    ht_can.url = "github:hytech-racing/ht_can/141";
+    ht_can.url = "github:hytech-racing/ht_can";
     ht_can.inputs.nixpkgs.follows = "nixpkgs";
     ht_can.inputs.nix-proto.follows = "nix-proto";
 
     vn_driver_lib.url = "github:RCMast3r/vn_driver_lib/fix/boost-compatible";
 
     db-core-src = {
-      url = "github:hytech-racing/drivebrain_core/feature/low_level_inputs";
+      url = "github:hytech-racing/drivebrain_core";
       flake = false;
     };
 
-    nanopb-proto-api = {
-      url = "github:nanopb/nanopb";
+    db-simulink-gen-src = {
+      url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel14/gen_rel.tar.gz";
       flake = false;
     };
   };
-  outputs = { self, nixpkgs, flake-parts, nebs-packages, easy_cmake, nix-proto, foxglove-schemas-src, ht_can, HT_proto, vn_driver_lib, db-core-src, nanopb-proto-api, ... }@inputs:
+  outputs = { self, nixpkgs, flake-parts, nebs-packages, easy_cmake, nix-proto, foxglove-schemas-src, ht_can, HT_proto, vn_driver_lib, db-core-src, db-simulink-gen-src, ... }@inputs:
+    
     let
-      nanopb-api = nix-proto.mkProtoDerivation {
-        name = "nanopb-api";
-        version = "0.0.0";
-        # need to remove the makefile from the proto boi because nix will attempt to build that shit
-        src = builtins.filterSource (path: _: baseNameOf path != "Makefile") "${nanopb-proto-api}/generator/proto";
-      };
-
-      drivebrain_core_msgs = { nanopb-api }: nix-proto.mkProtoDerivation {
-        name = "drivebrain_core_msgs";
-        version = (HT_proto.rev or "asdf");
-        
-        src = "${HT_proto}/proto";
-        # protoDeps = [ nanopb-api ];
-      };
-
-      nix-proto-foxglove-overlays = nix-proto.generateOverlays' {
-
-        inherit nanopb-api;
-        inherit drivebrain_core_msgs;
-
+    
+      nix-proto-overlays = nix-proto.generateOverlays' {
         foxglove-schemas = nix-proto.mkProtoDerivation {
           name = "foxglove-schemas";
           version = "1.0.1";
@@ -76,17 +52,28 @@
             namespace = "foxglove";
           };
         };
-
-
+        drivebrain_core_msgs = nix-proto.mkProtoDerivation {
+          name = "drivebrain_core_msgs";
+          
+          version = (if HT_proto ? rev then HT_proto.rev else "unknown");
+          src = "${HT_proto}/proto";
+          };
+        
+        simulink_automation_msgs = nix-proto.mkProtoDerivation {
+          name = "simulink_automation_msgs";
+          version = "1.0.1";
+          src = "${db-simulink-gen-src}/proto_outputs";
+        };
         db_service = nix-proto.mkProtoDerivation
-          {
-            name = "db_service";
-            version = "0.0.1";
-            src = nix-proto.lib.srcFromNamespace {
-              root = ./proto;
-              namespace = "db_service";
+            {
+              name = "db_service";
+              version = "0.0.1";
+              src = nix-proto.lib.srcFromNamespace {
+                root = ./proto;
+                namespace = "db_service";
+              };
             };
-          };
+        
       };
 
       db_core_overlay = final: prev: {
@@ -97,6 +84,9 @@
         (final: prev: {
           drivebrain_software = final.callPackage ./default.nix { };
         })
+        (final: prev: {
+          drivebrain-simulink-gen-pkg = final.callPackage ./simulink_automation.nix { inherit db-simulink-gen-src; };
+        })
         (self: super: {
           python311 = super.python311.override {
             packageOverrides = pyself: pysuper: {
@@ -112,7 +102,7 @@
         }
         )
         db_core_overlay
-      ] ++ (nix-proto.lib.overlayToList nix-proto-foxglove-overlays);
+      ] ++ (nix-proto.lib.overlayToList nix-proto-overlays);
 
     in
     flake-parts.lib.mkFlake { inherit inputs; }
@@ -143,12 +133,13 @@
                 easy_cmake.overlays.default
                 ht_can.overlays.default
                 self.overlays.default
-              ] ++ (nix-proto.lib.overlayToList nix-proto-foxglove-overlays);
+              ] ++ (nix-proto.lib.overlayToList nix-proto-overlays);
               config = { };
             };
             packages.default = pkgs.drivebrain_software;
             packages.drivebrain_software = pkgs.drivebrain_software;
             packages.drivebrain_core = pkgs.drivebrain_core;
+            packages.drivebrain-simulink-gen-pkg = pkgs.drivebrain-simulink-gen-pkg;
 
             devShells.default = pkgs.mkShell rec {
               name = "nix-devshell";
@@ -156,13 +147,15 @@
                 let icon = "f121";
                 in ''
                   dbc_path=${pkgs.ht_can_pkg}
+                  test_path=${db-simulink-gen-src}
                   export DBC_PATH=$dbc_path
                   export PS1="$(echo -e '\u${icon}') {\[$(tput sgr0)\]\[\033[38;5;228m\]\w\[$(tput sgr0)\]\[\033[38;5;15m\]} (${name}) \\$ \[$(tput sgr0)\]"
                   alias build="rm -rf build && mkdir build && cd build && cmake .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON && make -j && cd .."
+                  alias build_debug="rm -rf build && mkdir build && cd build && cmake .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug && make -j && cd .."
                   alias br="cd build && cmake .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON && make -j && cd .."
-                  alias run="./build/alpha_build config/drivebrain_config.json $DBC_PATH/hytech.dbc"
                 '';
-              packages = [ pkgs.mcap-cli ];
+              nativeBuildInputs = [ pkgs.drivebrain_core_msgs_proto_cpp ];
+              packages = [ pkgs.mcap-cli pkgs.valgrind pkgs.grpcui pkgs.python311Packages.cantools pkgs.python311Packages.tkinter ];
               inputsFrom = [
                 pkgs.drivebrain_software
               ];
@@ -178,7 +171,6 @@
                   dbc_path=${pkgs.ht_can_pkg}
                   export DBC_PATH=$dbc_path
                   export PS1="$(echo -e '\u${icon}') {\[$(tput sgr0)\]\[\033[38;5;228m\]\w\[$(tput sgr0)\]\[\033[38;5;15m\]} (${name}) \\$ \[$(tput sgr0)\]"
-                  alias run="./build/alpha_build config/drivebrain_config.json $DBC_PATH/hytech.dbc"
                 '';
 
               nativeBuiltInputs = [ pkgs.drivebrain_core_msgs_proto_cpp ];
@@ -198,7 +190,7 @@
                   easy_cmake.overlays.default
                   ht_can.overlays.default
                   self.overlays.default
-                ] ++ (nix-proto.lib.overlayToList nix-proto-foxglove-overlays);
+                ] ++ (nix-proto.lib.overlayToList nix-proto-overlays);
               };
           };
       };
diff --git a/module.nix b/module.nix
index 00170bae..c6c62b6d 100644
--- a/module.nix
+++ b/module.nix
@@ -18,7 +18,7 @@ in
 
       serviceConfig = {
         After = [ "network.target" ];
-        ExecStart = "${pkgs.drivebrain_software}/bin/alpha_build /home/nixos/config/drivebrain_config.json ${pkgs.ht_can_pkg}/hytech.dbc";
+        ExecStart = "${pkgs.drivebrain_software}/bin/drivebrain_exe /home/nixos/config/drivebrain_config.json ${pkgs.ht_can_pkg}/hytech.dbc";
         ExecStop = "/bin/kill -9 $MAINPID";
         Restart = "on-failure";
       };
diff --git a/simulink_automation.nix b/simulink_automation.nix
index 72c5ba0a..ae16c35f 100644
--- a/simulink_automation.nix
+++ b/simulink_automation.nix
@@ -1,10 +1,10 @@
-{ pkgs, stdenv, cmake, drivebrain_core, simulink-automation-src }:
+{ pkgs, stdenv, cmake, drivebrain_core, db-simulink-gen-src, simulink_automation_msgs_proto_cpp }:
 
 stdenv.mkDerivation {
-    name = "Simulink-automation";
-    src = simulink-automation-src;
+    name = "matlab-math";
+    src = "${db-simulink-gen-src}/matlab_math.tar.gz";
     version = "1.0.0";
     nativeBuildInputs = [ cmake ];
-    propagatedBuildInputs = [ cmake drivebrain_core ];
+    propagatedBuildInputs = [ drivebrain_core simulink_automation_msgs_proto_cpp];
     # cmakeFlags = [ "-DCMAKE_FIND_DEBUG_MODE=ON" ];
 }
\ No newline at end of file
diff --git a/suggestions.md b/suggestions.md
new file mode 100644
index 00000000..2143d22f
--- /dev/null
+++ b/suggestions.md
@@ -0,0 +1,2 @@
+1) Fix init/constructor ambigiousness
+2) Add logging features
\ No newline at end of file
diff --git a/test/controller_manager_integration.cpp b/test/controller_manager_integration.cpp
new file mode 100755
index 00000000..b561220e
--- /dev/null
+++ b/test/controller_manager_integration.cpp
@@ -0,0 +1,75 @@
+#include "SimpleSpeedController.hpp"
+#include "SimpleTorqueController.hpp"
+
+#include <Controller.hpp>
+#include <ControllerManager.hpp>
+#include <JsonFileHandler.hpp>
+#include <VehicleDataTypes.hpp>
+#include <hytech.pb.h>
+
+#include <cstdio>
+#include <iostream>
+#include <memory>
+#include <chrono>
+core::JsonFileHandler _config("../config/test_tcmux_integration.json");
+std::shared_ptr<control::SimpleTorqueController> controller1(std::make_shared<control::SimpleTorqueController>(_config));
+std::shared_ptr<control::SimpleSpeedController> controller2(std::make_shared<control::SimpleSpeedController>(_config));
+
+         // for 1000 rpm
+        // _max_switch_rpm = ((*max_switch_speed) * constants::METERS_PER_SECOND_TO_RPM);
+        // METERS_PER_SECOND_TO_RPM    = 1.0 / RPM_TO_METERS_PER_SECOND;
+        // RPM_TO_METERS_PER_SECOND    = WHEEL_DIAMETER * 3.1415 / GEARBOX_RATIO / 60.0;
+        // GEARBOX_RATIO               = 11.86;
+        // WHEEL_DIAMETER              = 0.4064;
+
+
+int main(int argc, char **argv) {
+    controller1->init();
+    controller2->init();
+    // important to init controllers. maybe put this in constructor?
+    control::ControllerManager<control::Controller<core::ControllerOutput, core::VehicleState>, 2 > _controllerManager(
+        _config, {controller1, controller2}
+    );
+    _controllerManager.init();
+
+
+
+
+
+    core::VehicleState vehicle_state = {};
+    vehicle_state.is_ready_to_drive = true;
+    vehicle_state.input.requested_accel = 0.2;
+    vehicle_state.input.requested_brake = 0.0;
+    vehicle_state.state_is_valid = true;
+    std::cout << "Testing torque controller (index 0)" << std::endl;
+  
+    vehicle_state.current_rpms = {1000, 1000, 1000, 1000};
+    _controllerManager.swap_active_controller(1, vehicle_state);
+    std::cout << "Expected output: 0(failure due to high rpms). Actual output: " << _controllerManager.get_active_controller_index() << std::endl;
+    
+  
+    vehicle_state.current_rpms = {990, 990, 990, 990};
+    _controllerManager.swap_active_controller(1, vehicle_state);
+    std::cout << "Expected output: 0(failure due to high accel). Actual output: " << _controllerManager.get_active_controller_index() << std::endl;
+  
+    vehicle_state.input.requested_accel = 0.0;
+  
+    _controllerManager.swap_active_controller(0, vehicle_state);
+    std::cout << "Expected output: 0(failure due to same controller). Actual output: " << _controllerManager.get_active_controller_index() << std::endl;
+    // controller1.get_config();
+    vehicle_state.current_rpms = {990, 990, 990, 990};
+    _controllerManager.swap_active_controller(1, vehicle_state);
+    std::cout << "Expected output: 1(success). Actual output: " << _controllerManager.get_active_controller_index() << std::endl;
+  
+  
+  
+  
+    std::cout << "Current controller index: " << _controllerManager.get_active_controller_index() << std::endl;
+    // vehicle_state.current_rpms = {0, 0, 0, 0};
+  
+    _controllerManager.swap_active_controller(0, vehicle_state);
+    std::cout << "Expected output: 0(success). Actual output: " << _controllerManager.get_active_controller_index() << std::endl;
+    std::cout << "Current controller index: " << _controllerManager.get_active_controller_index() << std::endl;
+
+    return 0;
+}
\ No newline at end of file
diff --git a/test/sender.py b/test/sender.py
new file mode 100644
index 00000000..7d36f90a
--- /dev/null
+++ b/test/sender.py
@@ -0,0 +1,170 @@
+import tkinter as tk
+from tkinter import ttk
+import cantools
+import can
+import threading
+import time
+
+DBC_FILE = "config/hytech.dbc"
+CAN_INTERFACE = "vcan1"
+
+# Load CAN
+bus = can.Bus(channel=CAN_INTERFACE, interface="socketcan")
+db = cantools.database.load_file(DBC_FILE)
+
+
+class MessageFrame(tk.LabelFrame):
+    def __init__(self, master, message, *args, **kwargs):
+        super().__init__(master, text=message.name, *args, **kwargs)
+        self.message = message
+        self.entries = {}
+        self.build_fields()
+
+    def build_fields(self):
+        for i, signal in enumerate(self.message.signals):
+            ttk.Label(self, text=signal.name).grid(row=i, column=0, sticky="e", padx=2, pady=2)
+            entry = ttk.Entry(self)
+            entry.grid(row=i, column=1, padx=2, pady=2)
+            self.entries[signal.name] = entry
+
+    def get_encoded_message(self):
+        try:
+            data = {name: float(entry.get()) for name, entry in self.entries.items()}
+            encoded = self.message.encode(data)
+            return can.Message(arbitration_id=self.message.frame_id, is_extended_id=False, data=encoded)
+        except Exception as e:
+            print(f"[{self.message.name}] Encoding error:", e)
+            return None
+
+class ScrollableFrame(ttk.Frame):
+    def __init__(self, container, *args, **kwargs):
+        super().__init__(container, *args, **kwargs)
+        canvas = tk.Canvas(self)
+        scrollbar = ttk.Scrollbar(self, orient="vertical", command=canvas.yview)
+        self.scrollable_frame = ttk.Frame(canvas)
+
+        self.scrollable_frame.bind(
+            "<Configure>",
+            lambda e: canvas.configure(
+                scrollregion=canvas.bbox("all")
+            )
+        )
+
+        self.canvas = canvas
+
+        self.canvas_frame = canvas.create_window((0, 0), window=self.scrollable_frame, anchor="nw")
+
+        canvas.configure(yscrollcommand=scrollbar.set)
+
+        canvas.pack(side="left", fill="both", expand=True)
+        scrollbar.pack(side="right", fill="y")
+
+        # Fix width resizing
+        self.bind("<Configure>", self._resize)
+
+    def _resize(self, event):
+        self.canvas.itemconfig(self.canvas_frame, width=event.width)
+
+class ScrollableSelectorFrame(tk.LabelFrame):
+    def __init__(self, parent, label="Select Messages", height=500):
+        super().__init__(parent, text=label)
+
+        # Canvas and scrollbar
+        self.canvas = tk.Canvas(self, height=height)
+        self.scrollbar = ttk.Scrollbar(self, orient="vertical", command=self.canvas.yview)
+        self.scrollable_frame = ttk.Frame(self.canvas)
+
+        self.scrollable_frame.bind(
+            "<Configure>",
+            lambda e: self.canvas.configure(
+                scrollregion=self.canvas.bbox("all")
+            )
+        )
+
+        self.canvas_frame = self.canvas.create_window((0, 0), window=self.scrollable_frame, anchor="nw")
+        self.canvas.configure(yscrollcommand=self.scrollbar.set)
+
+        self.canvas.pack(side="left", fill="both", expand=True)
+        self.scrollbar.pack(side="right", fill="y")
+
+        # Resize canvas frame with window
+        self.bind("<Configure>", self._on_resize)
+
+    def _on_resize(self, event):
+        # Make the canvas window match the width of the label frame
+        self.canvas.itemconfig(self.canvas_frame, width=self.canvas.winfo_width())
+
+
+class CANMultiSenderGUI(tk.Tk):
+    def __init__(self, db):
+        super().__init__()
+        self.title("CAN Multi-Message Sender")
+        self.geometry("900x900")
+        self.db = db
+        self.frames = {}
+        self.check_vars = {}
+        self.running = False
+
+        self.build_ui()
+
+    def build_ui(self):
+        selector_wrapper = ttk.LabelFrame(self, text="Select Messages")
+        selector_wrapper.pack(fill="x", padx=5, pady=5)
+
+        self.selector_frame = ScrollableSelectorFrame(self, label="Select Messages", height=400)
+        self.selector_frame.pack(fill="x", padx=5, pady=5)
+
+        for msg in self.db.messages:
+            var = tk.BooleanVar()
+            cb = ttk.Checkbutton(
+                self.selector_frame.scrollable_frame,
+                text=msg.name,
+                variable=var,
+                command=self.refresh_messages
+            )
+            cb.pack(anchor="w")
+            self.check_vars[msg.name] = var
+
+        # Scrollable message container
+        self.scrollable_message_area = ScrollableFrame(self)
+        self.scrollable_message_area.pack(fill="both", expand=True, padx=5, pady=5)
+
+        # Start/Stop button
+        self.toggle_btn = ttk.Button(self, text="Start Sending", command=self.toggle_sending)
+        self.toggle_btn.pack(pady=10)
+
+    def refresh_messages(self):
+        # Clear all frames
+        for widget in self.scrollable_message_area.scrollable_frame.winfo_children():
+            widget.destroy()
+        self.frames.clear()
+
+        for name, var in self.check_vars.items():
+            if var.get():
+                msg = self.db.get_message_by_name(name)
+                frame = MessageFrame(self.scrollable_message_area.scrollable_frame, msg)
+                frame.pack(fill="x", padx=5, pady=5)
+                self.frames[name] = frame
+
+    def toggle_sending(self):
+        self.running = not self.running
+        if self.running:
+            self.toggle_btn.config(text="Stop Sending")
+            threading.Thread(target=self.send_loop, daemon=True).start()
+        else:
+            self.toggle_btn.config(text="Start Sending")
+
+    def send_loop(self):
+        while self.running:
+            for msg_name, frame in self.frames.items():
+                message = frame.get_encoded_message()
+                if message:
+                    try:
+                        bus.send(message)
+                    except Exception as e:
+                        print(f"Send error for {msg_name}:", e)
+            time.sleep(0.004)
+
+if __name__ == "__main__":
+    app = CANMultiSenderGUI(db)
+    app.mainloop()
\ No newline at end of file
diff --git a/test/test_configurable.cpp b/test/test_configurable.cpp
index 8c25ecf4..5f61d40f 100644
--- a/test/test_configurable.cpp
+++ b/test/test_configurable.cpp
@@ -1,3 +1,96 @@
+#include <Controller.hpp>
 #include <Configurable.hpp>
-#include <JsonFileHandler.hpp>
+#include <Logger.hpp>
+#include <chrono>
+#include <hytech_msgs.pb.h>
+#include <Literals.hpp>
+#include <hytech.pb.h>
+#include <VehicleDataTypes.hpp>
 
+#include <DrivebrainMCAPLogger.hpp>
+
+#include <MsgLogger.hpp>
+#include <memory>
+#include <nlohmann/json_fwd.hpp>
+#include <thread>
+#include <type_traits>
+#include <utility>
+#include <mutex>
+#include <variant>
+
+// ABOUT: this controller is an implementation of mode 0
+
+// this controller will be reactionary for now
+// namespace control
+// {
+
+    // TODO make the output CAN message for the drivetrain, rpms telem is just a standin for now
+class ConfigureableTest : public core::common::Configurable
+{
+public:
+    ConfigureableTest(core::Logger &logger, core::JsonFileHandler &json_file_handler) : _logger(logger), Configurable(json_file_handler, "ConfigureableTest") {}
+    bool init() override {
+        std::optional test_float = get_parameter_value<float>("test_float");
+        std::optional test_bool = get_parameter_value<bool>("test_bool");
+        std::optional test_double = get_parameter_value<double>("test_double");
+        std::optional test_string = get_parameter_value<std::string>("test_string");
+        std::optional test_int = get_parameter_value<int>("test_int");
+        // _configured = true;
+        set_configured();
+        return true;
+    }
+private:
+    core::Logger &_logger;
+};
+
+class ConfigureableTest2 : public core::common::Configurable
+{
+public:
+    ConfigureableTest2(core::Logger &logger, core::JsonFileHandler &json_file_handler) : _logger(logger), Configurable(json_file_handler, "asdf2") {}
+    bool init() override {
+        std::optional test_float = get_parameter_value<float>("test_val");
+        // _configured = true;
+        set_configured();
+        return true;
+    }
+private:
+    core::Logger &_logger;
+};
+
+int main()
+{
+    // TODO:
+
+    // - [x] test new configureable instantiation to ensure that schema gets incrementally created as new params are "gotten"
+    // - [ ] test writing of the schema and creation of the config topic in the mcap file that gets created
+    // - [ ] test the writing of the config json message containing all the config data from every component
+    core::JsonFileHandler config("../config/config_test.json");
+    core::Logger logger(core::LogLevel::INFO);
+
+    auto test = std::make_shared<ConfigureableTest>(logger, config);
+    auto test_inst = std::make_shared<ConfigureableTest2>(logger, config);
+    
+    
+
+    std::vector<std::weak_ptr<core::common::Configurable>> configureable_components;
+    configureable_components.push_back(test);
+    configureable_components.push_back(test_inst);
+
+    auto standin_foxglove_ws_send = [](std::shared_ptr<google::protobuf::Message> msg){};
+    auto mcap_logger = std::make_shared<common::DrivebrainMCAPLogger>("temp", configureable_components);
+
+    test->init(); // everything has to be initialized BEFORE the message logger is created.
+    test_inst->init();
+    auto msg_logger = std::make_unique<core::MsgLogger<std::shared_ptr<google::protobuf::Message>>>(".mcap", true, 
+        std::bind(&common::DrivebrainMCAPLogger::log_msg, mcap_logger, std::placeholders::_1),
+        std::bind(&common::DrivebrainMCAPLogger::close_current_mcap, mcap_logger),
+        std::bind(&common::DrivebrainMCAPLogger::open_new_mcap, mcap_logger, std::placeholders::_1),
+        standin_foxglove_ws_send,
+        std::bind(&common::DrivebrainMCAPLogger::init_param_schema, mcap_logger),
+        std::bind(&common::DrivebrainMCAPLogger::log_params, mcap_logger));
+    std::this_thread::sleep_for(std::chrono::duration_cast<std::chrono::seconds>(std::chrono::seconds(2)));
+    
+    // std::cout << test.get_schema().dump() << std::endl;
+    std::this_thread::sleep_for(std::chrono::duration_cast<std::chrono::seconds>(std::chrono::seconds(10)));
+    return 0;
+}
\ No newline at end of file
diff --git a/test/test_json_schema_creation.cpp b/test/test_json_schema_creation.cpp
new file mode 100644
index 00000000..c0127e09
--- /dev/null
+++ b/test/test_json_schema_creation.cpp
@@ -0,0 +1,6 @@
+#include <nlohmann/json.hpp>
+
+int main()
+{
+    return 0;
+}
\ No newline at end of file
diff --git a/test/test_mcu.cpp b/test/test_mcu.cpp
index ba647248..51beee6c 100644
--- a/test/test_mcu.cpp
+++ b/test/test_mcu.cpp
@@ -5,6 +5,7 @@
 #include <unistd.h>
 #include <thread>
 #include "hytech_msgs.pb.h"
+
 void sendSpeedControlInMessage(const std::string &ip, int port)
 {
     // Create a UDP socket
diff --git a/test/test_msglogger.cpp b/test/test_msglogger.cpp
index 21a95405..7d98b37e 100644
--- a/test/test_msglogger.cpp
+++ b/test/test_msglogger.cpp
@@ -1,91 +1,91 @@
-#include <MsgLogger.hpp>
-#include <fstream>      // std::ofstream
+// #include <MsgLogger.hpp>
+// #include <fstream>      // std::ofstream
 
-class IntegerFileWriter
-{
-public:
-    // Constructor (optional file name can be set here, but now handled in openFile)
-    IntegerFileWriter() {}
+// class IntegerFileWriter
+// {
+// public:
+//     // Constructor (optional file name can be set here, but now handled in openFile)
+//     IntegerFileWriter() {}
 
-    // Function to open the file with a given filename
-    void openFile(const std::string &filename)
-    {
-        filename_ = filename;
-        file_.open(filename_, std::ios::out | std::ios::trunc); // Open the file in output mode (truncate by default)
-        if (!file_.is_open())
-        {
-            std::cerr << "Unable to open file: " << filename_ << std::endl;
-        }
-        else
-        {
-            std::cout << "File opened: " << filename_ << std::endl;
-        }
-    }
+//     // Function to open the file with a given filename
+//     void openFile(const std::string &filename)
+//     {
+//         filename_ = filename;
+//         file_.open(filename_, std::ios::out | std::ios::trunc); // Open the file in output mode (truncate by default)
+//         if (!file_.is_open())
+//         {
+//             std::cerr << "Unable to open file: " << filename_ << std::endl;
+//         }
+//         else
+//         {
+//             std::cout << "File opened: " << filename_ << std::endl;
+//         }
+//     }
 
-    // Function to write an integer to the file
-    void writeInteger(int value)
-    {
-        if (file_.is_open())
-        {
-            file_ << value << "\n";
-            // std::cout << "Integer written to file: " << value << std::endl;
-        }
-        else
-        {
-            std::cerr << "File is not open. Cannot write integer." << std::endl;
-        }
-    }
+//     // Function to write an integer to the file
+//     void writeInteger(int value)
+//     {
+//         if (file_.is_open())
+//         {
+//             file_ << value << "\n";
+//             // std::cout << "Integer written to file: " << value << std::endl;
+//         }
+//         else
+//         {
+//             std::cerr << "File is not open. Cannot write integer." << std::endl;
+//         }
+//     }
 
-    // Function to close the file
-    void closeFile()
-    {
-        if (file_.is_open())
-        {
-            file_.close();
-            std::cout << "File closed." << std::endl;
-        }
-        else
-        {
-            std::cerr << "File is not open. Cannot close." << std::endl;
-        }
-    }
+//     // Function to close the file
+//     void closeFile()
+//     {
+//         if (file_.is_open())
+//         {
+//             file_.close();
+//             std::cout << "File closed." << std::endl;
+//         }
+//         else
+//         {
+//             std::cerr << "File is not open. Cannot close." << std::endl;
+//         }
+//     }
 
-    // Destructor to ensure the file is closed when the object is destroyed
-    ~IntegerFileWriter()
-    {
-        if (file_.is_open())
-        {
-            file_.close();
-        }
-    }
+//     // Destructor to ensure the file is closed when the object is destroyed
+//     ~IntegerFileWriter()
+//     {
+//         if (file_.is_open())
+//         {
+//             file_.close();
+//         }
+//     }
 
-private:
-    std::string filename_;
-    std::ofstream file_; // File stream member to manage the file state
-};
+// private:
+//     std::string filename_;
+//     std::ofstream file_; // File stream member to manage the file state
+// };
 
-int main()
-{
+// int main()
+// {
 
-    std::function<void(int)> live_telem_func = [&](int n) { std::cout << "live out " << n <<std::endl; };
-    IntegerFileWriter fw;
-    core::MsgLogger<int> logger_test(std::string(".txt"), true,
-                               std::bind(&IntegerFileWriter::writeInteger, std::ref(fw), std::placeholders::_1),
-                               std::bind(&IntegerFileWriter::closeFile, std::ref(fw)),
-                               std::bind(&IntegerFileWriter::openFile, std::ref(fw), std::placeholders::_1),
-                               live_telem_func);
+//     std::function<void(int)> live_telem_func = [&](int n) { std::cout << "live out " << n <<std::endl; };
+//     IntegerFileWriter fw;
+//     core::MsgLogger<int> logger_test(std::string(".txt"), true,
+//                                std::bind(&IntegerFileWriter::writeInteger, std::ref(fw), std::placeholders::_1),
+//                                std::bind(&IntegerFileWriter::closeFile, std::ref(fw)),
+//                                std::bind(&IntegerFileWriter::openFile, std::ref(fw), std::placeholders::_1),
+//                                live_telem_func);
     
-    int out =0;
-    while(out < 100)
-    {
-        out++;
-        logger_test.log_msg(out);
-    }
+//     int out =0;
+//     while(out < 100)
+//     {
+//         out++;
+//         logger_test.log_msg(out);
+//     }
 
-    while(true)
-    {
-        std::this_thread::sleep_for(std::chrono::milliseconds(3));
-    }
-    std::cout << "done logging, stopping" <<std::endl; 
-    logger_test.stop_logging_to_file();
-}
\ No newline at end of file
+//     while(true)
+//     {
+//         std::this_thread::sleep_for(std::chrono::milliseconds(3));
+//     }
+//     std::cout << "done logging, stopping" <<std::endl; 
+//     logger_test.stop_logging_to_file();
+// }
\ No newline at end of file
diff --git a/test/test_scale_comms.cpp b/test/test_scale_comms.cpp
new file mode 100644
index 00000000..090db593
--- /dev/null
+++ b/test/test_scale_comms.cpp
@@ -0,0 +1,15 @@
+#include <ScaleComms.hpp>
+#include <spdlog/spdlog.h>
+
+
+int main()
+{
+    spdlog::set_level(spdlog::level::debug);
+    core::JsonFileHandler config("config/test_scale_comms.json");
+    boost::asio::io_context io_context;
+    comms::ScaleComms scale_comms(config, io_context);
+    (void)scale_comms.init();
+    
+    io_context.run();
+    return 0;
+}
diff --git a/test/test_send_can.py b/test/test_send_can.py
new file mode 100644
index 00000000..ec9ba568
--- /dev/null
+++ b/test/test_send_can.py
@@ -0,0 +1,25 @@
+import socket
+import time
+import can
+import cantools
+from pprint import pprint
+import os
+
+from enum import Enum
+
+bus1 = can.Bus(channel="vcan1", interface='socketcan')
+
+def main():
+    db = cantools.database.load_file("hytech_141.dbc")
+    dynamics = db.get_message_by_name("INV1_DYNAMICS")
+
+    inverter_on_msg = dynamics.encode({'actual_power_w': 0, 'actual_torque_nm': 0, 'actual_speed_rpm': 6969})
+
+    while(1):
+        msg = can.Message(arbitration_id=dynamics.frame_id, is_extended_id=False, data = inverter_on_msg)
+        bus1.send(msg)
+        
+        time.sleep(0.004)
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file
diff --git a/test/test_speedtech_comms.cpp b/test/test_speedtech_comms.cpp
new file mode 100644
index 00000000..66d6f3b1
--- /dev/null
+++ b/test/test_speedtech_comms.cpp
@@ -0,0 +1,14 @@
+#include <SpeedTechComms.hpp>
+#include <spdlog/spdlog.h>
+
+int main()
+{
+    spdlog::set_level(spdlog::level::debug);
+    core::JsonFileHandler config("config/test_speedtech_comms.json");
+    boost::asio::io_context io_context;
+    comms::SpeedTechComms speed_tech_lap_timer_driver(config, io_context);
+    (void)speed_tech_lap_timer_driver.init();
+    
+    io_context.run();
+    return 0;
+}
\ No newline at end of file
diff --git a/test/test_surrey_aero_comms.cpp b/test/test_surrey_aero_comms.cpp
new file mode 100644
index 00000000..9a1340e6
--- /dev/null
+++ b/test/test_surrey_aero_comms.cpp
@@ -0,0 +1,15 @@
+#include <SurreyAeroComms.hpp>
+#include <spdlog/spdlog.h>
+
+
+int main()
+{
+    spdlog::set_level(spdlog::level::debug);
+    core::JsonFileHandler config("config/test_surrey_aero_comms.json");
+    boost::asio::io_context io_context;
+    comms::SurreyAeroComms aero_comms(config, io_context);
+    (void)aero_comms.init();
+    
+    io_context.run();
+    return 0;
+}
diff --git a/unit_test/ControllerTesting.cpp b/unit_test/ControllerTesting.cpp
new file mode 100644
index 00000000..f60013eb
--- /dev/null
+++ b/unit_test/ControllerTesting.cpp
@@ -0,0 +1,96 @@
+#include "LoadCellVectoringTorqueController.hpp"
+#include <Literals.hpp>
+#include <gtest/gtest.h>
+#include <LoadCellVectoringTorqueController.hpp>
+#include <VehicleDataTypes.hpp>
+#include <JsonFileHandler.hpp>
+#include <Logger.hpp>
+#include <Utils.hpp>
+
+class ControllerTesting : public testing::Test {
+
+protected:
+    core::Logger logger;
+    core::JsonFileHandler config;
+    control::LoadCellVectoringTorqueController lctv;
+    core::VehicleState in;
+
+    ControllerTesting()
+        : logger(core::LogLevel::INFO), 
+        config("../config/drivebrain_config.json"),
+        lctv(config),
+        in()
+    {
+        in.is_ready_to_drive = true;
+        in.current_rpms = { 0.0, 0.0, 0.0, 0.0 };
+        in.current_body_vel_ms = { 0.0 , 0.0 , 0.0 };
+        in.input = { 0.0, 0.0 };
+    }
+
+    void SetUp() override {
+        lctv.init();
+    }
+};
+
+
+TEST_F(ControllerTesting, FullBrakeRequestLCTV)
+{
+    in.input.requested_brake = 1.0;
+    in.loadcells = {1.0, 1.0, 1.0, 1.0}; // simulate balanced weight
+
+    auto cmd = lctv.step_controller(in);
+    auto res = std::get_if<core::SpeedControlOut>(&cmd.out);
+
+    // All RPMs should be 0
+    ASSERT_NEAR(res->desired_rpms.FL, 0.0, 1e-3);
+    ASSERT_NEAR(res->desired_rpms.FR, 0.0, 1e-3);
+    ASSERT_NEAR(res->desired_rpms.RL, 0.0, 1e-3);
+    ASSERT_NEAR(res->desired_rpms.RR, 0.0, 1e-3);
+
+    float regen_torque_pool = 1.0 * std::get<float>(lctv.get_cached_param("max_regen_torque"));
+
+    float front_scale = (2.0 - std::get<float>(lctv.get_cached_param("rear_torque_scale")));
+    float rear_scale = std::get<float>(lctv.get_cached_param("rear_torque_scale"));
+
+    ASSERT_NEAR(res->torque_lim_nm.FL, regen_torque_pool * 1.0 * front_scale, 0.01);
+    ASSERT_NEAR(res->torque_lim_nm.FR, regen_torque_pool * 1.0 * front_scale, 0.01);
+    ASSERT_NEAR(res->torque_lim_nm.RL, regen_torque_pool * 1.0 * rear_scale, 0.01);
+    ASSERT_NEAR(res->torque_lim_nm.RR, regen_torque_pool * 1.0 * rear_scale, 0.01);
+}
+
+TEST_F(ControllerTesting, FullPositiveAccelProportionalToLoad)
+{
+    in.input.requested_accel = 1.0;
+    in.loadcells = {1.2f, 0.8f, 1.2f, 0.8f}; // FL, FR, RL, RR
+
+    auto cmd = lctv.step_controller(in);
+    auto res = std::get_if<core::SpeedControlOut>(&cmd.out);
+    ASSERT_NE(res, nullptr);
+
+    float speed_set = std::get<float>(lctv.get_cached_param("positive_speed_set"));
+    float max_torque = std::get<float>(lctv.get_cached_param("max_torque"));
+    float rpm_expected = speed_set * constants::METERS_PER_SECOND_TO_RPM;
+
+    // Confirm all wheels are set to the correct RPM
+    ASSERT_NEAR(res->desired_rpms.FL, rpm_expected, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR, rpm_expected, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL, rpm_expected, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR, rpm_expected, 1.0);
+
+    // Total available torque across all wheels
+    float torque_budget = max_torque * 4.0f;
+
+    // Sum of normalized corner loads
+    float total_load = 1.2f + 0.8f + 1.2f + 0.8f;
+
+    // Expected torque = (normalized_load / total_load) * torque_budget
+    float FL_torque = (1.2f / total_load) * torque_budget;
+    float FR_torque = (0.8f / total_load) * torque_budget;
+    float RL_torque = (1.2f / total_load) * torque_budget;
+    float RR_torque = (0.8f / total_load) * torque_budget;
+
+    ASSERT_NEAR(res->torque_lim_nm.FL, FL_torque, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.FR, FR_torque, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, RL_torque, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, RR_torque, 1.0);
+}
\ No newline at end of file
diff --git a/unit_test/SimpleControllerTest.cpp b/unit_test/SimpleControllerTest.cpp
deleted file mode 100644
index cc70780a..00000000
--- a/unit_test/SimpleControllerTest.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-#include <gtest/gtest.h>
-#include <SimpleController.hpp>
-#include <VehicleDataTypes.hpp>
-#include <JsonFileHandler.hpp>
-#include <Logger.hpp>
-#include <Utils.hpp>
-
-class SimpleControllerTest : public testing::Test {
-
-    protected:
-        core::Logger logger;
-        core::JsonFileHandler config;
-        control::SimpleController simple_controller;
-
-        SimpleControllerTest()
-            : logger(core::LogLevel::INFO), 
-            config("../config/test_config/can_driver.json"), // TODO probably want a better way to get param path
-            simple_controller(logger, config) {
-        }
-
-        void SetUp() override {
-            simple_controller.init();
-        }
-
-        void TearDown() override {
-            // Shouldn't need anything here
-        }
-
-};
-
-TEST_F(SimpleControllerTest, MaxRPM) {
-    core::VehicleState in;
-    auto res = simple_controller.step_controller(in);
-    ASSERT_LT(res.desired_rpms.FL, 20000);
-}
diff --git a/unit_test/SimpleSpeedControllerTest.cpp b/unit_test/SimpleSpeedControllerTest.cpp
new file mode 100644
index 00000000..33b99f75
--- /dev/null
+++ b/unit_test/SimpleSpeedControllerTest.cpp
@@ -0,0 +1,332 @@
+#include <Literals.hpp>
+#include <cassert>
+#include <gtest/gtest.h>
+#include <SimpleSpeedController.hpp>
+#include <VehicleDataTypes.hpp>
+#include <JsonFileHandler.hpp>
+#include <Logger.hpp>
+#include <Utils.hpp>
+
+class SimpleSpeedControllerTest : public testing::Test {
+
+protected:
+    core::Logger logger;
+    core::JsonFileHandler config;
+    core::JsonFileHandler fail_config;
+    control::SimpleSpeedController simple_controller;
+    control::SimpleSpeedController fail_controller;
+    core::VehicleState in;
+
+    SimpleSpeedControllerTest()
+        : logger(core::LogLevel::INFO), 
+        config("../config/drivebrain_config.json"),
+        fail_config("../config/fail_config.json"),
+        simple_controller(config),
+        fail_controller(fail_config),
+        in()
+    {
+        in.is_ready_to_drive = true;
+        in.current_rpms = { 0.0, 0.0, 0.0, 0.0 };
+        in.current_body_vel_ms = { 0.0 , 0.0 , 0.0 };
+        in.input = { 0.0, 0.0 };
+    }
+
+    void SetUp() override {
+        assert(simple_controller.init());
+    }
+};
+
+
+TEST_F(SimpleSpeedControllerTest, InitHasConfig)
+{
+    EXPECT_TRUE(simple_controller.init());
+}
+
+TEST_F(SimpleSpeedControllerTest, InitDoesNotHaveConfig)
+{
+    EXPECT_FALSE(fail_controller.init());
+}
+
+TEST_F(SimpleSpeedControllerTest, NoPedalInput)
+{
+
+    in.input.requested_accel = 0.0;
+    in.input.requested_brake = 0.0;   
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::SpeedControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_rpms.FL,std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR,std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL,std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR,std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+
+    ASSERT_NEAR(res->torque_lim_nm.FR, 0.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.FL, 0.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, 0.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, 0.0, 1.0); 
+}
+
+TEST_F(SimpleSpeedControllerTest, SmallPositiveAccelRequest)
+{
+    in.input.requested_accel = 0.2;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::SpeedControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_rpms.FL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+
+    ASSERT_NEAR(res->torque_lim_nm.FR, 4.48, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.FL, 4.48, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, 4.48, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, 4.48, 1.0);
+}
+
+TEST_F(SimpleSpeedControllerTest, FullPositiveAccelRequest)
+{
+    in.input.requested_accel = 1;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::SpeedControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_rpms.FL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+
+    ASSERT_NEAR(res->torque_lim_nm.FR, 22.4, 2.0);
+    ASSERT_NEAR(res->torque_lim_nm.FL, 22.4, 2.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, 22.4, 2.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, 22.4, 2.0);
+}
+
+TEST_F(SimpleSpeedControllerTest, SmallNegativeAccelRequest)
+{
+    in.input.requested_accel = 0.2;
+    in.input.requested_brake = 0.8;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::SpeedControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_rpms.FL, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR, 0.0, 1.0);
+
+    ASSERT_NEAR(res->torque_lim_nm.FL, 6.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.FR, 6.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, 6.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, 6.0, 1.0);
+}
+
+TEST_F(SimpleSpeedControllerTest, FullNegativeAccelRequest)
+{
+    in.input.requested_brake = 1;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::SpeedControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_rpms.FL, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR, 0.0, 1.0);
+
+    ASSERT_NEAR(res->torque_lim_nm.FL, 10.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.FR, 10.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, 10.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, 10.0, 1.0);
+}
+
+TEST_F(SimpleSpeedControllerTest, FullBrakeAndAccelRequest)
+{
+    in.input.requested_brake = 1;
+    in.input.requested_accel = 1;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::SpeedControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_rpms.FL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.FR, 0.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.FL, 0.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, 0.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, 0.0, 1.0);
+}
+
+
+
+
+
+
+
+
+
+
+// Ben's tests
+
+TEST_F(SimpleSpeedControllerTest, SimpleAccel) {
+
+    // // --- Test Case 1: Simple Acceleration ---
+    {
+        core::VehicleState in;
+        veh_vec<float> current_rpms; 
+        current_rpms.FL = 100; 
+        current_rpms.FR = 100;
+        current_rpms.RL = 100;
+        current_rpms.RR = 100;
+        in.current_rpms = current_rpms;
+        in.input.requested_accel = 0.1;
+        in.input.requested_brake = 0;
+        in.prev_MCU_recv_millis = 0;
+
+        auto res = std::get<core::SpeedControlOut>(simple_controller.step_controller(in).out);
+        std::cout <<"asdf: " << res.desired_rpms.FL <<std::endl;
+        // assert 
+        ASSERT_NEAR(res.desired_rpms.FL, 25082, 1);
+        ASSERT_NEAR(res.desired_rpms.FR, 25082, 1);
+        ASSERT_NEAR(res.desired_rpms.RL, 25082, 1);
+        ASSERT_NEAR(res.desired_rpms.RR, 25082, 1);
+
+        // std::cout << res.torque_lim_nm.FL <<std::endl;
+        ASSERT_NEAR(res.torque_lim_nm.FL, 2.1, 0.01);
+        ASSERT_NEAR(res.torque_lim_nm.FR, 2.1, 0.01);
+        ASSERT_NEAR(res.torque_lim_nm.RL, 2.1, 0.01);
+        ASSERT_NEAR(res.torque_lim_nm.RR, 2.1, 0.01);
+    }
+}
+
+TEST_F(SimpleSpeedControllerTest, FullBraking)
+{
+    // --- Test Case 2: Full Braking ---
+    {
+        core::VehicleState in;
+        veh_vec<float> current_rpms;
+        current_rpms.FL = 100;
+        current_rpms.FR = 100;
+        current_rpms.RL = 100;
+        current_rpms.RR = 100;
+        in.current_rpms = current_rpms;
+        in.input.requested_accel = 0;
+        in.input.requested_brake = 1.0;
+        in.prev_MCU_recv_millis = 0;
+
+        auto res = std::get<core::SpeedControlOut>(simple_controller.step_controller(in).out);
+        ASSERT_EQ(res.desired_rpms.FL, 0);
+        ASSERT_EQ(res.desired_rpms.FR, 0);
+        ASSERT_EQ(res.desired_rpms.RL, 0);
+        ASSERT_EQ(res.desired_rpms.RR, 0);
+        ASSERT_EQ(res.torque_lim_nm.FL, 10);
+        ASSERT_EQ(res.torque_lim_nm.FR, 10);
+        ASSERT_EQ(res.torque_lim_nm.RL, 10);
+        ASSERT_EQ(res.torque_lim_nm.RR, 10);
+    }
+}
+
+TEST_F(SimpleSpeedControllerTest, ZerlAccelZeroBrake)
+{
+    // --- Test Case 3: Zero Acceleration and Zero Brake (Coasting) ---
+    {
+        core::VehicleState in;
+        veh_vec<float> current_rpms;
+        current_rpms.FL = 100;
+        current_rpms.FR = 100;
+        current_rpms.RL = 100;
+        current_rpms.RR = 100;
+        in.current_rpms = current_rpms;
+        in.input.requested_accel = 0;
+        in.input.requested_brake = 0.0;
+        in.prev_MCU_recv_millis = 0;
+
+        auto res = std::get<core::SpeedControlOut>(simple_controller.step_controller(in).out);
+        
+        ASSERT_EQ(res.torque_lim_nm.FL, 0);
+        ASSERT_EQ(res.torque_lim_nm.FR, 0);
+        ASSERT_EQ(res.torque_lim_nm.RL, 0);
+        ASSERT_EQ(res.torque_lim_nm.RR, 0);
+    }
+}
+
+
+TEST_F(SimpleSpeedControllerTest, VariableRequests)
+{
+    in.input.requested_brake = 1;
+    in.input.requested_accel = 1;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::SpeedControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_rpms.FL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+
+    ASSERT_NEAR(res->torque_lim_nm.FR, 0.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.FL, 0.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, 0.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, 0.0, 1.0);
+
+    in.input.requested_accel = 0;
+    cmd = simple_controller.step_controller(in);
+
+    ASSERT_NEAR(res->desired_rpms.FL, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR, 0.0, 1.0);
+
+    ASSERT_NEAR(res->torque_lim_nm.FL, 10.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.FR, 10.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, 10.0, 1.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, 10.0, 1.0);
+
+    in.input.requested_accel = 1;
+    in.input.requested_brake = 0;
+    cmd = simple_controller.step_controller(in);
+
+    ASSERT_NEAR(res->desired_rpms.FL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.FR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RL, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+    ASSERT_NEAR(res->desired_rpms.RR, std::get<float>(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0);
+
+    ASSERT_NEAR(res->torque_lim_nm.FR, 22.4, 2.0);
+    ASSERT_NEAR(res->torque_lim_nm.FL, 22.4, 2.0);
+    ASSERT_NEAR(res->torque_lim_nm.RR, 22.4, 2.0);
+    ASSERT_NEAR(res->torque_lim_nm.RL, 22.4, 2.0);
+}
+
+
+
+TEST_F(SimpleSpeedControllerTest, TestPowerLimit)
+{
+    // // --- Test Case 4: Power Limiting Scenario ---
+    core::VehicleState in;
+    veh_vec<float> current_rpms;
+    current_rpms.FL = 10000;
+    current_rpms.FR = 10000;
+    current_rpms.RL = 10000;
+    current_rpms.RR = 10000;
+    in.current_rpms = current_rpms;
+    in.input.requested_accel = 1.0;
+    in.input.requested_brake = 0;
+    in.prev_MCU_recv_millis = 0;
+
+    auto res = std::get<core::SpeedControlOut>(simple_controller.step_controller(in).out);
+
+    ASSERT_NEAR(res.desired_rpms.FL, 25082, 1);
+    ASSERT_NEAR(res.desired_rpms.FR, 25082, 1);
+    ASSERT_NEAR(res.desired_rpms.RL, 25082, 1);
+    ASSERT_NEAR(res.desired_rpms.RR, 25082, 1);
+
+    ASSERT_LT(res.torque_lim_nm.FL, 21);  // Expect power limiting applied
+    ASSERT_LT(res.torque_lim_nm.FR, 21);
+    ASSERT_LT(res.torque_lim_nm.RL, 21);
+    ASSERT_LT(res.torque_lim_nm.RR, 21);
+
+
+    constexpr long double RPM_TO_RAD_PER_SECOND = 2.0 * 3.1415 / 60.0;
+    
+    float net_power = 0;
+    net_power += ::abs(res.torque_lim_nm.FL) * (current_rpms.FL * constants::RPM_TO_RAD_PER_SECOND);
+    net_power += ::abs(res.torque_lim_nm.FR) * (current_rpms.FR * constants::RPM_TO_RAD_PER_SECOND);
+    net_power += ::abs(res.torque_lim_nm.RL) * (current_rpms.RL * constants::RPM_TO_RAD_PER_SECOND);
+    net_power += ::abs(res.torque_lim_nm.RR) * (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND);
+
+    ASSERT_NEAR(net_power, 63000.0, 0.001);  // Expect power limiting applied and ensure near 63kw (hard-coded limit)
+}
diff --git a/unit_test/SimpleTorqueControllerTest.cpp b/unit_test/SimpleTorqueControllerTest.cpp
new file mode 100644
index 00000000..ccb65fbb
--- /dev/null
+++ b/unit_test/SimpleTorqueControllerTest.cpp
@@ -0,0 +1,150 @@
+#include <gtest/gtest.h>
+#include <SimpleTorqueController.hpp>
+#include <VehicleDataTypes.hpp>
+#include <JsonFileHandler.hpp>
+#include <Logger.hpp>
+#include <Utils.hpp>
+
+class SimpleTorqueControllerTest : public testing::Test {
+
+protected:
+    core::Logger logger;
+    core::JsonFileHandler config;
+    core::JsonFileHandler fail_config;
+    control::SimpleTorqueController simple_controller;
+    control::SimpleTorqueController fail_controller;
+    core::VehicleState in;
+
+    SimpleTorqueControllerTest()
+        : logger(core::LogLevel::INFO), 
+        config("../config/simple_torque_controller_test.json"),
+        fail_config("../config/fail_config.json"),
+        simple_controller(config),
+        fail_controller(fail_config),
+        in()
+    {
+        in.is_ready_to_drive = true;
+        in.current_rpms = { 0.0, 0.0, 0.0, 0.0 };
+        in.current_body_vel_ms = { 0.0 , 0.0 , 0.0 };
+        in.input = { 0.0, 0.0 };
+    }
+
+    void SetUp() override {
+        simple_controller.init();
+    }
+};
+
+
+
+TEST_F(SimpleTorqueControllerTest, InitHasConfig)
+{
+    EXPECT_TRUE(simple_controller.init());
+}
+
+TEST_F(SimpleTorqueControllerTest, InitDoesNotHaveConfig)
+{
+    EXPECT_FALSE(fail_controller.init());
+}
+
+TEST_F(SimpleTorqueControllerTest, NoPedalInput)
+{
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::TorqueControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_torques_nm.FR, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.FL, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RR, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RL, 0.0, 1.0); 
+}
+
+TEST_F(SimpleTorqueControllerTest, SmallPositiveAccelRequest)
+{
+    in.input.requested_accel = 0.2;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::TorqueControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_torques_nm.FR, 4.48, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.FL, 4.48, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RR, 4.48, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RL, 4.48, 1.0);
+}
+
+TEST_F(SimpleTorqueControllerTest, FullPositiveAccelRequest)
+{
+    in.input.requested_accel = 1;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::TorqueControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_torques_nm.FR, 21, 2.0);
+    ASSERT_NEAR(res->desired_torques_nm.FL, 21, 2.0);
+    ASSERT_NEAR(res->desired_torques_nm.RR, 21, 2.0);
+    ASSERT_NEAR(res->desired_torques_nm.RL, 21, 2.0);
+}
+
+TEST_F(SimpleTorqueControllerTest, SmallNegativeAccelRequest)
+{
+    in.input.requested_accel = 0.2;
+    in.input.requested_brake = 0.8;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::TorqueControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_torques_nm.FL, 6.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.FR, 6.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RL, 6.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RR, 6.0, 1.0);
+}
+
+TEST_F(SimpleTorqueControllerTest, FullNegativeAccelRequest)
+{
+    in.input.requested_brake = 1;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::TorqueControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_torques_nm.FL, 10.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.FR, 10.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RL, 10.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RR, 10.0, 1.0);
+}
+
+TEST_F(SimpleTorqueControllerTest, FullBrakeAndAccelRequest)
+{
+    in.input.requested_brake = 1;
+    in.input.requested_accel = 1;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::TorqueControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_torques_nm.FR, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.FL, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RR, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RL, 0.0, 1.0);
+}
+
+TEST_F(SimpleTorqueControllerTest, VariableRequests)
+{
+    in.input.requested_brake = 1;
+    in.input.requested_accel = 1;
+    auto cmd = simple_controller.step_controller(in);
+    auto res = std::get_if<core::TorqueControlOut>(&cmd.out);
+
+    ASSERT_NEAR(res->desired_torques_nm.FR, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.FL, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RR, 0.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RL, 0.0, 1.0);
+
+    in.input.requested_accel = 0;
+    cmd = simple_controller.step_controller(in);
+
+    ASSERT_NEAR(res->desired_torques_nm.FL, 10.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.FR, 10.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RL, 10.0, 1.0);
+    ASSERT_NEAR(res->desired_torques_nm.RR, 10.0, 1.0);
+
+    in.input.requested_accel = 1;
+    in.input.requested_brake = 0;
+    cmd = simple_controller.step_controller(in);
+
+    ASSERT_NEAR(res->desired_torques_nm.FR, 21, 2.0);
+    ASSERT_NEAR(res->desired_torques_nm.FL, 21, 2.0);
+    ASSERT_NEAR(res->desired_torques_nm.RR, 21, 2.0);
+    ASSERT_NEAR(res->desired_torques_nm.RL, 21, 2.0);
+}
diff --git a/unit_test/main.cpp b/unit_test/main.cpp
index 2dc3787b..7b493a48 100644
--- a/unit_test/main.cpp
+++ b/unit_test/main.cpp
@@ -1,5 +1,68 @@
 #include <gtest/gtest.h>
 
+#include <gtest/gtest.h>
+#include <VehicleDataTypes.hpp>
+#include <JsonFileHandler.hpp>
+#include <Logger.hpp>
+#include <Utils.hpp>
+
+// class SimpleControllerTest : public testing::Test {
+
+//     protected:
+//         core::Logger logger;
+//         core::JsonFileHandler config;
+//         std::unique_ptr<control::SimpleController> simple_controller;
+
+//         SimpleControllerTest()
+//         :
+//         logger(core::LogLevel::INFO),
+//         config(std::string("../config/drivebrain_config.json"))
+//         {
+//             simple_controller = std::make_unique<control::SimpleController>(logger, config);
+//         }
+        
+
+//         void SetUp() override {
+//             ASSERT_NE(simple_controller, nullptr);
+//             simple_controller->init();
+//         }
+// };
+
+// TEST(SimpleControllerTest, SimpleAccel) {
+//     core::Logger logger(core::LogLevel::INFO);
+//     core::JsonFileHandler config("../config/drivebrain_config.json");
+//     control::SimpleController simple_controller(config);
+//     core::VehicleState in{};
+//     veh_vec<float> current_rpms{}; 
+
+//     current_rpms.FL = 100.0f; 
+//     current_rpms.FR = 100.0f;
+//     current_rpms.RL = 100.0f;
+//     current_rpms.RR = 100.0f;
+
+//     in.current_rpms = current_rpms;
+//     in.input.requested_accel = 1.0f;
+//     in.input.requested_brake = 0.0f;
+//     in.prev_MCU_recv_millis = 0;
+
+//     // Debugging Checks
+//     ASSERT_TRUE(std::isfinite(in.input.requested_accel));
+//     ASSERT_TRUE(std::isfinite(in.input.requested_brake));
+
+//     auto res = simple_controller.step_controller(in);
+
+
+//     // EXPECT_FLOAT_EQ(res.desired_rpms.FL, 180.0f);
+//     // ASSERT_EQ(res.desired_rpms.FR, 180);
+//     // ASSERT_EQ(res.desired_rpms.RL, 180);
+//     // ASSERT_EQ(res.desired_rpms.RR, 180);
+//     // ASSERT_EQ(res.torque_lim_nm.FL, 210);
+//     // ASSERT_EQ(res.torque_lim_nm.FR, 210);
+//     // ASSERT_EQ(res.torque_lim_nm.RL, 210);
+//     // ASSERT_EQ(res.torque_lim_nm.RR, 210);
+
+// }
+
 int main(int argc, char **argv) {
     ::testing::InitGoogleTest(&argc, argv);
     return RUN_ALL_TESTS();
diff --git a/unit_test/test_controller_manager.cpp b/unit_test/test_controller_manager.cpp
new file mode 100644
index 00000000..7b946e7b
--- /dev/null
+++ b/unit_test/test_controller_manager.cpp
@@ -0,0 +1,210 @@
+#include <gtest/gtest.h>
+#include "ControllerManager.hpp"
+#include "Controller.hpp"
+#include "Controllers.hpp"
+#include "SimpleSpeedController.hpp"
+#include "SimpleTorqueController.hpp"
+
+#include <VehicleDataTypes.hpp>
+#include <memory>
+
+class ControllerManagerTest : public ::testing::Test {
+protected:
+    std::shared_ptr<control::SimpleSpeedController> simpleSpeedController1;
+    std::shared_ptr<control::SimpleTorqueController> simpleTorqueController1;
+    std::shared_ptr<control::SimpleSpeedController> simpleSpeedController2;
+    std::shared_ptr<control::SimpleTorqueController> simpleTorqueController2;
+    control::ControllerManager<control::Controller<core::ControllerOutput, core::VehicleState>, 2> controller_manager_2speed;
+    control::ControllerManager<control::Controller<core::ControllerOutput, core::VehicleState>, 2> controller_manager_2torque;
+    control::ControllerManager<control::Controller<core::ControllerOutput, core::VehicleState>, 2> controller_manager_diff;
+    core::JsonFileHandler json_file_handler; 
+
+    core::VehicleState vehicle_state;
+    core::ControllerOutput torque_controller_output;
+    core::ControllerOutput speed_controller_output;
+
+    ControllerManagerTest()
+        : 
+          json_file_handler("../config/test_controller_manager.json"),
+          simpleSpeedController1(std::make_shared<control::SimpleSpeedController>(json_file_handler)),
+          simpleSpeedController2(std::make_shared<control::SimpleSpeedController>(json_file_handler)),
+          simpleTorqueController1(std::make_shared<control::SimpleTorqueController>(json_file_handler)),
+          simpleTorqueController2(std::make_shared<control::SimpleTorqueController>(json_file_handler)),
+          controller_manager_2speed(json_file_handler, {simpleSpeedController1, simpleSpeedController2}),
+          controller_manager_2torque(json_file_handler, {simpleTorqueController1, simpleTorqueController2}),
+          controller_manager_diff(json_file_handler, {simpleSpeedController1, simpleTorqueController1})
+    {
+    }
+
+    void SetUp() override {
+        vehicle_state.is_ready_to_drive = true;
+        vehicle_state.input.requested_accel = 0.0;
+        vehicle_state.input.requested_brake = 0.0;
+        vehicle_state.current_rpms = {1000, 1000, 1000, 1000};
+
+        simpleSpeedController1->init();
+        simpleSpeedController2->init();
+        simpleTorqueController1->init();
+        simpleTorqueController2->init();
+
+
+        (void)controller_manager_2speed.init();
+        controller_manager_2speed.update_controllers({simpleSpeedController1, simpleSpeedController2});
+        (void)controller_manager_2torque.init();
+        controller_manager_2torque.update_controllers({simpleTorqueController1, simpleTorqueController2});
+        (void)controller_manager_diff.init();
+        controller_manager_diff.update_controllers({simpleSpeedController1, simpleTorqueController1});
+        // std::cout << "set up" << std::endl;
+    }
+
+    void TearDown() override {
+        // Clean up after each test if necessary
+    }
+};
+
+// Test the initialization
+TEST_F(ControllerManagerTest, InitializationSuccess) {
+    ASSERT_TRUE(controller_manager_2speed.init());
+    ASSERT_TRUE(controller_manager_2torque.init());
+    ASSERT_TRUE(controller_manager_diff.init());
+}
+
+// Test active controller timestep retrieval
+TEST_F(ControllerManagerTest, GetActiveControllerTimestep) {
+    EXPECT_EQ(controller_manager_2speed.get_active_controller_timestep(), 0.001f);
+    EXPECT_EQ(controller_manager_2torque.get_active_controller_timestep(), 0.001f);
+    EXPECT_EQ(controller_manager_diff.get_active_controller_timestep(), 0.001f);
+}
+
+// Test stepping the active controller
+TEST_F(ControllerManagerTest, StepActiveTorqueController) {
+    vehicle_state.input.requested_accel = 1.0;
+    core::ControllerOutput output = controller_manager_2torque.step_active_controller(vehicle_state);
+    
+    ASSERT_TRUE(std::holds_alternative<core::TorqueControlOut>(output.out));
+    auto torque_output = std::get<core::TorqueControlOut>(output.out);
+    EXPECT_EQ(torque_output.desired_torques_nm.FL, 21.0);
+    EXPECT_EQ(torque_output.desired_torques_nm.FR, 21.0);
+    EXPECT_EQ(torque_output.desired_torques_nm.RL, 21.0);
+    EXPECT_EQ(torque_output.desired_torques_nm.RR, 21.0);
+}
+
+TEST_F(ControllerManagerTest, StepActiveSpeedController) {
+    vehicle_state.input.requested_accel = 1.0;
+    core::ControllerOutput output = controller_manager_2speed.step_active_controller(vehicle_state);
+    
+    ASSERT_TRUE(std::holds_alternative<core::SpeedControlOut>(output.out));
+    auto speed_output = std::get<core::SpeedControlOut>(output.out);
+    EXPECT_EQ(speed_output.torque_lim_nm.FL, 21.0);
+    EXPECT_EQ(speed_output.torque_lim_nm.FR, 21.0);
+    EXPECT_EQ(speed_output.torque_lim_nm.RL, 21.0);
+    EXPECT_EQ(speed_output.torque_lim_nm.RR, 21.0);
+}
+
+//swap between same controller outputs
+TEST_F(ControllerManagerTest, SwapSameTypes) {
+    vehicle_state.current_rpms = {100, 100, 100, 100};
+    ASSERT_FALSE(controller_manager_diff.swap_active_controller(2, vehicle_state));
+
+    core::ControllerOutput output = controller_manager_diff.step_active_controller(vehicle_state);
+    ASSERT_TRUE(std::holds_alternative<core::SpeedControlOut>(output.out));
+}
+
+//swap between same controller outputs
+TEST_F(ControllerManagerTest, SwapSameIndex) {
+    vehicle_state.current_rpms = {100, 100, 100, 100};
+    ASSERT_FALSE(controller_manager_diff.swap_active_controller(0, vehicle_state));
+
+    core::ControllerOutput output = controller_manager_diff.step_active_controller(vehicle_state);
+    ASSERT_TRUE(std::holds_alternative<core::SpeedControlOut>(output.out));
+}
+
+// Test switching controllers
+TEST_F(ControllerManagerTest, SwapBetweenTypes) {
+    vehicle_state.current_rpms = {100, 100, 100, 100};
+    ASSERT_TRUE(controller_manager_diff.swap_active_controller(1, vehicle_state));
+
+    core::ControllerOutput output = controller_manager_diff.step_active_controller(vehicle_state);
+    ASSERT_TRUE(std::holds_alternative<core::TorqueControlOut>(output.out));
+
+    ASSERT_TRUE(controller_manager_diff.swap_active_controller(0, vehicle_state));
+
+    output = controller_manager_diff.step_active_controller(vehicle_state);
+    ASSERT_TRUE(std::holds_alternative<core::SpeedControlOut>(output.out));
+}
+
+//switching where kachow
+TEST_F(ControllerManagerTest, SwapSpeedTooHigh) {
+    vehicle_state.current_rpms = {10000, 10000, 10000, 10000};
+    ASSERT_FALSE(controller_manager_diff.swap_active_controller(1, vehicle_state));
+
+    core::ControllerOutput output = controller_manager_diff.step_active_controller(vehicle_state);
+    ASSERT_TRUE(std::holds_alternative<core::SpeedControlOut>(output.out));
+}
+
+// Test out of range index
+TEST_F(ControllerManagerTest, SwapControllerFailure_OutOfRange) {
+    ASSERT_FALSE(controller_manager_diff.swap_active_controller(2, vehicle_state));
+    EXPECT_EQ(controller_manager_diff.get_current_ctr_manager_state().current_status, core::control::ControllerManagerStatus::ERROR_CONTROLLER_INDEX_OUT_OF_RANGE);
+
+    ASSERT_FALSE(controller_manager_diff.swap_active_controller(-7, vehicle_state));
+    EXPECT_EQ(controller_manager_diff.get_current_ctr_manager_state().current_status, core::control::ControllerManagerStatus::ERROR_CONTROLLER_INDEX_OUT_OF_RANGE);
+}
+
+// Test high RPM
+TEST_F(ControllerManagerTest, SwapControllerFailure_HighRPM) {
+    vehicle_state.current_rpms = {100000, 10000, 10000, 10000};
+
+    ASSERT_FALSE(controller_manager_2speed.swap_active_controller(1, vehicle_state));
+    EXPECT_EQ(controller_manager_2speed.get_current_ctr_manager_state().current_status, core::control::ControllerManagerStatus::ERROR_SPEED_TOO_HIGH);
+}
+
+TEST_F(ControllerManagerTest, SwapControllerFailure_HighRPM_onewheel) {
+    vehicle_state.current_rpms = {100000, 0, 0, 0};
+
+    ASSERT_FALSE(controller_manager_2speed.swap_active_controller(1, vehicle_state));
+    EXPECT_EQ(controller_manager_2speed.get_current_ctr_manager_state().current_status, core::control::ControllerManagerStatus::ERROR_SPEED_TOO_HIGH);
+}
+
+//test foot on accelerator over/under threshold
+TEST_F(ControllerManagerTest, SwapAccelerator) {
+    vehicle_state.current_rpms = {0, 0, 0, 0};
+    vehicle_state.input.requested_accel = .3;
+    ASSERT_FALSE(controller_manager_2speed.swap_active_controller(1, vehicle_state));
+    EXPECT_EQ(controller_manager_2speed.get_current_ctr_manager_state().current_status, core::control::ControllerManagerStatus::ERROR_DRIVER_ON_PEDAL);
+
+    vehicle_state.input.requested_accel = .1;
+    ASSERT_TRUE(controller_manager_2speed.swap_active_controller(1, vehicle_state));
+    EXPECT_EQ(controller_manager_2speed.get_current_ctr_manager_state().current_status, core::control::ControllerManagerStatus::NO_ERROR);
+}
+
+//real conroller stuff
+TEST_F(ControllerManagerTest, StepSimpleSpeedController) {
+    vehicle_state.input.requested_accel = .2;
+    core::ControllerOutput output = controller_manager_2speed.step_active_controller(vehicle_state);
+    
+    ASSERT_TRUE(std::holds_alternative<core::SpeedControlOut>(output.out));
+    auto _output = std::get<core::SpeedControlOut>(output.out);
+
+    float torque_config = json_file_handler.get_config()["SimpleSpeedController"]["max_torque"];
+    EXPECT_NEAR(_output.torque_lim_nm.FL, (float)(0.2f*(torque_config)), 0.001f);
+    EXPECT_NEAR(_output.torque_lim_nm.FR, (float)(0.2f*(torque_config)), 0.001f);
+    EXPECT_NEAR(_output.torque_lim_nm.RL, (float)(0.2f*(torque_config)), 0.001f);
+    EXPECT_NEAR(_output.torque_lim_nm.RR, (float)(0.2f*(torque_config)), 0.001f);
+}
+
+TEST_F(ControllerManagerTest, SwapSimpleSpeedControllers) {
+    vehicle_state.current_rpms = {100, 100, 100, 100};
+    ASSERT_TRUE(controller_manager_2speed.swap_active_controller(1, vehicle_state));
+
+    core::ControllerOutput output = controller_manager_2speed.step_active_controller(vehicle_state);
+    ASSERT_TRUE(std::holds_alternative<core::SpeedControlOut>(output.out));
+}
+
+TEST_F(ControllerManagerTest, SwapDiffSimpleControllers) {
+    vehicle_state.current_rpms = {100, 100, 100, 100};
+    ASSERT_TRUE(controller_manager_diff.swap_active_controller(1, vehicle_state));
+
+    core::ControllerOutput output = controller_manager_diff.step_active_controller(vehicle_state);
+    ASSERT_TRUE(std::holds_alternative<core::TorqueControlOut>(output.out));
+}
\ No newline at end of file
diff --git a/vcan_bringup.sh b/vcan_bringup.sh
index 4ee1ec88..7aa856d7 100755
--- a/vcan_bringup.sh
+++ b/vcan_bringup.sh
@@ -6,4 +6,7 @@ modprobe vcan
 # Create the virtual CAN interface.
 ip link add dev vcan0 type vcan
 # Bring the virtual CAN interface online.
-ip link set up vcan0
\ No newline at end of file
+ip link set up vcan0
+ip link add dev vcan1 type vcan
+# Bring the virtual CAN interface online.
+ip link set up vcan1
\ No newline at end of file