From da7032b90e0dc2cbb46636f849d20a07a127dd2b Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 1 Feb 2025 23:09:13 -0500 Subject: [PATCH 01/87] (wip) working on the json integration --- asdf | 20 ++++++++++ .../include/JSONUtils.hpp | 13 +++++++ .../include/ProtobufUtils.hpp | 1 + .../drivebrain_comms/src/foxglove_server.cpp | 1 + .../drivebrain_mcap_logger/README.md | 18 +++++++++ .../include/MCAPProtobufLogger.hpp | 37 ++++++++++++++----- .../src/MCAPProtobufLogger.cpp | 10 +++-- 7 files changed, 88 insertions(+), 12 deletions(-) create mode 100644 asdf create mode 100644 drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp create mode 100644 drivebrain_core_impl/drivebrain_mcap_logger/README.md diff --git a/asdf b/asdf new file mode 100644 index 0000000..463081d --- /dev/null +++ b/asdf @@ -0,0 +1,20 @@ +{ + "type": "object", + "properties": { + "member_1": { + "type": "object" , + "properties": { + "y": { "type": "number" }, + "z": { "type": "number" } + } + }, + "member_2": { + "type": "object" , + "properties": { + "y": { "type": "number" }, + "z": { "type": "number" } + } + } + }, + "required": ["member_1", "member_2"] +} \ 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 0000000..83247db --- /dev/null +++ b/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp @@ -0,0 +1,13 @@ +#ifndef __JSONUTILS_H__ +#define __JSONUTILS_H__ + +#include + +#include + +namespace util +{ + +} + +#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 df3fbe4..8b8f07d 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 + #include #include #include diff --git a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp index 72761dc..7c95a68 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp @@ -96,6 +96,7 @@ core::FoxgloveWSServer::FoxgloveWSServer(std::vectoraddChannels(channels); _server->setHandlers(std::move(hdlrs)); _server->start("0.0.0.0", 5555); diff --git a/drivebrain_core_impl/drivebrain_mcap_logger/README.md b/drivebrain_core_impl/drivebrain_mcap_logger/README.md new file mode 100644 index 0000000..9e54258 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_mcap_logger/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_mcap_logger/include/MCAPProtobufLogger.hpp b/drivebrain_core_impl/drivebrain_mcap_logger/include/MCAPProtobufLogger.hpp index 38f3461..f8f6ca7 100644 --- a/drivebrain_core_impl/drivebrain_mcap_logger/include/MCAPProtobufLogger.hpp +++ b/drivebrain_core_impl/drivebrain_mcap_logger/include/MCAPProtobufLogger.hpp @@ -1,44 +1,63 @@ #ifndef __MCAPPROTOBUFLOGGER_H__ #define __MCAPPROTOBUFLOGGER_H__ +#include + +#include + #include + #include -#include -#include -#include #include #include -#include -#include +#include +#include +#include +#include +#include #include + + +// - [ ] 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 MCAPProtobufLogger { public: - struct ProtobufRawMessage + struct RawMessage { 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 out_msg); + void log_json_struct(const std::string & topic, const nhlohmann::json::object & out_json ); + void open_new_mcap(const std::string &name); void close_current_mcap(); private: void _handle_log_to_file(); private: - core::common::ThreadSafeDeque _input_deque; + core::common::ThreadSafeDeque _input_deque; std::thread _log_thread; bool _running = false; mcap::McapWriterOptions _options; diff --git a/drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp b/drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp index cefa208..7937912 100644 --- a/drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp +++ b/drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp @@ -12,7 +12,7 @@ namespace common { - MCAPProtobufLogger::MCAPProtobufLogger(const std::string &base_dir) + MCAPProtobufLogger::MCAPProtobufLogger(const std::string &base_dir, std::function get_json_schema) : _options(mcap::McapWriterOptions("")) { auto optional_map = util::generate_name_to_id_map({"hytech_msgs.proto", "hytech.proto"}); @@ -32,6 +32,7 @@ namespace common _options.noChunking = true; _log_thread = std::thread(&MCAPProtobufLogger::_handle_log_to_file, this); } + MCAPProtobufLogger::~MCAPProtobufLogger() { { @@ -77,6 +78,8 @@ namespace common add_schema_func(schema_only_descriptors, true); add_schema_func(receiving_descriptors, false); + + spdlog::info("Added message descriptions to MCAP"); } @@ -130,7 +133,7 @@ namespace common void MCAPProtobufLogger::log_msg(std::shared_ptr msg_out) { mcap::Timestamp log_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - MCAPProtobufLogger::ProtobufRawMessage msg_to_enque; + MCAPProtobufLogger::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; @@ -140,7 +143,8 @@ namespace common _input_deque.deque.push_back(msg_to_enque); _input_deque.cv.notify_all(); } - } + + void MCAPProtobufLogger::log_parameters() } \ No newline at end of file From 63cc72e6323560910f300c1dd2fbe220ff3f0f10 Mon Sep 17 00:00:00 2001 From: Krish Date: Fri, 7 Feb 2025 02:10:54 -0500 Subject: [PATCH 02/87] added power limiting to simple controller and it builds now --- .../src/SimpleController.cpp | 58 ++++++++ flake.lock | 140 ++++++++++++++---- flake.nix | 19 ++- 3 files changed, 183 insertions(+), 34 deletions(-) diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp index 9764773..70afb95 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp @@ -75,6 +75,8 @@ core::SpeedControlOut control::SimpleController::step_controller(const core::Veh // accelRequest goes between 1.0 and -1.0 float accelRequest = (in.input.requested_accel) - (in.input.requested_brake); + veh_vec current_rpms = in.current_rpms; + torque_nm torqueRequest; // hytech_msgs::MCUCommandData cmd_out; @@ -112,5 +114,61 @@ core::SpeedControlOut control::SimpleController::step_controller(const core::Veh cmd_out.torque_lim_nm.RR = (torqueRequest * cur_config.rear_torque_scale); } + // Apply power limit (basically a re-implementation of MCU) + float net_torque_mag = 0; + float net_power = 0; + + net_torque_mag += abs(cmd_out.torque_lim_nm.FL); + net_torque_mag += abs(cmd_out.torque_lim_nm.FR); + net_torque_mag += abs(cmd_out.torque_lim_nm.RL); + net_torque_mag += abs(cmd_out.torque_lim_nm.RR); + + net_power += abs(cmd_out.torque_lim_nm.FL) * (current_rpms.FL * constants::RPM_TO_RAD_PER_SECOND); + net_power += abs(cmd_out.torque_lim_nm.FR) * (current_rpms.FR * constants::RPM_TO_RAD_PER_SECOND); + net_power += abs(cmd_out.torque_lim_nm.RL) * (current_rpms.RL * constants::RPM_TO_RAD_PER_SECOND); + net_power += abs(cmd_out.torque_lim_nm.RR) * (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND); + + if (net_power > constants::POWER_LIMIT) { + + /* 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 = abs(cmd_out.torque_lim_nm.FL / net_torque_mag); + float power_per_corner_FL = (torque_percent_FL * constants::POWER_LIMIT); + + // 3. Divide power by rads per seconds to get torque per corner + cmd_out.torque_lim_nm.FL = abs(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 = abs(cmd_out.torque_lim_nm.FR / net_torque_mag); + float power_per_corner_FR = (torque_percent_FR * constants::POWER_LIMIT); + + // 3. Divide power by rads per seconds to get torque per corner + cmd_out.torque_lim_nm.FR = abs(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 = abs(cmd_out.torque_lim_nm.RL / net_torque_mag); + float power_per_corner_RL = (torque_percent_RL * constants::POWER_LIMIT); + + // 3. Divide power by rads per seconds to get torque per corner + cmd_out.torque_lim_nm.RL = abs(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 = abs(cmd_out.torque_lim_nm.RR / net_torque_mag); + float power_per_corner_RR = (torque_percent_RR * constants::POWER_LIMIT); + + // 3. Divide power by rads per seconds to get torque per corner + cmd_out.torque_lim_nm.RR = abs(power_per_corner_RR / (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND)); + + } + + + return cmd_out; } diff --git a/flake.lock b/flake.lock index 80b2eaf..9e9b2d3 100644 --- a/flake.lock +++ b/flake.lock @@ -52,11 +52,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1731609678, - "narHash": "sha256-AC96vyQBl/h6/4o1QuNGimJywy2T7p6tfe2ePWLwkqM=", + "lastModified": 1738910731, + "narHash": "sha256-yJQkdoHPzZynNtk8N4H6GHN92/DjurF9yyW8pjNeMh8=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "945279a1b4da90b4aab78346d8e1155c43c94281", + "rev": "791a6207e863521bc8f488c54e5c323d432151da", "type": "github" }, "original": { @@ -150,11 +150,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": { @@ -181,6 +181,24 @@ "type": "github" } }, + "flake-parts_3": { + "inputs": { + "nixpkgs-lib": "nixpkgs-lib_2" + }, + "locked": { + "lastModified": 1736143030, + "narHash": "sha256-+hu54pAoLDEZT9pjHlqL9DNzWz0NbUn8NEAHP7PQPzU=", + "owner": "hercules-ci", + "repo": "flake-parts", + "rev": "b905f6fc23a9051a6e1b741e1438dbfc0634c6de", + "type": "github" + }, + "original": { + "owner": "hercules-ci", + "repo": "flake-parts", + "type": "github" + } + }, "flow-ipc-src": { "flake": false, "locked": { @@ -216,11 +234,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": { @@ -232,11 +250,11 @@ "foxglove-ws-protocol-src": { "flake": false, "locked": { - "lastModified": 1723342974, - "narHash": "sha256-H3+xOxcKwHyrbT1d5P4KvpoMCfj3mCK9mxeegeFIHmA=", + "lastModified": 1726702470, + "narHash": "sha256-Ke2QydcCwnTeR0tiR5xe2FYb4yklSbn883CH6E0cSAc=", "owner": "RCMast3r", "repo": "ws-protocol", - "rev": "bdae89350a6d0b463fa648023171d10b6eb5d594", + "rev": "8c2b06a315d03f2d3b395bba5d97b03cf63ed908", "type": "github" }, "original": { @@ -245,6 +263,23 @@ "type": "github" } }, + "gtsam-src": { + "flake": false, + "locked": { + "lastModified": 1693794078, + "narHash": "sha256-HjpGrHclpm2XsicZty/rX/RM/762wzmj4AAoEfni8es=", + "owner": "borglab", + "repo": "gtsam", + "rev": "4f66a491ffc83cf092d0d818b11dc35135521612", + "type": "github" + }, + "original": { + "owner": "borglab", + "ref": "4.2.0", + "repo": "gtsam", + "type": "github" + } + }, "ht_can": { "inputs": { "nix-proto": [ @@ -296,18 +331,20 @@ "flow-ipc-src": "flow-ipc-src", "foxglove-mcap-src": "foxglove-mcap-src", "foxglove-ws-protocol-src": "foxglove-ws-protocol-src", + "gtsam-src": "gtsam-src", "libsocketcanpp-src": "libsocketcanpp-src", "nix-proto": "nix-proto", "nixpkgs": [ "nixpkgs" - ] + ], + "soem-src": "soem-src" }, "locked": { - "lastModified": 1728342286, - "narHash": "sha256-BiaoZV0F8yvmM3P4rKxEM8RWZw6JZs/zIdx1RjieOLw=", + "lastModified": 1738120152, + "narHash": "sha256-TQATVFi6pNa4C4i3JT+VkWv07h2O+olYxABMNlHl/9Y=", "owner": "RCMast3r", "repo": "nebs_packages", - "rev": "2d0f791a05fe5e3d1fb241d574be1624bbcff2ca", + "rev": "0b6409851032e9eb364b149e971d0565d57a0bd7", "type": "github" }, "original": { @@ -333,11 +370,11 @@ }, "nix-filter_2": { "locked": { - "lastModified": 1710156097, - "narHash": "sha256-1Wvk8UP7PXdf8bCCaEoMnOT1qe5/Duqgj+rL8sRQsSM=", + "lastModified": 1731533336, + "narHash": "sha256-oRam5PS1vcrr5UPgALW0eo1m/5/pls27Z/pabHNy2Ms=", "owner": "numtide", "repo": "nix-filter", - "rev": "3342559a24e85fc164b295c3444e8a139924675b", + "rev": "f7653272fd234696ae94229839a99b73c9ab7de0", "type": "github" }, "original": { @@ -368,18 +405,20 @@ }, "nix-proto_2": { "inputs": { + "flake-parts": "flake-parts_3", "nix-filter": "nix-filter_2", "nix-std": "nix-std_2", "nixpkgs": [ "nixpkgs" - ] + ], + "treefmt-nix": "treefmt-nix" }, "locked": { - "lastModified": 1723991236, - "narHash": "sha256-gGzv9rJaq/tTYuQOVlXgtSblfr66qWNthBvwTPjSRto=", + "lastModified": 1737939731, + "narHash": "sha256-4/cB3MxV38yTDfs+iNjJl0d0EBN3A0505CKC5IZQZWI=", "owner": "notalltim", "repo": "nix-proto", - "rev": "0b579a596058d5f9ec423379f973ae34b6cb59f6", + "rev": "fe40ca85ecc6217d3625c5477000e72d323be9e0", "type": "github" }, "original": { @@ -446,6 +485,18 @@ "url": "https://github.com/NixOS/nixpkgs/archive/356624c12086a18f2ea2825fed34523d60ccc4e3.tar.gz" } }, + "nixpkgs-lib_2": { + "locked": { + "lastModified": 1735774519, + "narHash": "sha256-CewEm1o2eVAnoqb6Ml+Qi9Gg/EfNAxbRx1lANGVyoLI=", + "type": "tarball", + "url": "https://github.com/NixOS/nixpkgs/archive/e9b51731911566bbf7e4895475a87fe06961de0b.tar.gz" + }, + "original": { + "type": "tarball", + "url": "https://github.com/NixOS/nixpkgs/archive/e9b51731911566bbf7e4895475a87fe06961de0b.tar.gz" + } + }, "nixpkgs_2": { "locked": { "lastModified": 1719426051, @@ -464,11 +515,11 @@ }, "nixpkgs_3": { "locked": { - "lastModified": 1731386116, - "narHash": "sha256-lKA770aUmjPHdTaJWnP3yQ9OI1TigenUqVC3wweqZuI=", + "lastModified": 1735563628, + "narHash": "sha256-OnSAY7XDSx7CtDoqNh8jwVwh4xNL/2HaJxGjryLWzX8=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "689fed12a013f56d4c4d3f612489634267d86529", + "rev": "b134951a4c9f3c995fd7be05f3243f8ecd65d798", "type": "github" }, "original": { @@ -538,6 +589,22 @@ "url": "https://github.com/hytech-racing/simulink_automation/releases/download/CodeGen_2024.11.13_05-40/matlab_math.tar.gz" } }, + "soem-src": { + "flake": false, + "locked": { + "lastModified": 1737971577, + "narHash": "sha256-DK+fymGO4Q7XdXRLdVrGTU89zazMuL8+fNttFYCtS/o=", + "owner": "OpenEtherCATsociety", + "repo": "SOEM", + "rev": "2752dc25882ab24d7cfcad674226b65270fb0c61", + "type": "github" + }, + "original": { + "owner": "OpenEtherCATsociety", + "repo": "SOEM", + "type": "github" + } + }, "systems": { "locked": { "lastModified": 1681028828, @@ -598,6 +665,27 @@ "type": "github" } }, + "treefmt-nix": { + "inputs": { + "nixpkgs": [ + "nix-proto", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1737483750, + "narHash": "sha256-5An1wq5U8sNycOBBg3nsDDgpwBmR9liOpDGlhliA6Xo=", + "owner": "numtide", + "repo": "treefmt-nix", + "rev": "f2cc121df15418d028a59c9737d38e3a90fbaf8f", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "treefmt-nix", + "type": "github" + } + }, "utils": { "inputs": { "systems": "systems" diff --git a/flake.nix b/flake.nix index ef36931..5eee659 100644 --- a/flake.nix +++ b/flake.nix @@ -58,21 +58,24 @@ root = "${foxglove-schemas-src}/schemas/proto"; namespace = "foxglove"; }; + + }; drivebrain_core_msgs = nix-proto.mkProtoDerivation { name = "drivebrain_core_msgs"; version = HT_proto.rev; src = "${HT_proto}/proto"; - }; + }; 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: { From bff923494b2bb235dea114b49df1c6783cd3d150 Mon Sep 17 00:00:00 2001 From: Krish Date: Fri, 7 Feb 2025 02:31:47 -0500 Subject: [PATCH 03/87] added test cases --- CMakeLists.txt | 2 +- unit_test/SimpleControllerTest.cpp | 97 ++++++++++++++++++++++++++++-- 2 files changed, 94 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0d317e7..2632d45 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -280,7 +280,7 @@ target_link_libraries(alpha_test PUBLIC gtest ) -add_test(NAME MyTest COMMAND alpha_test) +add_test(NAME Drivebrain_Tests COMMAND alpha_test) target_link_libraries(mcu_standin PUBLIC drivebrain_core_msgs_proto_cpp::drivebrain_core_msgs_proto_cpp diff --git a/unit_test/SimpleControllerTest.cpp b/unit_test/SimpleControllerTest.cpp index cc70780..fe33948 100644 --- a/unit_test/SimpleControllerTest.cpp +++ b/unit_test/SimpleControllerTest.cpp @@ -28,8 +28,97 @@ class SimpleControllerTest : public testing::Test { }; -TEST_F(SimpleControllerTest, MaxRPM) { - core::VehicleState in; - auto res = simple_controller.step_controller(in); - ASSERT_LT(res.desired_rpms.FL, 20000); +TEST_F(SimpleControllerTest, Outputs) { + + // --- Test Case 1: Simple Acceleration --- + { + core::VehicleState in; + veh_vec 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 = 10; + in.input.requested_brake = 0; + in.prev_MCU_recv_millis = 0; + + auto res = simple_controller.step_controller(in); + ASSERT_EQ(res.desired_rpms.FL, 180); + 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); + } + + // --- Test Case 2: Full Braking --- + { + core::VehicleState in; + veh_vec 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 = 10; + in.prev_MCU_recv_millis = 0; + + auto res = simple_controller.step_controller(in); + 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, 60); + ASSERT_EQ(res.torque_lim_nm.FR, 60); + ASSERT_EQ(res.torque_lim_nm.RL, 60); + ASSERT_EQ(res.torque_lim_nm.RR, 60); + } + + // --- Test Case 3: Zero Acceleration and Zero Brake (Coasting) --- + { + core::VehicleState in; + veh_vec 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; + in.prev_MCU_recv_millis = 0; + + auto res = simple_controller.step_controller(in); + 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, 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 Case 4: Power Limiting Scenario --- + { + core::VehicleState in; + veh_vec current_rpms; + current_rpms.FL = 5000; + current_rpms.FR = 5000; + current_rpms.RL = 5000; + current_rpms.RR = 5000; + in.current_rpms = current_rpms; + in.input.requested_accel = 10; + in.input.requested_brake = 0; + in.prev_MCU_recv_millis = 0; + + auto res = simple_controller.step_controller(in); + ASSERT_LT(res.torque_lim_nm.FL, 210); // Expect power limiting applied + ASSERT_LT(res.torque_lim_nm.FR, 210); + ASSERT_LT(res.torque_lim_nm.RL, 210); + ASSERT_LT(res.torque_lim_nm.RR, 210); + } } From 4bfe03c513b7256dbab77d2be417b4fa28432cda Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 13 Feb 2025 12:01:55 -0500 Subject: [PATCH 04/87] wip params --- drivebrain_app/src/DriveBrainApp.cpp | 2 ++ .../include/ParamLogger.hpp | 23 +++++++++++++++++++ .../src/ParamLogger.cpp | 13 +++++++++++ .../drivebrain_comms/include/VNComms.hpp | 2 +- .../drivebrain_comms/src/CANComms.cpp | 1 + .../drivebrain_comms/src/VNComms.cpp | 17 +++++++++----- .../src/SimpleController.cpp | 2 +- .../README.md | 0 .../include/MCAPProtobufLogger.hpp | 0 .../src/MCAPProtobufLogger.cpp | 0 .../drivebrain_logging/src/ParamLogger.cpp | 1 + flake.nix | 6 ++--- test/test_json_schema_creation.cpp | 6 +++++ 13 files changed, 62 insertions(+), 11 deletions(-) create mode 100644 drivebrain_core_impl/drivebrain_common_utils/include/ParamLogger.hpp create mode 100644 drivebrain_core_impl/drivebrain_common_utils/src/ParamLogger.cpp rename drivebrain_core_impl/{drivebrain_mcap_logger => drivebrain_logging}/README.md (100%) rename drivebrain_core_impl/{drivebrain_mcap_logger => drivebrain_logging}/include/MCAPProtobufLogger.hpp (100%) rename drivebrain_core_impl/{drivebrain_mcap_logger => drivebrain_logging}/src/MCAPProtobufLogger.cpp (100%) create mode 100644 drivebrain_core_impl/drivebrain_logging/src/ParamLogger.cpp create mode 100644 test/test_json_schema_creation.cpp diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 73cce9a..68914a3 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -82,6 +82,8 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if (!_controller->init()) { throw std::runtime_error("Failed to initialize controller"); } + + // TODO add here the creation of the config logger } DriveBrainApp::~DriveBrainApp() { diff --git a/drivebrain_core_impl/drivebrain_common_utils/include/ParamLogger.hpp b/drivebrain_core_impl/drivebrain_common_utils/include/ParamLogger.hpp new file mode 100644 index 0000000..e3063b7 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_common_utils/include/ParamLogger.hpp @@ -0,0 +1,23 @@ +#ifndef __PARAMLOGGER_H__ +#define __PARAMLOGGER_H__ + +#include + +#include +#include +#include +// #include + +// handles the logging of the params from all of the configurable components + +// functions: +// 1. verifies that all components have been configured upon construction +// (NOTE: this boi needs to be constructed only after the components have been initialized for the first time) +// 2. creates the json schema given the map of parameters +// 3. gathers the maps of params contained within the components + +namespace util +{ +std::optional get_schema(std::vector configurable_components); +} +#endif // __PARAMLOGGER_H__ \ No newline at end of file diff --git a/drivebrain_core_impl/drivebrain_common_utils/src/ParamLogger.cpp b/drivebrain_core_impl/drivebrain_common_utils/src/ParamLogger.cpp new file mode 100644 index 0000000..127bfd5 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_common_utils/src/ParamLogger.cpp @@ -0,0 +1,13 @@ +#include "ParamLogger.hpp" + +namespace util +{ + std::optional get_schema(std::vector configurable_components) + { + + for(auto component : configurable_components) + { + + } + } +} \ 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 04902c9..b9761fc 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp @@ -41,7 +41,7 @@ namespace comms { class VNDriver : public core::common::Configurable { public: - VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, ::core::StateEstimator &state_estimator, boost::asio::io_context &io_context); + VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, ::core::StateEstimator &state_estimator, boost::asio::io_context &io_context, bool &init_successful); bool init(); struct config { int baud_rate; diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index 088ff63..3ec2539 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -65,6 +65,7 @@ bool comms::CANDriver::init() { _logger.log_string("inited, started read", core::LogLevel::INFO); _do_read(); + _configured = true; return true; } diff --git a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp index 7b90871..89deda3 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp @@ -37,7 +37,7 @@ namespace comms { spdlog::warn("Error: {}", ec.message()); _logger.log_string("Failed to open vn driver device.", core::LogLevel::INFO); - return 1; + return false; } // Set the baud rate of the device along with other configs @@ -52,22 +52,27 @@ namespace comms _configure_binary_outputs(); - return 0; + _configured = true; + return true; } - VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, core::StateEstimator &state_estimator, boost::asio::io_context& io) + VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, core::StateEstimator &state_estimator, boost::asio::io_context& io, bool &init_successful) : core::common::Configurable(logger, json_file_handler, "VNDriver"), _logger(logger), _state_estimator(state_estimator), _message_logger(message_logger), _serial(io) { - init(); + init_successful = init(); // Starts read - _logger.log_string("Starting vn driver recieve.", core::LogLevel::INFO); + if(init_successful) + { + _logger.log_string("Starting vn driver recieve.", core::LogLevel::INFO); - _start_recieve(); + _start_recieve(); + } + } void VNDriver::log_proto_message(std::shared_ptr msg) diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp index 9764773..0d79fdc 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp @@ -59,7 +59,7 @@ bool control::SimpleController::init() _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)); - + _configured = true; return true; } diff --git a/drivebrain_core_impl/drivebrain_mcap_logger/README.md b/drivebrain_core_impl/drivebrain_logging/README.md similarity index 100% rename from drivebrain_core_impl/drivebrain_mcap_logger/README.md rename to drivebrain_core_impl/drivebrain_logging/README.md diff --git a/drivebrain_core_impl/drivebrain_mcap_logger/include/MCAPProtobufLogger.hpp b/drivebrain_core_impl/drivebrain_logging/include/MCAPProtobufLogger.hpp similarity index 100% rename from drivebrain_core_impl/drivebrain_mcap_logger/include/MCAPProtobufLogger.hpp rename to drivebrain_core_impl/drivebrain_logging/include/MCAPProtobufLogger.hpp diff --git a/drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/MCAPProtobufLogger.cpp similarity index 100% rename from drivebrain_core_impl/drivebrain_mcap_logger/src/MCAPProtobufLogger.cpp rename to drivebrain_core_impl/drivebrain_logging/src/MCAPProtobufLogger.cpp 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 0000000..8b13789 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_logging/src/ParamLogger.cpp @@ -0,0 +1 @@ + diff --git a/flake.nix b/flake.nix index ef36931..a728ad9 100644 --- a/flake.nix +++ b/flake.nix @@ -42,8 +42,8 @@ }; simulink-automation-src = { - url = "https://github.com/hytech-racing/simulink_automation/releases/download/CodeGen_2024.11.13_05-40/matlab_math.tar.gz"; - flake = false; + url = "https://github.com/hytech-racing/simulink_automation/releases/download/CodeGen_2024.11.13_05-40/matlab_math.tar.gz"; + flake = false; }; }; @@ -140,7 +140,7 @@ alias run="./build/alpha_build config/drivebrain_config.json $DBC_PATH/hytech.dbc" ''; nativeBuildInputs = [ pkgs.drivebrain_core_msgs_proto_cpp ]; - packages = [ pkgs.mcap-cli ]; + packages = [ pkgs.mcap-cli pkgs.ethercat ]; inputsFrom = [ pkgs.drivebrain_software ]; diff --git a/test/test_json_schema_creation.cpp b/test/test_json_schema_creation.cpp new file mode 100644 index 0000000..c0127e0 --- /dev/null +++ b/test/test_json_schema_creation.cpp @@ -0,0 +1,6 @@ +#include + +int main() +{ + return 0; +} \ No newline at end of file From 9c6d015a9b32865562f490179cb986436a2c3c7f Mon Sep 17 00:00:00 2001 From: JohnNesbit Date: Sun, 2 Mar 2025 14:00:30 -0500 Subject: [PATCH 05/87] Initial TCMUX Commit --- CMakeLists.txt | 9 +- config/drivebrain_config.json | 20 +- drivebrain_app/debug_main.cpp | 3 +- drivebrain_app/include/DriveBrainApp.hpp | 11 +- drivebrain_app/main.cpp | 3 +- drivebrain_app/src/DriveBrainApp.cpp | 71 ++++-- .../include/DBServiceImpl.hpp | 11 +- .../drivebrain_comms/src/DBServiceImpl.cpp | 14 +- .../include/Controllers.hpp | 3 +- ...ntroller.hpp => SimpleSpeedController.hpp} | 9 +- .../include/SimpleTorqueController.hpp | 43 ++++ ...ntroller.cpp => SimpleSpeedController.cpp} | 66 +++--- .../src/SimpleTorqueController.cpp | 94 ++++++++ flake.lock | 87 ++++++-- flake.nix | 2 +- unit_test/SimpleControllerTest.cpp | 35 --- unit_test/SimpleSpeedControllerTest.cpp | 196 +++++++++++++++++ unit_test/SimpleTorqueControllerTest.cpp | 151 +++++++++++++ unit_test/test_controller_manager.cpp | 203 ++++++++++++++++++ 19 files changed, 910 insertions(+), 121 deletions(-) rename drivebrain_core_impl/drivebrain_control/include/{SimpleController.hpp => SimpleSpeedController.hpp} (70%) create mode 100644 drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp rename drivebrain_core_impl/drivebrain_control/src/{SimpleController.cpp => SimpleSpeedController.cpp} (55%) create mode 100644 drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp delete mode 100644 unit_test/SimpleControllerTest.cpp create mode 100644 unit_test/SimpleSpeedControllerTest.cpp create mode 100644 unit_test/SimpleTorqueControllerTest.cpp create mode 100644 unit_test/test_controller_manager.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c093d5..73d94ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,12 +112,14 @@ target_link_libraries(drivebrain_comms PUBLIC libvncxx::libvncxx drivebrain_estimation spdlog::spdlog + drivebrain_control ) 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/include/SimpleController.hpp target_include_directories(drivebrain_control PUBLIC $ @@ -293,7 +295,10 @@ enable_testing() add_executable(alpha_test unit_test/main.cpp - unit_test/SimpleControllerTest.cpp + unit_test/SimpleSpeedControllerTest.cpp + unit_test/SimpleTorqueControllerTest.cpp + unit_test/test_controller_manager.cpp + ) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 63a0c1a..8937c85 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -1,14 +1,26 @@ { "CANDriver": { "canbus_device": "vcan0", - "path_to_dbc": "/home/ben/drivebrain_software/config/hytech.dbc" + "path_to_dbc": "./config/hytech.dbc" }, - "SimpleController": { - "max_torque": 21, + "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" : 3.0 + }, + "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_switch_requested_rpm": 20000.0, + "max_torque_switch_nm": 10.0, + "max_accel_switch_float": 0.1 }, "VNDriver": { "device_name": "/dev/ttyUSB0", diff --git a/drivebrain_app/debug_main.cpp b/drivebrain_app/debug_main.cpp index ae4509f..fea11f9 100644 --- a/drivebrain_app/debug_main.cpp +++ b/drivebrain_app/debug_main.cpp @@ -1,7 +1,8 @@ #include #include -#include +#include +#include #include #include #include diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 79e04f4..119af27 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -3,7 +3,8 @@ #include #include -#include +#include +#include #include #include #include @@ -64,7 +65,13 @@ class DriveBrainApp { std::vector _configurable_components; std::unique_ptr _mcap_logger; - std::unique_ptr _controller; + + // TCMUX + control::SimpleSpeedController controller1; + control::SimpleTorqueController controller2; + control::ControllerManager, 2 > _controllerManager; + std::function switch_modes; + // std::unique_ptr _matlab_math; std::unique_ptr _foxglove_server; std::shared_ptr>> _message_logger; diff --git a/drivebrain_app/main.cpp b/drivebrain_app/main.cpp index 6025b2c..a9a5b19 100644 --- a/drivebrain_app/main.cpp +++ b/drivebrain_app/main.cpp @@ -1,6 +1,7 @@ #include #include -#include +#include +#include #include #include #include diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 8b323bd..8d8bdda 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -13,15 +13,26 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d , _logger(core::LogLevel::INFO) , _config(_param_path) , _settings(settings) - + , controller1(control::SimpleSpeedController(_logger, _config)) + , controller2(control::SimpleTorqueController(_logger, _config)) + , _controllerManager(_logger, _config, {&controller1, &controller2}) // Initialize correctly { spdlog::set_level(spdlog::level::warn); _mcap_logger = std::make_unique("temp"); - - _controller = std::make_unique(_logger, _config); - _configurable_components.push_back(_controller.get()); + + + //control::SimpleSpeedController controller1(_logger, _config); + //control::SimpleTorqueController controller2(_logger, _config); + _configurable_components.push_back(&controller1); + _configurable_components.push_back(&controller2); + //_controllerManager = control::ControllerManager, 2 >(_logger, _config, {&controller1 , &controller2}); + _configurable_components.push_back(&_controllerManager); + + bool successful_controller1_init = controller1.init(); + bool successful_controller2_init = controller2.init(); + bool successful_manager_init = _controllerManager.init(); // bool matlab_construction_failed = false; // _matlab_math = std::make_unique( @@ -59,9 +70,16 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d _vn_driver = std::make_unique(_config, _logger, _message_logger, *_state_estimator, _io_context); } - if (!_controller->init()) { - throw std::runtime_error("Failed to initialize controller"); + if (!successful_controller1_init || !successful_controller2_init) { + throw std::runtime_error("Failed to initialize a controller"); + } + if (!successful_manager_init) { + throw std::runtime_error("Failed to initialize controller manager"); } + switch_modes = + [this](size_t mode) -> bool { + return _controllerManager.swap_active_controller(mode, _state_estimator->get_latest_state_and_validity().first); + }; } DriveBrainApp::~DriveBrainApp() { @@ -90,7 +108,7 @@ void DriveBrainApp::_process_loop() { // auto out_msg = std::make_shared(); auto desired_rpm_msg = std::make_shared(); auto torque_limit_msg = std::make_shared(); - auto loop_time = _controller->get_dt_sec(); + auto loop_time = _controllerManager.get_active_controller_timestep(); auto loop_time_micros = (int)(loop_time * 1000000.0f); std::chrono::microseconds loop_chrono_time(loop_time_micros); @@ -99,32 +117,57 @@ void DriveBrainApp::_process_loop() { 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 out_struct = _controllerManager.step_active_controller(state_and_validity.first); + + //logic for retrieving whichever type is currently in the variant, i dont think we need to check if it has monostate + core::SpeedControlOut speed_cmd_out; + core::TorqueControlOut torque_cmd_out; + if(std::holds_alternative(out_struct.out)) + { + speed_cmd_out = std::get(out_struct.out); + } + else if(std::holds_alternative(out_struct.out)) + { + torque_cmd_out = std::get(out_struct.out); + } + //for when we have both controllers the vision is if(speed_cmd_out) -> else auto temp_desired_torques = state_and_validity.first.matlab_math_temp_out; - _state_estimator->set_previous_control_output(out_struct); + + + //_state_estimator->set_previous_control_output(out_struct); + // TCMUX code works with ControllerOutput, but the state estimator only works with speed controls it looks like + // so we grab the speed if that is what the variant contains + // this means that we actuall can only use one Controller, the speed one, so we gotta re-write state estimator :/ + if (auto speedOut = std::get_if(&out_struct.out)) { + _state_estimator->set_previous_control_output(*speedOut); + } else { + // if the std::variant does not contain SpeedControlOut(we're so cooked) + std::cerr << "Error: ControllerOutput is monostate or TorqueControlOut" << std::endl; + } + 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); + desired_rpm_msg->set_drivebrain_set_rpm_fl(speed_cmd_out.desired_rpms.FL); } 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); + desired_rpm_msg->set_drivebrain_set_rpm_fr(speed_cmd_out.desired_rpms.FR); } 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); + desired_rpm_msg->set_drivebrain_set_rpm_rl(speed_cmd_out.desired_rpms.RL); } 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); + desired_rpm_msg->set_drivebrain_set_rpm_rr(speed_cmd_out.desired_rpms.RR); } torque_limit_msg->set_drivebrain_torque_fl(::abs(temp_desired_torques.res_torque_lim_nm.FL)); @@ -162,7 +205,7 @@ void DriveBrainApp::run() { if (!_settings.run_db_service) return; - _db_service = std::make_unique(_message_logger); + _db_service = std::make_unique(_message_logger, switch_modes); spdlog::info("started db service thread"); try { while (!stop_signal.load()) { diff --git a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp index 863d651..3b5f738 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp @@ -10,6 +10,8 @@ #include #include +#include +#include class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Service { @@ -18,14 +20,15 @@ class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Servi }; 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>> logger_inst); - void run_server(); + DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch); void run_server(); void stop_server(); private: std::shared_ptr>> _logger_inst; + std::function _mode_switch; std::unique_ptr _server; // Store server instance here }; diff --git a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp index c9a41ba..8033e21 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp @@ -35,8 +35,20 @@ grpc::Status DBInterfaceImpl::RequestCurrentLoggerStatus(grpc::ServerContext *co } } -DBInterfaceImpl::DBInterfaceImpl(std::shared_ptr>> 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(rq->requested_controller_index()))){ + response->set_status("switch"); + } + else{ + response->set_status("no switch"); + } + + return grpc::Status::OK; +} + +DBInterfaceImpl::DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch) + : _logger_inst(logger_inst), _mode_switch(mode_switch){ } void DBInterfaceImpl::stop_server() { if (_server) { diff --git a/drivebrain_core_impl/drivebrain_control/include/Controllers.hpp b/drivebrain_core_impl/drivebrain_control/include/Controllers.hpp index ba91a7f..5290fc3 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 +#include +#include diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp similarity index 70% rename from drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp rename to drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp index ca7a689..c2c8a4d 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp @@ -3,8 +3,6 @@ #include #include #include -#include -#include #include #include #include @@ -16,7 +14,7 @@ namespace control { // TODO make the output CAN message for the drivetrain, rpms telem is just a standin for now - class SimpleController : Controller, public core::common::Configurable + class SimpleSpeedController : public Controller, public core::common::Configurable { public: // rear_torque_scale: @@ -33,12 +31,13 @@ namespace control 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") {} + SimpleSpeedController(core::Logger &logger, core::JsonFileHandler &json_file_handler) : Configurable(logger, json_file_handler, "SimpleSpeedController") {} + SimpleSpeedController(core::Logger &logger, core::JsonFileHandler &json_file_handler, std::string config) : Configurable(logger, json_file_handler, config) {} float get_dt_sec() override { return (0.001); } 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 &new_param_map); 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 0000000..0800606 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp @@ -0,0 +1,43 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include + +// ABOUT: this controller is an implementation of mode 0 but with torque type shit +namespace control +{ + class SimpleTorqueController : public Controller, 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::Logger &logger, core::JsonFileHandler &json_file_handler) : Configurable(logger, json_file_handler, "SimpleTorqueController") {} + SimpleTorqueController(core::Logger &logger, core::JsonFileHandler &json_file_handler, std::string config) : Configurable(logger, json_file_handler, config) {} + float get_dt_sec() override { + return (0.001); + } + bool init() override; + core::ControllerOutput step_controller(const core::VehicleState &in) override; + + private: + void _handle_param_updates(const std::unordered_map &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/SimpleController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp similarity index 55% rename from drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp rename to drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp index 9764773..2066875 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp @@ -1,9 +1,9 @@ -#include +#include #include #include #include -void control::SimpleController::_handle_param_updates(const std::unordered_map &new_param_map) +void control::SimpleSpeedController::_handle_param_updates(const std::unordered_map &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(&new_param_map.at("max_torque"))) @@ -43,14 +43,13 @@ void control::SimpleController::_handle_param_updates(const std::unordered_map("max_torque"); - std::optional max_regen_torque = get_live_parameter("max_regen_torque"); - std::optional rear_torque_scale = get_live_parameter("rear_torque_scale"); - std::optional regen_torque_scale = get_live_parameter("regen_torque_scale"); - std::optional positive_speed_set = get_live_parameter("positive_speed_set"); - + auto max_torque = get_live_parameter("max_torque"); + auto max_regen_torque = get_live_parameter("max_regen_torque"); + auto rear_torque_scale = get_live_parameter("rear_torque_scale"); + auto regen_torque_scale = get_live_parameter("regen_torque_scale"); + auto positive_speed_set = get_live_parameter("positive_speed_set"); if (!(max_torque && max_regen_torque && rear_torque_scale && regen_torque_scale && positive_speed_set)) { return false; @@ -58,12 +57,12 @@ bool control::SimpleController::init() _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)); + param_update_handler_sig.connect(boost::bind(&control::SimpleSpeedController::_handle_param_updates, this, std::placeholders::_1)); return true; } -core::SpeedControlOut control::SimpleController::step_controller(const core::VehicleState &in) +core::ControllerOutput control::SimpleSpeedController::step_controller(const core::VehicleState &in) { config cur_config; { @@ -77,9 +76,12 @@ core::SpeedControlOut control::SimpleController::step_controller(const core::Veh torque_nm torqueRequest; - // hytech_msgs::MCUCommandData cmd_out; - core::SpeedControlOut cmd_out; - cmd_out.mcu_recv_millis = in.prev_MCU_recv_millis; // heartbeat + core::SpeedControlOut type_set; + core::ControllerOutput cmd_out; + cmd_out.out = type_set; + auto& speed_out = std::get(cmd_out.out); + + speed_out.mcu_recv_millis = in.prev_MCU_recv_millis; // heartbeat if (accelRequest >= 0.0) { @@ -87,29 +89,29 @@ core::SpeedControlOut control::SimpleController::step_controller(const core::Veh 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); + 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); } 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); + 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); } 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 0000000..14d38fa --- /dev/null +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp @@ -0,0 +1,94 @@ +#include +#include +#include + +void control::SimpleTorqueController::_handle_param_updates(const std::unordered_map &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(&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(&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(&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(&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("max_torque"); + auto max_regen_torque = get_live_parameter("max_regen_torque"); + auto rear_torque_scale = get_live_parameter("rear_torque_scale"); + auto regen_torque_scale = get_live_parameter("regen_torque_scale"); + if (!(max_torque && max_regen_torque && rear_torque_scale && regen_torque_scale)) + { + 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)); + + 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(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/flake.lock b/flake.lock index 2087e11..2b6016f 100644 --- a/flake.lock +++ b/flake.lock @@ -52,17 +52,17 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1739899350, - "narHash": "sha256-9BYbFPgHMOP756AhWICIzg6qxTdBcT831bO0QhI3WPM=", - "owner": "hytech-racing", + "lastModified": 1740936071, + "narHash": "sha256-Js+xaJFIjGIGWwmlOp4sl4FgNGxP7qbEXg6kkzGVfbs=", + "owner": "JohnNesbit", "repo": "drivebrain_core", - "rev": "a90ba76b5ff105814c1fd64e8112b5eae5f5bae4", + "rev": "17d90e3eacf9d4a23ba43dc8dfabf1412b0b588a", "type": "github" }, "original": { - "owner": "hytech-racing", - "ref": "feature/low_level_inputs", + "owner": "JohnNesbit", "repo": "drivebrain_core", + "rev": "17d90e3eacf9d4a23ba43dc8dfabf1412b0b588a", "type": "github" } }, @@ -151,11 +151,11 @@ ] }, "locked": { - "lastModified": 1730504689, - "narHash": "sha256-hgmguH29K2fvs9szpq2r3pz2/8cJd2LPS+b4tfNFCwE=", + "lastModified": 1740872218, + "narHash": "sha256-ZaMw0pdoUKigLpv9HiNDH2Pjnosg7NBYMJlHTIsHEUo=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "506278e768c2a08bec68eb62932193e341f55c90", + "rev": "3876f6b87db82f33775b1ef5ea343986105db764", "type": "github" }, "original": { @@ -235,11 +235,11 @@ "foxglove-schemas-src": { "flake": false, "locked": { - "lastModified": 1721832154, - "narHash": "sha256-Jp47MYWu3Sh60prjNALs1OaFnPZb7tXRNzFbSRZBXU0=", + "lastModified": 1740803626, + "narHash": "sha256-3gNItgIP1CWjxCwjD5NoK2oCe0tRqQ+0rvInSfh2VXg=", "owner": "foxglove", "repo": "schemas", - "rev": "f028ba1154faa1226a9993cc2223855123dbf817", + "rev": "399da6847abc0d0bcde103acf59e238af1652c70", "type": "github" }, "original": { @@ -264,6 +264,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": { @@ -325,11 +373,11 @@ "nanopb-proto-api": { "flake": false, "locked": { - "lastModified": 1736537572, - "narHash": "sha256-+vIvF7t58oB+QrxUBlHoQxLpbkOKv1Oy07ggyXnn4go=", + "lastModified": 1740503649, + "narHash": "sha256-5TOMOQ+8+7UYkhlx856+XYUWRWX63gLIoO9FCyXpshc=", "owner": "nanopb", "repo": "nanopb", - "rev": "fc7d53c3fea3ce04af5b98234feaa1c7400f73d6", + "rev": "8e9d5a10125b596188e95e925937d380a3c79e8e", "type": "github" }, "original": { @@ -348,6 +396,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 +408,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": { diff --git a/flake.nix b/flake.nix index 5a0247b..1e1fcec 100644 --- a/flake.nix +++ b/flake.nix @@ -37,7 +37,7 @@ 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:JohnNesbit/drivebrain_core/17d90e3eacf9d4a23ba43dc8dfabf1412b0b588a"; flake = false; }; diff --git a/unit_test/SimpleControllerTest.cpp b/unit_test/SimpleControllerTest.cpp deleted file mode 100644 index cc70780..0000000 --- a/unit_test/SimpleControllerTest.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include -#include -#include -#include -#include -#include - -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 0000000..b62f6eb --- /dev/null +++ b/unit_test/SimpleSpeedControllerTest.cpp @@ -0,0 +1,196 @@ +#include +#include +#include +#include +#include +#include + +class SimpleSpeedControllerTest : public testing::Test { + +protected: + core::Logger logger; + core::JsonFileHandler config; + control::SimpleSpeedController simple_controller; + control::SimpleSpeedController fail_controller; + core::VehicleState in; + + SimpleSpeedControllerTest() + : logger(core::LogLevel::INFO), + config("../config/drivebrain_config.json"), + simple_controller(logger, config), + fail_controller(logger, config, "no_config_here"), + 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(SimpleSpeedControllerTest, ConstructorInitializesProperly) +{ + EXPECT_NEAR(simple_controller.get_dt_sec(), 0.001, 0.01); +} + +TEST_F(SimpleSpeedControllerTest, InitHasConfig) +{ + EXPECT_TRUE(simple_controller.init()); +} + +TEST_F(SimpleSpeedControllerTest, InitDoesNotHaveConfig) +{ + EXPECT_FALSE(fail_controller.init()); +} + +TEST_F(SimpleSpeedControllerTest, NoPedalInput) +{ + auto cmd = simple_controller.step_controller(in); + auto res = std::get_if(&cmd.out); + + ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 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(&cmd.out); + + ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 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(&cmd.out); + + ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 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(&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(&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(&cmd.out); + + ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 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, VariableRequests) +{ + in.input.requested_brake = 1; + in.input.requested_accel = 1; + auto cmd = simple_controller.step_controller(in); + auto res = std::get_if(&cmd.out); + + ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 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, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 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); +} diff --git a/unit_test/SimpleTorqueControllerTest.cpp b/unit_test/SimpleTorqueControllerTest.cpp new file mode 100644 index 0000000..0c02d8f --- /dev/null +++ b/unit_test/SimpleTorqueControllerTest.cpp @@ -0,0 +1,151 @@ +#include +#include +#include +#include +#include +#include + +class SimpleTorqueControllerTest : public testing::Test { + +protected: + core::Logger logger; + core::JsonFileHandler config; + control::SimpleTorqueController simple_controller; + control::SimpleTorqueController fail_controller; + core::VehicleState in; + + SimpleTorqueControllerTest() + : logger(core::LogLevel::INFO), + config("../config/drivebrain_config.json"), + simple_controller(logger, config), + fail_controller(logger, config, "no_config_here"), + 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, ConstructorInitializesProperly) +{ + EXPECT_NEAR(simple_controller.get_dt_sec(), 0.001, 0.01); +} + +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(&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(&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(&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(&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(&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(&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(&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/test_controller_manager.cpp b/unit_test/test_controller_manager.cpp new file mode 100644 index 0000000..6444813 --- /dev/null +++ b/unit_test/test_controller_manager.cpp @@ -0,0 +1,203 @@ +#include +#include "ControllerManager.hpp" +#include "Controller.hpp" +#include "Controllers.hpp" +#include "SimpleSpeedController.hpp" +#include "SimpleTorqueController.hpp" + +#include + +class ControllerManagerTest : public ::testing::Test { +protected: + control::SimpleSpeedController simpleSpeedController1; + control::SimpleTorqueController simpleTorqueController1; + control::SimpleSpeedController simpleSpeedController2; + control::SimpleTorqueController simpleTorqueController2; + control::ControllerManager, 2> controller_manager_2speed; + control::ControllerManager, 2> controller_manager_2torque; + control::ControllerManager, 2> controller_manager_diff; + core::JsonFileHandler json_file_handler; + core::Logger logger; + + core::VehicleState vehicle_state; + core::ControllerOutput torque_controller_output; + core::ControllerOutput speed_controller_output; + + ControllerManagerTest() + : logger(core::LogLevel::NONE), + json_file_handler("../config/drivebrain_config.json"), + simpleSpeedController1(logger, json_file_handler), + simpleSpeedController2(logger, json_file_handler), + simpleTorqueController1(logger, json_file_handler), + simpleTorqueController2(logger, json_file_handler), + controller_manager_2speed(logger, json_file_handler, {&simpleSpeedController1, &simpleSpeedController2}), + controller_manager_2torque(logger, json_file_handler, {&simpleTorqueController1, &simpleTorqueController2}), + controller_manager_diff(logger, 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}; + + controller_manager_2speed.init(); + controller_manager_2torque.init(); + controller_manager_diff.init(); + simpleSpeedController1.init(); + simpleSpeedController2.init(); + simpleTorqueController1.init(); + simpleTorqueController2.init(); + + // 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(output.out)); + auto torque_output = std::get(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(output.out)); + auto speed_output = std::get(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(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(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(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(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(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.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(output.out)); + auto _output = std::get(output.out); + ASSERT_TRUE(_output.desired_rpms.FL - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); + ASSERT_TRUE(_output.desired_rpms.FR - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); + ASSERT_TRUE(_output.desired_rpms.RL - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); + ASSERT_TRUE(_output.desired_rpms.RR - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); +} + +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(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(output.out)); +} \ No newline at end of file From 0b51feb415bc703b6fe7a230b6aa547d227bd998 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 2 Mar 2025 14:05:45 -0500 Subject: [PATCH 06/87] erm, WIP --- CMakeLists.txt | 2 +- config/drivebrain_config.json | 44 +---- .../include/SimpleController.hpp | 2 + .../src/SimpleController.cpp | 60 ++++--- flake.nix | 2 +- unit_test/SimpleControllerTest.cpp | 160 ++++++++++-------- unit_test/main.cpp | 64 +++++++ 7 files changed, 190 insertions(+), 144 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2632d45..2062510 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -259,7 +259,7 @@ enable_testing() add_executable(alpha_test unit_test/main.cpp - unit_test/SimpleControllerTest.cpp + # unit_test/SimpleControllerTest.cpp ) target_link_libraries(alpha_build PUBLIC diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 63a0c1a..15c7696 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -8,53 +8,13 @@ "max_regen_torque": 10.0, "rear_torque_scale": 1.0, "regen_torque_scale": 0.6, - "positive_speed_set" : 3 + "positive_speed_set" : 3, + "max_power_kw": 63.0 }, "VNDriver": { "device_name": "/dev/ttyUSB0", "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 } } \ No newline at end of file diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp index ca7a689..ac2637e 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp @@ -32,6 +32,7 @@ namespace control float rear_torque_scale; float regen_torque_scale; speed_m_s positive_speed_set; + float max_power_kw; }; SimpleController(core::Logger &logger, core::JsonFileHandler &json_file_handler) : Configurable(logger, json_file_handler, "SimpleController") {} float get_dt_sec() override { @@ -42,6 +43,7 @@ namespace control private: void _handle_param_updates(const std::unordered_map &new_param_map); + core::SpeedControlOut _apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms); private: std::mutex _config_mutex; config _config; diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp index 70afb95..c1383cf 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp @@ -50,8 +50,9 @@ bool control::SimpleController::init() std::optional rear_torque_scale = get_live_parameter("rear_torque_scale"); std::optional regen_torque_scale = get_live_parameter("regen_torque_scale"); std::optional positive_speed_set = get_live_parameter("positive_speed_set"); + std::optional max_power_kw = get_live_parameter("max_power_kw"); - if (!(max_torque && max_regen_torque && rear_torque_scale && regen_torque_scale && positive_speed_set)) + if (!(max_torque && max_regen_torque && rear_torque_scale && regen_torque_scale && positive_speed_set && max_power_kw)) { return false; } @@ -114,61 +115,66 @@ core::SpeedControlOut control::SimpleController::step_controller(const core::Veh cmd_out.torque_lim_nm.RR = (torqueRequest * cur_config.rear_torque_scale); } + cmd_out = _apply_power_limit(cmd_out, in.current_rpms); + + return cmd_out; +} + +core::SpeedControlOut control::SimpleController::_apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms) +{ + 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 += abs(cmd_out.torque_lim_nm.FL); - net_torque_mag += abs(cmd_out.torque_lim_nm.FR); - net_torque_mag += abs(cmd_out.torque_lim_nm.RL); - net_torque_mag += abs(cmd_out.torque_lim_nm.RR); + net_torque_mag += ::abs(cmd_out.torque_lim_nm.FL); + net_torque_mag += ::abs(cmd_out.torque_lim_nm.FR); + net_torque_mag += ::abs(cmd_out.torque_lim_nm.RL); + net_torque_mag += ::abs(cmd_out.torque_lim_nm.RR); - net_power += abs(cmd_out.torque_lim_nm.FL) * (current_rpms.FL * constants::RPM_TO_RAD_PER_SECOND); - net_power += abs(cmd_out.torque_lim_nm.FR) * (current_rpms.FR * constants::RPM_TO_RAD_PER_SECOND); - net_power += abs(cmd_out.torque_lim_nm.RL) * (current_rpms.RL * constants::RPM_TO_RAD_PER_SECOND); - net_power += abs(cmd_out.torque_lim_nm.RR) * (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND); + net_power += ::abs(cmd_out.torque_lim_nm.FL) * (current_rpms.FL * constants::RPM_TO_RAD_PER_SECOND); + net_power += ::abs(cmd_out.torque_lim_nm.FR) * (current_rpms.FR * constants::RPM_TO_RAD_PER_SECOND); + net_power += ::abs(cmd_out.torque_lim_nm.RL) * (current_rpms.RL * constants::RPM_TO_RAD_PER_SECOND); + net_power += ::abs(cmd_out.torque_lim_nm.RR) * (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND); - if (net_power > constants::POWER_LIMIT) { + auto max_power_watt = _config.max_power_kw * 1000.0f; + if (net_power > max_power_watt) { /* 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 = abs(cmd_out.torque_lim_nm.FL / net_torque_mag); - float power_per_corner_FL = (torque_percent_FL * constants::POWER_LIMIT); + float torque_percent_FL = ::abs(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 = abs(power_per_corner_FL / (current_rpms.FL * constants::RPM_TO_RAD_PER_SECOND)); + cmd_out.torque_lim_nm.FL = ::abs(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 = abs(cmd_out.torque_lim_nm.FR / net_torque_mag); - float power_per_corner_FR = (torque_percent_FR * constants::POWER_LIMIT); + float torque_percent_FR = ::abs(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 = abs(power_per_corner_FR / (current_rpms.FR * constants::RPM_TO_RAD_PER_SECOND)); + cmd_out.torque_lim_nm.FR = ::abs(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 = abs(cmd_out.torque_lim_nm.RL / net_torque_mag); - float power_per_corner_RL = (torque_percent_RL * constants::POWER_LIMIT); + float torque_percent_RL = ::abs(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 = abs(power_per_corner_RL / (current_rpms.RL * constants::RPM_TO_RAD_PER_SECOND)); + cmd_out.torque_lim_nm.RL = ::abs(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 = abs(cmd_out.torque_lim_nm.RR / net_torque_mag); - float power_per_corner_RR = (torque_percent_RR * constants::POWER_LIMIT); + float torque_percent_RR = ::abs(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 = abs(power_per_corner_RR / (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND)); + cmd_out.torque_lim_nm.RR = ::abs(power_per_corner_RR / (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND)); } - - - - return cmd_out; -} +} \ No newline at end of file diff --git a/flake.nix b/flake.nix index 5eee659..c090d83 100644 --- a/flake.nix +++ b/flake.nix @@ -143,7 +143,7 @@ alias run="./build/alpha_build config/drivebrain_config.json $DBC_PATH/hytech.dbc" ''; nativeBuildInputs = [ pkgs.drivebrain_core_msgs_proto_cpp ]; - packages = [ pkgs.mcap-cli ]; + packages = [ pkgs.mcap-cli pkgs.valgrind ]; inputsFrom = [ pkgs.drivebrain_software ]; diff --git a/unit_test/SimpleControllerTest.cpp b/unit_test/SimpleControllerTest.cpp index fe33948..8081cc0 100644 --- a/unit_test/SimpleControllerTest.cpp +++ b/unit_test/SimpleControllerTest.cpp @@ -8,18 +8,24 @@ class SimpleControllerTest : public testing::Test { protected: + SimpleControllerTest() + : + logger(core::LogLevel::INFO) + { + // logger = core::Logger(); + config = core::JsonFileHandler(std::string("../config/drivebrain_config.json")); + simple_controller = std::make_unique(logger, config); + } + core::Logger logger; core::JsonFileHandler config; - control::SimpleController simple_controller; + + // 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) { - } + std::unique_ptr simple_controller; void SetUp() override { - simple_controller.init(); + simple_controller->init(); } void TearDown() override { @@ -28,9 +34,9 @@ class SimpleControllerTest : public testing::Test { }; -TEST_F(SimpleControllerTest, Outputs) { +TEST_F(SimpleControllerTest, SimpleAccel) { - // --- Test Case 1: Simple Acceleration --- + // // --- Test Case 1: Simple Acceleration --- { core::VehicleState in; veh_vec current_rpms; @@ -39,11 +45,11 @@ TEST_F(SimpleControllerTest, Outputs) { current_rpms.RL = 100; current_rpms.RR = 100; in.current_rpms = current_rpms; - in.input.requested_accel = 10; + in.input.requested_accel = 1; in.input.requested_brake = 0; in.prev_MCU_recv_millis = 0; - auto res = simple_controller.step_controller(in); + auto res = simple_controller->step_controller(in); ASSERT_EQ(res.desired_rpms.FL, 180); ASSERT_EQ(res.desired_rpms.FR, 180); ASSERT_EQ(res.desired_rpms.RL, 180); @@ -53,72 +59,80 @@ TEST_F(SimpleControllerTest, Outputs) { ASSERT_EQ(res.torque_lim_nm.RL, 210); ASSERT_EQ(res.torque_lim_nm.RR, 210); } +} +TEST_F(SimpleControllerTest, FullBraking) +{ + // // --- Test Case 2: Full Braking --- + // { + // core::VehicleState in; + // veh_vec 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; - // --- Test Case 2: Full Braking --- - { - core::VehicleState in; - veh_vec 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 = 10; - in.prev_MCU_recv_millis = 0; - - auto res = simple_controller.step_controller(in); - 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, 60); - ASSERT_EQ(res.torque_lim_nm.FR, 60); - ASSERT_EQ(res.torque_lim_nm.RL, 60); - ASSERT_EQ(res.torque_lim_nm.RR, 60); - } + // auto res = simple_controller.step_controller(in); + // 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, 60); + // ASSERT_EQ(res.torque_lim_nm.FR, 60); + // ASSERT_EQ(res.torque_lim_nm.RL, 60); + // ASSERT_EQ(res.torque_lim_nm.RR, 60); + // } +} +TEST_F(SimpleControllerTest, ZerlAccelZeroBrake) +{ // --- Test Case 3: Zero Acceleration and Zero Brake (Coasting) --- - { - core::VehicleState in; - veh_vec 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; - in.prev_MCU_recv_millis = 0; + // { + // core::VehicleState in; + // veh_vec 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; + // in.prev_MCU_recv_millis = 0; - auto res = simple_controller.step_controller(in); - 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, 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); - } + // auto res = simple_controller.step_controller(in); + // 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, 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 Case 4: Power Limiting Scenario --- - { - core::VehicleState in; - veh_vec current_rpms; - current_rpms.FL = 5000; - current_rpms.FR = 5000; - current_rpms.RL = 5000; - current_rpms.RR = 5000; - in.current_rpms = current_rpms; - in.input.requested_accel = 10; - in.input.requested_brake = 0; - in.prev_MCU_recv_millis = 0; +TEST_F(SimpleControllerTest, TestPowerLimit) +{ + // // --- Test Case 4: Power Limiting Scenario --- + // { + // core::VehicleState in; + // veh_vec current_rpms; + // current_rpms.FL = 5000; + // current_rpms.FR = 5000; + // current_rpms.RL = 5000; + // current_rpms.RR = 5000; + // in.current_rpms = current_rpms; + // in.input.requested_accel = 0.10; + // in.input.requested_brake = 0; + // in.prev_MCU_recv_millis = 0; - auto res = simple_controller.step_controller(in); - ASSERT_LT(res.torque_lim_nm.FL, 210); // Expect power limiting applied - ASSERT_LT(res.torque_lim_nm.FR, 210); - ASSERT_LT(res.torque_lim_nm.RL, 210); - ASSERT_LT(res.torque_lim_nm.RR, 210); - } + // auto res = simple_controller.step_controller(in); + // ASSERT_LT(res.torque_lim_nm.FL, 210); // Expect power limiting applied + // ASSERT_LT(res.torque_lim_nm.FR, 210); + // ASSERT_LT(res.torque_lim_nm.RL, 210); + // ASSERT_LT(res.torque_lim_nm.RR, 210); + // } } diff --git a/unit_test/main.cpp b/unit_test/main.cpp index 2dc3787..d288aaf 100644 --- a/unit_test/main.cpp +++ b/unit_test/main.cpp @@ -1,5 +1,69 @@ #include +#include +#include +#include +#include +#include +#include + +// class SimpleControllerTest : public testing::Test { + +// protected: +// core::Logger logger; +// core::JsonFileHandler config; +// std::unique_ptr simple_controller; + +// SimpleControllerTest() +// : +// logger(core::LogLevel::INFO), +// config(std::string("../config/drivebrain_config.json")) +// { +// simple_controller = std::make_unique(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(logger, config); + core::VehicleState in{}; + veh_vec 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(); From 6e7ea4c1a9e0e75bef6027db2f29dd4cbf880bb2 Mon Sep 17 00:00:00 2001 From: JohnNesbit Date: Sun, 2 Mar 2025 16:51:16 -0500 Subject: [PATCH 07/87] Standardize SE input, change nix DB core source --- drivebrain_app/src/DriveBrainApp.cpp | 17 ++--------------- flake.lock | 10 +++++----- flake.nix | 2 +- 3 files changed, 8 insertions(+), 21 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 8d8bdda..980d8ab 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -121,30 +121,17 @@ void DriveBrainApp::_process_loop() { //logic for retrieving whichever type is currently in the variant, i dont think we need to check if it has monostate core::SpeedControlOut speed_cmd_out; - core::TorqueControlOut torque_cmd_out; if(std::holds_alternative(out_struct.out)) { speed_cmd_out = std::get(out_struct.out); } else if(std::holds_alternative(out_struct.out)) { - torque_cmd_out = std::get(out_struct.out); + speed_cmd_out = {0, std::get(out_struct.out).desired_torques_nm, std::get(out_struct.out).desired_torques_nm}; } //for when we have both controllers the vision is if(speed_cmd_out) -> else auto temp_desired_torques = state_and_validity.first.matlab_math_temp_out; - - - //_state_estimator->set_previous_control_output(out_struct); - // TCMUX code works with ControllerOutput, but the state estimator only works with speed controls it looks like - // so we grab the speed if that is what the variant contains - // this means that we actuall can only use one Controller, the speed one, so we gotta re-write state estimator :/ - if (auto speedOut = std::get_if(&out_struct.out)) { - _state_estimator->set_previous_control_output(*speedOut); - } else { - // if the std::variant does not contain SpeedControlOut(we're so cooked) - std::cerr << "Error: ControllerOutput is monostate or TorqueControlOut" << std::endl; - } - + _state_estimator->set_previous_control_output(speed_cmd_out); if(temp_desired_torques.res_torque_lim_nm.FL < 0) { desired_rpm_msg->set_drivebrain_set_rpm_fl(0); diff --git a/flake.lock b/flake.lock index 2b6016f..8130d17 100644 --- a/flake.lock +++ b/flake.lock @@ -52,17 +52,17 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1740936071, + "lastModified": 1740951635, "narHash": "sha256-Js+xaJFIjGIGWwmlOp4sl4FgNGxP7qbEXg6kkzGVfbs=", - "owner": "JohnNesbit", + "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "17d90e3eacf9d4a23ba43dc8dfabf1412b0b588a", + "rev": "2303c612b5b4fb9e4975811e77079f2b25193399", "type": "github" }, "original": { - "owner": "JohnNesbit", + "owner": "hytech-racing", + "ref": "feature/ControllerMUX", "repo": "drivebrain_core", - "rev": "17d90e3eacf9d4a23ba43dc8dfabf1412b0b588a", "type": "github" } }, diff --git a/flake.nix b/flake.nix index 1e1fcec..ca92047 100644 --- a/flake.nix +++ b/flake.nix @@ -37,7 +37,7 @@ vn_driver_lib.url = "github:RCMast3r/vn_driver_lib/fix/boost-compatible"; db-core-src = { - url = "github:JohnNesbit/drivebrain_core/17d90e3eacf9d4a23ba43dc8dfabf1412b0b588a"; + url = "github:hytech-racing/drivebrain_core/feature/ControllerMUX"; flake = false; }; From 5d979cd760bfc77e58fd8ba8baa0a54a986708c9 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 2 Mar 2025 22:48:53 -0500 Subject: [PATCH 08/87] progress --- .clangd | 3 ++ CMakeLists.txt | 12 +++-- asdf | 29 ++++++++---- config/config_test.json | 10 +++++ drivebrain_app/src/DriveBrainApp.cpp | 6 ++- .../drivebrain_comms/src/CANComms.cpp | 2 +- .../drivebrain_comms/src/VNComms.cpp | 2 +- .../include/SimpleController.hpp | 2 +- .../src/SimpleController.cpp | 2 +- .../include/MCAPProtobufLogger.hpp | 12 ++--- .../src/MCAPProtobufLogger.cpp | 11 ++--- flake.lock | 7 +-- flake.nix | 2 +- test/test_configurable.cpp | 45 ++++++++++++++++++- test/test_mcu.cpp | 1 + 15 files changed, 114 insertions(+), 32 deletions(-) create mode 100644 .clangd create mode 100644 config/config_test.json diff --git a/.clangd b/.clangd new file mode 100644 index 0000000..10aacf4 --- /dev/null +++ b/.clangd @@ -0,0 +1,3 @@ +CompileFlags: + CompilationDatabase: build/ + Add: [-std=c++17] \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 0d317e7..36da203 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -124,10 +124,10 @@ target_link_libraries(drivebrain_control PUBLIC 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/MCAPProtobufLogger.cpp) target_include_directories(drivebrain_mcap_logger PUBLIC - $ - $ + $ + $ ) target_link_libraries(drivebrain_mcap_logger PUBLIC @@ -223,6 +223,12 @@ 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 +) + # add_executable(test_mcap test/test_mcap/test_mcap_logging.cpp test/test_mcap/BuildFileDescriptorSet.cpp) diff --git a/asdf b/asdf index 463081d..f182ea6 100644 --- a/asdf +++ b/asdf @@ -1,20 +1,31 @@ { "type": "object", "properties": { - "member_1": { - "type": "object" , + "member_1": { + "type": "object", "properties": { - "y": { "type": "number" }, - "z": { "type": "number" } + "y": { + "type": "number" + }, + "z": { + "type": "number" + } } }, - "member_2": { - "type": "object" , + "member_2": { + "type": "object", "properties": { - "y": { "type": "number" }, - "z": { "type": "number" } + "y": { + "type": "number" + }, + "z": { + "type": "number" + } } } }, - "required": ["member_1", "member_2"] + "required": [ + "member_1", + "member_2" + ] } \ No newline at end of file diff --git a/config/config_test.json b/config/config_test.json new file mode 100644 index 0000000..cd472ce --- /dev/null +++ b/config/config_test.json @@ -0,0 +1,10 @@ +{ + "ConfigureableTest" : + { + "test_float": 1.0, + "test_bool": false, + "test_double": 2.0, + "test_string": "asdf", + "test_int": 2 + } +} \ No newline at end of file diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 68914a3..14d5b26 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -75,7 +75,11 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d _io_context, "192.168.1.30", 2001, 2000); _vn_driver = std::make_unique( - _config, _logger, _message_logger, *_state_estimator, _io_context); + _config, _logger, _message_logger, *_state_estimator, _io_context, construction_failed); + + if (construction_failed) { + throw std::runtime_error("Failed to construct VN driver"); + } _db_service = std::make_unique(_message_logger); diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index 3ec2539..b9ae500 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -65,7 +65,7 @@ bool comms::CANDriver::init() { _logger.log_string("inited, started read", core::LogLevel::INFO); _do_read(); - _configured = true; + // _configured = true; return true; } diff --git a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp index 89deda3..c78fe20 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp @@ -52,7 +52,7 @@ namespace comms _configure_binary_outputs(); - _configured = true; + // _configured = true; return true; } diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp index ca7a689..86b118f 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleController.hpp @@ -22,7 +22,7 @@ namespace control // 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 diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp index 0d79fdc..466105c 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleController.cpp @@ -59,7 +59,7 @@ bool control::SimpleController::init() _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)); - _configured = true; + // _configured = true; return true; } diff --git a/drivebrain_core_impl/drivebrain_logging/include/MCAPProtobufLogger.hpp b/drivebrain_core_impl/drivebrain_logging/include/MCAPProtobufLogger.hpp index f8f6ca7..9b8c3f5 100644 --- a/drivebrain_core_impl/drivebrain_logging/include/MCAPProtobufLogger.hpp +++ b/drivebrain_core_impl/drivebrain_logging/include/MCAPProtobufLogger.hpp @@ -3,7 +3,7 @@ #include -#include +#include #include @@ -18,7 +18,7 @@ #include #include - +#include // - [ ] add functionality for logging parameters json @@ -43,13 +43,13 @@ namespace common uint64_t log_time; }; - MCAPProtobufLogger(const std::string &base_dir); + MCAPProtobufLogger(const std::string &base_dir, std::function get_component_param_schemas); ~MCAPProtobufLogger(); /// @brief /// @param out_msg void log_msg(std::shared_ptr out_msg); - void log_json_struct(const std::string & topic, const nhlohmann::json::object & out_json ); + void log_json_struct(const std::string & topic, const nlohmann::json::object & out_json ); void open_new_mcap(const std::string &name); void close_current_mcap(); @@ -57,7 +57,7 @@ namespace common private: void _handle_log_to_file(); private: - core::common::ThreadSafeDeque _input_deque; + core::common::ThreadSafeDeque _input_deque; std::thread _log_thread; bool _running = false; mcap::McapWriterOptions _options; @@ -65,6 +65,8 @@ namespace common std::mutex _logger_mtx; std::unordered_map _msg_name_id_map; + std::function _get_params_schema; + }; } diff --git a/drivebrain_core_impl/drivebrain_logging/src/MCAPProtobufLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/MCAPProtobufLogger.cpp index 7937912..4f6c09f 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/MCAPProtobufLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/MCAPProtobufLogger.cpp @@ -12,8 +12,8 @@ namespace common { - MCAPProtobufLogger::MCAPProtobufLogger(const std::string &base_dir, std::function get_json_schema) - : _options(mcap::McapWriterOptions("")) + MCAPProtobufLogger::MCAPProtobufLogger(const std::string &base_dir, std::function get_component_param_schemas) + : _options(mcap::McapWriterOptions(""), _get_params_schema(get_component_param_schemas)) { auto optional_map = util::generate_name_to_id_map({"hytech_msgs.proto", "hytech.proto"}); if (optional_map) @@ -78,8 +78,9 @@ namespace common add_schema_func(schema_only_descriptors, true); add_schema_func(receiving_descriptors, false); + auto params_schema_json = _get_params_schema(); + - spdlog::info("Added message descriptions to MCAP"); } @@ -91,7 +92,7 @@ namespace common void MCAPProtobufLogger::_handle_log_to_file() { - core::common::ThreadSafeDeque q; + core::common::ThreadSafeDeque q; // this will occasionally take a while (~200ms) to complete a loop iteration so this is in its own thread while (true) @@ -146,5 +147,5 @@ namespace common } - void MCAPProtobufLogger::log_parameters() + // void MCAPProtobufLogger::log_parameters() } \ No newline at end of file diff --git a/flake.lock b/flake.lock index 80b2eaf..e4d2715 100644 --- a/flake.lock +++ b/flake.lock @@ -52,15 +52,16 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1731609678, - "narHash": "sha256-AC96vyQBl/h6/4o1QuNGimJywy2T7p6tfe2ePWLwkqM=", + "lastModified": 1740960142, + "narHash": "sha256-LHW67AeOSl6TRC6op0sLu2GzxUTuevYdTMCbrHxBsgU=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "945279a1b4da90b4aab78346d8e1155c43c94281", + "rev": "602aa3541f1a53d3d5ea78fb6126afca3c458c54", "type": "github" }, "original": { "owner": "hytech-racing", + "ref": "feature/param_rec_support", "repo": "drivebrain_core", "type": "github" } diff --git a/flake.nix b/flake.nix index a728ad9..3bf8dce 100644 --- a/flake.nix +++ b/flake.nix @@ -37,7 +37,7 @@ vn_driver_lib.url = "github:RCMast3r/vn_driver_lib/fix/boost-compatible"; db-core-src = { - url = "github:hytech-racing/drivebrain_core"; + url = "github:hytech-racing/drivebrain_core/feature/param_rec_support"; flake = false; }; diff --git a/test/test_configurable.cpp b/test/test_configurable.cpp index 8c25ecf..015f1a7 100644 --- a/test/test_configurable.cpp +++ b/test/test_configurable.cpp @@ -1,3 +1,46 @@ +#include #include -#include +#include +#include +#include +#include +#include +#include +#include +// 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(_logger, json_file_handler, "ConfigureableTest") {} + bool init() override { + std::optional test_float = get_parameter_value("test_float"); + std::optional test_bool = get_parameter_value("test_bool"); + std::optional test_double = get_parameter_value("test_double"); + std::optional test_string = get_parameter_value("test_string"); + std::optional test_int = get_parameter_value("test_int"); + // _configured = true; + return true; + } + private: + core::Logger &_logger; + }; + + +int main() +{ + core::JsonFileHandler config("../config/config_test.json"); + core::Logger logger(core::LogLevel::INFO); + + ConfigureableTest test(logger, config); + test.init(); + std::cout << test.get_schema().dump() << std::endl; + + return 0; +} \ No newline at end of file diff --git a/test/test_mcu.cpp b/test/test_mcu.cpp index ba64724..51beee6 100644 --- a/test/test_mcu.cpp +++ b/test/test_mcu.cpp @@ -5,6 +5,7 @@ #include #include #include "hytech_msgs.pb.h" + void sendSpeedControlInMessage(const std::string &ip, int port) { // Create a UDP socket From 002448c8d1d208a601544970413b4c5782d7469d Mon Sep 17 00:00:00 2001 From: JohnNesbit Date: Wed, 19 Mar 2025 01:17:32 -0400 Subject: [PATCH 09/87] Testing(does not compile) --- drivebrain_app/src/DriveBrainApp.cpp | 84 ++++++++----------- .../include/StateEstimator.hpp | 2 +- .../src/StateEstimator.cpp | 30 +++++-- flake.lock | 40 ++++----- flake.nix | 4 +- 5 files changed, 82 insertions(+), 78 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 980d8ab..7605411 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -108,6 +108,7 @@ void DriveBrainApp::_process_loop() { // auto out_msg = std::make_shared(); auto desired_rpm_msg = std::make_shared(); auto torque_limit_msg = std::make_shared(); + auto desired_torque_msg = std::make_shared(); auto loop_time = _controllerManager.get_active_controller_timestep(); auto loop_time_micros = (int)(loop_time * 1000000.0f); std::chrono::microseconds loop_chrono_time(loop_time_micros); @@ -116,56 +117,43 @@ void DriveBrainApp::_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 = _controllerManager.step_active_controller(state_and_validity.first); - - //logic for retrieving whichever type is currently in the variant, i dont think we need to check if it has monostate - core::SpeedControlOut speed_cmd_out; - if(std::holds_alternative(out_struct.out)) - { - speed_cmd_out = std::get(out_struct.out); - } - else if(std::holds_alternative(out_struct.out)) - { - speed_cmd_out = {0, std::get(out_struct.out).desired_torques_nm, std::get(out_struct.out).desired_torques_nm}; - } - //for when we have both controllers the vision is if(speed_cmd_out) -> else - auto temp_desired_torques = state_and_validity.first.matlab_math_temp_out; - _state_estimator->set_previous_control_output(speed_cmd_out); - - 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(speed_cmd_out.desired_rpms.FL); - } - - 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(speed_cmd_out.desired_rpms.FR); - } - 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(speed_cmd_out.desired_rpms.RL); - } - - 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(speed_cmd_out.desired_rpms.RR); - } - - 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)); + auto out_struct = _controllerManager.step_active_controller(state_and_validity.first); - { - 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); + // get current command + core::ControllerOutput cmd_out = out_struct.out; + + // push current command for next state estimator call + _state_estimator->set_previous_control_output(cmd_out); + + if (std::holds_alternative(current_state.prev_controller_output)) { // speed controller, set RPM + + // set RPMs in message to the RPMS given from the controller + desired_rpm_msg->set_drivebrain_set_rpm_fl(cmd_out.desired_rpms.FL); + desired_rpm_msg->set_drivebrain_set_rpm_fr(cmd_out.desired_rpms.FR); + desired_rpm_msg->set_drivebrain_set_rpm_rl(cmd_out.desired_rpms.RL); + desired_rpm_msg->set_drivebrain_set_rpm_rr(cmd_out.desired_rpms.RR); + + // same with torque limits + torque_limit_msg->set_drivebrain_torque_fl(::abs(cmd_out.torque_lim_nm.FL)); + torque_limit_msg->set_drivebrain_torque_fr(::abs(cmd_out.torque_lim_nm.FR)); + torque_limit_msg->set_drivebrain_torque_rl(::abs(cmd_out.torque_lim_nm.RL)); + torque_limit_msg->set_drivebrain_torque_rr(::abs(cmd_out.torque_lim_nm.RR)); + { + 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); + } + } else if (std::holds_alternative(current_state.prev_controller_output)){ // if it is a torque controller: + // set desired torque + desired_torque_msg->set_drivebrain_torque_fl(::abs(cmd_out.desired_torques_nm.FL)); + desired_torque_msg->set_drivebrain_torque_fr(::abs(cmd_out.desired_torques_nm.FR)); + desired_torque_msg->set_drivebrain_torque_rl(::abs(cmd_out.desired_torques_nm.RL)); + desired_torque_msg->set_drivebrain_torque_rr(::abs(cmd_out.desired_torques_nm.RR)); + { + std::unique_lock lk(_can_tx_queue.mtx); + _can_tx_queue.deque.push_back(desired_torque_msg); // use new protobuf struct + } } auto end_time = std::chrono::high_resolution_clock::now(); diff --git a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp index d2c532b..c3808ce 100644 --- a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp +++ b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp @@ -75,7 +75,7 @@ namespace core void handle_recv_process(std::shared_ptr message); std::pair get_latest_state_and_validity(); - void set_previous_control_output(SpeedControlOut prev_control_output); + void set_previous_control_output(ControllerOutput prev_control_output); private: void _recv_low_level_state(std::shared_ptr message); diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 3265154..460a230 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -162,7 +162,7 @@ bool StateEstimator::_validate_stamps(const std::array StateEstimator::get_latest_state_and_validit curr_rpms->set_rr(current_state.current_rpms.RR); msg_out->set_state_is_valid(state_is_valid); - msg_out->set_steering_angle_deg(current_state.steering_angle_deg); + 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 (std::holds_alternative(current_state.prev_controller_output)) { + + // 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 + auto prev_driver_torque_req = msg_out->mutable_driver_torque(); + prev_driver_torque_req->set_fl(current_state.prev_controller_output.desired_torques_nm.FL); + prev_driver_torque_req->set_fr(current_state.prev_controller_output.desired_torques_nm.FR); + prev_driver_torque_req->set_rl(current_state.prev_controller_output.desired_torques_nm.RL); + prev_driver_torque_req->set_rr(current_state.prev_controller_output.desired_torques_nm.RR); + + } else (std::holds_alternative(current_state.prev_controller_output)) { // assuming no monostate possible here + + msg_out->set_is_using_torque_controller(false); + 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); + } auto log_start = std::chrono::high_resolution_clock::now(); _message_logger->log_msg(static_cast>(msg_out)); diff --git a/flake.lock b/flake.lock index 8130d17..3f19421 100644 --- a/flake.lock +++ b/flake.lock @@ -3,16 +3,16 @@ "HT_proto": { "flake": false, "locked": { - "lastModified": 1739773905, - "narHash": "sha256-VWA9cM2Ra/xWUoWZw+inJ1HesMEXrQmNiquJUuyT2R8=", + "lastModified": 1742359623, + "narHash": "sha256-Lt+eedvEagB9fOCNGQiDFn6jIjPje0zzY9xF4QJEE9w=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "d0399cbef06ecc51afab470db698b6e3d1cf62a4", + "rev": "11ba10f971a5beb52044c0b5d07c81867f7ad20b", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "2025-02-17T06_32_03", + "ref": "TCMUX", "repo": "HT_proto", "type": "github" } @@ -52,11 +52,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1740951635, - "narHash": "sha256-Js+xaJFIjGIGWwmlOp4sl4FgNGxP7qbEXg6kkzGVfbs=", + "lastModified": 1742359707, + "narHash": "sha256-znSvZ/mamfFNWMnuBbU3PTxCDfKt3qUIOohw2a2yU40=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "2303c612b5b4fb9e4975811e77079f2b25193399", + "rev": "6a6eb84f89e93b71e6d29bce32176f571b49ddfd", "type": "github" }, "original": { @@ -151,11 +151,11 @@ ] }, "locked": { - "lastModified": 1740872218, - "narHash": "sha256-ZaMw0pdoUKigLpv9HiNDH2Pjnosg7NBYMJlHTIsHEUo=", + "lastModified": 1741352980, + "narHash": "sha256-+u2UunDA4Cl5Fci3m7S643HzKmIDAe+fiXrLqYsR2fs=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "3876f6b87db82f33775b1ef5ea343986105db764", + "rev": "f4330d22f1c5d2ba72d3d22df5597d123fdb60a9", "type": "github" }, "original": { @@ -235,11 +235,11 @@ "foxglove-schemas-src": { "flake": false, "locked": { - "lastModified": 1740803626, - "narHash": "sha256-3gNItgIP1CWjxCwjD5NoK2oCe0tRqQ+0rvInSfh2VXg=", + "lastModified": 1742332163, + "narHash": "sha256-GdHx4NtpCdipBbuToHtE+joryJt/YG4vOqGGH8Raqbw=", "owner": "foxglove", "repo": "schemas", - "rev": "399da6847abc0d0bcde103acf59e238af1652c70", + "rev": "1ded9d0b71f0fdaf3a7064506be055aa009c1fc9", "type": "github" }, "original": { @@ -340,16 +340,16 @@ "utils": "utils_2" }, "locked": { - "lastModified": 1739624640, - "narHash": "sha256-g12QwiJYTQmlNPyCrqetNCMuZVsLL1wzHe5vBVITfvs=", + "lastModified": 1742359175, + "narHash": "sha256-Xf12l65WGnoS74G4eCw52eN8gC525urAU31w8hXwYwg=", "owner": "hytech-racing", "repo": "ht_can", - "rev": "0a3f0cd7062cab8d494eb62c6f622de6e913a114", + "rev": "553a9f088139c7b026a817ccca10c0f92d785dcd", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "141", + "ref": "TCMUX", "repo": "ht_can", "type": "github" } @@ -373,11 +373,11 @@ "nanopb-proto-api": { "flake": false, "locked": { - "lastModified": 1740503649, - "narHash": "sha256-5TOMOQ+8+7UYkhlx856+XYUWRWX63gLIoO9FCyXpshc=", + "lastModified": 1741586908, + "narHash": "sha256-byx2L+x45QNM0EPOnuZjph2p/2K9mYlm1H7f/8pyg10=", "owner": "nanopb", "repo": "nanopb", - "rev": "8e9d5a10125b596188e95e925937d380a3c79e8e", + "rev": "e00d04e2c575c19b7e582c8713be2a42098a8c87", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index ca92047..4fe92d9 100644 --- a/flake.nix +++ b/flake.nix @@ -21,7 +21,7 @@ type = "github"; owner = "hytech-racing"; repo = "HT_proto"; - ref = "2025-02-17T06_32_03"; + ref = "TCMUX"; flake = false; }; @@ -30,7 +30,7 @@ flake = false; }; - ht_can.url = "github:hytech-racing/ht_can/141"; + ht_can.url = "github:hytech-racing/ht_can/TCMUX"; ht_can.inputs.nixpkgs.follows = "nixpkgs"; ht_can.inputs.nix-proto.follows = "nix-proto"; From 9febd94deb2f24f529c8aa3923173713b7a5e43a Mon Sep 17 00:00:00 2001 From: JohnNesbit Date: Wed, 19 Mar 2025 02:04:27 -0400 Subject: [PATCH 10/87] Does not compile --- drivebrain_app/src/DriveBrainApp.cpp | 4 +-- .../src/StateEstimator.cpp | 28 ++++++++----------- flake.lock | 6 ++-- 3 files changed, 17 insertions(+), 21 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 7605411..495a433 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -121,10 +121,10 @@ void DriveBrainApp::_process_loop() { auto out_struct = _controllerManager.step_active_controller(state_and_validity.first); // get current command - core::ControllerOutput cmd_out = out_struct.out; + std::variant cmd_out = out_struct.out; // push current command for next state estimator call - _state_estimator->set_previous_control_output(cmd_out); + _state_estimator->set_previous_control_output(out_struct); if (std::holds_alternative(current_state.prev_controller_output)) { // speed controller, set RPM diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 460a230..7109c81 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -224,28 +224,24 @@ std::pair StateEstimator::get_latest_state_and_validit curr_rpms->set_rr(current_state.current_rpms.RR); msg_out->set_state_is_valid(state_is_valid); - msg_out->set_steering_angle_deg(current_state.steering_angle_deg); - - if (std::holds_alternative(current_state.prev_controller_output)) { + msg_out->set_steering_angle_deg(current_state.steering_angle_deg); + auto prev_driver_torque_req = msg_out->mutable_driver_torque(); + if (const core::TorqueControlOut* torqueControl = std::get_if(¤t_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 - auto prev_driver_torque_req = msg_out->mutable_driver_torque(); - prev_driver_torque_req->set_fl(current_state.prev_controller_output.desired_torques_nm.FL); - prev_driver_torque_req->set_fr(current_state.prev_controller_output.desired_torques_nm.FR); - prev_driver_torque_req->set_rl(current_state.prev_controller_output.desired_torques_nm.RL); - prev_driver_torque_req->set_rr(current_state.prev_controller_output.desired_torques_nm.RR); - - } else (std::holds_alternative(current_state.prev_controller_output)) { // assuming no monostate possible here + prev_driver_torque_req->set_fl(torqueControl->desired_torques_nm.FL); + prev_driver_torque_req->set_fr(torqueControl->desired_torques_nm.FR); + prev_driver_torque_req->set_rl(torqueControl->desired_torques_nm.RL); + prev_driver_torque_req->set_rr(torqueControl->desired_torques_nm.RR); + } else if (const core::SpeedControlOut* speedControl = std::get_if(¤t_state.prev_controller_output.out)) { // assuming no monostate possible here msg_out->set_is_using_torque_controller(false); - 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); + prev_driver_torque_req->set_fl(speedControl->torque_lim_nm.FL); + prev_driver_torque_req->set_fr(speedControl->torque_lim_nm.FR); + prev_driver_torque_req->set_rl(speedControl->torque_lim_nm.RL); + prev_driver_torque_req->set_rr(speedControl->torque_lim_nm.RR); } auto log_start = std::chrono::high_resolution_clock::now(); diff --git a/flake.lock b/flake.lock index 3f19421..e272f50 100644 --- a/flake.lock +++ b/flake.lock @@ -52,11 +52,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1742359707, - "narHash": "sha256-znSvZ/mamfFNWMnuBbU3PTxCDfKt3qUIOohw2a2yU40=", + "lastModified": 1742363870, + "narHash": "sha256-XAtlcAYMD2TprujUXV5kBB2SaxBl88bu5qUmCmwhR5U=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "6a6eb84f89e93b71e6d29bce32176f571b49ddfd", + "rev": "9bf16a0b3727e87ddf5737ecdc3ef08b20e39baf", "type": "github" }, "original": { From f690ec752f2876f94f97ec9de7c195b1c9d113d7 Mon Sep 17 00:00:00 2001 From: JohnNesbit Date: Wed, 19 Mar 2025 02:28:33 -0400 Subject: [PATCH 11/87] Fix type errors --- drivebrain_app/src/DriveBrainApp.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 495a433..1be094c 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -121,12 +121,12 @@ void DriveBrainApp::_process_loop() { auto out_struct = _controllerManager.step_active_controller(state_and_validity.first); // get current command - std::variant cmd_out = out_struct.out; + std::variant cmd_out = out_struct.out; // push current command for next state estimator call _state_estimator->set_previous_control_output(out_struct); - if (std::holds_alternative(current_state.prev_controller_output)) { // speed controller, set RPM + if (std::holds_alternative(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(cmd_out.desired_rpms.FL); @@ -144,7 +144,7 @@ void DriveBrainApp::_process_loop() { _can_tx_queue.deque.push_back(desired_rpm_msg); _can_tx_queue.deque.push_back(torque_limit_msg); } - } else if (std::holds_alternative(current_state.prev_controller_output)){ // if it is a torque controller: + } else if (std::holds_alternative(cmd_out)){ // if it is a torque controller: // set desired torque desired_torque_msg->set_drivebrain_torque_fl(::abs(cmd_out.desired_torques_nm.FL)); desired_torque_msg->set_drivebrain_torque_fr(::abs(cmd_out.desired_torques_nm.FR)); From 2ddc84715c4f918546e213f6b6e337352ccfb08f Mon Sep 17 00:00:00 2001 From: JohnNesbit Date: Wed, 19 Mar 2025 02:39:46 -0400 Subject: [PATCH 12/87] type errors --- drivebrain_app/src/DriveBrainApp.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 1be094c..16e8bc8 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -126,30 +126,30 @@ void DriveBrainApp::_process_loop() { // push current command for next state estimator call _state_estimator->set_previous_control_output(out_struct); - if (std::holds_alternative(cmd_out)) { // speed controller, set RPM + if (const core::SpeedControlOut* speedControl = std::get_if(&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(cmd_out.desired_rpms.FL); - desired_rpm_msg->set_drivebrain_set_rpm_fr(cmd_out.desired_rpms.FR); - desired_rpm_msg->set_drivebrain_set_rpm_rl(cmd_out.desired_rpms.RL); - desired_rpm_msg->set_drivebrain_set_rpm_rr(cmd_out.desired_rpms.RR); + 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); // same with torque limits - torque_limit_msg->set_drivebrain_torque_fl(::abs(cmd_out.torque_lim_nm.FL)); - torque_limit_msg->set_drivebrain_torque_fr(::abs(cmd_out.torque_lim_nm.FR)); - torque_limit_msg->set_drivebrain_torque_rl(::abs(cmd_out.torque_lim_nm.RL)); - torque_limit_msg->set_drivebrain_torque_rr(::abs(cmd_out.torque_lim_nm.RR)); + 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(_can_tx_queue.mtx); _can_tx_queue.deque.push_back(desired_rpm_msg); _can_tx_queue.deque.push_back(torque_limit_msg); } - } else if (std::holds_alternative(cmd_out)){ // if it is a torque controller: + } else if (const core::TorqueControlOut* torqueControl = std::get_if(&cmd_out)){ // if it is a torque controller: // set desired torque - desired_torque_msg->set_drivebrain_torque_fl(::abs(cmd_out.desired_torques_nm.FL)); - desired_torque_msg->set_drivebrain_torque_fr(::abs(cmd_out.desired_torques_nm.FR)); - desired_torque_msg->set_drivebrain_torque_rl(::abs(cmd_out.desired_torques_nm.RL)); - desired_torque_msg->set_drivebrain_torque_rr(::abs(cmd_out.desired_torques_nm.RR)); + desired_torque_msg->set_drivebrain_torque_fl(::abs(torqueControl->desired_torques_nm.FL)); + desired_torque_msg->set_drivebrain_torque_fr(::abs(torqueControl->desired_torques_nm.FR)); + desired_torque_msg->set_drivebrain_torque_rl(::abs(torqueControl->desired_torques_nm.RL)); + desired_torque_msg->set_drivebrain_torque_rr(::abs(torqueControl->desired_torques_nm.RR)); { std::unique_lock lk(_can_tx_queue.mtx); _can_tx_queue.deque.push_back(desired_torque_msg); // use new protobuf struct From 2cb4d88f58ebd4910186c2bb5aa0f695235fd359 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 30 Mar 2025 23:02:33 -0400 Subject: [PATCH 13/87] added comment --- unit_test/SimpleControllerTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unit_test/SimpleControllerTest.cpp b/unit_test/SimpleControllerTest.cpp index c4a1474..da81f69 100644 --- a/unit_test/SimpleControllerTest.cpp +++ b/unit_test/SimpleControllerTest.cpp @@ -146,5 +146,5 @@ TEST_F(SimpleControllerTest, TestPowerLimit) 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 + ASSERT_NEAR(net_power, 63000.0, 0.001); // Expect power limiting applied and ensure near 63kw (hard-coded limit) } From 73ab49b9bc76dd61ef22c1d994ce59feaba23634 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 30 Mar 2025 23:09:31 -0400 Subject: [PATCH 14/87] updating to latest branch of drivebrain core --- flake.lock | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flake.lock b/flake.lock index 7ba885a..fa9f156 100644 --- a/flake.lock +++ b/flake.lock @@ -52,11 +52,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1742990902, - "narHash": "sha256-/4LdFMVwQ78cBKrDnWuXBuU01H6vkqruvECS0NKXwvA=", + "lastModified": 1743390522, + "narHash": "sha256-xiyJgBOU2cO8dYYnKN1EJGEO7z4Cxhb4URs6ie27EGM=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "0e8641c7aab075875a7661edb13903edfafec6ea", + "rev": "baf0b9958966e4d96f21a4304f8e85fc9cdcbca6", "type": "github" }, "original": { From d13c58d8bed34d867d487869763c95ecbff673b5 Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Sun, 30 Mar 2025 23:40:18 -0400 Subject: [PATCH 15/87] TCMux integration test --- CMakeLists.txt | 13 +++ config/test_tcmux_integration.json | 21 +++++ .../include/SimpleTorqueController.hpp | 5 ++ .../src/SimpleTorqueController.cpp | 1 + flake.lock | 8 +- flake.nix | 2 +- suggestions.md | 2 + test/controller_manager_integration.cpp | 82 +++++++++++++++++++ 8 files changed, 129 insertions(+), 5 deletions(-) create mode 100644 config/test_tcmux_integration.json create mode 100644 suggestions.md create mode 100755 test/controller_manager_integration.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 73d94ab..ed13e33 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -327,3 +327,16 @@ install(TARGETS 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/config/test_tcmux_integration.json b/config/test_tcmux_integration.json new file mode 100644 index 0000000..f3cdd5d --- /dev/null +++ b/config/test_tcmux_integration.json @@ -0,0 +1,21 @@ +{ + "ControllerManager": { + "max_controller_switch_speed_ms": 1.793, + "max_switch_requested_rpm": 20000.0, + "max_torque_switch_nm": 10.0, + "max_accel_switch_float": 0.1 + }, + "SimpleTorqueController": { + "max_torque": 21.0, + "max_regen_torque": 10.0, + "rear_torque_scale": 1.0, + "regen_torque_scale": 0.6 + }, + "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/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp index 0800606..c0a5181 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp @@ -34,6 +34,11 @@ namespace control 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 &new_param_map); private: diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp index 14d38fa..c6ab861 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp @@ -41,6 +41,7 @@ bool control::SimpleTorqueController::init() auto regen_torque_scale = get_live_parameter("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; } diff --git a/flake.lock b/flake.lock index e272f50..7757053 100644 --- a/flake.lock +++ b/flake.lock @@ -52,16 +52,16 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1742363870, - "narHash": "sha256-XAtlcAYMD2TprujUXV5kBB2SaxBl88bu5qUmCmwhR5U=", + "lastModified": 1743114541, + "narHash": "sha256-0eUt8hM2MImnL6xFi3O7HGbUfwWd/gEy9qyEytB/AZs=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "9bf16a0b3727e87ddf5737ecdc3ef08b20e39baf", + "rev": "10d686445c5792d3f23aa2d25119236b048c04bf", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "feature/ControllerMUX", + "ref": "fix/resolve_merge_conflicts", "repo": "drivebrain_core", "type": "github" } diff --git a/flake.nix b/flake.nix index 4fe92d9..e5d1e50 100644 --- a/flake.nix +++ b/flake.nix @@ -37,7 +37,7 @@ vn_driver_lib.url = "github:RCMast3r/vn_driver_lib/fix/boost-compatible"; db-core-src = { - url = "github:hytech-racing/drivebrain_core/feature/ControllerMUX"; + url = "github:hytech-racing/drivebrain_core/fix/resolve_merge_conflicts"; flake = false; }; diff --git a/suggestions.md b/suggestions.md new file mode 100644 index 0000000..2143d22 --- /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 0000000..cdc1b0f --- /dev/null +++ b/test/controller_manager_integration.cpp @@ -0,0 +1,82 @@ +#include "SimpleSpeedController.hpp" +#include "SimpleTorqueController.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +core::JsonFileHandler _config("../config/test_tcmux_integration.json"); +core::Logger _logger(core::LogLevel::INFO); +control::SimpleTorqueController controller1(_logger, _config); +control::SimpleSpeedController controller2(_logger, _config); + + // std::cout << _controllerManager.ma + + // 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, 2 > _controllerManager( + _logger, _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 From dd58bc126e6e85caaf2f9434dfcc2668cc53e829 Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Mon, 31 Mar 2025 14:51:08 -0400 Subject: [PATCH 16/87] Added shared_ptr functionality to controllers --- .gitignore | 4 ++- CMakeLists.txt | 1 - drivebrain_app/include/DriveBrainApp.hpp | 5 +-- drivebrain_app/src/DriveBrainApp.cpp | 16 +++++----- .../include/DBServiceImpl.hpp | 1 - flake.lock | 6 ++-- test/controller_manager_integration.cpp | 10 +++--- unit_test/test_controller_manager.cpp | 31 ++++++++++--------- 8 files changed, 38 insertions(+), 36 deletions(-) diff --git a/.gitignore b/.gitignore index bdb4440..235512b 100644 --- a/.gitignore +++ b/.gitignore @@ -9,4 +9,6 @@ core.* result-man/ result-man .cache/ -*.dbc \ No newline at end of file +*.dbc + +compile_commands.json \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index ed13e33..d98f219 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,7 +112,6 @@ target_link_libraries(drivebrain_comms PUBLIC libvncxx::libvncxx drivebrain_estimation spdlog::spdlog - drivebrain_control ) make_cmake_package(drivebrain_comms drivebrain) diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 119af27..a2f9e63 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -1,6 +1,7 @@ // DriveBrainApp.hpp #pragma once +#include "SimpleTorqueController.hpp" #include #include #include @@ -67,8 +68,8 @@ class DriveBrainApp { std::unique_ptr _mcap_logger; // TCMUX - control::SimpleSpeedController controller1; - control::SimpleTorqueController controller2; + std::shared_ptr controller1; + std::shared_ptr controller2; control::ControllerManager, 2 > _controllerManager; std::function switch_modes; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 16e8bc8..b2c8d1b 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -1,6 +1,7 @@ // DriveBrainApp.cpp #include "DriveBrainApp.hpp" +#include "SimpleTorqueController.hpp" #include "hytech.pb.h" #include #include @@ -13,25 +14,24 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d , _logger(core::LogLevel::INFO) , _config(_param_path) , _settings(settings) - , controller1(control::SimpleSpeedController(_logger, _config)) - , controller2(control::SimpleTorqueController(_logger, _config)) - , _controllerManager(_logger, _config, {&controller1, &controller2}) // Initialize correctly + , controller1(std::make_shared(_logger, _config)) + , controller2(std::make_shared(_logger, _config)) + , _controllerManager(_logger, _config, {controller1, controller2}) // Initialize correctly { spdlog::set_level(spdlog::level::warn); _mcap_logger = std::make_unique("temp"); - //control::SimpleSpeedController controller1(_logger, _config); //control::SimpleTorqueController controller2(_logger, _config); - _configurable_components.push_back(&controller1); - _configurable_components.push_back(&controller2); + _configurable_components.push_back(controller1.get()); + _configurable_components.push_back(controller2.get()); //_controllerManager = control::ControllerManager, 2 >(_logger, _config, {&controller1 , &controller2}); _configurable_components.push_back(&_controllerManager); - bool successful_controller1_init = controller1.init(); - bool successful_controller2_init = controller2.init(); + bool successful_controller1_init = controller1->init(); + bool successful_controller2_init = controller2->init(); bool successful_manager_init = _controllerManager.init(); // bool matlab_construction_failed = false; diff --git a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp index 3b5f738..a09b049 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp @@ -10,7 +10,6 @@ #include #include -#include #include class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Service { diff --git a/flake.lock b/flake.lock index 7757053..617b429 100644 --- a/flake.lock +++ b/flake.lock @@ -52,11 +52,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1743114541, - "narHash": "sha256-0eUt8hM2MImnL6xFi3O7HGbUfwWd/gEy9qyEytB/AZs=", + "lastModified": 1743446752, + "narHash": "sha256-5XPD41jIX7rApUfNRnujFt+FXLQr8M+VrReYOWHTKIY=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "10d686445c5792d3f23aa2d25119236b048c04bf", + "rev": "c91f915c0d33ed04ef11e9613e57c739538ad10d", "type": "github" }, "original": { diff --git a/test/controller_manager_integration.cpp b/test/controller_manager_integration.cpp index cdc1b0f..ad115b7 100755 --- a/test/controller_manager_integration.cpp +++ b/test/controller_manager_integration.cpp @@ -17,8 +17,8 @@ #include core::JsonFileHandler _config("../config/test_tcmux_integration.json"); core::Logger _logger(core::LogLevel::INFO); -control::SimpleTorqueController controller1(_logger, _config); -control::SimpleSpeedController controller2(_logger, _config); +std::shared_ptr controller1(std::make_shared(_logger, _config)); +std::shared_ptr controller2(std::make_shared(_logger, _config)); // std::cout << _controllerManager.ma @@ -31,11 +31,11 @@ control::SimpleSpeedController controller2(_logger, _config); int main(int argc, char **argv) { - controller1.init(); - controller2.init(); + controller1->init(); + controller2->init(); // important to init controllers. maybe put this in constructor? control::ControllerManager, 2 > _controllerManager( - _logger, _config, {&controller1, &controller2} + _logger, _config, {controller1, controller2} ); _controllerManager.init(); diff --git a/unit_test/test_controller_manager.cpp b/unit_test/test_controller_manager.cpp index 6444813..a95d797 100644 --- a/unit_test/test_controller_manager.cpp +++ b/unit_test/test_controller_manager.cpp @@ -6,13 +6,14 @@ #include "SimpleTorqueController.hpp" #include +#include class ControllerManagerTest : public ::testing::Test { protected: - control::SimpleSpeedController simpleSpeedController1; - control::SimpleTorqueController simpleTorqueController1; - control::SimpleSpeedController simpleSpeedController2; - control::SimpleTorqueController simpleTorqueController2; + std::shared_ptr simpleSpeedController1; + std::shared_ptr simpleTorqueController1; + std::shared_ptr simpleSpeedController2; + std::shared_ptr simpleTorqueController2; control::ControllerManager, 2> controller_manager_2speed; control::ControllerManager, 2> controller_manager_2torque; control::ControllerManager, 2> controller_manager_diff; @@ -26,13 +27,13 @@ class ControllerManagerTest : public ::testing::Test { ControllerManagerTest() : logger(core::LogLevel::NONE), json_file_handler("../config/drivebrain_config.json"), - simpleSpeedController1(logger, json_file_handler), - simpleSpeedController2(logger, json_file_handler), - simpleTorqueController1(logger, json_file_handler), - simpleTorqueController2(logger, json_file_handler), - controller_manager_2speed(logger, json_file_handler, {&simpleSpeedController1, &simpleSpeedController2}), - controller_manager_2torque(logger, json_file_handler, {&simpleTorqueController1, &simpleTorqueController2}), - controller_manager_diff(logger, json_file_handler, {&simpleSpeedController1, &simpleTorqueController1}) + simpleSpeedController1(std::make_shared(logger, json_file_handler)), + simpleSpeedController2(std::make_shared(logger, json_file_handler)), + simpleTorqueController1(std::make_shared(logger, json_file_handler)), + simpleTorqueController2(std::make_shared(logger, json_file_handler)), + controller_manager_2speed(logger, json_file_handler, {simpleSpeedController1, simpleSpeedController2}), + controller_manager_2torque(logger, json_file_handler, {simpleTorqueController1, simpleTorqueController2}), + controller_manager_diff(logger, json_file_handler, {simpleSpeedController1, simpleTorqueController1}) { } @@ -45,10 +46,10 @@ class ControllerManagerTest : public ::testing::Test { controller_manager_2speed.init(); controller_manager_2torque.init(); controller_manager_diff.init(); - simpleSpeedController1.init(); - simpleSpeedController2.init(); - simpleTorqueController1.init(); - simpleTorqueController2.init(); + simpleSpeedController1->init(); + simpleSpeedController2->init(); + simpleTorqueController1->init(); + simpleTorqueController2->init(); // std::cout << "set up" << std::endl; } From 7b8543af79cc4697baa5f58116a58f9a5e858142 Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Mon, 31 Mar 2025 18:45:11 -0400 Subject: [PATCH 17/87] Intermediate status commit --- CMakeLists.txt | 12 +- drivebrain_app/src/DriveBrainApp.cpp | 15 +- .../include/DBServiceImpl.hpp | 10 +- .../drivebrain_comms/src/DBServiceImpl.cpp | 111 ++++-------- .../include/SimpleSpeedController.hpp | 3 +- .../include/SimpleTorqueController.hpp | 4 +- .../src/SimpleSpeedController.cpp | 6 +- flake.lock | 26 +-- flake.nix | 2 +- test/controller_manager_integration.cpp | 11 +- test/test_msglogger.cpp | 162 +++++++++--------- unit_test/SimpleSpeedControllerTest.cpp | 6 +- unit_test/SimpleTorqueControllerTest.cpp | 6 +- unit_test/main.cpp | 1 - unit_test/test_controller_manager.cpp | 17 +- 15 files changed, 161 insertions(+), 231 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b055634..c78a4d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -238,13 +238,13 @@ target_link_libraries(test_db_grpc PUBLIC gRPC::grpc++_reflection ) -add_executable(test_config_schema test/test_configurable.cpp) +# 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 -) +#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) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index e49f35e..aabbb6c 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -1,6 +1,7 @@ // DriveBrainApp.cpp #include "DriveBrainApp.hpp" +#include "SimpleSpeedController.hpp" #include "SimpleTorqueController.hpp" #include "hytech.pb.h" #include @@ -14,9 +15,9 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d , _logger(core::LogLevel::INFO) , _config(_param_path) , _settings(settings) - , controller1(std::make_shared(_logger, _config)) - , controller2(std::make_shared(_logger, _config)) - , _controllerManager(_logger, _config, {controller1, controller2}) // Initialize correctly + , controller1(std::make_shared(_config)) + , controller2(std::make_shared(_config)) + , _controllerManager(_config, {controller1, controller2}) // Initialize correctly { // spdlog::info("top o"); std::vector> configurable_components; @@ -30,11 +31,11 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d }; - _controller = std::make_shared(_config); - if (!_controller->init()) { + controller1 = std::make_shared(_config); + if (!controller1->init()) { throw std::runtime_error("Failed to initialize controller"); } - configurable_components.push_back(std::reinterpret_pointer_cast(_controller)); + configurable_components.push_back(std::reinterpret_pointer_cast(controller1)); spdlog::info("made controller"); @@ -200,7 +201,7 @@ void DriveBrainApp::run() { if (!_settings.run_db_service) return; - _db_service = std::make_unique(_message_logger, switch_modes); + _db_service = std::make_unique(_message_logger); spdlog::info("started db service thread"); try { while (!stop_signal.load()) { diff --git a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp index a09b049..863d651 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp @@ -10,7 +10,6 @@ #include #include -#include class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Service { @@ -19,15 +18,14 @@ class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Servi }; 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* 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; - + grpc::Status RequestCurrentLoggerStatus(grpc::ServerContext* context, const google::protobuf::Empty* request, db_service::v1::service::LoggerStatus* response) override; + public: - DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch); void run_server(); + DBInterfaceImpl(std::shared_ptr>> logger_inst); + void run_server(); void stop_server(); private: std::shared_ptr>> _logger_inst; - std::function _mode_switch; std::unique_ptr _server; // Store server instance here }; diff --git a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp index 8033e21..863d651 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp @@ -1,78 +1,33 @@ -#include -#include "spdlog/spdlog.h" - -grpc::Status DBInterfaceImpl::RequestStopLogging(grpc::ServerContext *context, const google::protobuf::Empty *rq, db_service::v1::service::LoggerStatus *response) -{ - { - spdlog::warn("requested stopping of logging"); - _logger_inst->stop_logging_to_file(); - auto status = _logger_inst->get_logger_status(); - - response->set_active_or_previous_log_file_name(std::get<0>(status)); - response->set_currently_logging(std::get<1>(status)); - return grpc::Status::OK; - } -} -grpc::Status DBInterfaceImpl::RequestStartLogging(grpc::ServerContext *context, const google::protobuf::Empty *rq, db_service::v1::service::LoggerStatus *response) -{ - { - spdlog::warn("requested start of logging"); - _logger_inst->start_logging_to_new_file(); - auto status = _logger_inst->get_logger_status(); - response->set_active_or_previous_log_file_name(std::get<0>(status)); - response->set_currently_logging(std::get<1>(status)); - return grpc::Status::OK; - } -} -grpc::Status DBInterfaceImpl::RequestCurrentLoggerStatus(grpc::ServerContext *context, const google::protobuf::Empty *rq, db_service::v1::service::LoggerStatus *response) -{ - { - spdlog::warn("requested status of logger"); - auto status = _logger_inst->get_logger_status(); - response->set_active_or_previous_log_file_name(std::get<0>(status)); - response->set_currently_logging(std::get<1>(status)); - return grpc::Status::OK; - } -} - -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(rq->requested_controller_index()))){ - response->set_status("switch"); - } - else{ - response->set_status("no switch"); - } - - return grpc::Status::OK; -} - -DBInterfaceImpl::DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch) - : _logger_inst(logger_inst), _mode_switch(mode_switch){ -} -void DBInterfaceImpl::stop_server() { - if (_server) { - spdlog::warn("Shutting down the server..."); - - _server->Shutdown(); - } -} -void DBInterfaceImpl::run_server() -{ - std::string server_address = "0.0.0.0:6969"; - - grpc::reflection::InitProtoReflectionServerBuilderPlugin(); - grpc::ServerBuilder builder; - // Listen on the given address without any authentication mechanism. - builder.AddListeningPort(server_address, grpc::InsecureServerCredentials()); - // Register "service" as the instance through which we'll communicate with - // clients. In this case it corresponds to an *synchronous* service. - builder.RegisterService(this); - // Finally assemble the server. - _server = builder.BuildAndStart(); - spdlog::warn("Server listening on "); - - // 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(); -} +#ifndef DBSERVICE_IMPL_HPP +#define DBSERVICE_IMPL_HPP + +#include +#include +#include +#include +#include + +#include + +#include + +class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Service { + + struct InitStruct { + std::shared_ptr>> 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; + + public: + DBInterfaceImpl(std::shared_ptr>> logger_inst); + void run_server(); + void stop_server(); + private: + std::shared_ptr>> _logger_inst; + std::unique_ptr _server; // Store server instance here + +}; + +#endif \ No newline at end of file diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp index a04a01c..af2afc5 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp @@ -1,7 +1,6 @@ #pragma once #include #include -#include #include #include #include @@ -32,7 +31,7 @@ namespace control speed_m_s positive_speed_set; float max_power_kw; }; - SimpleController(core::JsonFileHandler &json_file_handler) : Configurable(json_file_handler, "SimpleController") {} + SimpleSpeedController(core::JsonFileHandler &json_file_handler) : Configurable(json_file_handler, "SimpleSpeedController") {} float get_dt_sec() override { return (0.001); } diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp index c0a5181..1791add 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleTorqueController.hpp @@ -1,7 +1,6 @@ #pragma once #include #include -#include #include #include #include @@ -26,8 +25,7 @@ namespace control float rear_torque_scale; float regen_torque_scale; }; - SimpleTorqueController(core::Logger &logger, core::JsonFileHandler &json_file_handler) : Configurable(logger, json_file_handler, "SimpleTorqueController") {} - SimpleTorqueController(core::Logger &logger, core::JsonFileHandler &json_file_handler, std::string config) : Configurable(logger, json_file_handler, config) {} + SimpleTorqueController(core::JsonFileHandler &json_file_handler) : Configurable(json_file_handler, "SimpleTorqueController") {} float get_dt_sec() override { return (0.001); } diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp index 012c6df..6c017ed 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp @@ -59,7 +59,7 @@ bool control::SimpleSpeedController::init() _config = {*max_torque, *max_regen_torque, *rear_torque_scale, *regen_torque_scale, *positive_speed_set, *max_power_kw}; - param_update_handler_sig.connect(boost::bind(&control::SimpleController::_handle_param_updates, this, std::placeholders::_1)); + param_update_handler_sig.connect(boost::bind(&control::SimpleSpeedController::_handle_param_updates, this, std::placeholders::_1)); // _configured = true; return true; } @@ -119,12 +119,12 @@ core::ControllerOutput control::SimpleSpeedController::step_controller(const cor } - cmd_out = _apply_power_limit(cmd_out, in.current_rpms); + cmd_out.out = _apply_power_limit(type_set, in.current_rpms); return cmd_out; } -core::SpeedControlOut control::SimpleController::_apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms) +core::SpeedControlOut control::SimpleSpeedController::_apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms) { auto cmd_out = current_control; std::cout <<"cmd_out.torque_lim_nm.FL " << cmd_out.torque_lim_nm.FL < #include #include -#include -#include -#include #include -#include #include #include @@ -16,9 +12,8 @@ #include #include core::JsonFileHandler _config("../config/test_tcmux_integration.json"); -core::Logger _logger(core::LogLevel::INFO); -std::shared_ptr controller1(std::make_shared(_logger, _config)); -std::shared_ptr controller2(std::make_shared(_logger, _config)); +std::shared_ptr controller1(std::make_shared(_config)); +std::shared_ptr controller2(std::make_shared(_config)); // std::cout << _controllerManager.ma @@ -35,7 +30,7 @@ int main(int argc, char **argv) { controller2->init(); // important to init controllers. maybe put this in constructor? control::ControllerManager, 2 > _controllerManager( - _logger, _config, {controller1, controller2} + _config, {controller1, controller2} ); _controllerManager.init(); diff --git a/test/test_msglogger.cpp b/test/test_msglogger.cpp index 21a9540..7d98b37 100644 --- a/test/test_msglogger.cpp +++ b/test/test_msglogger.cpp @@ -1,91 +1,91 @@ -#include -#include // std::ofstream +// #include +// #include // 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 live_telem_func = [&](int n) { std::cout << "live out " << n < 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 live_telem_func = [&](int n) { std::cout << "live out " << n < 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" < #include -#include #include #include #include diff --git a/unit_test/test_controller_manager.cpp b/unit_test/test_controller_manager.cpp index a95d797..a2d4200 100644 --- a/unit_test/test_controller_manager.cpp +++ b/unit_test/test_controller_manager.cpp @@ -18,22 +18,21 @@ class ControllerManagerTest : public ::testing::Test { control::ControllerManager, 2> controller_manager_2torque; control::ControllerManager, 2> controller_manager_diff; core::JsonFileHandler json_file_handler; - core::Logger logger; core::VehicleState vehicle_state; core::ControllerOutput torque_controller_output; core::ControllerOutput speed_controller_output; ControllerManagerTest() - : logger(core::LogLevel::NONE), + : json_file_handler("../config/drivebrain_config.json"), - simpleSpeedController1(std::make_shared(logger, json_file_handler)), - simpleSpeedController2(std::make_shared(logger, json_file_handler)), - simpleTorqueController1(std::make_shared(logger, json_file_handler)), - simpleTorqueController2(std::make_shared(logger, json_file_handler)), - controller_manager_2speed(logger, json_file_handler, {simpleSpeedController1, simpleSpeedController2}), - controller_manager_2torque(logger, json_file_handler, {simpleTorqueController1, simpleTorqueController2}), - controller_manager_diff(logger, json_file_handler, {simpleSpeedController1, simpleTorqueController1}) + simpleSpeedController1(std::make_shared(json_file_handler)), + simpleSpeedController2(std::make_shared(json_file_handler)), + simpleTorqueController1(std::make_shared(json_file_handler)), + simpleTorqueController2(std::make_shared(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}) { } From b4fedbc78bae4ab98d60b3b6b069dfdd25a0b183 Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Mon, 31 Mar 2025 19:00:00 -0400 Subject: [PATCH 18/87] Finally --- CMakeLists.txt | 12 +-- .../drivebrain_comms/src/DBServiceImpl.cpp | 87 +++++++++++++------ test/controller_manager_integration.cpp | 2 - 3 files changed, 66 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c78a4d1..b055634 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -238,13 +238,13 @@ target_link_libraries(test_db_grpc PUBLIC gRPC::grpc++_reflection ) -# add_executable(test_config_schema test/test_configurable.cpp) +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 -#) +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) diff --git a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp index 863d651..d2140a9 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp @@ -1,33 +1,66 @@ -#ifndef DBSERVICE_IMPL_HPP -#define DBSERVICE_IMPL_HPP +#include +#include "spdlog/spdlog.h" -#include -#include -#include -#include -#include +grpc::Status DBInterfaceImpl::RequestStopLogging(grpc::ServerContext *context, const google::protobuf::Empty *rq, db_service::v1::service::LoggerStatus *response) +{ + { + spdlog::warn("requested stopping of logging"); + _logger_inst->stop_logging_to_file(); + auto status = _logger_inst->get_logger_status(); -#include + response->set_active_or_previous_log_file_name(std::get<0>(status)); + response->set_currently_logging(std::get<1>(status)); + return grpc::Status::OK; + } +} +grpc::Status DBInterfaceImpl::RequestStartLogging(grpc::ServerContext *context, const google::protobuf::Empty *rq, db_service::v1::service::LoggerStatus *response) +{ + { + spdlog::warn("requested start of logging"); + _logger_inst->start_logging_to_new_file(); + auto status = _logger_inst->get_logger_status(); + response->set_active_or_previous_log_file_name(std::get<0>(status)); + response->set_currently_logging(std::get<1>(status)); + return grpc::Status::OK; + } +} +grpc::Status DBInterfaceImpl::RequestCurrentLoggerStatus(grpc::ServerContext *context, const google::protobuf::Empty *rq, db_service::v1::service::LoggerStatus *response) +{ + { + spdlog::warn("requested status of logger"); + auto status = _logger_inst->get_logger_status(); + response->set_active_or_previous_log_file_name(std::get<0>(status)); + response->set_currently_logging(std::get<1>(status)); + return grpc::Status::OK; + } +} -#include +DBInterfaceImpl::DBInterfaceImpl(std::shared_ptr>> logger_inst) : _logger_inst(logger_inst) +{ +} +void DBInterfaceImpl::stop_server() { + if (_server) { + spdlog::warn("Shutting down the server..."); -class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Service { + _server->Shutdown(); + } +} +void DBInterfaceImpl::run_server() +{ + std::string server_address = "0.0.0.0:6969"; - struct InitStruct { - std::shared_ptr>> 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; - - public: - DBInterfaceImpl(std::shared_ptr>> logger_inst); - void run_server(); - void stop_server(); - private: - std::shared_ptr>> _logger_inst; - std::unique_ptr _server; // Store server instance here + grpc::reflection::InitProtoReflectionServerBuilderPlugin(); + grpc::ServerBuilder builder; + // Listen on the given address without any authentication mechanism. + builder.AddListeningPort(server_address, grpc::InsecureServerCredentials()); + // Register "service" as the instance through which we'll communicate with + // clients. In this case it corresponds to an *synchronous* service. + builder.RegisterService(this); + // Finally assemble the server. + _server = builder.BuildAndStart(); + spdlog::warn("Server listening on "); -}; - -#endif \ No newline at end of file + // 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/test/controller_manager_integration.cpp b/test/controller_manager_integration.cpp index 55a1325..b561220 100755 --- a/test/controller_manager_integration.cpp +++ b/test/controller_manager_integration.cpp @@ -15,8 +15,6 @@ core::JsonFileHandler _config("../config/test_tcmux_integration.json"); std::shared_ptr controller1(std::make_shared(_config)); std::shared_ptr controller2(std::make_shared(_config)); - // std::cout << _controllerManager.ma - // 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; From dc19a14beb783f56272311196fbb39e2a1e23b39 Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Mon, 31 Mar 2025 19:47:23 -0400 Subject: [PATCH 19/87] . --- CMakeLists.txt | 1 - config/drivebrain_config.json | 6 + flake.lock | 8 +- flake.nix | 2 +- unit_test/SimpleControllerTest.cpp | 150 ------------------------ unit_test/SimpleSpeedControllerTest.cpp | 134 +++++++++++++++++++++ 6 files changed, 145 insertions(+), 156 deletions(-) delete mode 100644 unit_test/SimpleControllerTest.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b055634..c2cf9c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -313,7 +313,6 @@ add_executable(alpha_test unit_test/SimpleSpeedControllerTest.cpp unit_test/SimpleTorqueControllerTest.cpp unit_test/test_controller_manager.cpp - ) target_link_libraries(alpha_test PUBLIC diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index af7809d..2c31958 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -16,5 +16,11 @@ "baud_rate": 921600, "port": 1, "freq_divisor": 1 + }, + "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/flake.lock b/flake.lock index 5c9e808..d4d6523 100644 --- a/flake.lock +++ b/flake.lock @@ -52,16 +52,16 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1743454261, - "narHash": "sha256-cftrGlquFrDt9K63b/CPamaD0yVJkVoYV3zBcOWCJEM=", + "lastModified": 1743463637, + "narHash": "sha256-RnNkdGOJ37JjVTWL+H6gSVJBOemy5jpTvL7zXh/m1Mk=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "9b481592d60f7d4b00bf9e28dd33c9391fe5357e", + "rev": "3e87f39c88c58dcdf6402a4e7667b915e54198e7", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "fix/removing_logger_references_and_controller_pointers", + "ref": "main", "repo": "drivebrain_core", "type": "github" } diff --git a/flake.nix b/flake.nix index 76a0ffe..3c94c2c 100644 --- a/flake.nix +++ b/flake.nix @@ -37,7 +37,7 @@ vn_driver_lib.url = "github:RCMast3r/vn_driver_lib/fix/boost-compatible"; db-core-src = { - url = "github:hytech-racing/drivebrain_core/fix/removing_logger_references_and_controller_pointers"; + url = "github:hytech-racing/drivebrain_core/main"; flake = false; }; diff --git a/unit_test/SimpleControllerTest.cpp b/unit_test/SimpleControllerTest.cpp deleted file mode 100644 index da81f69..0000000 --- a/unit_test/SimpleControllerTest.cpp +++ /dev/null @@ -1,150 +0,0 @@ -#include -#include -#include -#include -#include -#include - -class SimpleControllerTest : public testing::Test { - - protected: - SimpleControllerTest() : config(std::string("../config/drivebrain_config.json")){ - // config = core::JsonFileHandler(); - simple_controller = std::make_unique(config); - } - - core::JsonFileHandler config; - - // control::SimpleController simple_controller({}, {}); - - std::unique_ptr simple_controller; - - void SetUp() override { - simple_controller->init(); - } - - void TearDown() override { - // Shouldn't need anything here - } - -}; - -TEST_F(SimpleControllerTest, SimpleAccel) { - - // // --- Test Case 1: Simple Acceleration --- - { - core::VehicleState in; - veh_vec 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 = simple_controller->step_controller(in); - std::cout <<"asdf: " << res.desired_rpms.FL < 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 = simple_controller->step_controller(in); - 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(SimpleControllerTest, ZerlAccelZeroBrake) -{ - // --- Test Case 3: Zero Acceleration and Zero Brake (Coasting) --- - { - core::VehicleState in; - veh_vec 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 = simple_controller->step_controller(in); - - 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(SimpleControllerTest, TestPowerLimit) -{ - // // --- Test Case 4: Power Limiting Scenario --- - core::VehicleState in; - veh_vec 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 = simple_controller->step_controller(in); - - 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/SimpleSpeedControllerTest.cpp b/unit_test/SimpleSpeedControllerTest.cpp index c509de7..fa9a21c 100644 --- a/unit_test/SimpleSpeedControllerTest.cpp +++ b/unit_test/SimpleSpeedControllerTest.cpp @@ -152,6 +152,100 @@ TEST_F(SimpleSpeedControllerTest, FullBrakeAndAccelRequest) 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 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(simple_controller.step_controller(in).out); + std::cout <<"asdf: " << res.desired_rpms.FL < 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(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 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(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; @@ -196,3 +290,43 @@ TEST_F(SimpleSpeedControllerTest, VariableRequests) 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 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(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) +} From 1a6bb3f6c7b84bb9212aab3fb277eb55c63ae3a4 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 31 Mar 2025 19:57:09 -0400 Subject: [PATCH 20/87] discovering borked unit tests for drivebrain tc mux --- config/drivebrain_config.json | 6 ++++++ unit_test/test_controller_manager.cpp | 1 + 2 files changed, 7 insertions(+) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 2c31958..318d49e 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -22,5 +22,11 @@ "max_regen_torque": 10.0, "rear_torque_scale": 1.0, "regen_torque_scale": 0.6 + }, + "ControllerManager": { + "max_controller_switch_speed_ms": 1.793, + "max_switch_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/unit_test/test_controller_manager.cpp b/unit_test/test_controller_manager.cpp index a2d4200..ed6db57 100644 --- a/unit_test/test_controller_manager.cpp +++ b/unit_test/test_controller_manager.cpp @@ -164,6 +164,7 @@ TEST_F(ControllerManagerTest, SwapControllerFailure_HighRPM_onewheel) { //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); From d4c9292cbeb6afa15039eecccd5a040c194c4cc8 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 31 Mar 2025 20:03:16 -0400 Subject: [PATCH 21/87] working through fixing tests --- config/drivebrain_config.json | 2 +- unit_test/SimpleSpeedControllerTest.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 318d49e..e8a48d8 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -24,7 +24,7 @@ "regen_torque_scale": 0.6 }, "ControllerManager": { - "max_controller_switch_speed_ms": 1.793, + "max_controller_switch_speed_ms": 5.0, "max_switch_requested_rpm": 20000.0, "max_torque_switch_nm": 10.0, "max_accel_switch_float": 0.1 diff --git a/unit_test/SimpleSpeedControllerTest.cpp b/unit_test/SimpleSpeedControllerTest.cpp index fa9a21c..e85c63e 100644 --- a/unit_test/SimpleSpeedControllerTest.cpp +++ b/unit_test/SimpleSpeedControllerTest.cpp @@ -51,6 +51,7 @@ TEST_F(SimpleSpeedControllerTest, InitDoesNotHaveConfig) TEST_F(SimpleSpeedControllerTest, NoPedalInput) { + auto cmd = simple_controller.step_controller(in); auto res = std::get_if(&cmd.out); From 2f18e18be7ab17ded1ea2e58e96526f47b6917f5 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 31 Mar 2025 20:17:32 -0400 Subject: [PATCH 22/87] fixing simple controller tests --- .../src/SimpleSpeedController.cpp | 17 ++++-- unit_test/SimpleSpeedControllerTest.cpp | 54 ++++++++++--------- 2 files changed, 40 insertions(+), 31 deletions(-) diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp index 6c017ed..3c7d7fb 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp @@ -5,7 +5,7 @@ void control::SimpleSpeedController::_handle_param_updates(const std::unordered_map &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 + // 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(&new_param_map.at("max_torque"))) { @@ -41,6 +41,13 @@ void control::SimpleSpeedController::_handle_param_updates(const std::unordered_ _config.positive_speed_set = *pval; spdlog::info("Setting new positive speed set: {}", _config.positive_speed_set); } + if (auto pval = std::get_if(&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); + } + } bool control::SimpleSpeedController::init() @@ -78,10 +85,10 @@ core::ControllerOutput control::SimpleSpeedController::step_controller(const cor veh_vec current_rpms = in.current_rpms; - torque_nm torqueRequest; + torque_nm torqueRequest = {}; - core::SpeedControlOut type_set; - core::ControllerOutput cmd_out; + core::SpeedControlOut type_set = {}; + core::ControllerOutput cmd_out = {}; cmd_out.out = type_set; auto& speed_out = std::get(cmd_out.out); @@ -119,7 +126,7 @@ core::ControllerOutput control::SimpleSpeedController::step_controller(const cor } - cmd_out.out = _apply_power_limit(type_set, in.current_rpms); + cmd_out.out = _apply_power_limit(speed_out, in.current_rpms); return cmd_out; } diff --git a/unit_test/SimpleSpeedControllerTest.cpp b/unit_test/SimpleSpeedControllerTest.cpp index e85c63e..b8f1cc3 100644 --- a/unit_test/SimpleSpeedControllerTest.cpp +++ b/unit_test/SimpleSpeedControllerTest.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -51,14 +52,16 @@ TEST_F(SimpleSpeedControllerTest, InitDoesNotHaveConfig) 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(&cmd.out); - ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FL,std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.FR,std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RL,std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RR,std::get(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); @@ -72,10 +75,10 @@ TEST_F(SimpleSpeedControllerTest, SmallPositiveAccelRequest) auto cmd = simple_controller.step_controller(in); auto res = std::get_if(&cmd.out); - ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, std::get(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); @@ -89,10 +92,10 @@ TEST_F(SimpleSpeedControllerTest, FullPositiveAccelRequest) auto cmd = simple_controller.step_controller(in); auto res = std::get_if(&cmd.out); - ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, std::get(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); @@ -142,11 +145,10 @@ TEST_F(SimpleSpeedControllerTest, FullBrakeAndAccelRequest) auto cmd = simple_controller.step_controller(in); auto res = std::get_if(&cmd.out); - ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 1.0); - + ASSERT_NEAR(res->desired_rpms.FL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, std::get(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); @@ -254,10 +256,10 @@ TEST_F(SimpleSpeedControllerTest, VariableRequests) auto cmd = simple_controller.step_controller(in); auto res = std::get_if(&cmd.out); - ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, std::get(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); @@ -281,10 +283,10 @@ TEST_F(SimpleSpeedControllerTest, VariableRequests) in.input.requested_brake = 0; cmd = simple_controller.step_controller(in); - ASSERT_NEAR(res->desired_rpms.FL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.FR, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RL, 1672.12, 1.0); - ASSERT_NEAR(res->desired_rpms.RR, 1672.12, 1.0); + ASSERT_NEAR(res->desired_rpms.FL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.FR, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RL, std::get(simple_controller.get_cached_param("positive_speed_set")) * constants::METERS_PER_SECOND_TO_RPM, 1.0); + ASSERT_NEAR(res->desired_rpms.RR, std::get(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); From db7d10c1d91926be7388b308aa3e348cc2bb5fc8 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 31 Mar 2025 20:29:33 -0400 Subject: [PATCH 23/87] bring back in previous changes --- .../drivebrain_comms/include/DBServiceImpl.hpp | 11 +++++++---- .../drivebrain_comms/src/DBServiceImpl.cpp | 14 +++++++++++++- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp index 863d651..3b5f738 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp @@ -10,6 +10,8 @@ #include #include +#include +#include class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Service { @@ -18,14 +20,15 @@ class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Servi }; 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>> logger_inst); - void run_server(); + DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch); void run_server(); void stop_server(); private: std::shared_ptr>> _logger_inst; + std::function _mode_switch; std::unique_ptr _server; // Store server instance here }; diff --git a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp index d2140a9..f3c3234 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp @@ -35,8 +35,20 @@ grpc::Status DBInterfaceImpl::RequestCurrentLoggerStatus(grpc::ServerContext *co } } -DBInterfaceImpl::DBInterfaceImpl(std::shared_ptr>> 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(rq->requested_controller_index()))){ + response->set_status("switch"); + } + else{ + response->set_status("no switch"); + } + + return grpc::Status::OK; +} + +DBInterfaceImpl::DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch) + : _logger_inst(logger_inst), _mode_switch(mode_switch){ } void DBInterfaceImpl::stop_server() { if (_server) { From 4d76db814f99d140a74e63127844b221d8d52915 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 31 Mar 2025 20:55:07 -0400 Subject: [PATCH 24/87] bringing up to speed --- CMakeLists.txt | 1 + drivebrain_app/src/DriveBrainApp.cpp | 13 +++++++++---- .../drivebrain_comms/src/CANComms.cpp | 1 + .../src/SimpleSpeedController.cpp | 1 + .../src/SimpleTorqueController.cpp | 2 +- .../drivebrain_estimation/src/StateEstimator.cpp | 6 +++++- .../drivebrain_logging/src/DrivebrainMCAPLogger.cpp | 3 +++ 7 files changed, 21 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c2cf9c5..96bf03a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -102,6 +102,7 @@ target_include_directories(drivebrain_comms PUBLIC target_link_libraries(drivebrain_comms PUBLIC drivebrain_core::drivebrain_core drivebrain_common_utils + drivebrain_control gRPC::grpc++_reflection foxglove_websocket::foxglove_websocket protobuf::libprotobuf diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index aabbb6c..215eb5a 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -4,6 +4,7 @@ #include "SimpleSpeedController.hpp" #include "SimpleTorqueController.hpp" #include "hytech.pb.h" +#include #include #include @@ -35,7 +36,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if (!controller1->init()) { throw std::runtime_error("Failed to initialize controller"); } - configurable_components.push_back(std::reinterpret_pointer_cast(controller1)); + configurable_components.push_back(std::static_pointer_cast(controller1)); spdlog::info("made controller"); @@ -52,14 +53,19 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if (construction_failed) { throw std::runtime_error("Failed to construct CAN driver"); } - configurable_components.push_back(std::reinterpret_pointer_cast(_driver)); + configurable_components.push_back(std::static_pointer_cast(_driver)); spdlog::info("made CAN driver"); _eth_driver = std::make_unique( _logger, _eth_tx_queue, _message_logger, *_state_estimator, _io_context, "192.168.1.30", 2001, 2000); spdlog::info("eth driver"); - _db_service = std::make_unique(_message_logger); + + 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(_message_logger, switch_modes); spdlog::info("made db service"); if(_settings.use_vectornav) { @@ -201,7 +207,6 @@ void DriveBrainApp::run() { if (!_settings.run_db_service) return; - _db_service = std::make_unique(_message_logger); spdlog::info("started db service thread"); try { while (!stop_signal.load()) { diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index a28b9c3..eefd1e7 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -66,6 +66,7 @@ bool comms::CANDriver::init() { _do_read(); // _configured = true; + set_configured(); return true; } diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp index 3c7d7fb..8a87bfc 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp @@ -68,6 +68,7 @@ bool control::SimpleSpeedController::init() param_update_handler_sig.connect(boost::bind(&control::SimpleSpeedController::_handle_param_updates, this, std::placeholders::_1)); // _configured = true; + set_configured(); return true; } diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp index c6ab861..1dd98e9 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp @@ -48,7 +48,7 @@ bool control::SimpleTorqueController::init() _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; } diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 49d293b..5099a50 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -245,7 +245,11 @@ std::pair StateEstimator::get_latest_state_and_validit } auto log_start = std::chrono::high_resolution_clock::now(); - _message_logger->log_msg(static_cast>(msg_out)); + if(_message_logger) + { + _message_logger->log_msg(static_cast>(msg_out)); + } + auto log_end = std::chrono::high_resolution_clock::now(); auto state_estim_end = std::chrono::high_resolution_clock::now(); diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp index b7edd73..a5987a7 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp @@ -236,13 +236,16 @@ namespace common { 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 + std::cout << "getting component index: "<is_configured()) { return std::nullopt; } + component_index++; top_level_schema["properties"][component->get_name()] = component->get_schema(); } return top_level_schema; From f6698231c4d827f6834db551f41cda53f1203639 Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Mon, 31 Mar 2025 20:56:09 -0400 Subject: [PATCH 25/87] Removed redundant print --- .../drivebrain_control/src/SimpleSpeedController.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp index 8a87bfc..31ca9ff 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp @@ -135,7 +135,6 @@ core::ControllerOutput control::SimpleSpeedController::step_controller(const cor core::SpeedControlOut control::SimpleSpeedController::_apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms) { auto cmd_out = current_control; - std::cout <<"cmd_out.torque_lim_nm.FL " << cmd_out.torque_lim_nm.FL < Date: Mon, 31 Mar 2025 20:58:20 -0400 Subject: [PATCH 26/87] Commented tcmux test for now --- unit_test/test_controller_manager.cpp | 282 +++++++++++++------------- 1 file changed, 141 insertions(+), 141 deletions(-) diff --git a/unit_test/test_controller_manager.cpp b/unit_test/test_controller_manager.cpp index ed6db57..80178a1 100644 --- a/unit_test/test_controller_manager.cpp +++ b/unit_test/test_controller_manager.cpp @@ -58,147 +58,147 @@ class ControllerManagerTest : public ::testing::Test { } }; -// 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); +// // 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(output.out)); - auto torque_output = std::get(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(output.out)); +// auto torque_output = std::get(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(output.out)); - auto speed_output = std::get(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(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(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(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(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(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(output.out)); +// auto speed_output = std::get(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(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(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(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(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(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(output.out)); - auto _output = std::get(output.out); - ASSERT_TRUE(_output.desired_rpms.FL - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); - ASSERT_TRUE(_output.desired_rpms.FR - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); - ASSERT_TRUE(_output.desired_rpms.RL - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); - ASSERT_TRUE(_output.desired_rpms.RR - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); -} - -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(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(output.out)); -} \ No newline at end of file +// ASSERT_TRUE(std::holds_alternative(output.out)); +// auto _output = std::get(output.out); +// ASSERT_TRUE(_output.desired_rpms.FL - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); +// ASSERT_TRUE(_output.desired_rpms.FR - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); +// ASSERT_TRUE(_output.desired_rpms.RL - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); +// ASSERT_TRUE(_output.desired_rpms.RR - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); +// } + +// 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(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(output.out)); +// } \ No newline at end of file From c48d5467a727ad0f005a3810a1471c3d44e271d9 Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Wed, 2 Apr 2025 21:37:05 -0400 Subject: [PATCH 27/87] DT_sec is now a param --- .../drivebrain_control/include/SimpleSpeedController.hpp | 4 +++- .../drivebrain_control/src/SimpleSpeedController.cpp | 7 +++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp index af2afc5..d49660b 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp @@ -30,10 +30,11 @@ namespace control 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::ControllerOutput step_controller(const core::VehicleState &in) override; @@ -41,6 +42,7 @@ namespace control private: void _handle_param_updates(const std::unordered_map &new_param_map); core::SpeedControlOut _apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms); + int dt_rate_hz; private: std::mutex _config_mutex; config _config; diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp index 31ca9ff..c7ea937 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp @@ -48,6 +48,13 @@ void control::SimpleSpeedController::_handle_param_updates(const std::unordered_ spdlog::info("Setting new max power limit kw: {}", _config.max_power_kw); } + if (auto pval = std::get_if(&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() From d0f5b5306509a6dbbaac1cde23bd280a908ac5da Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Wed, 2 Apr 2025 21:38:37 -0400 Subject: [PATCH 28/87] Removing redundancy --- .../drivebrain_control/include/SimpleSpeedController.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp index d49660b..06627f0 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp @@ -42,7 +42,6 @@ namespace control private: void _handle_param_updates(const std::unordered_map &new_param_map); core::SpeedControlOut _apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms); - int dt_rate_hz; private: std::mutex _config_mutex; config _config; From 7dba369a71a7242eb7038d8dae505cb56916cd6e Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 2 Apr 2025 21:49:33 -0400 Subject: [PATCH 29/87] adding more checks for ensuring that things have pointers --- drivebrain_app/debug_main.cpp | 2 +- drivebrain_app/src/DriveBrainApp.cpp | 1 - .../drivebrain_comms/src/MCUETHComms.cpp | 12 ++++++++++-- .../drivebrain_comms/src/VNComms.cpp | 12 ++++++++---- 4 files changed, 19 insertions(+), 8 deletions(-) diff --git a/drivebrain_app/debug_main.cpp b/drivebrain_app/debug_main.cpp index 6eb23b0..d7e653a 100644 --- a/drivebrain_app/debug_main.cpp +++ b/drivebrain_app/debug_main.cpp @@ -91,7 +91,7 @@ int main(int argc, char *argv[]) .run_db_service = true, .run_io_context = true, .run_process_loop = true, - .use_vectornav = false + .use_vectornav = true }; std::cout <<"creating app" <(); auto desired_rpm_msg = std::make_shared(); auto torque_limit_msg = std::make_shared(); auto desired_torque_msg = std::make_shared(); diff --git a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp index 73272f1..2ba607c 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp @@ -55,7 +55,11 @@ namespace comms for (const auto &msg : _input_deque_ref.deque) { _send_message(msg); - _message_logger->log_msg(msg); + if(_message_logger) + { + _message_logger->log_msg(msg); + } + } _input_deque_ref.deque.clear(); } @@ -81,7 +85,11 @@ namespace comms _mcu_msg->ParseFromArray(_recv_buffer.data(), size); auto out_msg = static_cast>(_mcu_msg); _state_estimator.handle_recv_process(out_msg); - _message_logger->log_msg(out_msg); + if(_message_logger) + { + _message_logger->log_msg(out_msg); + } + _start_receive(); } } diff --git a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp index 927d004..1f49cbe 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp @@ -57,17 +57,17 @@ namespace comms return true; } - VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, core::StateEstimator &state_estimator, boost::asio::io_context& io, bool &init_successful) + VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, core::StateEstimator &state_estimator, boost::asio::io_context& io, bool &init_not_successful) : core::common::Configurable(json_file_handler, "VNDriver"), _logger(logger), _state_estimator(state_estimator), _message_logger(message_logger), _serial(io) { - init_successful = init(); + init_not_successful = !init(); // Starts read - if(init_successful) + if(!init_not_successful) { _logger.log_string("Starting vn driver recieve.", core::LogLevel::INFO); @@ -79,7 +79,11 @@ namespace comms void VNDriver::log_proto_message(std::shared_ptr msg) { _state_estimator.handle_recv_process(static_cast>(msg)); - _message_logger->log_msg(static_cast>(msg)); + if(_message_logger) + { + _message_logger->log_msg(static_cast>(msg)); + } + } void VNDriver::_configure_binary_outputs() From d8f9ceb932b1d232e4778b5d0d632fd818a6c3ca Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 2 Apr 2025 22:36:07 -0400 Subject: [PATCH 30/87] fixing allocation / shared pointer issues --- config/drivebrain_config.json | 2 +- drivebrain_app/debug_main.cpp | 1 - drivebrain_app/include/DriveBrainApp.hpp | 2 +- drivebrain_app/src/DriveBrainApp.cpp | 5 +++++ drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp | 7 +++++++ drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp | 4 ++++ drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp | 4 ++++ drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp | 2 ++ .../drivebrain_comms/src/foxglove_server.cpp | 2 +- .../drivebrain_estimation/src/StateEstimator.cpp | 3 +++ 10 files changed, 28 insertions(+), 4 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index e8a48d8..ec880d3 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -29,4 +29,4 @@ "max_torque_switch_nm": 10.0, "max_accel_switch_float": 0.1 } -} \ No newline at end of file +} diff --git a/drivebrain_app/debug_main.cpp b/drivebrain_app/debug_main.cpp index d7e653a..ab85820 100644 --- a/drivebrain_app/debug_main.cpp +++ b/drivebrain_app/debug_main.cpp @@ -82,7 +82,6 @@ std::pair parse_arguments(int argc, char* argv[]) { int main(int argc, char *argv[]) { - try { auto [param_path, dbc_path] = parse_arguments(argc, argv); diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index f239248..9a23786 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -71,7 +71,7 @@ class DriveBrainApp { control::ControllerManager, 2 > _controllerManager; // std::unique_ptr _matlab_math; std::shared_ptr _foxglove_server; - std::shared_ptr>> _message_logger; + std::shared_ptr>> _message_logger = nullptr; std::unique_ptr _state_estimator; std::shared_ptr _driver; std::unique_ptr _eth_driver; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 350e8df..3a215a5 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -105,6 +105,11 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d std::bind(&common::DrivebrainMCAPLogger::init_param_schema, _mcap_logger), std::bind(&common::DrivebrainMCAPLogger::log_params, _mcap_logger)); + if(_driver) + { + _driver->update_msg_logger(_message_logger); + } + spdlog::info("constructed app"); // TODO add here the creation of the config logger } diff --git a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp index ccc02a3..4d0c117 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp @@ -81,6 +81,13 @@ namespace comms } ~CANDriver(); bool init(); + + + void update_msg_logger(std::shared_ptr message_logger) + { + _message_logger = message_logger; + } + void _handle_send_msg_from_queue(); std::shared_ptr pb_msg_recv(const can_frame &in_frame); void set_field_values_of_pb_msg(const std::unordered_map &field_values, std::shared_ptr message); diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index eefd1e7..34fb289 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -128,6 +128,8 @@ void comms::CANDriver::_handle_recv_CAN_frame(const struct can_frame &frame) { if(_message_logger) // this may not exist yet as this gets constr { _message_logger->log_msg(msg); + }else { + spdlog::warn("can comms message logger not real"); } } @@ -411,6 +413,8 @@ void comms::CANDriver::_handle_send_msg_from_queue() { if(_message_logger) { _message_logger->log_msg(msg); + } else { + spdlog::warn("message logger not real"); } } } diff --git a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp index 2ba607c..395e5e8 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp @@ -58,6 +58,8 @@ namespace comms if(_message_logger) { _message_logger->log_msg(msg); + } else { + spdlog::warn("message logger not real"); } } @@ -88,6 +90,8 @@ namespace comms if(_message_logger) { _message_logger->log_msg(out_msg); + }else { + spdlog::warn("message logger not real"); } _start_receive(); diff --git a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp index 1f49cbe..3de9923 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp @@ -82,6 +82,8 @@ namespace comms if(_message_logger) { _message_logger->log_msg(static_cast>(msg)); + } else { + spdlog::warn("message logger not real"); } } diff --git a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp index 0edb6a9..6162ad8 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp @@ -105,7 +105,7 @@ core::FoxgloveWSServer::FoxgloveWSServer(std::vector msg) { - + spdlog::info("sending live telem msg"); if (_id_name_map.find(msg->GetDescriptor()->name()) != _id_name_map.end()) { auto msg_chan_id = _id_name_map[msg->GetDescriptor()->name()]; diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 5099a50..905f46e 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -10,6 +10,7 @@ #include #include // from HT_proto +#include #include "hytech_msgs.pb.h" // from HT_proto #include "hytech.pb.h" // from HT_CAN @@ -248,6 +249,8 @@ std::pair StateEstimator::get_latest_state_and_validit if(_message_logger) { _message_logger->log_msg(static_cast>(msg_out)); + } else { + spdlog::warn("message logger not real"); } auto log_end = std::chrono::high_resolution_clock::now(); From dcb1a0fc30dd6270f9b74088747b67fc37b0cc78 Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Wed, 2 Apr 2025 22:55:03 -0400 Subject: [PATCH 31/87] Added set_msg_logger functionality, refactored unit tests --- config/drivebrain_config.json | 3 ++- .../drivebrain_comms/include/DBServiceImpl.hpp | 5 +++++ .../drivebrain_comms/include/MCUETHComms.hpp | 4 +++- drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp | 4 +++- .../drivebrain_estimation/include/StateEstimator.hpp | 4 ++++ unit_test/SimpleSpeedControllerTest.cpp | 4 ---- unit_test/SimpleTorqueControllerTest.cpp | 5 +---- 7 files changed, 18 insertions(+), 11 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index e8a48d8..33eb0b0 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -9,7 +9,8 @@ "rear_torque_scale": 1.0, "regen_torque_scale": 0.6, "positive_speed_set" : 45, - "max_power_kw": 63.0 + "max_power_kw": 63.0, + "dt_rate_hz": 1000 }, "VNDriver": { "device_name": "/dev/ttyUSB0", diff --git a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp index 3b5f738..81217e2 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 +#include #include #include #include @@ -26,6 +28,9 @@ class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Servi public: DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch); void run_server(); void stop_server(); + void update_msg_logger(std::shared_ptr> _logger_inst) { + _logger_inst = _logger_inst; + } private: std::shared_ptr>> _logger_inst; std::function _mode_switch; diff --git a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp index 3e97da3..56fe82f 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp @@ -48,7 +48,9 @@ namespace comms void _handle_send(std::array /*message*/, const boost::system::error_code & /*error*/, std::size_t /*bytes_transferred*/); - + void update_msg_logger(std::shared_ptr message_logger) { + _message_logger = message_logger; + } private: core::Logger &_logger; std::shared_ptr _message_logger; diff --git a/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp index b9761fc..92ba3a6 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp @@ -64,7 +64,9 @@ namespace comms { public: // Public methods void log_proto_message(std::shared_ptr msg); - + void update_msg_logger(std::shared_ptr message_logger) { + _message_logger = message_logger; + } 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_estimation/include/StateEstimator.hpp b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp index c3808ce..00e4dbc 100644 --- a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp +++ b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp @@ -76,6 +76,10 @@ namespace core void handle_recv_process(std::shared_ptr message); std::pair get_latest_state_and_validity(); void set_previous_control_output(ControllerOutput prev_control_output); + void update_msg_logger(std::shared_ptr message_logger) + { + _message_logger = message_logger; + } private: void _recv_low_level_state(std::shared_ptr message); diff --git a/unit_test/SimpleSpeedControllerTest.cpp b/unit_test/SimpleSpeedControllerTest.cpp index b8f1cc3..1984307 100644 --- a/unit_test/SimpleSpeedControllerTest.cpp +++ b/unit_test/SimpleSpeedControllerTest.cpp @@ -35,10 +35,6 @@ class SimpleSpeedControllerTest : public testing::Test { } }; -TEST_F(SimpleSpeedControllerTest, ConstructorInitializesProperly) -{ - EXPECT_NEAR(simple_controller.get_dt_sec(), 0.001, 0.01); -} TEST_F(SimpleSpeedControllerTest, InitHasConfig) { diff --git a/unit_test/SimpleTorqueControllerTest.cpp b/unit_test/SimpleTorqueControllerTest.cpp index dabb98a..3244db1 100644 --- a/unit_test/SimpleTorqueControllerTest.cpp +++ b/unit_test/SimpleTorqueControllerTest.cpp @@ -34,10 +34,7 @@ class SimpleTorqueControllerTest : public testing::Test { } }; -TEST_F(SimpleTorqueControllerTest, ConstructorInitializesProperly) -{ - EXPECT_NEAR(simple_controller.get_dt_sec(), 0.001, 0.01); -} + TEST_F(SimpleTorqueControllerTest, InitHasConfig) { From 7a7a3efe3e0fe87705823d1f314ce26cfe7520e3 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 2 Apr 2025 23:06:43 -0400 Subject: [PATCH 32/87] erm, fixed bugs --- config/drivebrain_config.json | 2 +- drivebrain_app/src/DriveBrainApp.cpp | 17 +++++++++++++++++ .../drivebrain_comms/include/DBServiceImpl.hpp | 2 +- .../drivebrain_comms/include/MCUETHComms.hpp | 8 ++++---- .../src/DrivebrainMCAPLogger.cpp | 2 +- 5 files changed, 24 insertions(+), 7 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index cda47c3..2277d40 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -13,7 +13,7 @@ "dt_rate_hz": 1000 }, "VNDriver": { - "device_name": "/dev/ttyUSB0", + "device_name": "/dev/ttyUSB1", "baud_rate": 921600, "port": 1, "freq_divisor": 1 diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 3a215a5..e9cc3db 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -109,6 +109,23 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { _driver->update_msg_logger(_message_logger); } + if(_vn_driver) + { + _vn_driver->update_msg_logger(_message_logger); + } + if(_state_estimator) + { + _state_estimator->update_msg_logger(_message_logger); + } + + if(_db_service) + { + _db_service->update_msg_logger(_message_logger); + } + if(_eth_driver) + { + _eth_driver->update_msg_logger(_message_logger); + } spdlog::info("constructed app"); // TODO add here the creation of the config logger diff --git a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp index 81217e2..1bb0adb 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp @@ -28,7 +28,7 @@ class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Servi public: DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch); void run_server(); void stop_server(); - void update_msg_logger(std::shared_ptr> _logger_inst) { + void update_msg_logger(std::shared_ptr>> _logger_inst) { _logger_inst = _logger_inst; } private: diff --git a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp index 56fe82f..bb643e9 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp @@ -39,7 +39,10 @@ namespace comms const std::string &send_ip, uint16_t recv_port, uint16_t send_port); - + void update_msg_logger(std::shared_ptr message_logger) { + _message_logger = message_logger; + } + private: void _handle_send_msg_from_queue(); void _send_message(std::shared_ptr msg_out); @@ -48,9 +51,6 @@ namespace comms void _handle_send(std::array /*message*/, const boost::system::error_code & /*error*/, std::size_t /*bytes_transferred*/); - void update_msg_logger(std::shared_ptr message_logger) { - _message_logger = message_logger; - } private: core::Logger &_logger; std::shared_ptr _message_logger; diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp index a5987a7..4f9abe5 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp @@ -129,7 +129,7 @@ namespace common spdlog::info("Logging message: {}", msg.message_name); spdlog::info("message size: {}", msg_to_log.dataSize); - spdlog::info(msg.serialized_data); + // spdlog::info(msg.serialized_data); } } From 58d1bc64ef4d3c44225d9f336196f739258a31e2 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 3 Apr 2025 11:56:49 -0400 Subject: [PATCH 33/87] removed commented code and added info print --- drivebrain_app/src/DriveBrainApp.cpp | 3 +++ .../drivebrain_logging/src/DrivebrainMCAPLogger.cpp | 1 - 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index e9cc3db..0a99446 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -6,6 +6,7 @@ #include "hytech.pb.h" #include #include +#include #include std::atomic DriveBrainApp::_stop_signal{false}; @@ -191,7 +192,9 @@ void DriveBrainApp::_process_loop() { 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); + spdlog::info("sent can"); } + } else if (const core::TorqueControlOut* torqueControl = std::get_if(&cmd_out)){ // if it is a torque controller: // set desired torque desired_torque_msg->set_drivebrain_torque_fl(::abs(torqueControl->desired_torques_nm.FL)); diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp index 4f9abe5..219b6e1 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp @@ -129,7 +129,6 @@ namespace common spdlog::info("Logging message: {}", msg.message_name); spdlog::info("message size: {}", msg_to_log.dataSize); - // spdlog::info(msg.serialized_data); } } From 047aee6188ffdd7b453fcb54b56d90939ab8b4f5 Mon Sep 17 00:00:00 2001 From: Mark Goltsman Date: Thu, 3 Apr 2025 19:49:14 -0400 Subject: [PATCH 34/87] Fixed thread infinite loop --- drivebrain_app/src/DriveBrainApp.cpp | 2 ++ .../drivebrain_comms/src/CANComms.cpp | 12 ++++++++++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 0a99446..3ebe9b5 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -192,6 +192,7 @@ void DriveBrainApp::_process_loop() { 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); + _can_tx_queue.cv.notify_all(); // notify the CAN thread to send the messages spdlog::info("sent can"); } @@ -204,6 +205,7 @@ void DriveBrainApp::_process_loop() { { std::unique_lock lk(_can_tx_queue.mtx); _can_tx_queue.deque.push_back(desired_torque_msg); // use new protobuf struct + _can_tx_queue.cv.notify_all(); // notify the CAN thread to send the messages } } diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index 34fb289..b380ab4 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -112,6 +112,7 @@ void comms::CANDriver::_do_read() { } void comms::CANDriver::_send_message(const struct can_frame &frame) { + std:: cout << "Sending CAN message with ID: {} and length: {}" << frame.can_id << frame.len; boost::asio::async_write( _socket, boost::asio::buffer(&frame, sizeof(frame)), [this](boost::system::error_code ec, std::size_t /*bytes_transferred*/) { @@ -390,16 +391,19 @@ void comms::CANDriver::_handle_send_msg_from_queue() { core::common::ThreadSafeDeque> q; while (_running) { { + std::unique_lock lk(_input_deque_ref.mtx); // TODO unfuck this, queue management shouldnt live within the queue // itself + std::cout << "Waiting for input deque to be filled"; _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()) { + std::cout << "Deque is empty, returning"; return; } - + std::cout << "Let;s go"; q.deque = _input_deque_ref.deque; _input_deque_ref.deque.clear(); } @@ -407,6 +411,10 @@ 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); From c88502cd0fb424a2f67fcebd32924bab071da280 Mon Sep 17 00:00:00 2001 From: Krish Date: Thu, 3 Apr 2025 19:54:51 -0400 Subject: [PATCH 35/87] correct dbc can description --- config/hytech.dbc | 1644 +++++++++-------- drivebrain_app/debug_main.cpp | 2 +- drivebrain_app/src/DriveBrainApp.cpp | 2 + .../drivebrain_comms/src/CANComms.cpp | 11 +- .../src/DrivebrainMCAPLogger.cpp | 6 +- 5 files changed, 931 insertions(+), 734 deletions(-) diff --git a/config/hytech.dbc b/config/hytech.dbc index dcdff98..c8b72d9 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) [-64|63.75] "deg" Vector__XXX + SG_ Pitch_angle : 16|9@1- (0.25,0) [-64|63.75] "deg" Vector__XXX + SG_ Height : 0|12@1- (0.035,0) [-71.68|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,227 +191,192 @@ 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|7] "" 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|7] "" Vector__XXX + SG_ Attitude_Filtering : 32|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ Sensor_X_Location : 20|12@1- (0.01,0) [-20.48|20.47] "m" Vector__XXX + SG_ Sensor_Y_Location : 8|12@1- (0.01,0) [-20.48|20.47] "m" Vector__XXX + SG_ Height_Offset : 0|8@1- (0.5,0) [-64|63.5] "cm" Vector__XXX -BO_ 236 DASHBOARD_STATE: 3 ECU - SG_ dial_state : 16|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ tcu_recording_state : 12|2@1+ (1,0) [0|0] "" Vector__XXX - SG_ drive_buzzer : 11|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ ssok_above_threshold : 10|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ shutdown_h_above_threshold : 9|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ right_shifter_button : 8|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ left_shifter_button : 7|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ led_dimmer_button : 6|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ torque_mode_button : 5|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ launch_ctrl_button : 4|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ motor_controller_cycle_button : 3|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ mode_button : 2|1@1+ (1,0) [0|0] "" Vector__XXX - 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_ 2033 DASHBOARD_BUZZER_CONTROL: 1 ECU + 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 @@ -410,16 +394,335 @@ BO_ 235 DASHBOARD_MCU_STATE: 6 ECU 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_ 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_ 236 DASHBOARD_STATE: 3 ECU + SG_ dial_state : 16|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ tcu_recording_state : 12|2@1+ (1,0) [0|0] "" Vector__XXX + SG_ drive_buzzer : 11|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ ssok_above_threshold : 10|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ shutdown_h_above_threshold : 9|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ right_shifter_button : 8|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ left_shifter_button : 7|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ led_dimmer_button : 6|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ torque_mode_button : 5|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ launch_ctrl_button : 4|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ motor_controller_cycle_button : 3|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ mode_button : 2|1@1+ (1,0) [0|0] "" Vector__XXX + 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_ 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_ 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_ 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_ 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) [-10.24|10.23] "rad/s" Vector__XXX + SG_ Yaw_Rate : 41|11@1- (0.01,0) [-10.24|10.23] "rad/s" Vector__XXX + SG_ Roll_Rate : 30|11@1- (0.01,0) [-10.24|10.23] "rad/s" Vector__XXX + SG_ Accel_Z : 20|10@1- (0.057,0) [-29.184|29.127] "m/s2" Vector__XXX + SG_ Accel_Y : 10|10@1- (0.057,0) [-29.184|29.127] "m/s2" Vector__XXX + SG_ Accel_X : 0|10@1- (0.057,0) [-29.184|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,31 +796,114 @@ 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_ 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_ 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_ 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_ 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_ 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_ 257 MCU_ERROR_STATES: 8 ECU + SG_ torque_controller_mux_status : 0|3@1+ (1,0) [0|0] "" Vector__XXX -BO_ 1076 RF_TTPMS_5: 8 ECU +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 SG_ RF_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX SG_ RF_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX @@ -565,70 +951,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 +962,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|7] "" Vector__XXX + SG_ IMU_Filtering : 40|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ Attitude_Filtering : 37|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ Sensor_Y_Location : 25|12@1- (0.01,0) [-20.48|20.47] "m" Vector__XXX + SG_ Sensor_X_Location : 13|12@1- (0.01,0) [-20.48|20.47] "m" Vector__XXX + SG_ Height_Offset : 5|8@1- (0.5,0) [-64|63.5] "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|7] "" Vector__XXX + +BO_ 2 State: 5 ECU + SG_ SpeedBeam_Health : 32|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ sensor_3_Validity : 30|2@1+ (1,0) [0|3] "" Vector__XXX + SG_ sensor_2_Validity : 28|2@1+ (1,0) [0|3] "" Vector__XXX + SG_ sensor_1_Validity : 26|2@1+ (1,0) [0|3] "" Vector__XXX + SG_ sensor_0_Validity : 24|2@1+ (1,0) [0|3] "" Vector__XXX + SG_ Firmware_Version : 8|16@1+ (1,0) [0|65535] "" Vector__XXX + SG_ Temperature_Internal : 0|8@1- (0.5,20) [-44|83.5] "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 +1040,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 +1055,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 +1086,49 @@ 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) [-10.24|10.22] "m/s" Vector__XXX + SG_ Velocity_y_CGCorrected : 43|10@1- (0.03,0) [-15.36|15.33] "m/s" Vector__XXX + SG_ Velocity_x_CGCorrected : 31|12@1- (0.02,0) [-40.96|40.94] "m/s" Vector__XXX + SG_ Velocity_z : 22|9@1- (0.025,0) [-6.4|6.375] "m/s" Vector__XXX + SG_ Velocity_y : 12|10@1- (0.03,0) [-15.36|15.33] "m/s" Vector__XXX + SG_ Velocity_x : 0|12@1- (0.02,0) [-40.96|40.94] "m/s" Vector__XXX + +BO_ 0 brake_temps: 8 ECU +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 +1138,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 +1232,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 +1255,46 @@ 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; +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 +1303,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 +1319,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 +1351,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 +1385,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 +1396,45 @@ 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/drivebrain_app/debug_main.cpp b/drivebrain_app/debug_main.cpp index ab85820..b76e362 100644 --- a/drivebrain_app/debug_main.cpp +++ b/drivebrain_app/debug_main.cpp @@ -90,7 +90,7 @@ int main(int argc, char *argv[]) .run_db_service = true, .run_io_context = true, .run_process_loop = true, - .use_vectornav = true + .use_vectornav = false }; std::cout <<"creating app" < 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]; @@ -388,17 +388,12 @@ comms::CANDriver::_get_CAN_msg(std::shared_ptr 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> q; + 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; - } + while (_input_deque_ref.deque.empty()) { } q.deque = _input_deque_ref.deque; _input_deque_ref.deque.clear(); diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp index 219b6e1..598ed64 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp @@ -124,11 +124,11 @@ namespace common { std::unique_lock lk(_logger_mtx); msg_to_log.channelId = _msg_name_id_map[msg.message_name]; - spdlog::info("logging at channel: {}", msg_to_log.channelId); + // spdlog::info("logging at channel: {}", msg_to_log.channelId); auto write_res = _writer.write(msg_to_log); - spdlog::info("Logging message: {}", msg.message_name); - spdlog::info("message size: {}", msg_to_log.dataSize); + // spdlog::info("Logging message: {}", msg.message_name); + // spdlog::info("message size: {}", msg_to_log.dataSize); } } From 394ebfc2ae95578a2a805bd221d1459f1862b5c9 Mon Sep 17 00:00:00 2001 From: Krish Date: Thu, 3 Apr 2025 20:03:44 -0400 Subject: [PATCH 36/87] bruh --- drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index 0d9e636..241c5cc 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -399,7 +399,7 @@ void comms::CANDriver::_handle_send_msg_from_queue() { 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.") + spdlog::info("Returning, deque empty or not running."); return; } q.deque = _input_deque_ref.deque; From e6176f7bd4ed8f37545266a7b903820ab4919c48 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 4 Apr 2025 03:03:19 -0400 Subject: [PATCH 37/87] tuning down the loop rate for better sending --- drivebrain_app/src/DriveBrainApp.cpp | 12 ++++++++---- .../drivebrain_comms/src/foxglove_server.cpp | 1 - .../drivebrain_control/src/SimpleSpeedController.cpp | 5 +++-- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 521b6f6..716847e 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -158,7 +158,8 @@ void DriveBrainApp::_process_loop() { auto desired_rpm_msg = std::make_shared(); auto torque_limit_msg = std::make_shared(); auto desired_torque_msg = std::make_shared(); - auto loop_time = _controllerManager.get_active_controller_timestep(); + // auto loop_time = _controllerManager.get_active_controller_timestep(); + auto loop_time = 0.005; auto loop_time_micros = (int)(loop_time * 1000000.0f); std::chrono::microseconds loop_chrono_time(loop_time_micros); @@ -182,18 +183,19 @@ void DriveBrainApp::_process_loop() { 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); - + _message_logger->log_msg(static_cast>(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)); + _message_logger->log_msg(static_cast>(torque_limit_msg)); { 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); _can_tx_queue.cv.notify_all(); // notify the CAN thread to send the messages - spdlog::info("sent can"); + // spdlog::info("sent can"); } } else if (const core::TorqueControlOut* torqueControl = std::get_if(&cmd_out)){ // if it is a torque controller: @@ -202,6 +204,7 @@ void DriveBrainApp::_process_loop() { desired_torque_msg->set_drivebrain_torque_fr(::abs(torqueControl->desired_torques_nm.FR)); desired_torque_msg->set_drivebrain_torque_rl(::abs(torqueControl->desired_torques_nm.RL)); desired_torque_msg->set_drivebrain_torque_rr(::abs(torqueControl->desired_torques_nm.RR)); + _message_logger->log_msg(static_cast>(desired_torque_msg)); { std::unique_lock lk(_can_tx_queue.mtx); _can_tx_queue.deque.push_back(desired_torque_msg); // use new protobuf struct @@ -264,4 +267,5 @@ void DriveBrainApp::run() { } } -// ./test_build -p ../config/drivebrain_config.json -d ../config/hytech.dbc \ No newline at end of file +// ./test_build -p ../config/drivebrain_config.json -d ../config/hytech.dbc +// ./alpha_build -p ../config/drivebrain_config.json -d ../config/hytech.dbc \ No newline at end of file diff --git a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp index 6162ad8..763dd81 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp @@ -105,7 +105,6 @@ core::FoxgloveWSServer::FoxgloveWSServer(std::vector msg) { - spdlog::info("sending live telem msg"); if (_id_name_map.find(msg->GetDescriptor()->name()) != _id_name_map.end()) { auto msg_chan_id = _id_name_map[msg->GetDescriptor()->name()]; diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp index c7ea937..ffd4e89 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp @@ -65,13 +65,14 @@ bool control::SimpleSpeedController::init() std::optional regen_torque_scale = get_live_parameter("regen_torque_scale"); std::optional positive_speed_set = get_live_parameter("positive_speed_set"); std::optional max_power_kw = get_live_parameter("max_power_kw"); + std::optional dt_rate_hz = get_live_parameter("dt_rate_hz"); - if (!(max_torque && max_regen_torque && rear_torque_scale && regen_torque_scale && positive_speed_set && max_power_kw)) + 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}; + _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; From 3c74f9a85bfd6eee822f65809fbd2796628cb7b0 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 10 Apr 2025 03:20:12 -0400 Subject: [PATCH 38/87] integrating second can and updating db core src --- CMakeLists.txt | 1 + config/drivebrain_config.json | 6 +- config/hytech.dbc | 2021 +++++++++-------- drivebrain_app/debug_main.cpp | 41 +- drivebrain_app/include/DriveBrainApp.hpp | 11 +- drivebrain_app/include/arg_parse.hpp | 14 + drivebrain_app/main.cpp | 40 +- drivebrain_app/src/DriveBrainApp.cpp | 72 +- drivebrain_app/src/arg_parse.cpp | 30 + .../drivebrain_comms/include/CANComms.hpp | 18 +- .../drivebrain_comms/include/MCUETHComms.hpp | 4 +- .../drivebrain_comms/include/VNComms.hpp | 8 +- .../drivebrain_comms/src/CANComms.cpp | 3 +- .../drivebrain_comms/src/MCUETHComms.cpp | 5 +- .../drivebrain_comms/src/VNComms.cpp | 19 +- .../src/DrivebrainMCAPLogger.cpp | 7 +- flake.lock | 6 +- test/test_send_can.py | 25 + vcan_bringup.sh | 5 +- 19 files changed, 1227 insertions(+), 1109 deletions(-) create mode 100644 drivebrain_app/include/arg_parse.hpp create mode 100644 drivebrain_app/src/arg_parse.cpp create mode 100644 test/test_send_can.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 96bf03a..398e236 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -166,6 +166,7 @@ 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 diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 2277d40..8d5c8ca 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -1,8 +1,12 @@ { - "CANDriver": { + "CANDriverPrimary": { "canbus_device": "vcan0", "path_to_dbc": "./config/hytech.dbc" }, + "CANDriverSecondary": { + "canbus_device": "vcan1", + "path_to_dbc": "./config/hytech.dbc" + }, "SimpleSpeedController": { "max_torque": 21.0, "max_regen_torque": 10.0, diff --git a/config/hytech.dbc b/config/hytech.dbc index c8b72d9..5ff6608 100644 --- a/config/hytech.dbc +++ b/config/hytech.dbc @@ -42,16 +42,6 @@ BO_ 1220 IZZE_IR_TEMPS: 8 Peripherals 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) [-64|63.75] "deg" Vector__XXX - SG_ Pitch_angle : 16|9@1- (0.25,0) [-64|63.75] "deg" Vector__XXX - SG_ Height : 0|12@1- (0.035,0) [-71.68|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 @@ -169,9 +159,6 @@ 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 @@ -191,208 +178,205 @@ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 164 MC1_TORQUE_COMMAND: 2 ECU + SG_ torque_command : 0|16@1- (0.1,0) [0|0] "% Mn" 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 165 MC2_TORQUE_COMMAND: 2 ECU + SG_ torque_command : 0|16@1- (0.1,0) [0|0] "% Mn" 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 166 MC3_TORQUE_COMMAND: 2 ECU + SG_ torque_command : 0|16@1- (0.1,0) [0|0] "% Mn" 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 167 MC4_TORQUE_COMMAND: 2 ECU + SG_ torque_command : 0|16@1- (0.1,0) [0|0] "% Mn" 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_ 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_ 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_ 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_ 6 Configuration: 6 ECU - SG_ Velocity_Filtering : 40|3@1+ (1,0) [0|7] "" 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|7] "" Vector__XXX - SG_ Attitude_Filtering : 32|3@1+ (1,0) [0|7] "" Vector__XXX - SG_ Sensor_X_Location : 20|12@1- (0.01,0) [-20.48|20.47] "m" Vector__XXX - SG_ Sensor_Y_Location : 8|12@1- (0.01,0) [-20.48|20.47] "m" Vector__XXX - SG_ Height_Offset : 0|8@1- (0.5,0) [-64|63.5] "cm" 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_ 2033 DASHBOARD_BUZZER_CONTROL: 1 ECU - SG_ dash_buzzer_flag : 0|1@1+ (1,0) [0|1] "" 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_ 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_ 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_ 236 DASHBOARD_STATE: 3 ECU SG_ dial_state : 16|8@1+ (1,0) [0|0] "" Vector__XXX @@ -410,52 +394,190 @@ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 1060 LF_TTPMS_1: 8 ECU + SG_ LF_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ LF_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ LF_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX + SG_ LF_TTPMS_SN : 7|16@0+ (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_ 1061 LF_TTPMS_2: 8 ECU + SG_ LF_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1062 LF_TTPMS_3: 8 ECU + SG_ LF_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1063 LF_TTPMS_4: 8 ECU + SG_ LF_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1064 LF_TTPMS_5: 8 ECU + SG_ LF_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1065 LF_TTPMS_6: 8 ECU + SG_ LF_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX + SG_ LF_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX + SG_ LF_TTPMS_TC : 7|16@0+ (1,0) [0|0] "" Vector__XXX + +BO_ 1066 LR_TTPMS_1: 8 ECU + SG_ LR_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ LR_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ LR_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX + SG_ LR_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" Vector__XXX + +BO_ 1067 LR_TTPMS_2: 8 ECU + SG_ LR_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1068 LR_TTPMS_3: 8 ECU + SG_ LR_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1069 LR_TTPMS_4: 8 ECU + SG_ LR_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1070 LR_TTPMS_5: 8 ECU + SG_ LR_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1071 LR_TTPMS_6: 8 ECU + SG_ LR_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX + SG_ LR_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + 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_ 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 + SG_ RF_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RF_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RF_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1077 RF_TTPMS_6: 8 ECU + SG_ RF_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX + SG_ RF_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RF_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX + SG_ RF_TTPMS_TC : 7|16@0+ (1,0) [0|0] "" Vector__XXX + +BO_ 1078 RR_TTPMS_1: 8 ECU + SG_ RR_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ RR_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ RR_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX + SG_ RR_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" Vector__XXX + +BO_ 1079 RR_TTPMS_2: 8 ECU + SG_ RR_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1080 RR_TTPMS_3: 8 ECU + SG_ RR_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1081 RR_TTPMS_4: 8 ECU + SG_ RR_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1082 RR_TTPMS_5: 8 ECU + SG_ RR_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1083 RR_TTPMS_6: 8 ECU + SG_ RR_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX + SG_ RR_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + 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 @@ -463,6 +585,12 @@ BO_ 512 DRIVETRAIN_RPMS_TELEM: 8 ECU 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 @@ -501,725 +629,619 @@ BO_ 514 DRIVETRAIN_STATUS_TELEM: 8 ECU 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_ 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 + SG_ thermistor_inv2 : 16|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX + SG_ thermistor_inv1 : 0|16@1+ (0.005,0) [0|0] "deg C" 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_ 230 SAB_THERMISTORS_2: 6 ECU + SG_ thermistor_pump : 32|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX + 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_ 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_ 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_ 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_ 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_ 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_ 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_ 1 IMU: 8 ECU - SG_ Pitch_Rate : 52|11@1- (0.01,0) [-10.24|10.23] "rad/s" Vector__XXX - SG_ Yaw_Rate : 41|11@1- (0.01,0) [-10.24|10.23] "rad/s" Vector__XXX - SG_ Roll_Rate : 30|11@1- (0.01,0) [-10.24|10.23] "rad/s" Vector__XXX - SG_ Accel_Z : 20|10@1- (0.057,0) [-29.184|29.127] "m/s2" Vector__XXX - SG_ Accel_Y : 10|10@1- (0.057,0) [-29.184|29.127] "m/s2" Vector__XXX - SG_ Accel_X : 0|10@1- (0.057,0) [-29.184|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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 224 VN_GPS_TIME_MSG: 8 ECU + SG_ vn_gps_time : 0|64@1+ (1,0) [0|0] "ns" 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_ 225 VN_STATUS: 8 ECU + SG_ vn_gps_status : 0|16@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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 201 TC_SIMPLE: 8 ECU + SG_ accel_request_state : 48|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ rear_regen_scale : 40|8@1- (0.00787402,0) [0|0] "" Vector__XXX + SG_ front_regen_scale : 32|8@1- (0.00787402,0) [0|0] "" Vector__XXX + SG_ rear_torque_scale : 24|8@1- (0.00787402,0) [0|0] "" Vector__XXX + SG_ front_torque_scale : 16|8@1- (0.00787402,0) [0|0] "" Vector__XXX + SG_ torque_request : 0|16@1- (0.001,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_ 202 TC_SIMPLE_LAUNCH: 8 ECU + SG_ speed_setpoint_rpm : 24|16@1- (1,0) [0|0] "rpm" Vector__XXX + SG_ initial_launch_target : 8|16@1+ (1,0) [0|0] "rpm" Vector__XXX + SG_ algo_active : 2|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ launch_control_state : 0|2@1+ (1,0) [0|0] "" 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_ 2047 VEHM_ALPHA: 8 ECU + SG_ vehm_alpha_deg_rr : 48|16@1- (0.001,0) [0|0] "deg" Vector__XXX + SG_ vehm_alpha_deg_rl : 32|16@1- (0.001,0) [0|0] "deg" Vector__XXX + SG_ vehm_alpha_deg_fr : 16|16@1- (0.001,0) [0|0] "deg" Vector__XXX + SG_ vehm_alpha_deg_fl : 0|16@1- (0.001,0) [0|0] "deg" 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_ 2031 VEHM_BETA: 8 ECU + SG_ vehm_beta_deg : 0|16@1- (0.001,0) [0|0] "deg" 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_ 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 + SG_ vehm_long_corner_vel_fr : 16|16@1- (0.01,0) [0|0] "m/s" Vector__XXX + SG_ vehm_long_corner_vel_fl : 0|16@1- (0.01,0) [0|0] "m/s" 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_ 1999 VEHM_SL: 8 ECU + SG_ vehm_sl_rr : 48|16@1- (0.0001,0) [0|0] "" Vector__XXX + SG_ vehm_sl_rl : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX + 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 1060 LF_TTPMS_1: 8 ECU - SG_ LF_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ LF_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ LF_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX - SG_ LF_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" 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_ 1061 LF_TTPMS_2: 8 ECU - SG_ LF_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1062 LF_TTPMS_3: 8 ECU - SG_ LF_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1063 LF_TTPMS_4: 8 ECU - SG_ LF_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1064 LF_TTPMS_5: 8 ECU - SG_ LF_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1065 LF_TTPMS_6: 8 ECU - SG_ LF_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ LF_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX - SG_ LF_TTPMS_TC : 7|16@0+ (1,0) [0|0] "" 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_ 1066 LR_TTPMS_1: 8 ECU - SG_ LR_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ LR_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ LR_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX - SG_ LR_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" 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_ 1067 LR_TTPMS_2: 8 ECU - SG_ LR_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1068 LR_TTPMS_3: 8 ECU - SG_ LR_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T5 : 7|16@0+ (0.1,-100) [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_ 1069 LR_TTPMS_4: 8 ECU - SG_ LR_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1070 LR_TTPMS_5: 8 ECU - SG_ LR_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1071 LR_TTPMS_6: 8 ECU - SG_ LR_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ LR_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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 - SG_ RF_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RF_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RF_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1077 RF_TTPMS_6: 8 ECU - SG_ RF_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ RF_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RF_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX - SG_ RF_TTPMS_TC : 7|16@0+ (1,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_ 1078 RR_TTPMS_1: 8 ECU - SG_ RR_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ RR_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ RR_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX - SG_ RR_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" 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_ 1079 RR_TTPMS_2: 8 ECU - SG_ RR_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" 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 + SG_ num_com_sats_pvt : 48|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ highest_cn0_2 : 40|8@1+ (0.5,0) [0|0] "dBHz" Vector__XXX + SG_ num_sats_rtk_2 : 32|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ num_sats_pvt_2 : 24|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ highest_cn0_1 : 16|8@1+ (0.5,0) [0|0] "dBHz" Vector__XXX + 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_ 1080 RR_TTPMS_3: 8 ECU - SG_ RR_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1081 RR_TTPMS_4: 8 ECU - SG_ RR_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1082 RR_TTPMS_5: 8 ECU - SG_ RR_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1083 RR_TTPMS_6: 8 ECU - SG_ RR_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ RR_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - 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_ 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_ 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 - SG_ thermistor_inv2 : 16|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX - SG_ thermistor_inv1 : 0|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX +BO_ 0 brake_temps: 8 ECU -BO_ 230 SAB_THERMISTORS_2: 6 ECU - SG_ thermistor_pump : 32|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX - 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_ 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_ 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_ 113 INV1_TEMPS: 8 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_ 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_ 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- (1,0) [0|0] "" Vector__XXX + SG_ actual_power_w : 0|32@1+ (1,0) [0|0] "" Vector__XXX -BO_ 3 Settings: 6 ECU - SG_ Velocity_Filtering : 43|3@1+ (1,0) [0|7] "" Vector__XXX - SG_ IMU_Filtering : 40|3@1+ (1,0) [0|7] "" Vector__XXX - SG_ Attitude_Filtering : 37|3@1+ (1,0) [0|7] "" Vector__XXX - SG_ Sensor_Y_Location : 25|12@1- (0.01,0) [-20.48|20.47] "m" Vector__XXX - SG_ Sensor_X_Location : 13|12@1- (0.01,0) [-20.48|20.47] "m" Vector__XXX - SG_ Height_Offset : 5|8@1- (0.5,0) [-64|63.5] "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|7] "" Vector__XXX - -BO_ 2 State: 5 ECU - SG_ SpeedBeam_Health : 32|3@1+ (1,0) [0|7] "" Vector__XXX - SG_ sensor_3_Validity : 30|2@1+ (1,0) [0|3] "" Vector__XXX - SG_ sensor_2_Validity : 28|2@1+ (1,0) [0|3] "" Vector__XXX - SG_ sensor_1_Validity : 26|2@1+ (1,0) [0|3] "" Vector__XXX - SG_ sensor_0_Validity : 24|2@1+ (1,0) [0|3] "" Vector__XXX - SG_ Firmware_Version : 8|16@1+ (1,0) [0|65535] "" Vector__XXX - SG_ Temperature_Internal : 0|8@1- (0.5,20) [-44|83.5] "C" 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_ 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_ 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_ 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_ 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_ 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_ 118 INV2_TEMPS: 8 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_ 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_ 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- (1,0) [0|0] "" Vector__XXX + SG_ actual_power_w : 0|32@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 - SG_ rear_regen_scale : 40|8@1- (0.00787402,0) [0|0] "" Vector__XXX - SG_ front_regen_scale : 32|8@1- (0.00787402,0) [0|0] "" Vector__XXX - SG_ rear_torque_scale : 24|8@1- (0.00787402,0) [0|0] "" Vector__XXX - SG_ front_torque_scale : 16|8@1- (0.00787402,0) [0|0] "" Vector__XXX - SG_ torque_request : 0|16@1- (0.001,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_ 202 TC_SIMPLE_LAUNCH: 8 ECU - SG_ speed_setpoint_rpm : 24|16@1- (1,0) [0|0] "rpm" Vector__XXX - SG_ initial_launch_target : 8|16@1+ (1,0) [0|0] "rpm" Vector__XXX - SG_ algo_active : 2|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ launch_control_state : 0|2@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_ 2047 VEHM_ALPHA: 8 ECU - SG_ vehm_alpha_deg_rr : 48|16@1- (0.001,0) [0|0] "deg" Vector__XXX - SG_ vehm_alpha_deg_rl : 32|16@1- (0.001,0) [0|0] "deg" Vector__XXX - SG_ vehm_alpha_deg_fr : 16|16@1- (0.001,0) [0|0] "deg" Vector__XXX - SG_ vehm_alpha_deg_fl : 0|16@1- (0.001,0) [0|0] "deg" 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_ 2031 VEHM_BETA: 8 ECU - SG_ vehm_beta_deg : 0|16@1- (0.001,0) [0|0] "deg" Vector__XXX +BO_ 129 INV3_TEMPS: 8 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_ 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_ 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- (1,0) [0|0] "" Vector__XXX + SG_ actual_power_w : 0|32@1+ (1,0) [0|0] "" 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 - SG_ vehm_long_corner_vel_fr : 16|16@1- (0.01,0) [0|0] "m/s" Vector__XXX - SG_ vehm_long_corner_vel_fl : 0|16@1- (0.01,0) [0|0] "m/s" 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_ 1999 VEHM_SL: 8 ECU - SG_ vehm_sl_rr : 48|16@1- (0.0001,0) [0|0] "" Vector__XXX - SG_ vehm_sl_rl : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX - 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_ 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_ 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_ 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_ 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_ 134 INV4_TEMPS: 8 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_ 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_ 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- (1,0) [0|0] "" Vector__XXX + SG_ actual_power_w : 0|32@1+ (1,0) [0|0] "" 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_ 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_ 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_ 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_ 234 VN_GNSS_COMP_SIG_HEALTH: 8 ECU - SG_ num_com_sats_rtk : 56|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ num_com_sats_pvt : 48|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ highest_cn0_2 : 40|8@1+ (0.5,0) [0|0] "dBHz" Vector__XXX - SG_ num_sats_rtk_2 : 32|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ num_sats_pvt_2 : 24|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ highest_cn0_1 : 16|8@1+ (0.5,0) [0|0] "dBHz" Vector__XXX - 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_ 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_ 224 VN_GPS_TIME_MSG: 8 ECU - SG_ vn_gps_time : 0|64@1+ (1,0) [0|0] "ns" 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_ 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_ 144 INV1_CONTROL_WORD: 8 ECU + SG_ remove_error : 3|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ driver_enable : 2|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ hv_enable : 1|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ inverter_enable : 0|1@1+ (1,0) [0|0] "" Vector__XXX + +BO_ 145 INV2_CONTROL_WORD: 8 ECU + SG_ remove_error : 3|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ driver_enable : 2|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ hv_enable : 1|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ inverter_enable : 0|1@1+ (1,0) [0|0] "" Vector__XXX + +BO_ 146 INV3_CONTROL_WORD: 8 ECU + SG_ remove_error : 3|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ driver_enable : 2|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ hv_enable : 1|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ inverter_enable : 0|1@1+ (1,0) [0|0] "" Vector__XXX + +BO_ 147 INV4_CONTROL_WORD: 8 ECU + SG_ remove_error : 3|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ driver_enable : 2|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ hv_enable : 1|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ inverter_enable : 0|1@1+ (1,0) [0|0] "" Vector__XXX + +BO_ 151 INV1_CONTROL_INPUT: 8 ECU + SG_ torque_setpoint : 48|16@1- (1,0) [0|0] "" Vector__XXX + SG_ negative_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX + SG_ positive_torque_limit : 16|16@1- (0.01,0) [0|0] "Mn" Vector__XXX + SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" 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_ 152 INV2_CONTROL_INPUT: 8 ECU + SG_ torque_setpoint : 48|16@1- (1,0) [0|0] "" Vector__XXX + SG_ negative_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX + SG_ positive_torque_limit : 16|16@1- (0.01,0) [0|0] "Mn" Vector__XXX + SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" 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_ 153 INV3_CONTROL_INPUT: 8 ECU + SG_ torque_setpoint : 48|16@1- (1,0) [0|0] "" Vector__XXX + SG_ negative_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX + SG_ positive_torque_limit : 16|16@1- (0.01,0) [0|0] "Mn" Vector__XXX + SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" Vector__XXX -BO_ 225 VN_STATUS: 8 ECU - SG_ vn_gps_status : 0|16@1+ (1,0) [0|0] "" Vector__XXX +BO_ 258 INV4_CONTROL_INPUT: 8 ECU + SG_ torque_setpoint : 48|16@1- (1,0) [0|0] "" Vector__XXX + SG_ negative_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX + SG_ positive_torque_limit : 16|16@1- (0.01,0) [0|0] "Mn" Vector__XXX + SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" 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_ 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_ 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_ 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_ 4 Velocities: 8 ECU - SG_ Velocity_z_CGCorrected : 53|9@1- (0.025,0) [-10.24|10.22] "m/s" Vector__XXX - SG_ Velocity_y_CGCorrected : 43|10@1- (0.03,0) [-15.36|15.33] "m/s" Vector__XXX - SG_ Velocity_x_CGCorrected : 31|12@1- (0.02,0) [-40.96|40.94] "m/s" Vector__XXX - SG_ Velocity_z : 22|9@1- (0.025,0) [-6.4|6.375] "m/s" Vector__XXX - SG_ Velocity_y : 12|10@1- (0.03,0) [-15.36|15.33] "m/s" Vector__XXX - SG_ Velocity_x : 0|12@1- (0.02,0) [-40.96|40.94] "m/s" 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_ 0 brake_temps: 8 ECU +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_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; -BO_TX_BU_ 216 : ECU,Peripherals; -BO_TX_BU_ 214 : ECU,Peripherals; -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_ 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_ 222 : ECU,Peripherals; +BO_TX_BU_ 226 : ECU,Peripherals; +BO_TX_BU_ 218 : ECU,Peripherals; +BO_TX_BU_ 216 : ECU,Peripherals; +BO_TX_BU_ 214 : ECU,Peripherals; +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_ 221 : ECU,Peripherals; +BO_TX_BU_ 2550588916 : ECU,Peripherals; +BO_TX_BU_ 2566869221 : 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_ 203 : 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_ 1060 : ECU,Peripherals; BO_TX_BU_ 1061 : ECU,Peripherals; BO_TX_BU_ 1062 : ECU,Peripherals; @@ -1232,17 +1254,6 @@ 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; @@ -1255,46 +1266,111 @@ 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_ 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_ 228 : 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_ 1963 : 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_ 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_ 234 : 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_ 303 : ECU,Peripherals; +BO_TX_BU_ 516 : ECU,Peripherals; +BO_TX_BU_ 241 : ECU,Peripherals; +BO_TX_BU_ 242 : ECU,Peripherals; BO_TX_BU_ 0 : ECU,Peripherals; +BO_TX_BU_ 112 : ECU,Peripherals; +BO_TX_BU_ 113 : ECU,Peripherals; +BO_TX_BU_ 114 : ECU,Peripherals; +BO_TX_BU_ 115 : ECU,Peripherals; +BO_TX_BU_ 116 : ECU,Peripherals; +BO_TX_BU_ 117 : ECU,Peripherals; +BO_TX_BU_ 118 : ECU,Peripherals; +BO_TX_BU_ 119 : ECU,Peripherals; +BO_TX_BU_ 120 : ECU,Peripherals; +BO_TX_BU_ 121 : ECU,Peripherals; +BO_TX_BU_ 128 : ECU,Peripherals; +BO_TX_BU_ 129 : ECU,Peripherals; +BO_TX_BU_ 130 : ECU,Peripherals; +BO_TX_BU_ 131 : ECU,Peripherals; +BO_TX_BU_ 132 : ECU,Peripherals; +BO_TX_BU_ 133 : ECU,Peripherals; +BO_TX_BU_ 134 : ECU,Peripherals; +BO_TX_BU_ 135 : ECU,Peripherals; +BO_TX_BU_ 136 : ECU,Peripherals; +BO_TX_BU_ 137 : ECU,Peripherals; +BO_TX_BU_ 192 : ECU,Peripherals; +BO_TX_BU_ 237 : ECU,Peripherals; +BO_TX_BU_ 144 : ECU,Peripherals; +BO_TX_BU_ 145 : ECU,Peripherals; +BO_TX_BU_ 146 : ECU,Peripherals; +BO_TX_BU_ 147 : ECU,Peripherals; +BO_TX_BU_ 151 : ECU,Peripherals; +BO_TX_BU_ 152 : ECU,Peripherals; +BO_TX_BU_ 153 : ECU,Peripherals; +BO_TX_BU_ 258 : ECU,Peripherals; +BO_TX_BU_ 259 : ECU,Peripherals; +BO_TX_BU_ 260 : ECU,Peripherals; +BO_TX_BU_ 261 : ECU,Peripherals; +BO_TX_BU_ 262 : 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."; @@ -1303,13 +1379,6 @@ 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."; @@ -1319,31 +1388,42 @@ 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_ BO_ 257 "this message will have states internal to the MCU code"; +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_ 195 torque_mode "torque mode"; CM_ SG_ 195 drive_mode "The current drive mode on the ECU irrespective of dial mapping"; -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_ 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_ 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"; @@ -1351,16 +1431,36 @@ 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_ 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"; +CM_ SG_ 201 front_regen_scale "the front/rear torque scaling"; +CM_ SG_ 201 rear_torque_scale "the front/rear torque scaling"; +CM_ SG_ 201 front_torque_scale "the front/rear torque scaling"; +CM_ SG_ 201 torque_request "The torque request calculated from the accelleration request and the maximum torque"; +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_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"; @@ -1370,24 +1470,6 @@ 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_ 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"; -CM_ SG_ 201 front_regen_scale "the front/rear torque scaling"; -CM_ SG_ 201 rear_torque_scale "the front/rear torque scaling"; -CM_ SG_ 201 front_torque_scale "the front/rear torque scaling"; -CM_ SG_ 201 torque_request "The torque request calculated from the accelleration request and the maximum torque"; -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_ 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"; @@ -1396,45 +1478,26 @@ 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"; +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_ 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"; -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_ 236 dial_state 0 "MODE_0" 1 "MODE_1" 2 "MODE_2" 3 "MODE_3" 4 "MODE_4" 5 "MODE_5" ; +VAL_ 235 dial_state 0 "MODE_0" 1 "MODE_1" 2 "MODE_2" 3 "MODE_3" 4 "MODE_4" 5 "MODE_5" ; 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" 3 "ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS" 4 "ERROR_CONTROLLER_NULL_POINTER" ; SIG_VALTYPE_ 1055 steering_digital_raw : 1; + diff --git a/drivebrain_app/debug_main.cpp b/drivebrain_app/debug_main.cpp index b76e362..8bf7328 100644 --- a/drivebrain_app/debug_main.cpp +++ b/drivebrain_app/debug_main.cpp @@ -41,6 +41,8 @@ #include +#include "arg_parse.hpp" + // TODO first application will have // - [x] message queue that can send messages between the CAN driver and the controller @@ -48,49 +50,18 @@ // - [ ] fix the CAN messages that cant currently be encoded into the protobuf messages // - [x] simple controller -std::atomic 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 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(¶m_path), "Path to the parameter JSON file") - ("dbc-path,d", po::value(&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" < _dbc_path; boost::asio::io_context _io_context; + boost::asio::io_context _io_context_secondary_can; core::common::ThreadSafeDeque> _rx_queue; - core::common::ThreadSafeDeque> _can_tx_queue; + core::common::ThreadSafeDeque> _primary_can_tx_queue; + core::common::ThreadSafeDeque> _secondary_can_tx_queue; core::common::ThreadSafeDeque> _eth_tx_queue; core::common::ThreadSafeDeque> _live_telem_queue; @@ -72,14 +75,16 @@ class DriveBrainApp { // std::unique_ptr _matlab_math; std::shared_ptr _foxglove_server; std::shared_ptr>> _message_logger = nullptr; - std::unique_ptr _state_estimator; - std::shared_ptr _driver; + std::shared_ptr _state_estimator; + std::shared_ptr _driver_primary_can; + std::shared_ptr _driver_secondary_can; std::unique_ptr _eth_driver; std::shared_ptr _vn_driver; std::unique_ptr _db_service; std::thread _process_thread; std::thread _io_context_thread; + std::thread _io_context_secondary_thread; std::thread _db_service_thread; const DriveBrainSettings _settings; diff --git a/drivebrain_app/include/arg_parse.hpp b/drivebrain_app/include/arg_parse.hpp new file mode 100644 index 0000000..05bd7bb --- /dev/null +++ b/drivebrain_app/include/arg_parse.hpp @@ -0,0 +1,14 @@ +#ifndef __ARG_PARSE_H__ +#define __ARG_PARSE_H__ + +#include + +#include +#include +#include + + +std::tuple 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 34b8ed8..0947640 100644 --- a/drivebrain_app/main.cpp +++ b/drivebrain_app/main.cpp @@ -39,7 +39,7 @@ #include #include - +#include "arg_parse.hpp" // TODO first application will have // - [x] message queue that can send messages between the CAN driver and the controller @@ -47,51 +47,19 @@ // - [ ] fix the CAN messages that cant currently be encoded into the protobuf messages // - [x] simple controller -std::atomic 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 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(¶m_path), "Path to the parameter JSON file") - ("dbc-path,d", po::value(&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 = true, + .use_secondary_can = use_secondary_can }; std::cout <<"creating app" < #include -std::atomic DriveBrainApp::_stop_signal{false}; +std::atomic stop_signal{false}; DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& dbc_path, const DriveBrainSettings& settings) : _param_path(param_path) @@ -47,17 +47,27 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d spdlog::info("made state estimator"); bool construction_failed = false; // this also calls init() in the constructor - _driver = std::make_shared( - _config, _logger, _message_logger,_can_tx_queue, _io_context, - _dbc_path, construction_failed, *_state_estimator); + + _driver_primary_can = std::make_shared( + _config, _logger, _message_logger, _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(std::static_pointer_cast(_driver)); + + _driver_secondary_can = std::make_shared( + _config, _logger, _message_logger, _secondary_can_tx_queue, _io_context_secondary_can, + _dbc_path, construction_failed, _state_estimator, "CANDriverSecondary"); + + if (construction_failed) { + throw std::runtime_error("Failed to construct CAN driver"); + } + configurable_components.push_back(std::static_pointer_cast(_driver_primary_can)); + configurable_components.push_back(std::static_pointer_cast(_driver_secondary_can)); spdlog::info("made CAN driver"); _eth_driver = std::make_unique( - _logger, _eth_tx_queue, _message_logger, *_state_estimator, + _logger, _eth_tx_queue, _message_logger, _state_estimator, _io_context, "192.168.1.30", 2001, 2000); spdlog::info("eth driver"); @@ -71,7 +81,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if(_settings.use_vectornav) { // on creation calls init() - _vn_driver = std::make_shared(_config, _logger, _message_logger, *_state_estimator, _io_context, construction_failed); + _vn_driver = std::make_shared(_config, _logger, _message_logger, _state_estimator, _io_context, construction_failed); if (construction_failed) { throw std::runtime_error("Failed to construct VN driver"); } @@ -106,10 +116,15 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d std::bind(&common::DrivebrainMCAPLogger::init_param_schema, _mcap_logger), std::bind(&common::DrivebrainMCAPLogger::log_params, _mcap_logger)); - if(_driver) + if(_driver_primary_can) + { + _driver_primary_can->update_msg_logger(_message_logger); + } + if(_driver_secondary_can) { - _driver->update_msg_logger(_message_logger); + _driver_secondary_can->update_msg_logger(_message_logger); } + if(_vn_driver) { _vn_driver->update_msg_logger(_message_logger); @@ -133,17 +148,23 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d } DriveBrainApp::~DriveBrainApp() { - _stop_signal.store(true); + stop_signal.store(true); if (_process_thread.joinable()) { _process_thread.join(); } spdlog::info("joined main process"); + _message_logger->halt(); _io_context.stop(); if (_io_context_thread.joinable()) { _io_context_thread.join(); } + + _io_context_secondary_can.stop(); + if (_io_context_secondary_thread.joinable()) { + _io_context_secondary_thread.join(); + } if (_db_service) { _db_service->stop_server(); @@ -158,12 +179,11 @@ void DriveBrainApp::_process_loop() { auto desired_rpm_msg = std::make_shared(); auto torque_limit_msg = std::make_shared(); auto desired_torque_msg = std::make_shared(); - // auto loop_time = _controllerManager.get_active_controller_timestep(); auto loop_time = 0.005; 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()) { auto start_time = std::chrono::high_resolution_clock::now(); auto state_and_validity = _state_estimator->get_latest_state_and_validity(); @@ -191,10 +211,10 @@ void DriveBrainApp::_process_loop() { torque_limit_msg->set_drivebrain_torque_rr(::abs(speedControl->torque_lim_nm.RR)); _message_logger->log_msg(static_cast>(torque_limit_msg)); { - 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); - _can_tx_queue.cv.notify_all(); // notify the CAN thread to send the messages + 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"); } @@ -206,9 +226,9 @@ void DriveBrainApp::_process_loop() { desired_torque_msg->set_drivebrain_torque_rr(::abs(torqueControl->desired_torques_nm.RR)); _message_logger->log_msg(static_cast>(desired_torque_msg)); { - std::unique_lock lk(_can_tx_queue.mtx); - _can_tx_queue.deque.push_back(desired_torque_msg); // use new protobuf struct - _can_tx_queue.cv.notify_all(); // notify the CAN thread to send the messages + 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 } } @@ -222,10 +242,10 @@ void DriveBrainApp::_process_loop() { } -std::atomic 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 } @@ -256,6 +276,16 @@ 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()); + } + }); + _process_thread = std::thread([this]() { if (!_settings.run_process_loop) return; _process_loop(); diff --git a/drivebrain_app/src/arg_parse.cpp b/drivebrain_app/src/arg_parse.cpp new file mode 100644 index 0000000..f633a65 --- /dev/null +++ b/drivebrain_app/src/arg_parse.cpp @@ -0,0 +1,30 @@ +#include "arg_parse.hpp" + +#include + +std::tuple 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(¶m_path), "Path to the parameter JSON file") + ("dbc-path,d", po::value(&dbc_path), "Path to the DBC file (optional)") + ("2-can,c", po::value(&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_comms/include/CANComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp index 4d0c117..00393b7 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp @@ -66,7 +66,7 @@ namespace comms /// @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 message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, core::StateEstimator &state_estimator) : + CANDriver(core::JsonFileHandler &json_file_handler, core::Logger& logger, std::shared_ptr message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator) : Configurable(json_file_handler, "CANDriver"), _logger(logger), _message_logger(message_logger), @@ -79,6 +79,20 @@ namespace comms _output_thread = std::thread(&comms::CANDriver::_handle_send_msg_from_queue, this); construction_failed = !init(); } + + CANDriver(core::JsonFileHandler &json_file_handler, core::Logger& logger, std::shared_ptr message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator, std::string driver_name) : + Configurable(json_file_handler, driver_name), + _logger(logger), + _message_logger(message_logger), + _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(); bool init(); @@ -132,6 +146,6 @@ namespace comms std::unordered_map _messages_names_and_ids; int _CAN_socket; // can socket bound to bool _running = false; - core::StateEstimator & _state_estimator; + std::shared_ptr _state_estimator; }; } diff --git a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp index bb643e9..9a28645 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp @@ -34,7 +34,7 @@ namespace comms MCUETHComms(core::Logger &logger, deqtype &in_deq, std::shared_ptr message_logger, - core::StateEstimator &state_estimator, + std::shared_ptr state_estimator, boost::asio::io_context &io_context, const std::string &send_ip, uint16_t recv_port, @@ -54,7 +54,7 @@ namespace comms private: core::Logger &_logger; std::shared_ptr _message_logger; - core::StateEstimator &_state_estimator; + std::shared_ptr _state_estimator; std::array _recv_buffer; std::array _send_buffer; diff --git a/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp index 92ba3a6..0d3d9c2 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp @@ -41,7 +41,7 @@ namespace comms { class VNDriver : public core::common::Configurable { public: - VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, ::core::StateEstimator &state_estimator, boost::asio::io_context &io_context, bool &init_successful); + VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, std::shared_ptr state_estimator, boost::asio::io_context &io_context, bool &init_successful); bool init(); struct config { int baud_rate; @@ -49,9 +49,7 @@ namespace comms { }; private: - // Private variables - core::Logger& _logger; - core::StateEstimator &_state_estimator; + std::shared_ptr _state_estimator; vn::protocol::uart::PacketFinder _processor; @@ -59,7 +57,7 @@ namespace comms { boost::array _input_buff; SerialPort _serial; std::shared_ptr _message_logger; - config _config; + config _config; public: // Public methods diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index 241c5cc..e1b07a1 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -112,7 +112,6 @@ void comms::CANDriver::_do_read() { } void comms::CANDriver::_send_message(const struct can_frame &frame) { - std:: cout << "Sending CAN message with ID: {} and length: {}" << frame.can_id << frame.len; boost::asio::async_write( _socket, boost::asio::buffer(&frame, sizeof(frame)), [this](boost::system::error_code ec, std::size_t /*bytes_transferred*/) { @@ -125,7 +124,7 @@ 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); + _state_estimator->handle_recv_process(msg); if(_message_logger) // this may not exist yet as this gets constr { _message_logger->log_msg(msg); diff --git a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp index 395e5e8..8e8c6b9 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp @@ -1,4 +1,5 @@ #include +#include "StateEstimator.hpp" #include "hytech_msgs.pb.h" #include @@ -9,7 +10,7 @@ namespace comms MCUETHComms::MCUETHComms(core::Logger &logger, deqtype &in_deq, std::shared_ptr message_logger, - core::StateEstimator &state_estimator, + std::shared_ptr state_estimator, boost::asio::io_context &io_context, const std::string &send_ip, uint16_t recv_port, @@ -86,7 +87,7 @@ namespace comms { _mcu_msg->ParseFromArray(_recv_buffer.data(), size); auto out_msg = static_cast>(_mcu_msg); - _state_estimator.handle_recv_process(out_msg); + _state_estimator->handle_recv_process(out_msg); if(_message_logger) { _message_logger->log_msg(out_msg); diff --git a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp index 3de9923..dc34c4f 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("device_name"); _config.baud_rate = get_parameter_value("baud_rate").value(); _config.freq_divisor = get_parameter_value("freq_divisor").value(); @@ -31,12 +30,12 @@ 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); + spdlog::info("failed to open vectornav serial port"); return false; } @@ -48,18 +47,15 @@ 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(); - // _configured = true; set_configured(); return true; } - VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, core::StateEstimator &state_estimator, boost::asio::io_context& io, bool &init_not_successful) + VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, std::shared_ptr state_estimator, boost::asio::io_context& io, bool &init_not_successful) : core::common::Configurable(json_file_handler, "VNDriver"), - _logger(logger), _state_estimator(state_estimator), _message_logger(message_logger), _serial(io) @@ -69,8 +65,7 @@ namespace comms // Starts read if(!init_not_successful) { - _logger.log_string("Starting vn driver recieve.", core::LogLevel::INFO); - + spdlog::info("Starting vn driver recieve."); _start_recieve(); } @@ -78,7 +73,7 @@ namespace comms void VNDriver::log_proto_message(std::shared_ptr msg) { - _state_estimator.handle_recv_process(static_cast>(msg)); + _state_estimator->handle_recv_process(static_cast>(msg)); if(_message_logger) { _message_logger->log_msg(static_cast>(msg)); diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp index 598ed64..043d0e1 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp @@ -124,11 +124,8 @@ namespace common { std::unique_lock lk(_logger_mtx); msg_to_log.channelId = _msg_name_id_map[msg.message_name]; - // spdlog::info("logging at channel: {}", msg_to_log.channelId); auto write_res = _writer.write(msg_to_log); - // spdlog::info("Logging message: {}", msg.message_name); - // spdlog::info("message size: {}", msg_to_log.dataSize); } } @@ -217,7 +214,7 @@ namespace common { std::unordered_map params_map = cc->get_all_params_map(); std::string param_parent = cc->get_name(); - std::cout << param_parent << std::endl; + // std::cout << param_parent << std::endl; std::vector param_names = cc->get_param_names(); for (auto i = params_map.begin(); i != params_map.end(); i++) { @@ -227,7 +224,7 @@ namespace common _get_params_as_json(param_parent, name, var_val, params_all); } } - std::cout << params_all.dump() < Date: Sat, 12 Apr 2025 07:21:46 -0400 Subject: [PATCH 39/87] ok --- drivebrain_app/main.cpp | 2 +- .../drivebrain_logging/src/DrivebrainMCAPLogger.cpp | 5 ----- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/drivebrain_app/main.cpp b/drivebrain_app/main.cpp index 0947640..94224ba 100644 --- a/drivebrain_app/main.cpp +++ b/drivebrain_app/main.cpp @@ -58,7 +58,7 @@ int main(int argc, char *argv[]) .run_db_service = true, .run_io_context = true, .run_process_loop = true, - .use_vectornav = true, + .use_vectornav = false, .use_secondary_can = use_secondary_can }; diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp index 043d0e1..04031b0 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp @@ -204,8 +204,6 @@ namespace common _input_deque.cv.notify_all(); } } - - } nlohmann::json DrivebrainMCAPLogger::_get_param_vals() { @@ -224,7 +222,6 @@ namespace common _get_params_as_json(param_parent, name, var_val, params_all); } } - // std::cout << params_all.dump() < Date: Sun, 20 Apr 2025 18:29:41 -0400 Subject: [PATCH 40/87] integrating acu eth comms --- CMakeLists.txt | 1 + drivebrain_app/include/DriveBrainApp.hpp | 2 + drivebrain_app/src/DriveBrainApp.cpp | 11 ++-- .../drivebrain_comms/include/ACUETHComms.hpp | 60 +++++++++++++++++++ .../drivebrain_comms/src/ACUETHComms.cpp | 53 ++++++++++++++++ flake.lock | 16 ++--- flake.nix | 4 +- 7 files changed, 132 insertions(+), 15 deletions(-) create mode 100644 drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp create mode 100644 drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 398e236..067a7ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,7 @@ add_library(drivebrain_comms SHARED 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/ACUETHComms.cpp drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp ) diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index eb1df35..2c3efaf 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -79,6 +80,7 @@ class DriveBrainApp { std::shared_ptr _driver_primary_can; std::shared_ptr _driver_secondary_can; std::unique_ptr _eth_driver; + std::unique_ptr _acu_eth_driver; std::shared_ptr _vn_driver; std::unique_ptr _db_service; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 200a9bd..5fdaff0 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -66,9 +66,10 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d configurable_components.push_back(std::static_pointer_cast(_driver_primary_can)); configurable_components.push_back(std::static_pointer_cast(_driver_secondary_can)); spdlog::info("made CAN driver"); - _eth_driver = std::make_unique( - _logger, _eth_tx_queue, _message_logger, _state_estimator, - _io_context, "192.168.1.30", 2001, 2000); + _acu_eth_driver = std::make_unique( + _logger, _message_logger, + _io_context, 7766 + ); spdlog::info("eth driver"); @@ -138,9 +139,9 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { _db_service->update_msg_logger(_message_logger); } - if(_eth_driver) + if(_acu_eth_driver) { - _eth_driver->update_msg_logger(_message_logger); + _acu_eth_driver->update_msg_logger(_message_logger); } spdlog::info("constructed app"); diff --git a/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp new file mode 100644 index 0000000..3365aab --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp @@ -0,0 +1,60 @@ +#ifndef __ACUCOMMS_H__ +#define __ACUCOMMS_H__ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include "hytech_msgs.pb.h" +#include + +// - [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 ACUETHComms + { + public: + using loggertype = core::MsgLogger>; + ACUETHComms() = delete; + ~ACUETHComms(); + ACUETHComms(core::Logger &logger, + std::shared_ptr message_logger, + boost::asio::io_context &io_context, + uint16_t recv_port); + void update_msg_logger(std::shared_ptr message_logger) { + _message_logger = message_logger; + } + + private: + void _handle_receive(const boost::system::error_code &error, std::size_t size); + void _start_receive(); + void _handle_send(std::array /*message*/, + const boost::system::error_code & /*error*/, + std::size_t /*bytes_transferred*/); + private: + core::Logger &_logger; + std::shared_ptr _message_logger; + std::array _recv_buffer; + boost::asio::ip::udp::socket _socket; + boost::asio::ip::udp::endpoint _remote_endpoint; + std::shared_ptr _acu_msg; + bool _running = false; + std::thread _output_thread; + }; + +} + +#endif // __ACUCOMMS_H__ \ No newline at end of file diff --git a/drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp new file mode 100644 index 0000000..0198c7f --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp @@ -0,0 +1,53 @@ +#include +#include "hytech_msgs.pb.h" +#include + + +using boost::asio::ip::udp; +namespace comms +{ + ACUETHComms::ACUETHComms(core::Logger &logger, + std::shared_ptr message_logger, + boost::asio::io_context &io_context, + uint16_t recv_port) : _logger(logger), + _message_logger(message_logger), + _socket(io_context, udp::endpoint(udp::v4(), recv_port)) + { + _acu_msg = std::make_shared(); + _start_receive(); + } + + ACUETHComms::~ACUETHComms() + { + _running = false; + spdlog::warn("Destructed ACU ETH COMMS"); + } + + void ACUETHComms::_handle_receive(const boost::system::error_code &error, std::size_t size) + { + + if (!error) + { + _acu_msg->ParseFromArray(_recv_buffer.data(), size); + auto out_msg = static_cast>(_acu_msg); + if (_message_logger) + { + _message_logger->log_msg(out_msg); + } else { + spdlog::warn("Message logger not real"); + } + + _start_receive(); + } + } + + void ACUETHComms::_start_receive() + { + using namespace boost::placeholders; + _socket.async_receive_from( + boost::asio::buffer(_recv_buffer), _remote_endpoint, + boost::bind(&ACUETHComms::_handle_receive, this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); + } +} \ No newline at end of file diff --git a/flake.lock b/flake.lock index 490cdf8..fb611ed 100644 --- a/flake.lock +++ b/flake.lock @@ -3,16 +3,16 @@ "HT_proto": { "flake": false, "locked": { - "lastModified": 1742359623, - "narHash": "sha256-Lt+eedvEagB9fOCNGQiDFn6jIjPje0zzY9xF4QJEE9w=", + "lastModified": 1744328254, + "narHash": "sha256-N4JJyjXAkPn8B2V7opNnhuvJZ8CD/V/g0FD/sCZcl/c=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "11ba10f971a5beb52044c0b5d07c81867f7ad20b", + "rev": "22d4c819aedd716d924275ff89be61b3644face7", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "TCMUX", + "ref": "22d4c819aedd716d924275ff89be61b3644face7", "repo": "HT_proto", "type": "github" } @@ -340,16 +340,16 @@ "utils": "utils_2" }, "locked": { - "lastModified": 1742359175, - "narHash": "sha256-Xf12l65WGnoS74G4eCw52eN8gC525urAU31w8hXwYwg=", + "lastModified": 1744371175, + "narHash": "sha256-wnar4mIdioNP2wKbF4hGNBT6uGpp2WL0UxHO50ZbDDc=", "owner": "hytech-racing", "repo": "ht_can", - "rev": "553a9f088139c7b026a817ccca10c0f92d785dcd", + "rev": "e60a00940dad34dcf396fcc8c850913331775509", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "TCMUX", + "ref": "156", "repo": "ht_can", "type": "github" } diff --git a/flake.nix b/flake.nix index 3c94c2c..67a72bd 100644 --- a/flake.nix +++ b/flake.nix @@ -21,7 +21,7 @@ type = "github"; owner = "hytech-racing"; repo = "HT_proto"; - ref = "TCMUX"; + ref = "22d4c819aedd716d924275ff89be61b3644face7"; flake = false; }; @@ -30,7 +30,7 @@ flake = false; }; - ht_can.url = "github:hytech-racing/ht_can/TCMUX"; + ht_can.url = "github:hytech-racing/ht_can/156"; ht_can.inputs.nixpkgs.follows = "nixpkgs"; ht_can.inputs.nix-proto.follows = "nix-proto"; From dd606cec02ad86a670e3fedb431724054cb261fa Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 20 Apr 2025 22:02:54 -0400 Subject: [PATCH 41/87] integrating ability to turn off vectornav --- config/drivebrain_config.json | 3 ++- config/test_tcmux_integration.json | 2 +- drivebrain_app/src/DriveBrainApp.cpp | 5 ++++- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 8d5c8ca..ac13278 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -33,5 +33,6 @@ "max_switch_requested_rpm": 20000.0, "max_torque_switch_nm": 10.0, "max_accel_switch_float": 0.1 - } + }, + "use_vectornav": false } diff --git a/config/test_tcmux_integration.json b/config/test_tcmux_integration.json index f3cdd5d..92e91d1 100644 --- a/config/test_tcmux_integration.json +++ b/config/test_tcmux_integration.json @@ -1,4 +1,4 @@ -{ +g{ "ControllerManager": { "max_controller_switch_speed_ms": 1.793, "max_switch_requested_rpm": 20000.0, diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 5fdaff0..0938300 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -79,8 +79,11 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d }; _db_service = std::make_unique(_message_logger, switch_modes); spdlog::info("made db service"); - if(_settings.use_vectornav) + + 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(_config, _logger, _message_logger, _state_estimator, _io_context, construction_failed); if (construction_failed) { From d39faaf06c0c56a40b472494b7b8911218582217 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 28 Apr 2025 21:56:03 -0400 Subject: [PATCH 42/87] yo --- CMakeLists.txt | 2 + config/drivebrain_config.json | 19 ++ drivebrain_app/src/DriveBrainApp.cpp | 15 +- .../include/JSONUtils.hpp | 13 +- .../drivebrain_estimation/include/HTMath.hpp | 22 ++ .../include/StateEstimator.hpp | 35 ++- .../src/StateEstimator.cpp | 285 +++++++++++------- flake.lock | 14 +- flake.nix | 2 +- 9 files changed, 271 insertions(+), 136 deletions(-) create mode 100644 drivebrain_core_impl/drivebrain_estimation/include/HTMath.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 067a7ee..0bcdef5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -163,6 +163,8 @@ target_include_directories(drivebrain_estimation PUBLIC $ ) +target_link_libraries(drivebrain_estimation PUBLIC drivebrain_core::drivebrain_core) + make_cmake_package(drivebrain_estimation drivebrain) add_library(drivebrain_app SHARED diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index ac13278..5653051 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -34,5 +34,24 @@ "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 + }, "use_vectornav": false + } diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 0938300..e6c42f1 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -25,13 +25,6 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d std::vector> configurable_components; spdlog::set_level(spdlog::level::info); - // TODO make this function that can get the config schemas from the configureable components. it also needs to join all of the schemas together - - auto get_schema = []() -> nlohmann::json - { - return nlohmann::json(); - }; - controller1 = std::make_shared(_config); if (!controller1->init()) { @@ -43,8 +36,14 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d - _state_estimator = std::make_unique(_logger, _message_logger); + _state_estimator = std::make_unique(_config, _message_logger); + if(!_state_estimator->init()) + { + throw std::runtime_error("Failed to initialize state estimator"); + } + configurable_components.push_back(std::static_pointer_cast(_state_estimator)); spdlog::info("made state estimator"); + bool construction_failed = false; // this also calls init() in the constructor diff --git a/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp b/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp index 83247db..ce293f8 100644 --- a/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp +++ b/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp @@ -1,13 +1,14 @@ #ifndef __JSONUTILS_H__ #define __JSONUTILS_H__ -#include - #include -namespace util -{ - -} +#define LOAD_PARAM_OR_FAIL(param_name, param_type, config_struct) \ + { \ + auto opt = get_parameter_value(#param_name); \ + if (!opt) \ + return false; \ + config_struct.param_name = *opt; \ + } #endif // __JSONUTILS_H__ \ 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 0000000..8867255 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_estimation/include/HTMath.hpp @@ -0,0 +1,22 @@ +#ifndef __HTMATH_H__ +#define __HTMATH_H__ + +#include +namespace math +{ + template + float normalize_linear_scale(T input_value, T min_val, T max_val, float new_min, float new_max) + { + float normalized = (static_cast(input_value) - static_cast(min_val)) / (static_cast(max_val) - static_cast(min_val)); + float scaled = new_min + normalized * (new_max - new_min); + return scaled; + } + template + 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 00e4dbc..f1c14d5 100644 --- a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp +++ b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp @@ -52,15 +52,36 @@ // 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::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; + } _config; using loggertype = core::MsgLogger>; public: using tsq = core::common::ThreadSafeDeque>; - StateEstimator(core::Logger &shared_logger, std::shared_ptr message_logger) - : _logger(shared_logger), _message_logger(message_logger) + StateEstimator(core::JsonFileHandler &json_file_handler, std::shared_ptr message_logger) : + + Configurable(json_file_handler, "StateEstimator") + , _message_logger(message_logger) // _matlab_estimator(matlab_estimator) { _vehicle_state = {}; // initialize to all zeros @@ -76,10 +97,13 @@ namespace core void handle_recv_process(std::shared_ptr message); std::pair get_latest_state_and_validity(); void set_previous_control_output(ControllerOutput prev_control_output); + core::VehicleState append_state_variables_from_raw_inputs(core::VehicleState vs, core::RawInputData raw_inputs); void update_msg_logger(std::shared_ptr message_logger) { _message_logger = message_logger; } + bool init(); + private: void _recv_low_level_state(std::shared_ptr message); @@ -92,10 +116,11 @@ namespace core template bool _validate_stamps(const std::array ×tamp_arr); + std::shared_ptr _set_computed_states(core::VehicleState current_state, std::shared_ptr msg_out); + void _set_float_veh_vec_message_member(veh_vec 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; diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 905f46e..d91bc64 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -2,44 +2,39 @@ #include -#include #include +#include #include #include #include -#include // from HT_proto -#include +#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 // from HT_proto +#include + +#include "JSONUtils.hpp" using namespace core; -void StateEstimator::handle_recv_process(std::shared_ptr message) -{ - if (message->GetTypeName() == "hytech_msgs.VNData") - { +void StateEstimator::handle_recv_process(std::shared_ptr message) { + if (message->GetTypeName() == "hytech_msgs.VNData") { auto in_msg = std::static_pointer_cast(message); - xyz_vec 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 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 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 ypr_rad = { - (in_msg->vn_ypr_rad().yaw()), - (in_msg->vn_ypr_rad().pitch()), - (in_msg->vn_ypr_rad().roll())}; + xyz_vec 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 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 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 ypr_rad = {(in_msg->vn_ypr_rad().yaw()), (in_msg->vn_ypr_rad().pitch()), + (in_msg->vn_ypr_rad().roll())}; { std::unique_lock lk(_state_mutex); @@ -48,79 +43,79 @@ void StateEstimator::handle_recv_process(std::shared_ptr message) -{ +void StateEstimator::_recv_low_level_state(std::shared_ptr message) { if (message->GetTypeName() == "hytech.rear_suspension") { - auto in_msg = std::static_pointer_cast(message); + auto in_msg = std::static_pointer_cast(message); { std::unique_lock lk(_state_mutex); - _timestamp_array[0] = std::chrono::duration_cast(std::chrono::high_resolution_clock::now().time_since_epoch()); + _timestamp_array[0] = std::chrono::duration_cast( + 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(); + _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(message); { std::unique_lock lk(_state_mutex); - _timestamp_array[1] = std::chrono::duration_cast(std::chrono::high_resolution_clock::now().time_since_epoch()); + _timestamp_array[1] = std::chrono::duration_cast( + 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(); + _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(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::high_resolution_clock::now().time_since_epoch()); + _timestamp_array[2] = std::chrono::duration_cast( + 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(message); { std::unique_lock lk(_state_mutex); - _timestamp_array[3] = std::chrono::duration_cast(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(); + _timestamp_array[3] = std::chrono::duration_cast( + 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(message); + // { + // std::unique_lock lk(_state_mutex); + // // TODO + // } + } + + else { _recv_inverter_states(message); } } // INV1_STATUS INV1_TEMPS INV1_DYNAMICS INV1_POWER INV1_FEEDBACK -void StateEstimator::_recv_inverter_states(std::shared_ptr msg) -{ +void StateEstimator::_recv_inverter_states(std::shared_ptr msg) { auto name = msg->GetTypeName(); - if( name == "hytech.inv1_status" ) - { + if (name == "hytech.inv1_status") { - } else if(name == "hytech.inv2_status") - { + } else if (name == "hytech.inv2_status") { - } else if(name == "hytech.inv1_dynamics") - { + } else if (name == "hytech.inv1_dynamics") { _handle_set_inverter_dynamics<0, hytech::inv1_dynamics>(msg); - } else if(name == "hytech.inv2_dynamics") - { + } else if (name == "hytech.inv2_dynamics") { _handle_set_inverter_dynamics<1, hytech::inv2_dynamics>(msg); - } else if(name == "hytech.inv3_dynamics") - { + } else if (name == "hytech.inv3_dynamics") { _handle_set_inverter_dynamics<2, hytech::inv3_dynamics>(msg); - } else if(name == "hytech.inv4_dynamics") - { + } else if (name == "hytech.inv4_dynamics") { _handle_set_inverter_dynamics<3, hytech::inv4_dynamics>(msg); } } @@ -130,16 +125,16 @@ void StateEstimator::_handle_set_inverter_dynamics(std::shared_ptr(msg); { std::unique_lock lk(_state_mutex); - // _raw_input_data.raw_inverter_torques.set_from_index(in_msg->actual_torque_nm()); - // _raw_input_data.raw_inverter_power.set_from_index(in_msg->actual_power_w()); - // _vehicle_state.current_rpms.set_from_index(in_msg->actual_speed_rpm()); + _raw_input_data.raw_inverter_torques.set_from_index(in_msg->actual_torque_nm()); + _raw_input_data.raw_inverter_power.set_from_index(in_msg->actual_power_w()); + _vehicle_state.current_rpms.set_from_index(in_msg->actual_speed_rpm()); } } // TODO parameterize the timeout threshold template -bool StateEstimator::_validate_stamps(const std::array ×tamp_arr) -{ +bool StateEstimator::_validate_stamps( + const std::array ×tamp_arr) { std::array timestamp_array_to_sort; { std::unique_lock lk(_state_mutex); @@ -155,22 +150,42 @@ bool StateEstimator::_validate_stamps(const std::array(std::chrono::high_resolution_clock::now().time_since_epoch()); + + auto curr_time = std::chrono::duration_cast( + 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(curr_time - max_stamp)) < threshold; + bool last_update_recent_enough = + (std::chrono::duration_cast(curr_time - max_stamp)) < threshold; return within_threshold && all_members_received && last_update_recent_enough; } -void StateEstimator::set_previous_control_output(core::ControllerOutput 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); + 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 StateEstimator::_set_ins_state_data(core::VehicleState current_state, std::shared_ptr msg_out) -{ +std::shared_ptr +StateEstimator::_set_ins_state_data(core::VehicleState current_state, + std::shared_ptr 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); @@ -181,7 +196,8 @@ std::shared_ptr 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); @@ -193,22 +209,25 @@ std::shared_ptr StateEstimator::_set_ins_state_data(co return msg_out; } -std::pair StateEstimator::get_latest_state_and_validity() -{ +std::pair 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, current_raw_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 msg_out = std::make_shared(); + std::shared_ptr msg_out = + std::make_shared(); msg_out->set_is_ready_to_drive(true); @@ -217,7 +236,7 @@ std::pair StateEstimator::get_latest_state_and_validit 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); @@ -225,48 +244,96 @@ std::pair StateEstimator::get_latest_state_and_validit curr_rpms->set_rr(current_state.current_rpms.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(); - - if (const core::TorqueControlOut* torqueControl = std::get_if(¤t_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 - prev_driver_torque_req->set_fl(torqueControl->desired_torques_nm.FL); - prev_driver_torque_req->set_fr(torqueControl->desired_torques_nm.FR); - prev_driver_torque_req->set_rl(torqueControl->desired_torques_nm.RL); - prev_driver_torque_req->set_rr(torqueControl->desired_torques_nm.RR); - - } else if (const core::SpeedControlOut* speedControl = std::get_if(¤t_state.prev_controller_output.out)) { // assuming no monostate possible here + msg_out->set_steering_angle_deg(current_state.steering_angle_deg); + auto prev_driver_torque_req = msg_out->mutable_driver_torque(); + + if (const core::TorqueControlOut *torqueControl = + std::get_if(¤t_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( + ¤t_state.prev_controller_output + .out)) { // assuming no monostate possible here msg_out->set_is_using_torque_controller(false); - prev_driver_torque_req->set_fl(speedControl->torque_lim_nm.FL); - prev_driver_torque_req->set_fr(speedControl->torque_lim_nm.FR); - prev_driver_torque_req->set_rl(speedControl->torque_lim_nm.RL); - prev_driver_torque_req->set_rr(speedControl->torque_lim_nm.RR); + _set_float_veh_vec_message_member(speedControl->torque_lim_nm, prev_driver_torque_req); } auto log_start = std::chrono::high_resolution_clock::now(); - if(_message_logger) - { + if (_message_logger) { _message_logger->log_msg(static_cast>(msg_out)); } else { spdlog::warn("message logger not real"); } - + 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(state_estim_end - state_estim_start); - + auto elapsed = + std::chrono::duration_cast(state_estim_end - state_estim_start); + constexpr bool debug = false; - if ( (elapsed > std::chrono::microseconds(6000)) && debug ) // 6ms + if ((elapsed > std::chrono::microseconds(6000)) && debug) // 6ms { std::cout << "WARNING: timing" << std::endl; - std::cout << "total: " << (static_cast(std::chrono::duration_cast(elapsed).count())) << " us\n"; - std::cout << "state mutex: " << (static_cast(std::chrono::duration_cast(state_mutex_end - state_mutex_start).count())) << " us\n"; - std::cout << "log time: " << (static_cast(std::chrono::duration_cast(log_end - log_start).count())) << " us\n"; + std::cout << "total: " + << (static_cast( + std::chrono::duration_cast(elapsed).count())) + << " us\n"; + std::cout << "state mutex: " + << (static_cast(std::chrono::duration_cast( + state_mutex_end - state_mutex_start) + .count())) + << " us\n"; + std::cout << "log time: " + << (static_cast( + std::chrono::duration_cast(log_end - log_start) + .count())) + << " us\n"; } return {current_state, state_is_valid}; +} + +bool StateEstimator::init() { + 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); + set_configured(); + return true; +} + +std::shared_ptr +StateEstimator::_set_computed_states(core::VehicleState current_state, + std::shared_ptr 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 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/flake.lock b/flake.lock index fb611ed..b92b617 100644 --- a/flake.lock +++ b/flake.lock @@ -3,16 +3,16 @@ "HT_proto": { "flake": false, "locked": { - "lastModified": 1744328254, - "narHash": "sha256-N4JJyjXAkPn8B2V7opNnhuvJZ8CD/V/g0FD/sCZcl/c=", + "lastModified": 1745886368, + "narHash": "sha256-d1KpRJ9Ov3mE8zGBLZNkHrFvuoKCsb+Pn3DRovF2VvA=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "22d4c819aedd716d924275ff89be61b3644face7", + "rev": "b63b352f4a4d3d0e9b0cdb90f876ae12e812b4a7", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "22d4c819aedd716d924275ff89be61b3644face7", + "ref": "b63b352f4a4d3d0e9b0cdb90f876ae12e812b4a7", "repo": "HT_proto", "type": "github" } @@ -52,11 +52,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1744269183, - "narHash": "sha256-WR4r7QAh07J2QmN4Kl4x6YG/IZCBATOGLhhmW+FRJ8o=", + "lastModified": 1745886021, + "narHash": "sha256-BKPcAybnaEat8tGx93dXpqEnjTlGy+RM+liL70FSJeY=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "706d6bcc9c0a22a3e1c2c86d1789514bfb4b07cd", + "rev": "bf99f0daf0acdc658ea83a74ae58977fe0096ab6", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index 67a72bd..db96d13 100644 --- a/flake.nix +++ b/flake.nix @@ -21,7 +21,7 @@ type = "github"; owner = "hytech-racing"; repo = "HT_proto"; - ref = "22d4c819aedd716d924275ff89be61b3644face7"; + ref = "b63b352f4a4d3d0e9b0cdb90f876ae12e812b4a7"; flake = false; }; From 05f7b840e540033fca2a092befd622e223505161 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 8 May 2025 23:49:19 -0400 Subject: [PATCH 43/87] working through required updates and debugging --- drivebrain_app/include/DriveBrainApp.hpp | 2 +- drivebrain_app/src/DriveBrainApp.cpp | 53 ++++++-- .../include/ParamLogger.hpp | 23 ---- .../src/ParamLogger.cpp | 13 -- .../drivebrain_comms/include/ACUETHComms.hpp | 10 +- .../drivebrain_comms/include/CANComms.hpp | 1 + .../drivebrain_comms/include/MCUETHComms.hpp | 1 - .../drivebrain_comms/include/VNComms.hpp | 3 + .../include/foxglove_server.hpp | 2 + .../drivebrain_comms/src/ACUETHComms.cpp | 7 +- .../drivebrain_comms/src/CANComms.cpp | 4 +- .../drivebrain_comms/src/MCUETHComms.cpp | 1 + .../src/SimpleSpeedController.cpp | 5 +- .../include/StateEstimator.hpp | 2 +- .../src/StateEstimator.cpp | 4 + .../src/DrivebrainMCAPLogger.cpp | 2 + flake.lock | 123 +++++++++++------- flake.nix | 17 +-- 18 files changed, 155 insertions(+), 118 deletions(-) delete mode 100644 drivebrain_core_impl/drivebrain_common_utils/include/ParamLogger.hpp delete mode 100644 drivebrain_core_impl/drivebrain_common_utils/src/ParamLogger.cpp diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 2c3efaf..5200b89 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -79,7 +79,7 @@ class DriveBrainApp { std::shared_ptr _state_estimator; std::shared_ptr _driver_primary_can; std::shared_ptr _driver_secondary_can; - std::unique_ptr _eth_driver; + // std::unique_ptr _eth_driver; std::unique_ptr _acu_eth_driver; std::shared_ptr _vn_driver; std::unique_ptr _db_service; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index e6c42f1..a8977bb 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -1,11 +1,13 @@ // DriveBrainApp.cpp #include "DriveBrainApp.hpp" +#include "ACUETHComms.hpp" #include "SimpleSpeedController.hpp" #include "SimpleTorqueController.hpp" #include "hytech.pb.h" #include #include +#include #include #include @@ -23,7 +25,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { // spdlog::info("top o"); std::vector> configurable_components; - spdlog::set_level(spdlog::level::info); + spdlog::set_level(spdlog::level::debug); controller1 = std::make_shared(_config); @@ -65,10 +67,10 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d configurable_components.push_back(std::static_pointer_cast(_driver_primary_can)); configurable_components.push_back(std::static_pointer_cast(_driver_secondary_can)); spdlog::info("made CAN driver"); + comms::ETHCommPorts ports = {7766, 5555, 4444}; _acu_eth_driver = std::make_unique( _logger, _message_logger, - _io_context, 7766 - ); + _io_context, ports); spdlog::info("eth driver"); @@ -109,7 +111,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d 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 + // // functions of the configurable components _message_logger = std::make_shared>>( ".mcap", true, std::bind(&common::DrivebrainMCAPLogger::log_msg, _mcap_logger, std::placeholders::_1), @@ -153,29 +155,41 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d 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("joined main process"); - - _message_logger->halt(); + + + 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 (_db_service) { _db_service->stop_server(); } if ( _db_service_thread.joinable()) { _db_service_thread.join(); + spdlog::info("joined db service"); } - spdlog::info("joined io context"); + spdlog::info("destructed db app"); } void DriveBrainApp::_process_loop() { @@ -187,6 +201,7 @@ void DriveBrainApp::_process_loop() { std::chrono::microseconds loop_chrono_time(loop_time_micros); 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(); @@ -206,13 +221,21 @@ void DriveBrainApp::_process_loop() { 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); - _message_logger->log_msg(static_cast>(desired_rpm_msg)); + if(_message_logger) + { + _message_logger->log_msg(static_cast>(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)); - _message_logger->log_msg(static_cast>(torque_limit_msg)); + if(_message_logger) + { + _message_logger->log_msg(static_cast>(torque_limit_msg)); + } + { std::unique_lock lk(_primary_can_tx_queue.mtx); _primary_can_tx_queue.deque.push_back(desired_rpm_msg); @@ -227,7 +250,11 @@ void DriveBrainApp::_process_loop() { desired_torque_msg->set_drivebrain_torque_fr(::abs(torqueControl->desired_torques_nm.FR)); desired_torque_msg->set_drivebrain_torque_rl(::abs(torqueControl->desired_torques_nm.RL)); desired_torque_msg->set_drivebrain_torque_rr(::abs(torqueControl->desired_torques_nm.RR)); - _message_logger->log_msg(static_cast>(desired_torque_msg)); + if(_message_logger) + { + _message_logger->log_msg(static_cast>(desired_torque_msg)); + } + { std::unique_lock lk(_primary_can_tx_queue.mtx); _primary_can_tx_queue.deque.push_back(desired_torque_msg); // use new protobuf struct @@ -262,6 +289,7 @@ void DriveBrainApp::run() { 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) { @@ -296,6 +324,7 @@ void DriveBrainApp::run() { while (!stop_signal.load()) { + spdlog::debug("looping DBAPP run"); std::this_thread::sleep_for(std::chrono::seconds(1)); } } diff --git a/drivebrain_core_impl/drivebrain_common_utils/include/ParamLogger.hpp b/drivebrain_core_impl/drivebrain_common_utils/include/ParamLogger.hpp deleted file mode 100644 index e3063b7..0000000 --- a/drivebrain_core_impl/drivebrain_common_utils/include/ParamLogger.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef __PARAMLOGGER_H__ -#define __PARAMLOGGER_H__ - -#include - -#include -#include -#include -// #include - -// handles the logging of the params from all of the configurable components - -// functions: -// 1. verifies that all components have been configured upon construction -// (NOTE: this boi needs to be constructed only after the components have been initialized for the first time) -// 2. creates the json schema given the map of parameters -// 3. gathers the maps of params contained within the components - -namespace util -{ -std::optional get_schema(std::vector configurable_components); -} -#endif // __PARAMLOGGER_H__ \ No newline at end of file diff --git a/drivebrain_core_impl/drivebrain_common_utils/src/ParamLogger.cpp b/drivebrain_core_impl/drivebrain_common_utils/src/ParamLogger.cpp deleted file mode 100644 index 127bfd5..0000000 --- a/drivebrain_core_impl/drivebrain_common_utils/src/ParamLogger.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "ParamLogger.hpp" - -namespace util -{ - std::optional get_schema(std::vector configurable_components) - { - - for(auto component : configurable_components) - { - - } - } -} \ No newline at end of file diff --git a/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp index 3365aab..0b717e3 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp @@ -10,6 +10,7 @@ #include #include +#include #include #include "hytech_msgs.pb.h" #include @@ -24,16 +25,23 @@ namespace comms { + struct ETHCommPorts + { + uint16_t acu_port; + uint16_t vcr_port; + uint16_t vcf_port; + }; class ACUETHComms { public: + using loggertype = core::MsgLogger>; ACUETHComms() = delete; ~ACUETHComms(); ACUETHComms(core::Logger &logger, std::shared_ptr message_logger, boost::asio::io_context &io_context, - uint16_t recv_port); + ETHCommPorts ports); void update_msg_logger(std::shared_ptr message_logger) { _message_logger = message_logger; } diff --git a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp index 00393b7..028b22e 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp @@ -114,6 +114,7 @@ namespace comms /// @return variant of types FieldVariant get_field_value(std::shared_ptr message, const std::string &field_name); + // for exposing to the test framework directly protected: // socket operations diff --git a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp index 9a28645..c6dcb22 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp @@ -6,7 +6,6 @@ #include #include -#include #include #include diff --git a/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp index 0d3d9c2..2b34b99 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/VNComms.hpp @@ -42,6 +42,9 @@ namespace comms { { public: VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, std::shared_ptr 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; diff --git a/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp b/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp index 90dc30c..16cbd2b 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp @@ -31,7 +31,9 @@ namespace core void send_live_telem_msg(std::shared_ptr msg); ~FoxgloveWSServer() { + spdlog::info("stopping foxglove server"); _server->stop(); + spdlog::info("stopped foxglove server and destructed foxglove server"); } private: diff --git a/drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp index 0198c7f..18fe1bc 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp @@ -6,12 +6,13 @@ using boost::asio::ip::udp; namespace comms { + ACUETHComms::ACUETHComms(core::Logger &logger, std::shared_ptr message_logger, boost::asio::io_context &io_context, - uint16_t recv_port) : _logger(logger), + ETHCommPorts ports) : _logger(logger), _message_logger(message_logger), - _socket(io_context, udp::endpoint(udp::v4(), recv_port)) + _socket(io_context, udp::endpoint(udp::v4(), ports.acu_port)) { _acu_msg = std::make_shared(); _start_receive(); @@ -22,7 +23,7 @@ namespace comms _running = false; spdlog::warn("Destructed ACU ETH COMMS"); } - + void ACUETHComms::_handle_receive(const boost::system::error_code &error, std::size_t size) { diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index e1b07a1..7f4d628 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("canbus_device"); @@ -391,7 +393,7 @@ void comms::CANDriver::_handle_send_msg_from_queue() { while (_running) { { - + spdlog::debug("looping _handle_send_msg_from_queue"); std::unique_lock lk(_input_deque_ref.mtx); _input_deque_ref.cv.wait( diff --git a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp index 8e8c6b9..3af48c8 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp @@ -43,6 +43,7 @@ namespace comms // we will assume that this queue only has messages that we want to send while (_running) { + spdlog::debug("looping _handle_send_msg_from_queue mcu eth"); { std::unique_lock lk(_input_deque_ref.mtx); // TODO unfuck this, queue management shouldnt live within the queue itself diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp index ffd4e89..697a534 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp @@ -118,6 +118,7 @@ core::ControllerOutput control::SimpleSpeedController::step_controller(const cor 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 = _apply_power_limit(speed_out, in.current_rpms); } else { @@ -132,11 +133,9 @@ core::ControllerOutput control::SimpleSpeedController::step_controller(const cor 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 } - - cmd_out.out = _apply_power_limit(speed_out, in.current_rpms); - return cmd_out; } diff --git a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp index f1c14d5..56a0777 100644 --- a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp +++ b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp @@ -92,7 +92,7 @@ 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 message); std::pair get_latest_state_and_validity(); diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index d91bc64..8ee0a7f 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -19,6 +19,10 @@ using namespace core; +StateEstimator::~StateEstimator() +{ + spdlog::info("destructed StateEstimator"); +} void StateEstimator::handle_recv_process(std::shared_ptr message) { if (message->GetTypeName() == "hytech_msgs.VNData") { auto in_msg = std::static_pointer_cast(message); diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp index 04031b0..c170720 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp @@ -37,6 +37,7 @@ namespace common DrivebrainMCAPLogger::~DrivebrainMCAPLogger() { + spdlog::info("started destruction of mcap logger"); { std::unique_lock lk(_input_deque.mtx); _running = false; @@ -101,6 +102,7 @@ namespace common 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; }); diff --git a/flake.lock b/flake.lock index b92b617..9280f5e 100644 --- a/flake.lock +++ b/flake.lock @@ -1,18 +1,20 @@ { "nodes": { "HT_proto": { - "flake": false, + "inputs": { + "flake-utils": "flake-utils", + "nixpkgs": "nixpkgs" + }, "locked": { - "lastModified": 1745886368, - "narHash": "sha256-d1KpRJ9Ov3mE8zGBLZNkHrFvuoKCsb+Pn3DRovF2VvA=", + "lastModified": 1746748475, + "narHash": "sha256-pj26RU9QyY8uJLwmgFPz+HMVWslHpQGHBkR7DhoJHr8=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "b63b352f4a4d3d0e9b0cdb90f876ae12e812b4a7", + "rev": "6d3404aa2bc9d1bde5cc65a3e5efa200b6a2f924", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "b63b352f4a4d3d0e9b0cdb90f876ae12e812b4a7", "repo": "HT_proto", "type": "github" } @@ -52,11 +54,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1745886021, - "narHash": "sha256-BKPcAybnaEat8tGx93dXpqEnjTlGy+RM+liL70FSJeY=", + "lastModified": 1746758396, + "narHash": "sha256-p/LvoVpxNLq14189+LNdMRiagNY/7JaxBMkDZJlpujU=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "bf99f0daf0acdc658ea83a74ae58977fe0096ab6", + "rev": "27b323bc09ab523a21529e72f6d7e4b2b1d77825", "type": "github" }, "original": { @@ -86,7 +88,7 @@ }, "devshell": { "inputs": { - "nixpkgs": "nixpkgs" + "nixpkgs": "nixpkgs_2" }, "locked": { "lastModified": 1722113426, @@ -126,7 +128,7 @@ }, "easy_cmake_2": { "inputs": { - "nixpkgs": "nixpkgs_4", + "nixpkgs": "nixpkgs_5", "utils": "utils_3" }, "locked": { @@ -200,6 +202,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": { @@ -340,16 +360,15 @@ "utils": "utils_2" }, "locked": { - "lastModified": 1744371175, - "narHash": "sha256-wnar4mIdioNP2wKbF4hGNBT6uGpp2WL0UxHO50ZbDDc=", + "lastModified": 1745452764, + "narHash": "sha256-kWgeQEj1HIrIUdGsZQqGqs/k6NgM6lGJsJ1o8atXnnE=", "owner": "hytech-racing", "repo": "ht_can", - "rev": "e60a00940dad34dcf396fcc8c850913331775509", + "rev": "b9f6fa362d0853d94b4f1c50306a4535d071856e", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "156", "repo": "ht_can", "type": "github" } @@ -370,22 +389,6 @@ "type": "github" } }, - "nanopb-proto-api": { - "flake": false, - "locked": { - "lastModified": 1741586908, - "narHash": "sha256-byx2L+x45QNM0EPOnuZjph2p/2K9mYlm1H7f/8pyg10=", - "owner": "nanopb", - "repo": "nanopb", - "rev": "e00d04e2c575c19b7e582c8713be2a42098a8c87", - "type": "github" - }, - "original": { - "owner": "nanopb", - "repo": "nanopb", - "type": "github" - } - }, "nebs-packages": { "inputs": { "commsdsl-src": "commsdsl-src", @@ -455,7 +458,7 @@ "inputs": { "nix-filter": "nix-filter", "nix-std": "nix-std", - "nixpkgs": "nixpkgs_2" + "nixpkgs": "nixpkgs_3" }, "locked": { "lastModified": 1723991236, @@ -527,16 +530,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" } @@ -566,6 +569,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=", @@ -581,7 +600,7 @@ "type": "github" } }, - "nixpkgs_3": { + "nixpkgs_4": { "locked": { "lastModified": 1735563628, "narHash": "sha256-OnSAY7XDSx7CtDoqNh8jwVwh4xNL/2HaJxGjryLWzX8=", @@ -597,7 +616,7 @@ "type": "github" } }, - "nixpkgs_4": { + "nixpkgs_5": { "locked": { "lastModified": 1695825837, "narHash": "sha256-4Ne11kNRnQsmSJCRSSNkFRSnHC4Y5gPDBIQGjjPfJiU=", @@ -613,7 +632,7 @@ "type": "github" } }, - "nixpkgs_5": { + "nixpkgs_6": { "locked": { "lastModified": 1695825837, "narHash": "sha256-4Ne11kNRnQsmSJCRSSNkFRSnHC4Y5gPDBIQGjjPfJiU=", @@ -637,10 +656,9 @@ "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" } }, @@ -720,6 +738,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": [ @@ -743,7 +776,7 @@ }, "utils": { "inputs": { - "systems": "systems" + "systems": "systems_2" }, "locked": { "lastModified": 1694529238, @@ -761,7 +794,7 @@ }, "utils_2": { "inputs": { - "systems": "systems_2" + "systems": "systems_3" }, "locked": { "lastModified": 1726560853, @@ -779,7 +812,7 @@ }, "utils_3": { "inputs": { - "systems": "systems_3" + "systems": "systems_4" }, "locked": { "lastModified": 1694529238, @@ -797,7 +830,7 @@ }, "utils_4": { "inputs": { - "systems": "systems_4" + "systems": "systems_5" }, "locked": { "lastModified": 1694529238, @@ -816,7 +849,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 db96d13..c8caec5 100644 --- a/flake.nix +++ b/flake.nix @@ -16,21 +16,14 @@ nix-proto.url = "github:notalltim/nix-proto"; nix-proto.inputs.nixpkgs.follows = "nixpkgs"; - HT_proto = - { - type = "github"; - owner = "hytech-racing"; - repo = "HT_proto"; - ref = "b63b352f4a4d3d0e9b0cdb90f876ae12e812b4a7"; - flake = false; - }; + HT_proto.url = "github:hytech-racing/HT_proto"; foxglove-schemas-src = { url = "github:foxglove/schemas"; flake = false; }; - ht_can.url = "github:hytech-racing/ht_can/156"; + ht_can.url = "github:hytech-racing/ht_can"; ht_can.inputs.nixpkgs.follows = "nixpkgs"; ht_can.inputs.nix-proto.follows = "nix-proto"; @@ -41,12 +34,8 @@ flake = false; }; - nanopb-proto-api = { - url = "github:nanopb/nanopb"; - 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, ... }@inputs: let nix-proto-foxglove-overlays = nix-proto.generateOverlays' { From 889502e8b37b1a0a045b741e7d4f4e50f1a0fffb Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 9 May 2025 10:32:27 -0400 Subject: [PATCH 44/87] switching to weak pointers for destruction capability for the configureable components --- asdf | 31 ------------- drivebrain_app/src/DriveBrainApp.cpp | 10 ++--- .../drivebrain_comms/include/ACUETHComms.hpp | 1 - .../include/foxglove_server.hpp | 4 +- .../drivebrain_comms/src/foxglove_server.cpp | 44 +++++++++++-------- .../include/DrivebrainMCAPLogger.hpp | 6 +-- .../src/DrivebrainMCAPLogger.cpp | 39 +++++++++------- flake.lock | 6 +-- test/test_configurable.cpp | 7 ++- 9 files changed, 64 insertions(+), 84 deletions(-) delete mode 100644 asdf diff --git a/asdf b/asdf deleted file mode 100644 index f182ea6..0000000 --- a/asdf +++ /dev/null @@ -1,31 +0,0 @@ -{ - "type": "object", - "properties": { - "member_1": { - "type": "object", - "properties": { - "y": { - "type": "number" - }, - "z": { - "type": "number" - } - } - }, - "member_2": { - "type": "object", - "properties": { - "y": { - "type": "number" - }, - "z": { - "type": "number" - } - } - } - }, - "required": [ - "member_1", - "member_2" - ] -} \ No newline at end of file diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index a8977bb..f762605 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -24,7 +24,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d , _controllerManager(_config, {controller1, controller2}) // Initialize correctly { // spdlog::info("top o"); - std::vector> configurable_components; + std::vector> configurable_components; spdlog::set_level(spdlog::level::debug); @@ -32,7 +32,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if (!controller1->init()) { throw std::runtime_error("Failed to initialize controller"); } - configurable_components.push_back(std::static_pointer_cast(controller1)); + configurable_components.push_back(controller1); spdlog::info("made controller"); @@ -43,7 +43,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { throw std::runtime_error("Failed to initialize state estimator"); } - configurable_components.push_back(std::static_pointer_cast(_state_estimator)); + configurable_components.push_back(_state_estimator); spdlog::info("made state estimator"); bool construction_failed = false; @@ -64,8 +64,8 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if (construction_failed) { throw std::runtime_error("Failed to construct CAN driver"); } - configurable_components.push_back(std::static_pointer_cast(_driver_primary_can)); - configurable_components.push_back(std::static_pointer_cast(_driver_secondary_can)); + configurable_components.push_back(_driver_primary_can); + configurable_components.push_back(_driver_secondary_can); spdlog::info("made CAN driver"); comms::ETHCommPorts ports = {7766, 5555, 4444}; _acu_eth_driver = std::make_unique( diff --git a/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp index 0b717e3..6b031af 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp @@ -6,7 +6,6 @@ #include #include -#include #include #include diff --git a/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp b/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp index 16cbd2b..da3cdf2 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/foxglove_server.hpp @@ -27,7 +27,7 @@ namespace core public: FoxgloveWSServer() = delete; - FoxgloveWSServer(std::vector> configurable_components); + FoxgloveWSServer(std::vector> configurable_components); void send_live_telem_msg(std::shared_ptr msg); ~FoxgloveWSServer() { @@ -63,7 +63,7 @@ namespace core std::optional _convert_foxglove_param(core::common::Configurable::ParamTypes curr_param_val, foxglove::Parameter incoming_param); void _handle_foxglove_send(); private: - std::vector> _components; + std::vector> _components; std::unique_ptr> _server; std::function _log_handler; foxglove::ServerOptions _server_options; diff --git a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp index 763dd81..b8448b6 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp @@ -23,7 +23,7 @@ static uint64_t nanosecondsSinceEpoch() .count()); } -core::FoxgloveWSServer::FoxgloveWSServer(std::vector> configurable_components) : _components(configurable_components) +core::FoxgloveWSServer::FoxgloveWSServer(std::vector> configurable_components) : _components(configurable_components) { _log_handler = [](foxglove::WebSocketLogLevel, char const *msg) { @@ -210,20 +210,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 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 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); @@ -232,17 +236,21 @@ void core::FoxgloveWSServer::_set_db_param(foxglove::Parameter param_update) std::vector core::FoxgloveWSServer::_get_current_params() { std::vector 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 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 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_logging/include/DrivebrainMCAPLogger.hpp b/drivebrain_core_impl/drivebrain_logging/include/DrivebrainMCAPLogger.hpp index af7fc8f..265066d 100644 --- a/drivebrain_core_impl/drivebrain_logging/include/DrivebrainMCAPLogger.hpp +++ b/drivebrain_core_impl/drivebrain_logging/include/DrivebrainMCAPLogger.hpp @@ -46,7 +46,7 @@ namespace common uint64_t log_time; }; - DrivebrainMCAPLogger(const std::string &base_dir, std::vector> configurable_components); + DrivebrainMCAPLogger(const std::string &base_dir, std::vector> configurable_components); ~DrivebrainMCAPLogger(); /// @brief @@ -91,9 +91,7 @@ namespace common std::mutex _logger_mtx; std::unordered_map _msg_name_id_map; - // std::function()> _get_params_schema; - // std::function _get_param_vals; - std::vector> _configurable_components; + std::vector> _configurable_components; }; } diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp index c170720..bb4bc46 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp @@ -14,7 +14,7 @@ namespace common { - DrivebrainMCAPLogger::DrivebrainMCAPLogger(const std::string &base_dir, std::vector> configurable_components) + DrivebrainMCAPLogger::DrivebrainMCAPLogger(const std::string &base_dir, std::vector> configurable_components) : _options(mcap::McapWriterOptions("")), _configurable_components(configurable_components) { auto optional_map = util::generate_name_to_id_map({"hytech_msgs.proto", "hytech.proto"}); @@ -210,19 +210,23 @@ namespace common nlohmann::json DrivebrainMCAPLogger::_get_param_vals() { nlohmann::json params_all; - for(const auto cc: _configurable_components) + for(const auto cc_locked: _configurable_components) { - 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 param_names = cc->get_param_names(); - for (auto i = params_map.begin(); i != params_map.end(); i++) + if(auto cc = cc_locked.lock()) { - auto name = i->first; - auto var_val = i->second; - - _get_params_as_json(param_parent, name, var_val, params_all); + 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 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(param_parent, name, var_val, params_all); + } } + } return params_all; } @@ -235,13 +239,16 @@ namespace common for(const auto component : _configurable_components ) { // TODO handle multiple instances of the same component - std::cout << "getting component index: "<is_configured()) + if(auto comp_locked = component.lock()) { - return std::nullopt; + std::cout << "getting component index: "<is_configured()) + { + return std::nullopt; + } + component_index++; + top_level_schema["properties"][comp_locked->get_name()] = comp_locked->get_schema(); } - component_index++; - top_level_schema["properties"][component->get_name()] = component->get_schema(); } return top_level_schema; } diff --git a/flake.lock b/flake.lock index 9280f5e..44dc865 100644 --- a/flake.lock +++ b/flake.lock @@ -54,11 +54,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1746758396, - "narHash": "sha256-p/LvoVpxNLq14189+LNdMRiagNY/7JaxBMkDZJlpujU=", + "lastModified": 1746801073, + "narHash": "sha256-vLj8vABRGTN6L8VQWrVf9NXLygsWXsK1AWGPXo2kxUs=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "27b323bc09ab523a21529e72f6d7e4b2b1d77825", + "rev": "75411bc00b60826eb109d55dc357497a5d9e44df", "type": "github" }, "original": { diff --git a/test/test_configurable.cpp b/test/test_configurable.cpp index 83b2801..5f61d40 100644 --- a/test/test_configurable.cpp +++ b/test/test_configurable.cpp @@ -72,10 +72,9 @@ int main() - std::vector> configureable_components; - auto test_inst_cast = std::reinterpret_pointer_cast(test); - configureable_components.push_back(test_inst_cast); - configureable_components.push_back(std::reinterpret_pointer_cast(test_inst)); + std::vector> configureable_components; + configureable_components.push_back(test); + configureable_components.push_back(test_inst); auto standin_foxglove_ws_send = [](std::shared_ptr msg){}; auto mcap_logger = std::make_shared("temp", configureable_components); From 4b0cc3d3e7dea209efc4e2ef44385804d46d78a6 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 9 May 2025 10:54:15 -0400 Subject: [PATCH 45/87] generalized ethernet recv driver and used it to implement vcf and vcr eth recv --- CMakeLists.txt | 2 - drivebrain_app/debug_main.cpp | 1 - drivebrain_app/include/DriveBrainApp.hpp | 8 +- drivebrain_app/main.cpp | 1 - drivebrain_app/src/DriveBrainApp.cpp | 18 ++- .../{ACUETHComms.hpp => ETHRecvComms.hpp} | 37 +++--- .../drivebrain_comms/include/ETHRecvComms.tpp | 55 ++++++++ .../drivebrain_comms/include/MCUETHComms.hpp | 72 ----------- .../drivebrain_comms/src/ACUETHComms.cpp | 54 -------- .../drivebrain_comms/src/MCUETHComms.cpp | 117 ------------------ 10 files changed, 86 insertions(+), 279 deletions(-) rename drivebrain_core_impl/drivebrain_comms/include/{ACUETHComms.hpp => ETHRecvComms.hpp} (62%) create mode 100644 drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp delete mode 100644 drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp delete mode 100644 drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp delete mode 100644 drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0bcdef5..25115f9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,8 +82,6 @@ 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/ACUETHComms.cpp drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp ) diff --git a/drivebrain_app/debug_main.cpp b/drivebrain_app/debug_main.cpp index 8bf7328..d0f6211 100644 --- a/drivebrain_app/debug_main.cpp +++ b/drivebrain_app/debug_main.cpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include #include diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 5200b89..246262c 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -7,7 +7,6 @@ #include #include #include -#include #include #include #include @@ -15,7 +14,7 @@ #include #include #include -#include +#include #include #include @@ -79,8 +78,9 @@ class DriveBrainApp { std::shared_ptr _state_estimator; std::shared_ptr _driver_primary_can; std::shared_ptr _driver_secondary_can; - // std::unique_ptr _eth_driver; - std::unique_ptr _acu_eth_driver; + std::unique_ptr> _acu_eth_driver; + std::unique_ptr> _vcr_eth_driver; + std::unique_ptr> _vcf_eth_driver; std::shared_ptr _vn_driver; std::unique_ptr _db_service; diff --git a/drivebrain_app/main.cpp b/drivebrain_app/main.cpp index 94224ba..9496f85 100644 --- a/drivebrain_app/main.cpp +++ b/drivebrain_app/main.cpp @@ -3,7 +3,6 @@ #include #include #include -#include #include #include #include diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index f762605..d8ca4b7 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -1,7 +1,6 @@ // DriveBrainApp.cpp #include "DriveBrainApp.hpp" -#include "ACUETHComms.hpp" #include "SimpleSpeedController.hpp" #include "SimpleTorqueController.hpp" #include "hytech.pb.h" @@ -67,12 +66,11 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d configurable_components.push_back(_driver_primary_can); configurable_components.push_back(_driver_secondary_can); spdlog::info("made CAN driver"); - comms::ETHCommPorts ports = {7766, 5555, 4444}; - _acu_eth_driver = std::make_unique( - _logger, _message_logger, - _io_context, ports); + _acu_eth_driver = std::make_unique>(_message_logger,_io_context, 7766); + _vcr_eth_driver = std::make_unique>(_message_logger,_io_context, 9999); + _vcf_eth_driver = std::make_unique>(_message_logger,_io_context, 4444); - spdlog::info("eth driver"); + spdlog::info("eth drivers"); auto switch_modes = [this](size_t mode) -> bool { @@ -147,6 +145,14 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { _acu_eth_driver->update_msg_logger(_message_logger); } + if(_vcr_eth_driver) + { + _vcr_eth_driver->update_msg_logger(_message_logger); + } + if(_vcf_eth_driver) + { + _vcf_eth_driver->update_msg_logger(_message_logger); + } spdlog::info("constructed app"); // TODO add here the creation of the config logger diff --git a/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp similarity index 62% rename from drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp rename to drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp index 6b031af..d25b947 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/ACUETHComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp @@ -1,5 +1,6 @@ -#ifndef __ACUCOMMS_H__ -#define __ACUCOMMS_H__ +#ifndef __ETHRECVCOMMS_H__ +#define __ETHRECVCOMMS_H__ + #include #include @@ -24,23 +25,16 @@ namespace comms { - struct ETHCommPorts - { - uint16_t acu_port; - uint16_t vcr_port; - uint16_t vcf_port; - }; - class ACUETHComms + + template + class ETHRecvComms { public: using loggertype = core::MsgLogger>; - ACUETHComms() = delete; - ~ACUETHComms(); - ACUETHComms(core::Logger &logger, - std::shared_ptr message_logger, - boost::asio::io_context &io_context, - ETHCommPorts ports); + ETHRecvComms() = delete; + ~ETHRecvComms(); + ETHRecvComms(std::shared_ptr message_logger, boost::asio::io_context &io_context, uint16_t recv_port); void update_msg_logger(std::shared_ptr message_logger) { _message_logger = message_logger; } @@ -48,20 +42,19 @@ namespace comms private: void _handle_receive(const boost::system::error_code &error, std::size_t size); void _start_receive(); - void _handle_send(std::array /*message*/, - const boost::system::error_code & /*error*/, - std::size_t /*bytes_transferred*/); + private: - core::Logger &_logger; std::shared_ptr _message_logger; - std::array _recv_buffer; + std::array _recv_buffer; boost::asio::ip::udp::socket _socket; boost::asio::ip::udp::endpoint _remote_endpoint; - std::shared_ptr _acu_msg; + std::shared_ptr _eth_msg; bool _running = false; std::thread _output_thread; }; } -#endif // __ACUCOMMS_H__ \ No newline at end of file +#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 0000000..5a3b25f --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp @@ -0,0 +1,55 @@ +#include +#include "hytech_msgs.pb.h" +#include + + +using boost::asio::ip::udp; +namespace comms +{ + template + ETHRecvComms::ETHRecvComms(std::shared_ptr message_logger, + boost::asio::io_context &io_context, + uint16_t port) : _message_logger(message_logger), + _socket(io_context, udp::endpoint(udp::v4(), port)) + { + _eth_msg = std::make_shared(); + _start_receive(); + } + + template + ETHRecvComms::~ETHRecvComms() + { + _running = false; + spdlog::warn("Destructed ETH COMMS"); + } + + template + void ETHRecvComms::_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>(_eth_msg); + if (_message_logger) + { + _message_logger->log_msg(out_msg); + } else { + spdlog::warn("Message logger not real"); + } + + _start_receive(); + } + } + + template + void ETHRecvComms::_start_receive() + { + using namespace boost::placeholders; + _socket.async_receive_from( + boost::asio::buffer(_recv_buffer), _remote_endpoint, + boost::bind(ÐRecvComms::_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/MCUETHComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp deleted file mode 100644 index c6dcb22..0000000 --- a/drivebrain_core_impl/drivebrain_comms/include/MCUETHComms.hpp +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef __ETHCOMMS_H__ -#define __ETHCOMMS_H__ - -#include -#include -#include - -#include -#include -#include - -#include -#include "hytech_msgs.pb.h" -#include - -// - [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>; - using loggertype = core::MsgLogger>; - MCUETHComms() = delete; - ~MCUETHComms(); - MCUETHComms(core::Logger &logger, - deqtype &in_deq, - std::shared_ptr message_logger, - std::shared_ptr state_estimator, - boost::asio::io_context &io_context, - const std::string &send_ip, - uint16_t recv_port, - uint16_t send_port); - void update_msg_logger(std::shared_ptr message_logger) { - _message_logger = message_logger; - } - - private: - void _handle_send_msg_from_queue(); - void _send_message(std::shared_ptr msg_out); - void _handle_receive(const boost::system::error_code &error, std::size_t size); - void _start_receive(); - void _handle_send(std::array /*message*/, - const boost::system::error_code & /*error*/, - std::size_t /*bytes_transferred*/); - private: - core::Logger &_logger; - std::shared_ptr _message_logger; - std::shared_ptr _state_estimator; - std::array _recv_buffer; - std::array _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 _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/src/ACUETHComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp deleted file mode 100644 index 18fe1bc..0000000 --- a/drivebrain_core_impl/drivebrain_comms/src/ACUETHComms.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include -#include "hytech_msgs.pb.h" -#include - - -using boost::asio::ip::udp; -namespace comms -{ - - ACUETHComms::ACUETHComms(core::Logger &logger, - std::shared_ptr message_logger, - boost::asio::io_context &io_context, - ETHCommPorts ports) : _logger(logger), - _message_logger(message_logger), - _socket(io_context, udp::endpoint(udp::v4(), ports.acu_port)) - { - _acu_msg = std::make_shared(); - _start_receive(); - } - - ACUETHComms::~ACUETHComms() - { - _running = false; - spdlog::warn("Destructed ACU ETH COMMS"); - } - - void ACUETHComms::_handle_receive(const boost::system::error_code &error, std::size_t size) - { - - if (!error) - { - _acu_msg->ParseFromArray(_recv_buffer.data(), size); - auto out_msg = static_cast>(_acu_msg); - if (_message_logger) - { - _message_logger->log_msg(out_msg); - } else { - spdlog::warn("Message logger not real"); - } - - _start_receive(); - } - } - - void ACUETHComms::_start_receive() - { - using namespace boost::placeholders; - _socket.async_receive_from( - boost::asio::buffer(_recv_buffer), _remote_endpoint, - boost::bind(&ACUETHComms::_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/MCUETHComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp deleted file mode 100644 index 3af48c8..0000000 --- a/drivebrain_core_impl/drivebrain_comms/src/MCUETHComms.cpp +++ /dev/null @@ -1,117 +0,0 @@ -#include -#include "StateEstimator.hpp" -#include "hytech_msgs.pb.h" -#include - - -using boost::asio::ip::udp; -namespace comms -{ - MCUETHComms::MCUETHComms(core::Logger &logger, - deqtype &in_deq, - std::shared_ptr message_logger, - std::shared_ptr 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(); - _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) - { - spdlog::debug("looping _handle_send_msg_from_queue mcu eth"); - { - 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); - if(_message_logger) - { - _message_logger->log_msg(msg); - } else { - spdlog::warn("message logger not real"); - } - - } - _input_deque_ref.deque.clear(); - } - } - } - void MCUETHComms::_send_message(std::shared_ptr 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>(_mcu_msg); - _state_estimator->handle_recv_process(out_msg); - if(_message_logger) - { - _message_logger->log_msg(out_msg); - }else { - spdlog::warn("message logger not real"); - } - - _start_receive(); - } - } - void MCUETHComms::_handle_send(std::array /*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 From 873a39503a077761fdcb9f61bed5c613def81984 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 9 May 2025 10:54:45 -0400 Subject: [PATCH 46/87] removed todo --- drivebrain_app/src/DriveBrainApp.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index d8ca4b7..a469cb7 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -155,7 +155,6 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d } spdlog::info("constructed app"); - // TODO add here the creation of the config logger } DriveBrainApp::~DriveBrainApp() { From 5a801a6c98fab08a200582a7e5c32744748571ca Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 9 May 2025 19:02:48 -0400 Subject: [PATCH 47/87] fixed dumb issue that caused stale message logger shared_ptr in drivebrain service --- drivebrain_app/src/DriveBrainApp.cpp | 4 +++- .../drivebrain_comms/include/DBServiceImpl.hpp | 4 ++-- drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp | 2 +- flake.lock | 6 +++--- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index a469cb7..c3d0aa2 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -24,7 +24,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { // spdlog::info("top o"); std::vector> configurable_components; - spdlog::set_level(spdlog::level::debug); + spdlog::set_level(spdlog::level::info); controller1 = std::make_shared(_config); @@ -154,6 +154,8 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d _vcf_eth_driver->update_msg_logger(_message_logger); } + // _message_logger->start_logging_params(); + spdlog::info("constructed app"); } diff --git a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp index 1bb0adb..8b39009 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp @@ -28,8 +28,8 @@ class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Servi public: DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch); void run_server(); void stop_server(); - void update_msg_logger(std::shared_ptr>> _logger_inst) { - _logger_inst = _logger_inst; + void update_msg_logger(std::shared_ptr>> logger_inst) { + _logger_inst = logger_inst; } private: std::shared_ptr>> _logger_inst; diff --git a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp index f3c3234..2014465 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(); diff --git a/flake.lock b/flake.lock index 44dc865..2b32655 100644 --- a/flake.lock +++ b/flake.lock @@ -54,11 +54,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1746801073, - "narHash": "sha256-vLj8vABRGTN6L8VQWrVf9NXLygsWXsK1AWGPXo2kxUs=", + "lastModified": 1746831657, + "narHash": "sha256-SwBOefZrjmU3xrZt2FUyIi3B25ojt8xiTotM+F6uyvs=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "75411bc00b60826eb109d55dc357497a5d9e44df", + "rev": "228e25eccd359f82aa9c64a2f7f304d675fd9779", "type": "github" }, "original": { From 365c18309e932f1e8d37a37253f866e1dcd5fdaa Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 9 May 2025 20:07:24 -0400 Subject: [PATCH 48/87] working first TODO of the mcap replay done --- CMakeLists.txt | 46 ++++++++++++---- cmake/drivebrain_utilsConfig.cmake.in | 11 ++++ drivebrain_utils/include/MCAPReplay.hpp | 42 +++++++++++++++ drivebrain_utils/src/MCAPReplay.cpp | 71 +++++++++++++++++++++++++ drivebrain_utils/src/replay_app.cpp | 28 ++++++++++ 5 files changed, 188 insertions(+), 10 deletions(-) create mode 100644 cmake/drivebrain_utilsConfig.cmake.in create mode 100644 drivebrain_utils/include/MCAPReplay.hpp create mode 100644 drivebrain_utils/src/MCAPReplay.cpp create mode 100644 drivebrain_utils/src/replay_app.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 25115f9..aea1b34 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,9 +56,9 @@ find_package(drivebrain_core REQUIRED) include(create_package) -##################### -# created libraries # -##################### +################################### +# created drivebrain sw libraries # +################################### # utils add_library(drivebrain_common_utils SHARED @@ -161,7 +161,13 @@ target_include_directories(drivebrain_estimation PUBLIC $ ) -target_link_libraries(drivebrain_estimation PUBLIC drivebrain_core::drivebrain_core) +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) @@ -183,15 +189,30 @@ target_link_libraries(drivebrain_app PUBLIC 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 + $ + $ +) + +target_link_libraries(drivebrain_utils PUBLIC protobuf::libprotobuf + mcap::mcap + Boost::program_options + spdlog::spdlog ) -make_cmake_package(drivebrain_app drivebrain) + +make_cmake_package(drivebrain_utils drivebrain) ############### # executables # @@ -207,6 +228,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 diff --git a/cmake/drivebrain_utilsConfig.cmake.in b/cmake/drivebrain_utilsConfig.cmake.in new file mode 100644 index 0000000..993932e --- /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/drivebrain_utils/include/MCAPReplay.hpp b/drivebrain_utils/include/MCAPReplay.hpp new file mode 100644 index 0000000..7a5faee --- /dev/null +++ b/drivebrain_utils/include/MCAPReplay.hpp @@ -0,0 +1,42 @@ +#ifndef __MCAPREPLAY_H__ +#define __MCAPREPLAY_H__ + +#include +#include + +#include +#include +#include + +#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 +// [ ] 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. +// [ ] use a CANDriver and send all CAN traffic from MCAP file to vcan +// [ ] use instances of (TODO) ETHSendComms to send the VCF/VCR/ACU data + +namespace util +{ + class MCAPReplay + { + public: + MCAPReplay(){} + + void start(std::string filename); + private: + bool _load_schema(const mcap::SchemaPtr schema, google::protobuf::SimpleDescriptorDatabase* proto_db); + + private: + + mcap::McapReader _mcap_reader; + }; + +} +#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 0000000..e45a852 --- /dev/null +++ b/drivebrain_utils/src/MCAPReplay.cpp @@ -0,0 +1,71 @@ +#include "MCAPReplay.hpp" + +#include +#include + +namespace gp = google::protobuf; + +namespace util { +bool MCAPReplay::_load_schema(const mcap::SchemaPtr schema, + google::protobuf::SimpleDescriptorDatabase *protoDb) { + gp::FileDescriptorSet fdSet; + if (!fdSet.ParseFromArray(schema->data.data(), static_cast(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()); + // std::cerr << "failed to add definition " << file.name() << "to protoDB" << + // std::endl; + return false; + } + } + } + return true; +} + +void MCAPReplay::start(std::string filename) { + const auto res = _mcap_reader.open(filename); + if (!res.ok()) { + + spdlog::error("Failed to open {}", filename); + return; + } + + auto msg_view = _mcap_reader.readMessages(); + + 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. + if (it->schema->encoding != "protobuf") { + continue; + } + + spdlog::info("name: {}", it->schema->name); + const gp::Descriptor *descriptor = protoPool.FindMessageTypeByName(it->schema->name); + + if (descriptor == nullptr) { + if (!_load_schema(it->schema, &protoDb)) { + spdlog::error("failed to load schema, exiting"); + _mcap_reader.close(); + return; + } + descriptor = protoPool.FindMessageTypeByName(it->schema->name); + if (descriptor == nullptr) { + spdlog::error("failed to find desc after loading pool, exiting"); + _mcap_reader.close(); + return; + } + } + } + + _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 0000000..d41a0a1 --- /dev/null +++ b/drivebrain_utils/src/replay_app.cpp @@ -0,0 +1,28 @@ +#include "MCAPReplay.hpp" +#include +#include + +int main(int argc, char* argv[]) +{ + namespace po = boost::program_options; + po::options_description desc("Allowed options"); + + + std::string mcap_filepath; + desc.add_options() + ("help,h", "produce help message") + ("mcap,m", po::value(&mcap_filepath), "Path to mcap file"); + + util::MCAPReplay replay_util; + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + po::notify(vm); + + if (vm.count("help")) { + std::cout << desc< Date: Sat, 10 May 2025 19:01:12 -0400 Subject: [PATCH 49/87] added in mcap replay that can replay onto a virtual CAN bus --- CMakeLists.txt | 1 + config/hytech.dbc | 2003 ++++++++--------- drivebrain_app/src/DriveBrainApp.cpp | 6 +- .../drivebrain_comms/include/CANComms.hpp | 8 +- .../drivebrain_comms/src/CANComms.cpp | 27 +- drivebrain_utils/include/MCAPReplay.hpp | 25 +- drivebrain_utils/src/MCAPReplay.cpp | 102 +- drivebrain_utils/src/replay_app.cpp | 9 +- 8 files changed, 1104 insertions(+), 1077 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aea1b34..b0e6891 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -209,6 +209,7 @@ target_link_libraries(drivebrain_utils PUBLIC mcap::mcap Boost::program_options spdlog::spdlog + drivebrain_comms # CANDriver ) diff --git a/config/hytech.dbc b/config/hytech.dbc index 5ff6608..95beb6d 100644 --- a/config/hytech.dbc +++ b/config/hytech.dbc @@ -42,6 +42,16 @@ BO_ 1220 IZZE_IR_TEMPS: 8 Peripherals 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 @@ -159,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 @@ -178,221 +191,194 @@ 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_ 203 BRAKE_PRESSURE_SENSOR: 2 ECU - SG_ brake_sensor_analog_read : 0|16@1+ (3.0517578125,-312.5) [0|0] "psi" 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_ 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_ 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_ 236 DASHBOARD_STATE: 3 ECU - SG_ dial_state : 16|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ tcu_recording_state : 12|2@1+ (1,0) [0|0] "" Vector__XXX - SG_ drive_buzzer : 11|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ ssok_above_threshold : 10|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ shutdown_h_above_threshold : 9|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ right_shifter_button : 8|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ left_shifter_button : 7|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ led_dimmer_button : 6|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ torque_mode_button : 5|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ launch_ctrl_button : 4|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ motor_controller_cycle_button : 3|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ mode_button : 2|1@1+ (1,0) [0|0] "" Vector__XXX - 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_ 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 @@ -410,174 +396,68 @@ BO_ 235 DASHBOARD_MCU_STATE: 6 ECU 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_ 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_ 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_ 1060 LF_TTPMS_1: 8 ECU - SG_ LF_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ LF_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ LF_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX - SG_ LF_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" Vector__XXX - -BO_ 1061 LF_TTPMS_2: 8 ECU - SG_ LF_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1062 LF_TTPMS_3: 8 ECU - SG_ LF_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1063 LF_TTPMS_4: 8 ECU - SG_ LF_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1064 LF_TTPMS_5: 8 ECU - SG_ LF_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1065 LF_TTPMS_6: 8 ECU - SG_ LF_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ LF_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LF_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX - SG_ LF_TTPMS_TC : 7|16@0+ (1,0) [0|0] "" Vector__XXX - -BO_ 1066 LR_TTPMS_1: 8 ECU - SG_ LR_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ LR_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ LR_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX - SG_ LR_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" Vector__XXX - -BO_ 1067 LR_TTPMS_2: 8 ECU - SG_ LR_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1068 LR_TTPMS_3: 8 ECU - SG_ LR_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1069 LR_TTPMS_4: 8 ECU - SG_ LR_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1070 LR_TTPMS_5: 8 ECU - SG_ LR_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ LR_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1071 LR_TTPMS_6: 8 ECU - SG_ LR_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ LR_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - 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_ 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 - SG_ RF_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RF_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RF_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1077 RF_TTPMS_6: 8 ECU - SG_ RF_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ RF_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RF_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX - SG_ RF_TTPMS_TC : 7|16@0+ (1,0) [0|0] "" Vector__XXX - -BO_ 1078 RR_TTPMS_1: 8 ECU - SG_ RR_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ RR_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX - SG_ RR_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX - SG_ RR_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" Vector__XXX - -BO_ 1079 RR_TTPMS_2: 8 ECU - SG_ RR_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - -BO_ 1080 RR_TTPMS_3: 8 ECU - SG_ RR_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX +BO_ 236 DASHBOARD_STATE: 3 ECU + SG_ dial_state : 16|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ tcu_recording_state : 12|2@1+ (1,0) [0|0] "" Vector__XXX + SG_ drive_buzzer : 11|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ ssok_above_threshold : 10|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ shutdown_h_above_threshold : 9|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ right_shifter_button : 8|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ left_shifter_button : 7|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ led_dimmer_button : 6|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ torque_mode_button : 5|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ launch_ctrl_button : 4|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ motor_controller_cycle_button : 3|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ mode_button : 2|1@1+ (1,0) [0|0] "" Vector__XXX + 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_ 1081 RR_TTPMS_4: 8 ECU - SG_ RR_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1082 RR_TTPMS_5: 8 ECU - SG_ RR_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - SG_ RR_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1083 RR_TTPMS_6: 8 ECU - SG_ RR_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ RR_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX - 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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 @@ -585,12 +465,6 @@ BO_ 512 DRIVETRAIN_RPMS_TELEM: 8 ECU 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 @@ -629,319 +503,495 @@ BO_ 514 DRIVETRAIN_STATUS_TELEM: 8 ECU 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 - SG_ thermistor_inv2 : 16|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX - SG_ thermistor_inv1 : 0|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX - -BO_ 230 SAB_THERMISTORS_2: 6 ECU - SG_ thermistor_pump : 32|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX - 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 224 VN_GPS_TIME_MSG: 8 ECU - SG_ vn_gps_time : 0|64@1+ (1,0) [0|0] "ns" 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_ 225 VN_STATUS: 8 ECU - SG_ vn_gps_status : 0|16@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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 201 TC_SIMPLE: 8 ECU - SG_ accel_request_state : 48|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ rear_regen_scale : 40|8@1- (0.00787402,0) [0|0] "" Vector__XXX - SG_ front_regen_scale : 32|8@1- (0.00787402,0) [0|0] "" Vector__XXX - SG_ rear_torque_scale : 24|8@1- (0.00787402,0) [0|0] "" Vector__XXX - SG_ front_torque_scale : 16|8@1- (0.00787402,0) [0|0] "" Vector__XXX - SG_ torque_request : 0|16@1- (0.001,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_ 202 TC_SIMPLE_LAUNCH: 8 ECU - SG_ speed_setpoint_rpm : 24|16@1- (1,0) [0|0] "rpm" Vector__XXX - SG_ initial_launch_target : 8|16@1+ (1,0) [0|0] "rpm" Vector__XXX - SG_ algo_active : 2|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ launch_control_state : 0|2@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_ 2047 VEHM_ALPHA: 8 ECU - SG_ vehm_alpha_deg_rr : 48|16@1- (0.001,0) [0|0] "deg" Vector__XXX - SG_ vehm_alpha_deg_rl : 32|16@1- (0.001,0) [0|0] "deg" Vector__XXX - SG_ vehm_alpha_deg_fr : 16|16@1- (0.001,0) [0|0] "deg" Vector__XXX - SG_ vehm_alpha_deg_fl : 0|16@1- (0.001,0) [0|0] "deg" 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_ 2031 VEHM_BETA: 8 ECU - SG_ vehm_beta_deg : 0|16@1- (0.001,0) [0|0] "deg" 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_ 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 - SG_ vehm_long_corner_vel_fr : 16|16@1- (0.01,0) [0|0] "m/s" Vector__XXX - SG_ vehm_long_corner_vel_fl : 0|16@1- (0.01,0) [0|0] "m/s" 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_ 1999 VEHM_SL: 8 ECU - SG_ vehm_sl_rr : 48|16@1- (0.0001,0) [0|0] "" Vector__XXX - SG_ vehm_sl_rl : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX - 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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 + SG_ LF_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ LF_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX + SG_ LF_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" Vector__XXX + +BO_ 1061 LF_TTPMS_2: 8 ECU + SG_ LF_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1062 LF_TTPMS_3: 8 ECU + SG_ LF_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1063 LF_TTPMS_4: 8 ECU + SG_ LF_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1064 LF_TTPMS_5: 8 ECU + SG_ LF_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1065 LF_TTPMS_6: 8 ECU + SG_ LF_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX + SG_ LF_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LF_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX + SG_ LF_TTPMS_TC : 7|16@0+ (1,0) [0|0] "" Vector__XXX + +BO_ 1066 LR_TTPMS_1: 8 ECU + SG_ LR_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ LR_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ LR_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX + SG_ LR_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" Vector__XXX + +BO_ 1067 LR_TTPMS_2: 8 ECU + SG_ LR_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1068 LR_TTPMS_3: 8 ECU + SG_ LR_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1069 LR_TTPMS_4: 8 ECU + SG_ LR_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1070 LR_TTPMS_5: 8 ECU + SG_ LR_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ LR_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + +BO_ 1071 LR_TTPMS_6: 8 ECU + SG_ LR_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX + SG_ LR_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + 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_ 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_ 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_ 257 MCU_ERROR_STATES: 8 ECU + SG_ torque_controller_mux_status : 0|3@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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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_ 1076 RF_TTPMS_5: 8 ECU + SG_ RF_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RF_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RF_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RF_TTPMS_T13 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1077 RF_TTPMS_6: 8 ECU + SG_ RF_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX + SG_ RF_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RF_TTPMS_RSSI : 23|16@0- (1,0) [0|0] "dBm" Vector__XXX + SG_ RF_TTPMS_TC : 7|16@0+ (1,0) [0|0] "" 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_ 1078 RR_TTPMS_1: 8 ECU + SG_ RR_TTPMS_P_GAUGE : 55|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ RR_TTPMS_P : 39|16@0+ (1,0) [0|0] "mbar" Vector__XXX + SG_ RR_TTPMS_BAT_V : 23|16@0+ (1,0) [0|0] "mV" Vector__XXX + SG_ RR_TTPMS_SN : 7|16@0+ (1,0) [0|0] "" 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_ 1079 RR_TTPMS_2: 8 ECU + SG_ RR_TTPMS_T4 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T3 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T2 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T1 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1080 RR_TTPMS_3: 8 ECU + SG_ RR_TTPMS_T8 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T7 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T6 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T5 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1081 RR_TTPMS_4: 8 ECU + SG_ RR_TTPMS_T12 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T11 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T10 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T9 : 7|16@0+ (0.1,-100) [0|0] "°C" 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_ 1082 RR_TTPMS_5: 8 ECU + SG_ RR_TTPMS_T16 : 55|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T15 : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T14 : 23|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + SG_ RR_TTPMS_T13 : 7|16@0+ (0.1,-100) [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_ 1083 RR_TTPMS_6: 8 ECU + SG_ RR_TTPMS_NODE_ID : 55|16@0+ (1,0) [0|0] "" Vector__XXX + SG_ RR_TTPMS_T : 39|16@0+ (0.1,-100) [0|0] "°C" Vector__XXX + 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_ 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_ 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 + SG_ thermistor_inv2 : 16|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX + SG_ thermistor_inv1 : 0|16@1+ (0.005,0) [0|0] "deg 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_ 230 SAB_THERMISTORS_2: 6 ECU + SG_ thermistor_pump : 32|16@1+ (0.005,0) [0|0] "deg C" Vector__XXX + 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_ 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_ 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_ 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_ 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_ 1265 TCMUX_STATUS_REPORT: 8 ECU SG_ torque_limit : 36|16@1+ (0.001,0) [0|0] "" Vector__XXX @@ -954,251 +1004,137 @@ BO_ 1265 TCMUX_STATUS_REPORT: 8 ECU 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_ 234 VN_GNSS_COMP_SIG_HEALTH: 8 ECU - SG_ num_com_sats_rtk : 56|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ num_com_sats_pvt : 48|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ highest_cn0_2 : 40|8@1+ (0.5,0) [0|0] "dBHz" Vector__XXX - SG_ num_sats_rtk_2 : 32|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ num_sats_pvt_2 : 24|8@1+ (1,0) [0|0] "" Vector__XXX - SG_ highest_cn0_1 : 16|8@1+ (0.5,0) [0|0] "dBHz" Vector__XXX - 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_ 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_ 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_ 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_ 0 brake_temps: 8 ECU - -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: 8 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_ 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- (1,0) [0|0] "" Vector__XXX - SG_ actual_power_w : 0|32@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_ 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_ 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_ 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_ 118 INV2_TEMPS: 8 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_ 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_ 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- (1,0) [0|0] "" Vector__XXX - SG_ actual_power_w : 0|32@1+ (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_ 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_ 201 TC_SIMPLE: 8 ECU + SG_ accel_request_state : 48|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ rear_regen_scale : 40|8@1- (0.00787402,0) [0|0] "" Vector__XXX + SG_ front_regen_scale : 32|8@1- (0.00787402,0) [0|0] "" Vector__XXX + SG_ rear_torque_scale : 24|8@1- (0.00787402,0) [0|0] "" Vector__XXX + SG_ front_torque_scale : 16|8@1- (0.00787402,0) [0|0] "" Vector__XXX + SG_ torque_request : 0|16@1- (0.001,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_ 202 TC_SIMPLE_LAUNCH: 8 ECU + SG_ speed_setpoint_rpm : 24|16@1- (1,0) [0|0] "rpm" Vector__XXX + SG_ initial_launch_target : 8|16@1+ (1,0) [0|0] "rpm" Vector__XXX + SG_ algo_active : 2|1@1+ (1,0) [0|0] "" Vector__XXX + SG_ launch_control_state : 0|2@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_ 2047 VEHM_ALPHA: 8 ECU + SG_ vehm_alpha_deg_rr : 48|16@1- (0.001,0) [0|0] "deg" Vector__XXX + SG_ vehm_alpha_deg_rl : 32|16@1- (0.001,0) [0|0] "deg" Vector__XXX + SG_ vehm_alpha_deg_fr : 16|16@1- (0.001,0) [0|0] "deg" Vector__XXX + SG_ vehm_alpha_deg_fl : 0|16@1- (0.001,0) [0|0] "deg" Vector__XXX -BO_ 129 INV3_TEMPS: 8 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_ 2031 VEHM_BETA: 8 ECU + SG_ vehm_beta_deg : 0|16@1- (0.001,0) [0|0] "deg" 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- (1,0) [0|0] "" Vector__XXX - SG_ actual_power_w : 0|32@1+ (1,0) [0|0] "" 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_ 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_ 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 + SG_ vehm_long_corner_vel_fr : 16|16@1- (0.01,0) [0|0] "m/s" Vector__XXX + SG_ vehm_long_corner_vel_fl : 0|16@1- (0.01,0) [0|0] "m/s" 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_ 1999 VEHM_SL: 8 ECU + SG_ vehm_sl_rr : 48|16@1- (0.0001,0) [0|0] "" Vector__XXX + SG_ vehm_sl_rl : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX + 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_ 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_ 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_ 134 INV4_TEMPS: 8 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_ 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_ 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- (1,0) [0|0] "" Vector__XXX - SG_ actual_power_w : 0|32@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_ 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_ 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_ 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_ 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_ 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_ 234 VN_GNSS_COMP_SIG_HEALTH: 8 ECU + SG_ num_com_sats_rtk : 56|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ num_com_sats_pvt : 48|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ highest_cn0_2 : 40|8@1+ (0.5,0) [0|0] "dBHz" Vector__XXX + SG_ num_sats_rtk_2 : 32|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ num_sats_pvt_2 : 24|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ highest_cn0_1 : 16|8@1+ (0.5,0) [0|0] "dBHz" Vector__XXX + 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_ 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_ 224 VN_GPS_TIME_MSG: 8 ECU + SG_ vn_gps_time : 0|64@1+ (1,0) [0|0] "ns" Vector__XXX -BO_ 144 INV1_CONTROL_WORD: 8 ECU - SG_ remove_error : 3|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ driver_enable : 2|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ hv_enable : 1|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ inverter_enable : 0|1@1+ (1,0) [0|0] "" Vector__XXX - -BO_ 145 INV2_CONTROL_WORD: 8 ECU - SG_ remove_error : 3|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ driver_enable : 2|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ hv_enable : 1|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ inverter_enable : 0|1@1+ (1,0) [0|0] "" Vector__XXX - -BO_ 146 INV3_CONTROL_WORD: 8 ECU - SG_ remove_error : 3|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ driver_enable : 2|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ hv_enable : 1|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ inverter_enable : 0|1@1+ (1,0) [0|0] "" Vector__XXX - -BO_ 147 INV4_CONTROL_WORD: 8 ECU - SG_ remove_error : 3|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ driver_enable : 2|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ hv_enable : 1|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ inverter_enable : 0|1@1+ (1,0) [0|0] "" Vector__XXX - -BO_ 151 INV1_CONTROL_INPUT: 8 ECU - SG_ torque_setpoint : 48|16@1- (1,0) [0|0] "" Vector__XXX - SG_ negative_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX - SG_ positive_torque_limit : 16|16@1- (0.01,0) [0|0] "Mn" Vector__XXX - SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" 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_ 152 INV2_CONTROL_INPUT: 8 ECU - SG_ torque_setpoint : 48|16@1- (1,0) [0|0] "" Vector__XXX - SG_ negative_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX - SG_ positive_torque_limit : 16|16@1- (0.01,0) [0|0] "Mn" Vector__XXX - SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" 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_ 153 INV3_CONTROL_INPUT: 8 ECU - SG_ torque_setpoint : 48|16@1- (1,0) [0|0] "" Vector__XXX - SG_ negative_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX - SG_ positive_torque_limit : 16|16@1- (0.01,0) [0|0] "Mn" Vector__XXX - SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" 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_ 258 INV4_CONTROL_INPUT: 8 ECU - SG_ torque_setpoint : 48|16@1- (1,0) [0|0] "" Vector__XXX - SG_ negative_torque_limit : 32|16@1- (0.01,0) [0|0] "Mn" Vector__XXX - SG_ positive_torque_limit : 16|16@1- (0.01,0) [0|0] "Mn" Vector__XXX - SG_ speed_setpoint_rpm : 0|16@1- (1,0) [0|0] "rpm" Vector__XXX +BO_ 225 VN_STATUS: 8 ECU + SG_ vn_gps_status : 0|16@1+ (1,0) [0|0] "" 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_ 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_ 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_ 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_ 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_ 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_ 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_ 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; @@ -1208,40 +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_ 203 : 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; @@ -1254,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; @@ -1266,111 +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_ 516 : ECU,Peripherals; -BO_TX_BU_ 241 : ECU,Peripherals; -BO_TX_BU_ 242 : 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_ 112 : ECU,Peripherals; -BO_TX_BU_ 113 : ECU,Peripherals; -BO_TX_BU_ 114 : ECU,Peripherals; -BO_TX_BU_ 115 : ECU,Peripherals; -BO_TX_BU_ 116 : ECU,Peripherals; -BO_TX_BU_ 117 : ECU,Peripherals; -BO_TX_BU_ 118 : ECU,Peripherals; -BO_TX_BU_ 119 : ECU,Peripherals; -BO_TX_BU_ 120 : ECU,Peripherals; -BO_TX_BU_ 121 : ECU,Peripherals; -BO_TX_BU_ 128 : ECU,Peripherals; -BO_TX_BU_ 129 : ECU,Peripherals; -BO_TX_BU_ 130 : ECU,Peripherals; -BO_TX_BU_ 131 : ECU,Peripherals; -BO_TX_BU_ 132 : ECU,Peripherals; -BO_TX_BU_ 133 : ECU,Peripherals; -BO_TX_BU_ 134 : ECU,Peripherals; -BO_TX_BU_ 135 : ECU,Peripherals; -BO_TX_BU_ 136 : ECU,Peripherals; -BO_TX_BU_ 137 : ECU,Peripherals; -BO_TX_BU_ 192 : ECU,Peripherals; -BO_TX_BU_ 237 : ECU,Peripherals; -BO_TX_BU_ 144 : ECU,Peripherals; -BO_TX_BU_ 145 : ECU,Peripherals; -BO_TX_BU_ 146 : ECU,Peripherals; -BO_TX_BU_ 147 : ECU,Peripherals; -BO_TX_BU_ 151 : ECU,Peripherals; -BO_TX_BU_ 152 : ECU,Peripherals; -BO_TX_BU_ 153 : ECU,Peripherals; -BO_TX_BU_ 258 : ECU,Peripherals; -BO_TX_BU_ 259 : ECU,Peripherals; -BO_TX_BU_ 260 : ECU,Peripherals; -BO_TX_BU_ 261 : ECU,Peripherals; -BO_TX_BU_ 262 : 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."; @@ -1379,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."; @@ -1388,42 +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_ 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"; @@ -1431,36 +1358,16 @@ 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_ 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"; -CM_ SG_ 201 front_regen_scale "the front/rear torque scaling"; -CM_ SG_ 201 rear_torque_scale "the front/rear torque scaling"; -CM_ SG_ 201 front_torque_scale "the front/rear torque scaling"; -CM_ SG_ 201 torque_request "The torque request calculated from the accelleration request and the maximum torque"; -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_ 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"; @@ -1470,6 +1377,24 @@ 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_ 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"; +CM_ SG_ 201 front_regen_scale "the front/rear torque scaling"; +CM_ SG_ 201 rear_torque_scale "the front/rear torque scaling"; +CM_ SG_ 201 front_torque_scale "the front/rear torque scaling"; +CM_ SG_ 201 torque_request "The torque request calculated from the accelleration request and the maximum torque"; +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_ 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"; @@ -1478,26 +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_ 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_ 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_ 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_ 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_ 225 vn_gps_status 0 "NO_FIX" 1 "TIME_ONLY" 2 "FIX_2D" 3 "FIX_3D" ; +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" ; SIG_VALTYPE_ 1055 steering_digital_raw : 1; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index c3d0aa2..e527a3b 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -49,7 +49,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d // this also calls init() in the constructor _driver_primary_can = std::make_shared( - _config, _logger, _message_logger, _primary_can_tx_queue, _io_context, + _config, _message_logger, _primary_can_tx_queue, _io_context, _dbc_path, construction_failed, _state_estimator, "CANDriverPrimary"); if (construction_failed) { @@ -57,7 +57,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d } _driver_secondary_can = std::make_shared( - _config, _logger, _message_logger, _secondary_can_tx_queue, _io_context_secondary_can, + _config, _message_logger, _secondary_can_tx_queue, _io_context_secondary_can, _dbc_path, construction_failed, _state_estimator, "CANDriverSecondary"); if (construction_failed) { @@ -154,7 +154,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d _vcf_eth_driver->update_msg_logger(_message_logger); } - // _message_logger->start_logging_params(); + _message_logger->start_logging_params(); spdlog::info("constructed app"); } diff --git a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp index 028b22e..a9812cf 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp @@ -2,7 +2,6 @@ // drivebrain includes #include #include -#include #include #include #include // generated from CAN description @@ -66,9 +65,8 @@ namespace comms /// @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 message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator) : + CANDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator) : Configurable(json_file_handler, "CANDriver"), - _logger(logger), _message_logger(message_logger), _input_deque_ref(in_deq), _socket(io_context), @@ -80,9 +78,8 @@ namespace comms construction_failed = !init(); } - CANDriver(core::JsonFileHandler &json_file_handler, core::Logger& logger, std::shared_ptr message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator, std::string driver_name) : + CANDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator, std::string driver_name) : Configurable(json_file_handler, driver_name), - _logger(logger), _message_logger(message_logger), _input_deque_ref(in_deq), _socket(io_context), @@ -131,7 +128,6 @@ namespace comms static std::string _to_lowercase(std::string s); private: - core::Logger& _logger; std::shared_ptr _message_logger; deqtype &_input_deque_ref; diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index 7f4d628..d370079 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -39,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 net; @@ -60,14 +57,13 @@ 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(); - // _configured = true; set_configured(); return true; } @@ -75,9 +71,7 @@ bool comms::CANDriver::init() { 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; } @@ -90,9 +84,7 @@ bool comms::CANDriver::_open_socket(const std::string &interface_name) { addr.can_ifindex = ifr.ifr_ifindex; if (::bind(raw_socket, reinterpret_cast(&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; } @@ -126,7 +118,10 @@ 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); + if(_state_estimator) + { + _state_estimator->handle_recv_process(msg); + } if(_message_logger) // this may not exist yet as this gets constr { _message_logger->log_msg(msg); @@ -380,7 +375,7 @@ comms::CANDriver::_get_CAN_msg(std::shared_ptr 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; } diff --git a/drivebrain_utils/include/MCAPReplay.hpp b/drivebrain_utils/include/MCAPReplay.hpp index 7a5faee..b65c067 100644 --- a/drivebrain_utils/include/MCAPReplay.hpp +++ b/drivebrain_utils/include/MCAPReplay.hpp @@ -8,6 +8,11 @@ #include #include + + +// Drivebrain impl components +#include + #define MCAP_IMPLEMENTATION #include "mcap/reader.hpp" @@ -16,26 +21,36 @@ // ethernet socket and a virtual CAN bus // [x] simple reading of mcap file -// [ ] read of mcap at real-time speed +// [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. -// [ ] use a CANDriver and send all CAN traffic from MCAP file to vcan +// [x] use a CANDriver and send all CAN traffic from MCAP file to vcan // [ ] use instances of (TODO) ETHSendComms to send the VCF/VCR/ACU data + // [ ] create ETHSendComms namespace util { class MCAPReplay { public: - MCAPReplay(){} + 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 _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> _primary_can_tx_queue; + boost::asio::io_context _io_context; + std::shared_ptr _driver_primary_can = nullptr; mcap::McapReader _mcap_reader; + + std::thread _io_context_thread; }; } diff --git a/drivebrain_utils/src/MCAPReplay.cpp b/drivebrain_utils/src/MCAPReplay.cpp index e45a852..dd9d516 100644 --- a/drivebrain_utils/src/MCAPReplay.cpp +++ b/drivebrain_utils/src/MCAPReplay.cpp @@ -1,11 +1,27 @@ #include "MCAPReplay.hpp" +#include #include #include +#include + 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(_config, nullptr, _primary_can_tx_queue, _io_context, + dbc_file_path, cf, nullptr, "CANDriverPrimary"); + if(cf) + { + throw std::runtime_error("Failed to initialize can driver"); + } +} + bool MCAPReplay::_load_schema(const mcap::SchemaPtr schema, google::protobuf::SimpleDescriptorDatabase *protoDb) { gp::FileDescriptorSet fdSet; @@ -19,8 +35,6 @@ bool MCAPReplay::_load_schema(const mcap::SchemaPtr schema, if (!protoDb->FindFileByName(file.name(), &unused)) { if (!protoDb->Add(file)) { spdlog::error("failed to add def {} to protoDB", file.name()); - // std::cerr << "failed to add definition " << file.name() << "to protoDB" << - // std::endl; return false; } } @@ -28,6 +42,35 @@ bool MCAPReplay::_load_schema(const mcap::SchemaPtr schema, return true; } +std::shared_ptr 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(protoFactory.GetPrototype(descriptor)->New()); + + + if (!msg->ParseFromArray(mcap_msg.data, static_cast(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()) { @@ -36,34 +79,63 @@ void MCAPReplay::start(std::string filename) { return; } - auto msg_view = _mcap_reader.readMessages(); + _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; } - spdlog::info("name: {}", it->schema->name); - const gp::Descriptor *descriptor = protoPool.FindMessageTypeByName(it->schema->name); + auto next_ns = std::chrono::nanoseconds(it->message.logTime); - if (descriptor == nullptr) { - if (!_load_schema(it->schema, &protoDb)) { - spdlog::error("failed to load schema, exiting"); - _mcap_reader.close(); - return; + // spdlog::info("time diff in ns: {}", std::chrono::duration_cast( + // 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); } - descriptor = protoPool.FindMessageTypeByName(it->schema->name); - if (descriptor == nullptr) { - spdlog::error("failed to find desc after loading pool, exiting"); - _mcap_reader.close(); - return; + } + + prev_ns = next_ns; + + auto msg = _get_pb_msg(protoPool, protoFactory, it->schema, &protoDb, it->message); + if (!it->schema->name.rfind("hytech.", 0)) // denotes CAN message + { + { + std::unique_lock lk(_primary_can_tx_queue.mtx); + _primary_can_tx_queue.deque.push_back(msg); + _primary_can_tx_queue.cv.notify_all(); } } + + 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(); diff --git a/drivebrain_utils/src/replay_app.cpp b/drivebrain_utils/src/replay_app.cpp index d41a0a1..e162346 100644 --- a/drivebrain_utils/src/replay_app.cpp +++ b/drivebrain_utils/src/replay_app.cpp @@ -8,12 +8,14 @@ int main(int argc, char* argv[]) po::options_description desc("Allowed options"); - std::string mcap_filepath; + 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(&mcap_filepath), "Path to mcap file"); + ("mcap,m", po::value(&mcap_filepath), "Path to mcap file") + ("config,c", po::value(&config_filepath), "Path to config file") + ("dbc,d", po::value(&dbc_filepath), "Path to dbc file"); - util::MCAPReplay replay_util; po::variables_map vm; po::store(po::parse_command_line(argc, argv, desc), vm); po::notify(vm); @@ -23,6 +25,7 @@ int main(int argc, char* argv[]) std::exit(0); } + util::MCAPReplay replay_util(config_filepath, dbc_filepath); replay_util.start(mcap_filepath); return 0; } \ No newline at end of file From 8e07f020c6a3cadc10d7fe30a58b84449deeb9dd Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 11 May 2025 14:00:10 -0400 Subject: [PATCH 50/87] Feature/replay eth (#40) * adding in replay with ethernet fake vn * fixing valgrind issues --- .gitignore | 3 +- CMakeLists.txt | 1 + README.md | 5 ++ config/drivebrain_config.json | 3 +- drivebrain_app/include/DriveBrainApp.hpp | 2 + drivebrain_app/src/DriveBrainApp.cpp | 14 ++- .../drivebrain_comms/include/ETHRecvComms.hpp | 3 +- .../drivebrain_comms/include/ETHRecvComms.tpp | 17 ++-- .../drivebrain_comms/include/ETHSendComms.hpp | 69 ++++++++++++++ .../drivebrain_comms/src/CANComms.cpp | 2 +- .../drivebrain_comms/src/ETHSendComms.cpp | 89 +++++++++++++++++++ .../include/SimpleSpeedController.hpp | 18 ++-- .../src/SimpleSpeedController.cpp | 2 +- drivebrain_utils/include/MCAPReplay.hpp | 6 ++ drivebrain_utils/src/MCAPReplay.cpp | 15 ++++ flake.lock | 7 +- flake.nix | 2 +- 17 files changed, 232 insertions(+), 26 deletions(-) create mode 100644 drivebrain_core_impl/drivebrain_comms/include/ETHSendComms.hpp create mode 100644 drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp diff --git a/.gitignore b/.gitignore index 235512b..ea51db7 100644 --- a/.gitignore +++ b/.gitignore @@ -11,4 +11,5 @@ result-man .cache/ *.dbc -compile_commands.json \ No newline at end of file +compile_commands.json +*.log \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index b0e6891..d8be2df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,7 @@ add_library(drivebrain_comms SHARED drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp + drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp ) target_compile_features(drivebrain_comms PUBLIC cxx_std_17) diff --git a/README.md b/README.md index d7b8780..1b916cc 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,8 @@ +# execution +- drivebrain: +``` +./build +``` # 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: diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 5653051..7857a02 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -52,6 +52,7 @@ "rr_sus_pot_max": 2000.0, "rr_sus_pot_max_mm": 1.0 }, - "use_vectornav": false + "use_vectornav": false, + "use_fake_vn": true } diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 246262c..7357f0e 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -81,6 +82,7 @@ class DriveBrainApp { std::unique_ptr> _acu_eth_driver; std::unique_ptr> _vcr_eth_driver; std::unique_ptr> _vcf_eth_driver; + std::unique_ptr> _fake_vn = nullptr; std::shared_ptr _vn_driver; std::unique_ptr _db_service; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index e527a3b..3657bf1 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -4,6 +4,7 @@ #include "SimpleSpeedController.hpp" #include "SimpleTorqueController.hpp" #include "hytech.pb.h" +#include #include #include #include @@ -66,9 +67,9 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d configurable_components.push_back(_driver_primary_can); configurable_components.push_back(_driver_secondary_can); spdlog::info("made CAN driver"); - _acu_eth_driver = std::make_unique>(_message_logger,_io_context, 7766); - _vcr_eth_driver = std::make_unique>(_message_logger,_io_context, 9999); - _vcf_eth_driver = std::make_unique>(_message_logger,_io_context, 4444); + _acu_eth_driver = std::make_unique>(_io_context, 7766); + _vcr_eth_driver = std::make_unique>(_io_context, 9999); + _vcf_eth_driver = std::make_unique>(_io_context, 4444); spdlog::info("eth drivers"); @@ -89,6 +90,9 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d 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_unique>( _io_context, 13111, _state_estimator); } @@ -132,6 +136,10 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { _vn_driver->update_msg_logger(_message_logger); } + if(_fake_vn) + { + _fake_vn->update_msg_logger(_message_logger); + } if(_state_estimator) { _state_estimator->update_msg_logger(_message_logger); diff --git a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp index d25b947..82ee63e 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp @@ -34,7 +34,7 @@ namespace comms using loggertype = core::MsgLogger>; ETHRecvComms() = delete; ~ETHRecvComms(); - ETHRecvComms(std::shared_ptr message_logger, boost::asio::io_context &io_context, uint16_t recv_port); + ETHRecvComms(boost::asio::io_context &io_context, uint16_t recv_port, std::shared_ptr state_estim=nullptr); void update_msg_logger(std::shared_ptr message_logger) { _message_logger = message_logger; } @@ -45,6 +45,7 @@ namespace comms private: std::shared_ptr _message_logger; + std::shared_ptr _state_estimator = nullptr; std::array _recv_buffer; boost::asio::ip::udp::socket _socket; boost::asio::ip::udp::endpoint _remote_endpoint; diff --git a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp index 5a3b25f..63373ff 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp +++ b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp @@ -7,10 +7,12 @@ using boost::asio::ip::udp; namespace comms { template - ETHRecvComms::ETHRecvComms(std::shared_ptr message_logger, - boost::asio::io_context &io_context, - uint16_t port) : _message_logger(message_logger), - _socket(io_context, udp::endpoint(udp::v4(), port)) + ETHRecvComms::ETHRecvComms(boost::asio::io_context &io_context, + uint16_t port, + std::shared_ptr state_estim) + : _message_logger(nullptr), + _state_estimator(state_estim), + _socket(io_context, udp::endpoint(udp::v4(), port)) { _eth_msg = std::make_shared(); _start_receive(); @@ -35,8 +37,13 @@ namespace comms { _message_logger->log_msg(out_msg); } else { - spdlog::warn("Message logger not real"); + spdlog::info("Message logger not real"); } + if(_state_estimator) + { + _state_estimator->handle_recv_process(out_msg); + } + _start_receive(); } 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 0000000..c4ad55a --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/include/ETHSendComms.hpp @@ -0,0 +1,69 @@ +#ifndef __ETHSENDCOMMS_H__ +#define __ETHSENDCOMMS_H__ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include "hytech_msgs.pb.h" +#include + +// - [ ] 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: + using deqtype = core::common::ThreadSafeDeque>; + using loggertype = core::MsgLogger>; + + ETHSendComms() = delete; + ~ETHSendComms(); + ETHSendComms(std::shared_ptr message_logger, boost::asio::io_context &io_context, uint16_t send_port, std::string send_ip, bool bind=true); + + void update_msg_logger(std::shared_ptr message_logger) { + _message_logger = message_logger; + } + + // 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 send_msg); + + private: + // is this required? i think that it is + void _send_completition_handler(std::array /*message*/, + const boost::system::error_code & /*error*/, + std::size_t /*bytes_transferred*/); + void _handle_send_msg_from_queue(); + void _send_message(std::shared_ptr msg_out); + + private: + + boost::asio::ip::udp::endpoint _endp; + + std::shared_ptr _message_logger; + std::array _recv_buffer; + boost::asio::ip::udp::socket _socket; + boost::asio::ip::udp::endpoint _remote_endpoint; + bool _running = false; + std::thread _output_thread; + std::array _send_buffer; + deqtype _queue; + }; + +} + +#endif // __ETHSENDCOMMS_H__ \ No newline at end of file diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index d370079..fe9a5e1 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -79,7 +79,7 @@ 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; 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 0000000..8c0990b --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp @@ -0,0 +1,89 @@ +#include "ETHSendComms.hpp" +namespace comms +{ +using boost::asio::ip::udp; +ETHSendComms::ETHSendComms(std::shared_ptr message_logger, boost::asio::io_context &io_context, uint16_t send_port, std::string send_ip, bool bind) +: _message_logger(message_logger), +_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 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); + if(_message_logger) + { + _message_logger->log_msg(msg); + } + + } + _queue.deque.clear(); + } + } +} + +// don do nothin +void ETHSendComms::_send_completition_handler(std::array /*message*/, + const boost::system::error_code & /*error*/, + std::size_t /*bytes_transferred*/) +{ + +} + +void ETHSendComms::_send_message(std::shared_ptr 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(ÐSendComms::_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_control/include/SimpleSpeedController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp index 06627f0..2fef8de 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp @@ -24,14 +24,14 @@ namespace control // 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; - float max_power_kw; - int dt_rate_hz; - }; + 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 (double) 1.0 / _config.dt_rate_hz; @@ -44,6 +44,6 @@ namespace control core::SpeedControlOut _apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms); private: std::mutex _config_mutex; - config _config; + config _config{}; }; } \ No newline at end of file diff --git a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp index 697a534..48eec3b 100644 --- a/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.cpp @@ -100,7 +100,7 @@ core::ControllerOutput control::SimpleSpeedController::step_controller(const cor core::ControllerOutput cmd_out = {}; cmd_out.out = type_set; auto& speed_out = std::get(cmd_out.out); - + speed_out = {}; speed_out.mcu_recv_millis = in.prev_MCU_recv_millis; // heartbeat if (accelRequest >= 0.0) diff --git a/drivebrain_utils/include/MCAPReplay.hpp b/drivebrain_utils/include/MCAPReplay.hpp index b65c067..1bcfee9 100644 --- a/drivebrain_utils/include/MCAPReplay.hpp +++ b/drivebrain_utils/include/MCAPReplay.hpp @@ -12,6 +12,8 @@ // Drivebrain impl components #include +#include + #define MCAP_IMPLEMENTATION #include "mcap/reader.hpp" @@ -48,6 +50,10 @@ namespace util core::common::ThreadSafeDeque> _primary_can_tx_queue; boost::asio::io_context _io_context; std::shared_ptr _driver_primary_can = nullptr; + std::shared_ptr _acu_sender = nullptr; + std::shared_ptr _vcr_sender = nullptr; + std::shared_ptr _acu_core_sender = nullptr; + std::shared_ptr _vn_sender = nullptr; mcap::McapReader _mcap_reader; std::thread _io_context_thread; diff --git a/drivebrain_utils/src/MCAPReplay.cpp b/drivebrain_utils/src/MCAPReplay.cpp index dd9d516..95bcfc3 100644 --- a/drivebrain_utils/src/MCAPReplay.cpp +++ b/drivebrain_utils/src/MCAPReplay.cpp @@ -20,6 +20,13 @@ MCAPReplay::MCAPReplay(std::string param_file_path, std::string dbc_file_path) { throw std::runtime_error("Failed to initialize can driver"); } + + _acu_sender = std::make_shared(nullptr, _io_context, 7766, "127.0.0.1", false); + // _acu_core_sender = std::make_shared(nullptr, _io_context, 7777, "127.0.0.1"); + // _vcr_sender = std::make_shared(nullptr, _io_context, 9999, "127.0.0.1"); + _vn_sender = std::make_shared(nullptr, _io_context, 13111, "127.0.0.1", false); // fake VN + + } bool MCAPReplay::_load_schema(const mcap::SchemaPtr schema, @@ -126,6 +133,14 @@ void MCAPReplay::start(std::string filename) { _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); } prev_start = loop_start; diff --git a/flake.lock b/flake.lock index 2b32655..8ac655f 100644 --- a/flake.lock +++ b/flake.lock @@ -6,16 +6,17 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1746748475, - "narHash": "sha256-pj26RU9QyY8uJLwmgFPz+HMVWslHpQGHBkR7DhoJHr8=", + "lastModified": 1746370014, + "narHash": "sha256-tJylKeVpbiAGPpUwZYwCcxNNmpm74mMVvMludIZaMVM=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "6d3404aa2bc9d1bde5cc65a3e5efa200b6a2f924", + "rev": "2afaef827a82f4dfc172d5ed6ed90000ba500513", "type": "github" }, "original": { "owner": "hytech-racing", "repo": "HT_proto", + "rev": "2afaef827a82f4dfc172d5ed6ed90000ba500513", "type": "github" } }, diff --git a/flake.nix b/flake.nix index c8caec5..e932f91 100644 --- a/flake.nix +++ b/flake.nix @@ -16,7 +16,7 @@ nix-proto.url = "github:notalltim/nix-proto"; nix-proto.inputs.nixpkgs.follows = "nixpkgs"; - HT_proto.url = "github:hytech-racing/HT_proto"; + HT_proto.url = "github:hytech-racing/HT_proto/2afaef827a82f4dfc172d5ed6ed90000ba500513"; foxglove-schemas-src = { url = "github:foxglove/schemas"; From edcde4bf9bbe905713d4482cc9bd0c61678adc57 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 11 May 2025 17:53:38 -0400 Subject: [PATCH 51/87] fixing unit tests for the controller manager and using new core version to enable safe controller implementation and fixing simple speed control --- README.md | 20 ++ config/drivebrain_config.json | 8 +- config/simple_torque_controller_test.json | 8 + config/test_controller_manager.json | 23 ++ config/test_tcmux_integration.json | 8 +- drivebrain_app/include/DriveBrainApp.hpp | 4 +- drivebrain_app/src/DriveBrainApp.cpp | 10 +- drivebrain_utils/include/MCAPReplay.hpp | 6 +- drivebrain_utils/src/MCAPReplay.cpp | 9 +- flake.lock | 6 +- flake.nix | 3 +- unit_test/SimpleTorqueControllerTest.cpp | 2 +- unit_test/test_controller_manager.cpp | 298 +++++++++++----------- 13 files changed, 230 insertions(+), 175 deletions(-) create mode 100644 config/simple_torque_controller_test.json create mode 100644 config/test_controller_manager.json diff --git a/README.md b/README.md index 1b916cc..68a9dc2 100644 --- a/README.md +++ b/README.md @@ -120,4 +120,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/alpha_build", + "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/config/drivebrain_config.json b/config/drivebrain_config.json index 7857a02..09f0bbf 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -22,15 +22,9 @@ "port": 1, "freq_divisor": 1 }, - "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_switch_requested_rpm": 20000.0, + "max_requested_rpm": 20000.0, "max_torque_switch_nm": 10.0, "max_accel_switch_float": 0.1 }, diff --git a/config/simple_torque_controller_test.json b/config/simple_torque_controller_test.json new file mode 100644 index 0000000..b5f2e0d --- /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 0000000..d54d120 --- /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_tcmux_integration.json b/config/test_tcmux_integration.json index 92e91d1..2c6377d 100644 --- a/config/test_tcmux_integration.json +++ b/config/test_tcmux_integration.json @@ -1,16 +1,10 @@ g{ "ControllerManager": { "max_controller_switch_speed_ms": 1.793, - "max_switch_requested_rpm": 20000.0, + "max_requested_rpm": 20000.0, "max_torque_switch_nm": 10.0, "max_accel_switch_float": 0.1 }, - "SimpleTorqueController": { - "max_torque": 21.0, - "max_regen_torque": 10.0, - "rear_torque_scale": 1.0, - "regen_torque_scale": 0.6 - }, "SimpleSpeedController": { "max_torque": 21.0, "max_regen_torque": 10.0, diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 7357f0e..3e30f60 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -1,7 +1,6 @@ // DriveBrainApp.hpp #pragma once -#include "SimpleTorqueController.hpp" #include #include #include @@ -71,8 +70,7 @@ class DriveBrainApp { std::vector> _configurable_components; std::shared_ptr _mcap_logger; std::shared_ptr controller1; - std::shared_ptr controller2; - control::ControllerManager, 2 > _controllerManager; + control::ControllerManager, 1> _controllerManager; // std::unique_ptr _matlab_math; std::shared_ptr _foxglove_server; std::shared_ptr>> _message_logger = nullptr; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 3657bf1..514fe45 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -2,7 +2,6 @@ #include "DriveBrainApp.hpp" #include "SimpleSpeedController.hpp" -#include "SimpleTorqueController.hpp" #include "hytech.pb.h" #include #include @@ -20,8 +19,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d , _config(_param_path) , _settings(settings) , controller1(std::make_shared(_config)) - , controller2(std::make_shared(_config)) - , _controllerManager(_config, {controller1, controller2}) // Initialize correctly + , _controllerManager(_config, {controller1}) // Initialize correctly { // spdlog::info("top o"); std::vector> configurable_components; @@ -32,6 +30,12 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if (!controller1->init()) { throw std::runtime_error("Failed to initialize controller"); } + // 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({controller1}); + if(!_controllerManager.init()){ + throw std::runtime_error("Failed to initialize controller manager"); + } + configurable_components.push_back(controller1); spdlog::info("made controller"); diff --git a/drivebrain_utils/include/MCAPReplay.hpp b/drivebrain_utils/include/MCAPReplay.hpp index 1bcfee9..2960880 100644 --- a/drivebrain_utils/include/MCAPReplay.hpp +++ b/drivebrain_utils/include/MCAPReplay.hpp @@ -27,8 +27,10 @@ // 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 -// [ ] use instances of (TODO) ETHSendComms to send the VCF/VCR/ACU data - // [ ] create ETHSendComms +// [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 { diff --git a/drivebrain_utils/src/MCAPReplay.cpp b/drivebrain_utils/src/MCAPReplay.cpp index 95bcfc3..d5d44f1 100644 --- a/drivebrain_utils/src/MCAPReplay.cpp +++ b/drivebrain_utils/src/MCAPReplay.cpp @@ -126,7 +126,14 @@ void MCAPReplay::start(std::string filename) { prev_ns = next_ns; auto msg = _get_pb_msg(protoPool, protoFactory, it->schema, &protoDb, it->message); - if (!it->schema->name.rfind("hytech.", 0)) // denotes CAN 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); diff --git a/flake.lock b/flake.lock index 8ac655f..138fc73 100644 --- a/flake.lock +++ b/flake.lock @@ -55,11 +55,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1746831657, - "narHash": "sha256-SwBOefZrjmU3xrZt2FUyIi3B25ojt8xiTotM+F6uyvs=", + "lastModified": 1747000350, + "narHash": "sha256-ARgNO0nuQd4IWO1sjaE1lOwYv9FVLHS++wNKV5RJa5M=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "228e25eccd359f82aa9c64a2f7f304d675fd9779", + "rev": "8bbea20847d0adfb80c71fa030389efe5db7c3a6", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index e932f91..4415f6e 100644 --- a/flake.nix +++ b/flake.nix @@ -134,8 +134,8 @@ 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" ''; nativeBuildInputs = [ pkgs.drivebrain_core_msgs_proto_cpp ]; packages = [ pkgs.mcap-cli pkgs.valgrind ]; @@ -154,7 +154,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 ]; diff --git a/unit_test/SimpleTorqueControllerTest.cpp b/unit_test/SimpleTorqueControllerTest.cpp index 3244db1..ccb65fb 100644 --- a/unit_test/SimpleTorqueControllerTest.cpp +++ b/unit_test/SimpleTorqueControllerTest.cpp @@ -17,7 +17,7 @@ class SimpleTorqueControllerTest : public testing::Test { SimpleTorqueControllerTest() : logger(core::LogLevel::INFO), - config("../config/drivebrain_config.json"), + config("../config/simple_torque_controller_test.json"), fail_config("../config/fail_config.json"), simple_controller(config), fail_controller(fail_config), diff --git a/unit_test/test_controller_manager.cpp b/unit_test/test_controller_manager.cpp index 80178a1..7b946e7 100644 --- a/unit_test/test_controller_manager.cpp +++ b/unit_test/test_controller_manager.cpp @@ -25,7 +25,7 @@ class ControllerManagerTest : public ::testing::Test { ControllerManagerTest() : - json_file_handler("../config/drivebrain_config.json"), + json_file_handler("../config/test_controller_manager.json"), simpleSpeedController1(std::make_shared(json_file_handler)), simpleSpeedController2(std::make_shared(json_file_handler)), simpleTorqueController1(std::make_shared(json_file_handler)), @@ -42,14 +42,18 @@ class ControllerManagerTest : public ::testing::Test { vehicle_state.input.requested_brake = 0.0; vehicle_state.current_rpms = {1000, 1000, 1000, 1000}; - controller_manager_2speed.init(); - controller_manager_2torque.init(); - controller_manager_diff.init(); 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; } @@ -58,147 +62,149 @@ class ControllerManagerTest : public ::testing::Test { } }; -// // 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); +// 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(output.out)); -// auto torque_output = std::get(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(output.out)); + auto torque_output = std::get(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(output.out)); -// auto speed_output = std::get(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(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(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(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(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(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(output.out)); + auto speed_output = std::get(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(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(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(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(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(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(output.out)); -// auto _output = std::get(output.out); -// ASSERT_TRUE(_output.desired_rpms.FL - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); -// ASSERT_TRUE(_output.desired_rpms.FR - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); -// ASSERT_TRUE(_output.desired_rpms.RL - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); -// ASSERT_TRUE(_output.desired_rpms.RR - constants::METERS_PER_SECOND_TO_RPM * 3 < 10.0); -// } - -// 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(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(output.out)); -// } \ No newline at end of file + ASSERT_TRUE(std::holds_alternative(output.out)); + auto _output = std::get(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(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(output.out)); +} \ No newline at end of file From d5d232a223af90a574653fdf47055119283b39a6 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 11 May 2025 18:00:41 -0400 Subject: [PATCH 52/87] updating to latest drivebrain_core --- flake.lock | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flake.lock b/flake.lock index 138fc73..4a52e54 100644 --- a/flake.lock +++ b/flake.lock @@ -55,11 +55,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1747000350, - "narHash": "sha256-ARgNO0nuQd4IWO1sjaE1lOwYv9FVLHS++wNKV5RJa5M=", + "lastModified": 1747000808, + "narHash": "sha256-bH5V0QOYEmJJrKpbVN4AlBCMDe/aVjCey8vphhY+0u4=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "8bbea20847d0adfb80c71fa030389efe5db7c3a6", + "rev": "114cc790d7bddf0b9da618622974fdc5be12f937", "type": "github" }, "original": { From e3d250bcfeb719c56830a5fc5cc7a9bd23d3945e Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 11 May 2025 18:06:11 -0400 Subject: [PATCH 53/87] updating readme --- README.md | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 68a9dc2..7fb240c 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,31 @@ # execution + +```bash +./build/alpha_build -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 +./build/alpha_build -p ./config/drivebrain_config.json -d ./config/hytech.dbc -c false +``` + +- replay app: + +```bash +./build/replay_app -m -c ./config/drivebrain_config.json -d ./config/hytech.dbc ``` # About From 073d85cc31fcf43011539f8443699ae2cdea31f3 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 11 May 2025 18:39:53 -0400 Subject: [PATCH 54/87] if the state is not valid, stop outputting the drivebrain commands. this should trigger the vehicle firmware to switch to its fallback controller in the drivebrain control mode --- drivebrain_app/src/DriveBrainApp.cpp | 93 ++++++++++--------- .../src/StateEstimator.cpp | 1 + 2 files changed, 52 insertions(+), 42 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 514fe45..9b2ad86 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -224,7 +224,7 @@ void DriveBrainApp::_process_loop() { auto start_time = std::chrono::high_resolution_clock::now(); auto state_and_validity = _state_estimator->get_latest_state_and_validity(); - + auto out_struct = _controllerManager.step_active_controller(state_and_validity.first); // get current command @@ -232,52 +232,61 @@ void DriveBrainApp::_process_loop() { // 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) + { - if (const core::SpeedControlOut* speedControl = std::get_if(&cmd_out)) { // speed controller, set RPM + if (const core::SpeedControlOut* speedControl = std::get_if(&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>(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)); - if(_message_logger) - { - _message_logger->log_msg(static_cast>(torque_limit_msg)); - } + // set RPMs in message to the RPMS given from the controller - { - 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"); - } + 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); - } else if (const core::TorqueControlOut* torqueControl = std::get_if(&cmd_out)){ // if it is a torque controller: - // set desired torque - desired_torque_msg->set_drivebrain_torque_fl(::abs(torqueControl->desired_torques_nm.FL)); - desired_torque_msg->set_drivebrain_torque_fr(::abs(torqueControl->desired_torques_nm.FR)); - desired_torque_msg->set_drivebrain_torque_rl(::abs(torqueControl->desired_torques_nm.RL)); - desired_torque_msg->set_drivebrain_torque_rr(::abs(torqueControl->desired_torques_nm.RR)); - if(_message_logger) - { - _message_logger->log_msg(static_cast>(desired_torque_msg)); - } + if(_message_logger) + { + _message_logger->log_msg(static_cast>(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_torque_msg); // use new protobuf struct - _primary_can_tx_queue.cv.notify_all(); // notify the CAN thread to send the messages + if(_message_logger) + { + _message_logger->log_msg(static_cast>(torque_limit_msg)); + } + + { + 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(&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); + + if(_message_logger) + { + _message_logger->log_msg(static_cast>(desired_torque_msg)); + } + + { + 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 + } } } diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 8ee0a7f..41df9f0 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -182,6 +182,7 @@ StateEstimator::append_state_variables_from_raw_inputs(core::VehicleState vs, _config.rr_sus_pot_min_mm, _config.rr_sus_pot_max_mm); 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; From 1edc0cbcd41310f697ec36f536218d3b0c50b47d Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 11 May 2025 18:51:43 -0400 Subject: [PATCH 55/87] fixed name of drivebrain executable --- CMakeLists.txt | 19 +++++++------------ README.md | 6 +++--- drivebrain_app/src/DriveBrainApp.cpp | 3 --- module.nix | 2 +- 4 files changed, 11 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d8be2df..baa3084 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -300,23 +300,18 @@ 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 ) enable_testing() -# add_executable(alpha_test -# unit_test/main.cpp -# # unit_test/SimpleControllerTest.cpp -# ) - -target_link_libraries(alpha_build PUBLIC +target_link_libraries(drivebrain_exe PUBLIC drivebrain_app drivebrain_core::drivebrain_core drivebrain_control @@ -339,14 +334,14 @@ target_link_libraries(test_build PUBLIC enable_testing() -add_executable(alpha_test +add_executable(drivebrain_test unit_test/main.cpp unit_test/SimpleSpeedControllerTest.cpp unit_test/SimpleTorqueControllerTest.cpp unit_test/test_controller_manager.cpp ) -target_link_libraries(alpha_test PUBLIC +target_link_libraries(drivebrain_test PUBLIC drivebrain_core::drivebrain_core drivebrain_control drivebrain_comms @@ -354,7 +349,7 @@ target_link_libraries(alpha_test PUBLIC gtest ) -add_test(NAME Drivebrain_Tests 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 @@ -364,7 +359,7 @@ target_link_libraries(mcu_standin PUBLIC include(GNUInstallDirs) install(TARGETS - alpha_build + drivebrain_exe test_param_server test_build RUNTIME diff --git a/README.md b/README.md index 7fb240c..f5a42c8 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # execution ```bash -./build/alpha_build -h +./build/drivebrain_exe -h Allowed options: -h [ --help ] produce help message -p [ --param-path ] arg Path to the parameter JSON file @@ -19,7 +19,7 @@ Allowed options: ``` - drivebrain: ``` -./build/alpha_build -p ./config/drivebrain_config.json -d ./config/hytech.dbc -c false +./build/drivebrain_exe -p ./config/drivebrain_config.json -d ./config/hytech.dbc -c false ``` - replay app: @@ -156,7 +156,7 @@ example `c_cpp_properties.json` file to be placed in your `.vscode/`: "name": "Debug", "type": "cppdbg", // use "cppdbg" with gdb on Linux "request": "launch", - "program": "${workspaceFolder}/build/alpha_build", + "program": "${workspaceFolder}/build/drivebrain_exe", "args": ["-p", "./config/drivebrain_config.json", "-d", "./config/hytech.dbc", "-c", "false"], "cwd": "${workspaceFolder}", "stopAtEntry": false, diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 9b2ad86..f745717 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -356,6 +356,3 @@ void DriveBrainApp::run() { std::this_thread::sleep_for(std::chrono::seconds(1)); } } - -// ./test_build -p ../config/drivebrain_config.json -d ../config/hytech.dbc -// ./alpha_build -p ../config/drivebrain_config.json -d ../config/hytech.dbc \ No newline at end of file diff --git a/module.nix b/module.nix index 00170ba..c6c62b6 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"; }; From 8dbdbe0a371b972dfa81ce228c6c7e7dddd82016 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 14 May 2025 02:45:51 -0400 Subject: [PATCH 56/87] Feature/aero driver integration (#43) * working test of the surrey aero sensor driver * working on laptop integration test also works on car --- CMakeLists.txt | 6 + config/drivebrain_config.json | 6 +- config/test_surrey_aero_comms.json | 5 + drivebrain_app/include/DriveBrainApp.hpp | 5 +- drivebrain_app/src/DriveBrainApp.cpp | 36 ++++++ .../include/SurreyAeroComms.hpp | 47 ++++++++ .../drivebrain_comms/src/SurreyAeroComms.cpp | 114 ++++++++++++++++++ flake.lock | 16 +-- flake.nix | 4 +- test/test_surrey_aero_comms.cpp | 15 +++ 10 files changed, 242 insertions(+), 12 deletions(-) create mode 100644 config/test_surrey_aero_comms.json create mode 100644 drivebrain_core_impl/drivebrain_comms/include/SurreyAeroComms.hpp create mode 100644 drivebrain_core_impl/drivebrain_comms/src/SurreyAeroComms.cpp create mode 100644 test/test_surrey_aero_comms.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index baa3084..7eb6db2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,6 +84,7 @@ add_library(drivebrain_comms SHARED drivebrain_core_impl/drivebrain_comms/src/VNComms.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 ) target_compile_features(drivebrain_comms PUBLIC cxx_std_17) @@ -243,6 +244,11 @@ 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_dbcan test/test_dbcan.cpp) # target_link_libraries(test_dbcan PUBLIC diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 09f0bbf..1f88194 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -46,7 +46,11 @@ "rr_sus_pot_max": 2000.0, "rr_sus_pot_max_mm": 1.0 }, + "SurreyAeroComms": { + "port_name": "/dev/ttyACM0" + }, "use_vectornav": false, - "use_fake_vn": true + "use_fake_vn": false, + "use_surrey_aero": true } diff --git a/config/test_surrey_aero_comms.json b/config/test_surrey_aero_comms.json new file mode 100644 index 0000000..1e06ad8 --- /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/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 3e30f60..01ef03c 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -1,6 +1,7 @@ // DriveBrainApp.hpp #pragma once +#include "SurreyAeroComms.hpp" #include #include #include @@ -60,6 +61,7 @@ class DriveBrainApp { std::optional _dbc_path; boost::asio::io_context _io_context; boost::asio::io_context _io_context_secondary_can; + boost::asio::io_context _aero_usb_io_context; core::common::ThreadSafeDeque> _rx_queue; core::common::ThreadSafeDeque> _primary_can_tx_queue; @@ -77,6 +79,7 @@ class DriveBrainApp { std::shared_ptr _state_estimator; std::shared_ptr _driver_primary_can; std::shared_ptr _driver_secondary_can; + std::shared_ptr _aero_sensor_driver = nullptr; std::unique_ptr> _acu_eth_driver; std::unique_ptr> _vcr_eth_driver; std::unique_ptr> _vcf_eth_driver; @@ -88,6 +91,6 @@ class DriveBrainApp { std::thread _io_context_thread; std::thread _io_context_secondary_thread; std::thread _db_service_thread; - + std::thread _aero_usb_io_context_thread; const DriveBrainSettings _settings; }; \ No newline at end of file diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index f745717..d5cc39e 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -2,6 +2,7 @@ #include "DriveBrainApp.hpp" #include "SimpleSpeedController.hpp" +#include "SurreyAeroComms.hpp" #include "hytech.pb.h" #include #include @@ -97,6 +98,17 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d } else if(config_json.contains("use_fake_vn") && config_json["use_fake_vn"]) { _fake_vn = std::make_unique>( _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(_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"); } @@ -165,6 +177,10 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { _vcf_eth_driver->update_msg_logger(_message_logger); } + if(_aero_sensor_driver) + { + _aero_sensor_driver->set_msg_logger(_message_logger); + } _message_logger->start_logging_params(); @@ -199,6 +215,13 @@ DriveBrainApp::~DriveBrainApp() { 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 (_db_service) { @@ -345,6 +368,19 @@ void DriveBrainApp::run() { } }); + if(_aero_sensor_driver) + { + _aero_usb_io_context_thread = std::thread([this]() { + 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()); + } + }); + } + + _process_thread = std::thread([this]() { if (!_settings.run_process_loop) return; _process_loop(); 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 0000000..0a67bc6 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/include/SurreyAeroComms.hpp @@ -0,0 +1,47 @@ +#ifndef __SURREYAEROCOMMS_H__ +#define __SURREYAEROCOMMS_H__ + + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +#include + + + +namespace comms { + + class SurreyAeroComms : public core::common::Loggable>, 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::vector _extract_sensor_readings(const boost::array& buffer); + void _log_proto_message(const std::vector& readings); + private: + boost::asio::serial_port _serial; + std::chrono::steady_clock::time_point _timestamp_of_last_debug_p; + + boost::array _input_buff{}; + }; +} + +# +#endif // __SURREYAEROCOMMS_H__ \ 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 0000000..88a9231 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/src/SurreyAeroComms.cpp @@ -0,0 +1,114 @@ +#include "SurreyAeroComms.hpp" + +#include +#include + + +#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) { + std::vector sensor_readings = _extract_sensor_readings(_input_buff); + + _log_proto_message(sensor_readings); + _start_receive(); + }); +} + +std::vector SurreyAeroComms::_extract_sensor_readings(const boost::array& buffer) { + std::vector readings; + if (buffer[0] != '#') { + spdlog::error("invalid aero sensor frame received"); + return readings; + } + + + auto now_time = std::chrono::steady_clock::now(); + + auto elapsed = std::chrono::duration_cast(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& readings) { + auto msg_out = std::make_shared(); + 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/flake.lock b/flake.lock index 4a52e54..a3d5936 100644 --- a/flake.lock +++ b/flake.lock @@ -6,17 +6,17 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1746370014, - "narHash": "sha256-tJylKeVpbiAGPpUwZYwCcxNNmpm74mMVvMludIZaMVM=", + "lastModified": 1747196911, + "narHash": "sha256-Y8x8DywKp2/d3+9dSK7c4bh2CtNQOht/tvZhNptKpjU=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "2afaef827a82f4dfc172d5ed6ed90000ba500513", + "rev": "cfcf44824c5cbfebe137ce8d330203616f184697", "type": "github" }, "original": { "owner": "hytech-racing", + "ref": "2025-05-14T04_28_50", "repo": "HT_proto", - "rev": "2afaef827a82f4dfc172d5ed6ed90000ba500513", "type": "github" } }, @@ -55,16 +55,16 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1747000808, - "narHash": "sha256-bH5V0QOYEmJJrKpbVN4AlBCMDe/aVjCey8vphhY+0u4=", + "lastModified": 1747194373, + "narHash": "sha256-2VVlZo9MYAWx0u1WJNYhKHLEcKtq50DyvZVeRjdlGpk=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "114cc790d7bddf0b9da618622974fdc5be12f937", + "rev": "59bacd7e3e104138fc442b933404b54d8fc523c6", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "main", + "ref": "feat/loggable_w_position_type", "repo": "drivebrain_core", "type": "github" } diff --git a/flake.nix b/flake.nix index 4415f6e..06d2835 100644 --- a/flake.nix +++ b/flake.nix @@ -16,7 +16,7 @@ nix-proto.url = "github:notalltim/nix-proto"; nix-proto.inputs.nixpkgs.follows = "nixpkgs"; - HT_proto.url = "github:hytech-racing/HT_proto/2afaef827a82f4dfc172d5ed6ed90000ba500513"; + HT_proto.url = "github:hytech-racing/HT_proto/2025-05-14T04_28_50"; foxglove-schemas-src = { url = "github:foxglove/schemas"; @@ -30,7 +30,7 @@ vn_driver_lib.url = "github:RCMast3r/vn_driver_lib/fix/boost-compatible"; db-core-src = { - url = "github:hytech-racing/drivebrain_core/main"; + url = "github:hytech-racing/drivebrain_core/feat/loggable_w_position_type"; flake = false; }; diff --git a/test/test_surrey_aero_comms.cpp b/test/test_surrey_aero_comms.cpp new file mode 100644 index 0000000..9a1340e --- /dev/null +++ b/test/test_surrey_aero_comms.cpp @@ -0,0 +1,15 @@ +#include +#include + + +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; +} From 4a3b66ee75a7904c1898af1e08061cd18a8fe97c Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 19 May 2025 14:17:35 -0400 Subject: [PATCH 57/87] speed tech comms working in test binary. To be tested on car with drivebrain app (#42) * speed tech comms working in test binary. To be tested on car with drivebrain app. --- CMakeLists.txt | 8 ++ config/test_speedtech_comms.json | 6 + drivebrain_app/include/DriveBrainApp.hpp | 9 +- drivebrain_app/src/DriveBrainApp.cpp | 38 ++++++- .../include/SpeedTechComms.hpp | 51 +++++++++ .../drivebrain_comms/src/SpeedTechComms.cpp | 104 ++++++++++++++++++ .../drivebrain_comms/src/VNComms.cpp | 2 +- flake.nix | 6 +- test/test_speedtech_comms.cpp | 14 +++ 9 files changed, 229 insertions(+), 9 deletions(-) create mode 100644 config/test_speedtech_comms.json create mode 100644 drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp create mode 100644 drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp create mode 100644 test/test_speedtech_comms.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 7eb6db2..312c007 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,6 +85,7 @@ add_library(drivebrain_comms SHARED 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 ) target_compile_features(drivebrain_comms PUBLIC cxx_std_17) @@ -249,6 +250,13 @@ 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_dbcan test/test_dbcan.cpp) # target_link_libraries(test_dbcan PUBLIC diff --git a/config/test_speedtech_comms.json b/config/test_speedtech_comms.json new file mode 100644 index 0000000..f9e9158 --- /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/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 01ef03c..37cb699 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -2,6 +2,7 @@ #pragma once #include "SurreyAeroComms.hpp" +#include "SpeedTechComms.hpp" #include #include #include @@ -62,7 +63,7 @@ class DriveBrainApp { 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; core::common::ThreadSafeDeque> _rx_queue; core::common::ThreadSafeDeque> _primary_can_tx_queue; core::common::ThreadSafeDeque> _secondary_can_tx_queue; @@ -80,17 +81,21 @@ class DriveBrainApp { std::shared_ptr _driver_primary_can; std::shared_ptr _driver_secondary_can; std::shared_ptr _aero_sensor_driver = nullptr; + std::shared_ptr _lap_timer_driver; + bool _using_lap_timer = false; std::unique_ptr> _acu_eth_driver; std::unique_ptr> _vcr_eth_driver; std::unique_ptr> _vcf_eth_driver; std::unique_ptr> _fake_vn = nullptr; std::shared_ptr _vn_driver; std::unique_ptr _db_service; - + 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; + const DriveBrainSettings _settings; }; \ No newline at end of file diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index d5cc39e..44423c3 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -3,6 +3,7 @@ #include "SimpleSpeedController.hpp" #include "SurreyAeroComms.hpp" +#include "SpeedTechComms.hpp" #include "hytech.pb.h" #include #include @@ -41,8 +42,6 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d spdlog::info("made controller"); - - _state_estimator = std::make_unique(_config, _message_logger); if(!_state_estimator->init()) { @@ -69,6 +68,8 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if (construction_failed) { throw std::runtime_error("Failed to construct CAN driver"); } + + configurable_components.push_back(_driver_primary_can); configurable_components.push_back(_driver_secondary_can); spdlog::info("made CAN driver"); @@ -111,7 +112,13 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d spdlog::info("made surrey aero sensor"); } - + if(config_json.contains("use_laptimer") && config_json["use_laptimer"]) + { + _lap_timer_driver = std::make_shared(_config, _io_context_speed_tech_serial); + _using_lap_timer = true; + } else { + _using_lap_timer = false; + } // - [x] TODO figure out how im going to get the parameter schemas for each of the configureable components into the mcap logger if @@ -181,8 +188,11 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { _aero_sensor_driver->set_msg_logger(_message_logger); } + if(_lap_timer_driver) + { + _lap_timer_driver->update_msg_logger(_message_logger); + } - _message_logger->start_logging_params(); spdlog::info("constructed app"); } @@ -231,6 +241,15 @@ DriveBrainApp::~DriveBrainApp() { _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"); } @@ -380,6 +399,17 @@ void DriveBrainApp::run() { }); } + 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; 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 0000000..af5fb36 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp @@ -0,0 +1,51 @@ +#pragma once + +// drivebrain_core +#include +#include +#include + +#include "hytech_msgs.pb.h" + +// protobuf +#include +#include +#include + +// boost +#include +#include + + +// https://wiki.hytechracing.org/books/software/page/speedtech-lap-timing-protocol-description + +namespace comms { +class SpeedTechComms : 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(); + + void update_msg_logger(std::shared_ptr>> message_logger) { + _message_logger = message_logger; + } + + void process_buffer(const boost::array &buff, std::size_t length); + + private: + void _start_recv(); + + private: + boost::array _input_buff; + boost::asio::serial_port _serial; + std::shared_ptr>> _message_logger = nullptr; +}; +} // namespace comms \ No newline at end of file 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 0000000..08aa2cc --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp @@ -0,0 +1,104 @@ +#include + +#include +#include +#include +#include + +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 &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(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(lap_time_sec_int); + float lap_time_millis = static_cast(lap_time_msec_int); + auto lap_time = lap_time_seconds + (lap_time_millis/1000.0f); + + std::shared_ptr speed_tech_lap_time_msg = std::make_shared(); + speed_tech_lap_time_msg->set_laptime(lap_time); + speed_tech_lap_time_msg->set_lapcount(lap_count); + + spdlog::debug("lapcount {} laptime {}", lap_count, lap_time); + if(_message_logger) + { + _message_logger->log_msg(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/VNComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp index dc34c4f..85d552b 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp @@ -196,7 +196,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/flake.nix b/flake.nix index 06d2835..0d38b5a 100644 --- a/flake.nix +++ b/flake.nix @@ -36,8 +36,9 @@ }; outputs = { self, nixpkgs, flake-parts, nebs-packages, easy_cmake, nix-proto, foxglove-schemas-src, ht_can, HT_proto, vn_driver_lib, db-core-src, ... }@inputs: + let - + nix-proto-foxglove-overlays = nix-proto.generateOverlays' { foxglove-schemas = nix-proto.mkProtoDerivation { name = "foxglove-schemas"; @@ -49,7 +50,8 @@ }; drivebrain_core_msgs = nix-proto.mkProtoDerivation { name = "drivebrain_core_msgs"; - version = HT_proto.rev; + + version = (if HT_proto ? rev then HT_proto.rev else "unknown"); src = "${HT_proto}/proto"; }; db_service = nix-proto.mkProtoDerivation diff --git a/test/test_speedtech_comms.cpp b/test/test_speedtech_comms.cpp new file mode 100644 index 0000000..66d6f3b --- /dev/null +++ b/test/test_speedtech_comms.cpp @@ -0,0 +1,14 @@ +#include +#include + +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 From 977ea98139731bebb1adb9eba57cf62c996586f8 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 19 May 2025 17:43:24 -0400 Subject: [PATCH 58/87] Feature/vcr control copy (#44) * adding in load cell vectoring and making the power limit a shared util function between controllers and adding in unit test for load cell tv * integrating mode 1 into drivebrain app and rebasing onto dev --- CMakeLists.txt | 9 +- config/drivebrain_config.json | 10 ++ drivebrain_app/include/DriveBrainApp.hpp | 6 +- drivebrain_app/src/DriveBrainApp.cpp | 14 ++- .../include/JSONUtils.hpp | 34 +++++- .../LoadCellVectoringTorqueController.hpp | 49 ++++++++ .../include/SimplePowerLimiter.hpp | 9 ++ .../include/SimpleSpeedController.hpp | 1 - .../src/LoadCellVectoringTorqueController.cpp | 114 ++++++++++++++++++ .../src/SimplePowerLimiter.cpp | 81 +++++++++++++ .../src/SimpleSpeedController.cpp | 66 +--------- .../drivebrain_estimation/include/HTMath.hpp | 8 +- .../include/StateEstimator.hpp | 10 +- .../src/StateEstimator.cpp | 16 +++ flake.lock | 7 +- flake.nix | 2 +- unit_test/ControllerTesting.cpp | 96 +++++++++++++++ unit_test/SimpleSpeedControllerTest.cpp | 3 +- 18 files changed, 453 insertions(+), 82 deletions(-) create mode 100644 drivebrain_core_impl/drivebrain_control/include/LoadCellVectoringTorqueController.hpp create mode 100644 drivebrain_core_impl/drivebrain_control/include/SimplePowerLimiter.hpp create mode 100644 drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp create mode 100644 drivebrain_core_impl/drivebrain_control/src/SimplePowerLimiter.cpp create mode 100644 unit_test/ControllerTesting.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 312c007..3722c9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -120,7 +120,12 @@ target_link_libraries(drivebrain_comms PUBLIC make_cmake_package(drivebrain_comms drivebrain) -add_library(drivebrain_control SHARED drivebrain_core_impl/drivebrain_control/src/SimpleTorqueController.cpp drivebrain_core_impl/drivebrain_control/src/SimpleSpeedController.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 @@ -132,6 +137,7 @@ 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) @@ -353,6 +359,7 @@ add_executable(drivebrain_test unit_test/SimpleSpeedControllerTest.cpp unit_test/SimpleTorqueControllerTest.cpp unit_test/test_controller_manager.cpp + unit_test/ControllerTesting.cpp ) target_link_libraries(drivebrain_test PUBLIC diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 1f88194..9ef981c 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -16,6 +16,16 @@ "max_power_kw": 63.0, "dt_rate_hz": 1000 }, + "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/ttyUSB1", "baud_rate": 921600, diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 37cb699..d8a00d2 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -5,7 +5,10 @@ #include "SpeedTechComms.hpp" #include #include + #include +#include + #include #include #include @@ -73,7 +76,8 @@ class DriveBrainApp { std::vector> _configurable_components; std::shared_ptr _mcap_logger; std::shared_ptr controller1; - control::ControllerManager, 1> _controllerManager; + std::shared_ptr _mode1; + control::ControllerManager, 2> _controllerManager; // std::unique_ptr _matlab_math; std::shared_ptr _foxglove_server; std::shared_ptr>> _message_logger = nullptr; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 44423c3..52b3fa4 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -32,14 +32,22 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if (!controller1->init()) { throw std::runtime_error("Failed to initialize controller"); } + configurable_components.push_back(controller1); + spdlog::info("made mode 0 controller"); + + _mode1 = std::make_shared(_config); + if (!_mode1->init()) { + throw std::runtime_error("Failed to mode 1 controller"); + } + configurable_components.push_back(_mode1); + spdlog::info("made mode 1 controller"); // 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({controller1}); + _controllerManager.update_controllers({controller1, _mode1}); if(!_controllerManager.init()){ throw std::runtime_error("Failed to initialize controller manager"); } - configurable_components.push_back(controller1); - spdlog::info("made controller"); + _state_estimator = std::make_unique(_config, _message_logger); diff --git a/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp b/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp index ce293f8..8807ad8 100644 --- a/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp +++ b/drivebrain_core_impl/drivebrain_common_utils/include/JSONUtils.hpp @@ -3,12 +3,36 @@ #include +// Ensures a parameter is loaded or the function returns false #define LOAD_PARAM_OR_FAIL(param_name, param_type, config_struct) \ - { \ - auto opt = get_parameter_value(#param_name); \ - if (!opt) \ + do { \ + auto opt_##param_name = get_parameter_value(#param_name); \ + if (!opt_##param_name) \ return false; \ - config_struct.param_name = *opt; \ - } + (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_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(&it->second)) { \ + std::unique_lock 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_control/include/LoadCellVectoringTorqueController.hpp b/drivebrain_core_impl/drivebrain_control/include/LoadCellVectoringTorqueController.hpp new file mode 100644 index 0000000..0b57f5e --- /dev/null +++ b/drivebrain_core_impl/drivebrain_control/include/LoadCellVectoringTorqueController.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + +#include + +#include +#include + +namespace control +{ + + // TODO make the output CAN message for the drivetrain, rpms telem is just a standin for now + class LoadCellVectoringTorqueController : public Controller, 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 &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 0000000..6caceb8 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_control/include/SimplePowerLimiter.hpp @@ -0,0 +1,9 @@ +#include + +namespace control { +namespace util { +core::SpeedControlOut apply_power_limit(core::SpeedControlOut current_control, + veh_vec current_rpms, float max_power_kw); +} + +} // namespace control diff --git a/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp index 2fef8de..723957d 100644 --- a/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp +++ b/drivebrain_core_impl/drivebrain_control/include/SimpleSpeedController.hpp @@ -41,7 +41,6 @@ namespace control private: void _handle_param_updates(const std::unordered_map &new_param_map); - core::SpeedControlOut _apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms); private: std::mutex _config_mutex; config _config{}; 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 0000000..12d0efc --- /dev/null +++ b/drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp @@ -0,0 +1,114 @@ +#include "LoadCellVectoringTorqueController.hpp" +#include "SimplePowerLimiter.hpp" + +#include + +#include +#include + +#include + +void control::LoadCellVectoringTorqueController::_handle_param_updates(const std::unordered_map &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 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(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.normalized_corner_load.FL + in.normalized_corner_load.FR + in.normalized_corner_load.RL+ in.normalized_corner_load.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.normalized_corner_load.FL / sum_normal) ); + speed_out.torque_lim_nm.FR = ((2.0 - cur_config.rear_torque_scale) * accel_torque_pool * (in.normalized_corner_load.FR / sum_normal) ); + speed_out.torque_lim_nm.RL = (cur_config.rear_torque_scale * accel_torque_pool * (in.normalized_corner_load.RL / sum_normal) ); + speed_out.torque_lim_nm.RR = (cur_config.rear_torque_scale * accel_torque_pool * (in.normalized_corner_load.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.normalized_corner_load.FL / sum_normal) * (2.0 - cur_config.rear_torque_scale)); + speed_out.torque_lim_nm.FR = (regen_torque_pool * (in.normalized_corner_load.FR / sum_normal) * (2.0 - cur_config.rear_torque_scale)); + speed_out.torque_lim_nm.RL = (regen_torque_pool * (in.normalized_corner_load.RL / sum_normal) * cur_config.rear_torque_scale); + speed_out.torque_lim_nm.RR = (regen_torque_pool * (in.normalized_corner_load.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/SimplePowerLimiter.cpp b/drivebrain_core_impl/drivebrain_control/src/SimplePowerLimiter.cpp new file mode 100644 index 0000000..d1517d8 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_control/src/SimplePowerLimiter.cpp @@ -0,0 +1,81 @@ +#include "SimplePowerLimiter.hpp" + +#include + +namespace control { +namespace util { + core::SpeedControlOut apply_power_limit(core::SpeedControlOut current_control, + veh_vec 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 < max_power_watt) { + // std::cout << "erm why" << max_power_watt < #include +#include "SimplePowerLimiter.hpp" + void control::SimpleSpeedController::_handle_param_updates(const std::unordered_map &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 @@ -118,7 +120,7 @@ core::ControllerOutput control::SimpleSpeedController::step_controller(const cor 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 = _apply_power_limit(speed_out, in.current_rpms); + cmd_out.out = control::util::apply_power_limit(speed_out, in.current_rpms, cur_config.max_power_kw); } else { @@ -139,65 +141,3 @@ core::ControllerOutput control::SimpleSpeedController::step_controller(const cor return cmd_out; } -core::SpeedControlOut control::SimpleSpeedController::_apply_power_limit(core::SpeedControlOut current_control, veh_vec current_rpms) -{ - 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 += ::abs(cmd_out.torque_lim_nm.FL); - net_torque_mag += ::abs(cmd_out.torque_lim_nm.FR); - net_torque_mag += ::abs(cmd_out.torque_lim_nm.RL); - net_torque_mag += ::abs(cmd_out.torque_lim_nm.RR); - - net_power += ::abs(cmd_out.torque_lim_nm.FL) * (current_rpms.FL * constants::RPM_TO_RAD_PER_SECOND); - net_power += ::abs(cmd_out.torque_lim_nm.FR) * (current_rpms.FR * constants::RPM_TO_RAD_PER_SECOND); - net_power += ::abs(cmd_out.torque_lim_nm.RL) * (current_rpms.RL * constants::RPM_TO_RAD_PER_SECOND); - net_power += ::abs(cmd_out.torque_lim_nm.RR) * (current_rpms.RR * constants::RPM_TO_RAD_PER_SECOND); - - auto max_power_watt = _config.max_power_kw * 1000.0f; - // std::cout << "net power " << net_power < max_power_watt) { - // std::cout << "erm why" << max_power_watt < namespace math { + template + float linear_approx(T x_val, T scale, T offset) + { + return ((static_cast(x_val) * static_cast(scale)) + (static_cast(offset))); + } template 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(input_value) - static_cast(min_val)) / (static_cast(max_val) - static_cast(min_val)); - float scaled = new_min + normalized * (new_max - new_min); + float scaled = (new_min + (normalized * (new_max - new_min))); return scaled; } template diff --git a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp index 56a0777..1951f88 100644 --- a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp +++ b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp @@ -72,6 +72,14 @@ namespace core 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>; @@ -102,7 +110,7 @@ namespace core { _message_logger = message_logger; } - bool init(); + bool init() override final; private: diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 41df9f0..e8d3a6b 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -180,6 +180,11 @@ StateEstimator::append_state_variables_from_raw_inputs(core::VehicleState vs, 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.normalized_corner_load.FL = math::linear_approx(raw_data.raw_load_cell_values.FL, _config.fl_load_cell_scale, _config.fl_load_cell_offset); + vehicle_state.normalized_corner_load.FR = math::linear_approx(raw_data.raw_load_cell_values.FR, _config.fr_load_cell_scale, _config.fr_load_cell_offset); + vehicle_state.normalized_corner_load.RL = math::linear_approx(raw_data.raw_load_cell_values.RL, _config.rl_load_cell_scale, _config.rl_load_cell_offset); + vehicle_state.normalized_corner_load.RR = math::linear_approx(raw_data.raw_load_cell_values.RR, _config.rr_load_cell_scale, _config.rr_load_cell_offset); return vehicle_state; } @@ -306,6 +311,7 @@ std::pair StateEstimator::get_latest_state_and_validit } 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); @@ -322,6 +328,16 @@ bool StateEstimator::init() { 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; } diff --git a/flake.lock b/flake.lock index a3d5936..2205d79 100644 --- a/flake.lock +++ b/flake.lock @@ -55,16 +55,15 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1747194373, - "narHash": "sha256-2VVlZo9MYAWx0u1WJNYhKHLEcKtq50DyvZVeRjdlGpk=", + "lastModified": 1747667698, + "narHash": "sha256-paXLS7OZo3llCuw0OTWlDzxwMZJtAh5AwaUMfZc1RcQ=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "59bacd7e3e104138fc442b933404b54d8fc523c6", + "rev": "b1535620917e54c7a8b022385d78bbf253a51f09", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "feat/loggable_w_position_type", "repo": "drivebrain_core", "type": "github" } diff --git a/flake.nix b/flake.nix index 0d38b5a..2e6d681 100644 --- a/flake.nix +++ b/flake.nix @@ -30,7 +30,7 @@ vn_driver_lib.url = "github:RCMast3r/vn_driver_lib/fix/boost-compatible"; db-core-src = { - url = "github:hytech-racing/drivebrain_core/feat/loggable_w_position_type"; + url = "github:hytech-racing/drivebrain_core"; flake = false; }; diff --git a/unit_test/ControllerTesting.cpp b/unit_test/ControllerTesting.cpp new file mode 100644 index 0000000..cc80eb3 --- /dev/null +++ b/unit_test/ControllerTesting.cpp @@ -0,0 +1,96 @@ +#include "LoadCellVectoringTorqueController.hpp" +#include +#include +#include +#include +#include +#include +#include + +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.normalized_corner_load = {1.0, 1.0, 1.0, 1.0}; // simulate balanced weight + + auto cmd = lctv.step_controller(in); + auto res = std::get_if(&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(lctv.get_cached_param("max_regen_torque")); + + float front_scale = (2.0 - std::get(lctv.get_cached_param("rear_torque_scale"))); + float rear_scale = std::get(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.normalized_corner_load = {1.2f, 0.8f, 1.2f, 0.8f}; // FL, FR, RL, RR + + auto cmd = lctv.step_controller(in); + auto res = std::get_if(&cmd.out); + ASSERT_NE(res, nullptr); + + float speed_set = std::get(lctv.get_cached_param("positive_speed_set")); + float max_torque = std::get(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/SimpleSpeedControllerTest.cpp b/unit_test/SimpleSpeedControllerTest.cpp index 1984307..33b99f7 100644 --- a/unit_test/SimpleSpeedControllerTest.cpp +++ b/unit_test/SimpleSpeedControllerTest.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -31,7 +32,7 @@ class SimpleSpeedControllerTest : public testing::Test { } void SetUp() override { - simple_controller.init(); + assert(simple_controller.init()); } }; From 1e169f7f1e43442322ecc2f75bd6215be14ca365 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 19 May 2025 17:56:57 -0400 Subject: [PATCH 59/87] Feature/scale driver comms (#45) * integrating scale comms --- CMakeLists.txt | 9 ++ config/drivebrain_config.json | 17 +++- config/test_scale_comms.json | 6 ++ drivebrain_app/include/DriveBrainApp.hpp | 12 ++- drivebrain_app/src/DriveBrainApp.cpp | 39 +++++++- .../drivebrain_comms/include/ScaleComms.hpp | 40 +++++++++ .../include/SurreyAeroComms.hpp | 2 +- .../drivebrain_comms/src/ScaleComms.cpp | 89 +++++++++++++++++++ .../drivebrain_comms/src/SurreyAeroComms.cpp | 15 ++-- test/test_scale_comms.cpp | 15 ++++ 10 files changed, 229 insertions(+), 15 deletions(-) create mode 100644 config/test_scale_comms.json create mode 100644 drivebrain_core_impl/drivebrain_comms/include/ScaleComms.hpp create mode 100644 drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp create mode 100644 test/test_scale_comms.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 3722c9e..f17af09 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,6 +86,7 @@ add_library(drivebrain_comms SHARED 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) @@ -256,6 +257,7 @@ 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 @@ -263,6 +265,13 @@ target_link_libraries(test_speedtech_comms PUBLIC ) + +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 diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 9ef981c..215bce7 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -54,13 +54,26 @@ "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 + "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" + }, "use_vectornav": false, "use_fake_vn": false, - "use_surrey_aero": true + "use_surrey_aero": false, + "use_scale_comms": false } diff --git a/config/test_scale_comms.json b/config/test_scale_comms.json new file mode 100644 index 0000000..6032efe --- /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/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index d8a00d2..05f58f8 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -2,7 +2,9 @@ #pragma once #include "SurreyAeroComms.hpp" -#include "SpeedTechComms.hpp" + +#include +#include #include #include @@ -67,6 +69,8 @@ class DriveBrainApp { 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> _rx_queue; core::common::ThreadSafeDeque> _primary_can_tx_queue; core::common::ThreadSafeDeque> _secondary_can_tx_queue; @@ -93,13 +97,15 @@ class DriveBrainApp { std::unique_ptr> _fake_vn = nullptr; std::shared_ptr _vn_driver; std::unique_ptr _db_service; - + + std::shared_ptr _scale_comms = nullptr; 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; + }; \ No newline at end of file diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 52b3fa4..f92f632 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -128,6 +128,19 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d _using_lap_timer = false; } + if(config_json.contains("use_scale_comms") && config_json["use_scale_comms"]) + { + spdlog::info("making scale comms driver"); + _scale_comms = std::make_shared(_config, _scale_usb_io_context); + if(!_scale_comms->init()){ + throw std::runtime_error("failed to init scale comms"); + + } + spdlog::info("made scale comms driver"); + } + + + // - [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 @@ -200,6 +213,10 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { _lap_timer_driver->update_msg_logger(_message_logger); } + if(_scale_comms) + { + _scale_comms->set_msg_logger(_message_logger); + } spdlog::info("constructed app"); @@ -239,7 +256,14 @@ DriveBrainApp::~DriveBrainApp() { _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) { @@ -397,7 +421,7 @@ void DriveBrainApp::run() { if(_aero_sensor_driver) { - _aero_usb_io_context_thread = std::thread([this]() { + _aero_usb_io_context_thread = std::thread([this]() -> void { spdlog::info("Started _aero_usb_io_context_thread"); try { _aero_usb_io_context.run(); @@ -406,6 +430,17 @@ void DriveBrainApp::run() { } }); } + 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) { 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 0000000..fecf142 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/include/ScaleComms.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace comms { +class ScaleComms : public core::common::Loggable>, + public core::common::Configurable { + struct config { + int baud_rate; + std::string port_name; + } _config; + + struct ScaleData { + veh_vec 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 _parse_buffer(const boost::array& buffer, std::size_t bytes_count); + void _log_proto_message(const ScaleData & data); + private: + boost::asio::serial_port _serial; + boost::array _input_buff{}; +}; +} // namespace comms diff --git a/drivebrain_core_impl/drivebrain_comms/include/SurreyAeroComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/SurreyAeroComms.hpp index 0a67bc6..7173e9d 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/SurreyAeroComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/SurreyAeroComms.hpp @@ -33,7 +33,7 @@ namespace comms { void _start_receive(); void _configure_serial_port(boost::asio::serial_port& serial); bool _send_command(const std::string& command); - std::vector _extract_sensor_readings(const boost::array& buffer); + std::optional> _extract_sensor_readings(const boost::array& buffer); void _log_proto_message(const std::vector& readings); private: boost::asio::serial_port _serial; 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 0000000..277b0e6 --- /dev/null +++ b/drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp @@ -0,0 +1,89 @@ +#include "ScaleComms.hpp" + + +#include "JSONUtils.hpp" +#include + +#include + +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(_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::_parse_buffer(const boost::array& 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(_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(); + msg_out->set_weight_lf(data.corner_weights_lbs.FL); + msg_out->set_weight_lr(data.corner_weights_lbs.FR); + msg_out->set_weight_lr(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/SurreyAeroComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/SurreyAeroComms.cpp index 88a9231..a66ea44 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/SurreyAeroComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/SurreyAeroComms.cpp @@ -51,18 +51,22 @@ void SurreyAeroComms::_start_receive() { _serial.async_read_some( boost::asio::buffer(_input_buff), [&](const boost::system::error_code &ec, std::size_t bytesCount) { - std::vector sensor_readings = _extract_sensor_readings(_input_buff); + auto opt_readings = _extract_sensor_readings(_input_buff); + if(opt_readings) + { + std::vector sensor_readings = (*opt_readings); + _log_proto_message(sensor_readings); + } - _log_proto_message(sensor_readings); _start_receive(); }); } -std::vector SurreyAeroComms::_extract_sensor_readings(const boost::array& buffer) { +std::optional> SurreyAeroComms::_extract_sensor_readings(const boost::array& buffer) { std::vector readings; if (buffer[0] != '#') { spdlog::error("invalid aero sensor frame received"); - return readings; + return std::nullopt; } @@ -87,7 +91,6 @@ std::vector SurreyAeroComms::_extract_sensor_readings(const boost::array< _timestamp_of_last_debug_p = now_time; } - return readings; } @@ -99,8 +102,6 @@ void SurreyAeroComms::_log_proto_message(const std::vector& readings) { 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); diff --git a/test/test_scale_comms.cpp b/test/test_scale_comms.cpp new file mode 100644 index 0000000..090db59 --- /dev/null +++ b/test/test_scale_comms.cpp @@ -0,0 +1,15 @@ +#include +#include + + +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; +} From bc0919f14681a88c37ad54ec4084059aa2b33634 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 19 May 2025 19:36:38 -0400 Subject: [PATCH 60/87] mems, fixing copy paster error --- drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp index 277b0e6..3f3ad7b 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/ScaleComms.cpp @@ -69,7 +69,7 @@ void ScaleComms::_log_proto_message(const ScaleData & data) auto msg_out = std::make_shared(); msg_out->set_weight_lf(data.corner_weights_lbs.FL); msg_out->set_weight_lr(data.corner_weights_lbs.FR); - msg_out->set_weight_lr(data.corner_weights_lbs.RL); + msg_out->set_weight_rf(data.corner_weights_lbs.RL); msg_out->set_weight_rr(data.corner_weights_lbs.RR); this->log(msg_out); } From ae8e044b0c779c1858b09b6fa5790e3524e317fb Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 21 May 2025 16:46:59 -0400 Subject: [PATCH 61/87] fixing speedtech lap timer, forgot to init --- config/drivebrain_config.json | 13 +++++++++---- drivebrain_app/src/DriveBrainApp.cpp | 4 ++++ .../drivebrain_comms/include/SpeedTechComms.hpp | 2 +- 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 215bce7..a315867 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -12,7 +12,7 @@ "max_regen_torque": 10.0, "rear_torque_scale": 1.0, "regen_torque_scale": 0.6, - "positive_speed_set" : 45, + "positive_speed_set" : 15, "max_power_kw": 63.0, "dt_rate_hz": 1000 }, @@ -21,7 +21,7 @@ "max_regen_torque": 10.0, "rear_torque_scale": 1.0, "regen_torque_scale": 0.6, - "positive_speed_set" : 45, + "positive_speed_set" : 15, "max_power_kw": 63.0, "dt_rate_hz": 1000, "apply_vectoring_in_regen": true @@ -34,7 +34,7 @@ }, "ControllerManager": { "max_controller_switch_speed_ms": 5.0, - "max_requested_rpm": 20000.0, + "max_requested_rpm": 22000.0, "max_torque_switch_nm": 10.0, "max_accel_switch_float": 0.1 }, @@ -71,9 +71,14 @@ "baud_rate": 115200, "port_name": "/dev/ttyUSB0" }, + "SpeedTechComms" :{ + "baud_rate": 9600, + "port_name": "/dev/ttyUSB0" + }, "use_vectornav": false, "use_fake_vn": false, "use_surrey_aero": false, - "use_scale_comms": false + "use_scale_comms": false, + "use_laptimer": false } diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index f92f632..361f54f 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -123,6 +123,10 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d if(config_json.contains("use_laptimer") && config_json["use_laptimer"]) { _lap_timer_driver = std::make_shared(_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; diff --git a/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp index af5fb36..4464ed6 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp @@ -32,7 +32,7 @@ class SpeedTechComms : public core::common::Configurable { core::JsonFileHandler &json_file_handler, boost::asio::io_context &io_context); - bool init(); + bool init() override final; void update_msg_logger(std::shared_ptr>> message_logger) { _message_logger = message_logger; From d1854776ade854584d3cf9d50bc04b910ac0e1a8 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 22 May 2025 00:24:17 -0400 Subject: [PATCH 62/87] fixing unit tests --- config/drivebrain_config.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index a315867..e8b1714 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -12,7 +12,7 @@ "max_regen_torque": 10.0, "rear_torque_scale": 1.0, "regen_torque_scale": 0.6, - "positive_speed_set" : 15, + "positive_speed_set" : 45, "max_power_kw": 63.0, "dt_rate_hz": 1000 }, @@ -21,7 +21,7 @@ "max_regen_torque": 10.0, "rear_torque_scale": 1.0, "regen_torque_scale": 0.6, - "positive_speed_set" : 15, + "positive_speed_set" : 45, "max_power_kw": 63.0, "dt_rate_hz": 1000, "apply_vectoring_in_regen": true @@ -34,7 +34,7 @@ }, "ControllerManager": { "max_controller_switch_speed_ms": 5.0, - "max_requested_rpm": 22000.0, + "max_requested_rpm": 20000.0, "max_torque_switch_nm": 10.0, "max_accel_switch_float": 0.1 }, From 6056096d63f9a9c0d8b0b75673f6816c44b144be Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 23 May 2025 00:26:10 -0400 Subject: [PATCH 63/87] Feature/simulink automation integration (#46) * integrating simulink automation in flake * fixing config for unit tests * adding in new python test script for sending can messages and adding commented code for integration with generated controllers * adding in python packages to devshell for sender.py util script * new ht_proto * updating simulink model * fix append state variables --------- Co-authored-by: lukec --- CMakeLists.txt | 7 + README.md | 7 + config/drivebrain_config.json | 15 +- default.nix | 4 +- drivebrain_app/include/DriveBrainApp.hpp | 13 +- drivebrain_app/src/DriveBrainApp.cpp | 32 +++- .../drivebrain_comms/src/foxglove_server.cpp | 10 +- .../src/StateEstimator.cpp | 4 +- .../src/DrivebrainMCAPLogger.cpp | 21 ++- flake.lock | 28 ++- flake.nix | 29 ++- simulink_automation.nix | 8 +- test/sender.py | 170 ++++++++++++++++++ 13 files changed, 310 insertions(+), 38 deletions(-) create mode 100644 test/sender.py diff --git a/CMakeLists.txt b/CMakeLists.txt index f17af09..a696dc8 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) @@ -103,6 +106,7 @@ target_include_directories(drivebrain_comms PUBLIC ) target_link_libraries(drivebrain_comms PUBLIC + codegen::matlab_model drivebrain_core::drivebrain_core drivebrain_common_utils drivebrain_control @@ -151,6 +155,7 @@ target_include_directories(drivebrain_mcap_logger PUBLIC target_link_libraries(drivebrain_mcap_logger PUBLIC drivebrain_core::drivebrain_core + codegen::matlab_model drivebrain_common_utils protobuf::libprotobuf mcap::mcap @@ -193,6 +198,7 @@ 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 @@ -347,6 +353,7 @@ target_link_libraries(drivebrain_exe PUBLIC drivebrain_comms drivebrain_mcap_logger Boost::program_options + codegen::matlab_model ) add_executable(test_build diff --git a/README.md b/README.md index f5a42c8..4d067b0 100644 --- a/README.md +++ b/README.md @@ -27,6 +27,13 @@ Allowed options: ```bash ./build/replay_app -m -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: diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index e8b1714..dfec2a9 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -15,7 +15,18 @@ "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 + }, "LoadCellVectoringTorqueController": { "max_torque": 21.0, "max_regen_torque": 10.0, @@ -76,7 +87,7 @@ "port_name": "/dev/ttyUSB0" }, "use_vectornav": false, - "use_fake_vn": false, + "use_fake_vn": true, "use_surrey_aero": false, "use_scale_comms": false, "use_laptimer": false diff --git a/default.nix b/default.nix index 4526699..c6b9502 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/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 05f58f8..6d4de12 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -23,6 +23,8 @@ #include #include +#include + #include #include #include @@ -81,8 +83,13 @@ class DriveBrainApp { std::shared_ptr _mcap_logger; std::shared_ptr controller1; std::shared_ptr _mode1; - control::ControllerManager, 2> _controllerManager; - // std::unique_ptr _matlab_math; + + // const + // namespace matlab_model_gen { + + + control::ControllerManager, 2 + matlab_model_gen::num_controllers> _controllerManager; + std::shared_ptr _foxglove_server; std::shared_ptr>> _message_logger = nullptr; std::shared_ptr _state_estimator; @@ -99,6 +106,8 @@ class DriveBrainApp { std::unique_ptr _db_service; std::shared_ptr _scale_comms = nullptr; + + std::vector> _gend_controllers; std::thread _process_thread; std::thread _io_context_thread; std::thread _io_context_secondary_thread; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 361f54f..c6f60f3 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -41,20 +41,35 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d } configurable_components.push_back(_mode1); spdlog::info("made mode 1 controller"); + + + std::array>, 2 + matlab_model_gen::num_controllers> controllers{}; + + std::array>, 2> existing_controllers = {controller1, _mode1}; + + _gend_controllers = matlab_model_gen::create_controllers(_config, configurable_components); + if(_gend_controllers.size()+(existing_controllers.size()) != controllers.size()) + { + throw std::runtime_error("Failed to initialize matlab generated controllers! Wrong vector size!"); + } + + std::copy(existing_controllers.begin(), existing_controllers.end(), controllers.begin()); + + std::copy(_gend_controllers.begin(), _gend_controllers.end(), controllers.begin()+2); + + // 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({controller1, _mode1}); + _controllerManager.update_controllers(controllers); if(!_controllerManager.init()){ throw std::runtime_error("Failed to initialize controller manager"); } - - - _state_estimator = std::make_unique(_config, _message_logger); if(!_state_estimator->init()) { throw std::runtime_error("Failed to initialize state estimator"); } + configurable_components.push_back(_state_estimator); spdlog::info("made state estimator"); @@ -144,8 +159,6 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d } - - // - [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. @@ -222,7 +235,12 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d _scale_comms->set_msg_logger(_message_logger); } + for(auto controller : _gend_controllers) + { + controller->set_msg_logger(_message_logger); + } + _message_logger->start_logging_params(); spdlog::info("constructed app"); } @@ -346,7 +364,7 @@ void DriveBrainApp::_process_loop() { _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(&cmd_out)){ // if it is a torque controller: // set desired torque diff --git a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp index b8448b6..45e2f98 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/foxglove_server.cpp @@ -15,7 +15,7 @@ #include - +#include static uint64_t nanosecondsSinceEpoch() { return uint64_t(std::chrono::duration_cast( @@ -71,14 +71,18 @@ core::FoxgloveWSServer::FoxgloveWSServer(std::vector proto_names = {"hytech_msgs.proto", "hytech.proto"}; + + std::vector 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 channels; diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index e8d3a6b..2291c5a 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -185,6 +185,8 @@ StateEstimator::append_state_variables_from_raw_inputs(core::VehicleState vs, vehicle_state.normalized_corner_load.FR = math::linear_approx(raw_data.raw_load_cell_values.FR, _config.fr_load_cell_scale, _config.fr_load_cell_offset); vehicle_state.normalized_corner_load.RL = math::linear_approx(raw_data.raw_load_cell_values.RL, _config.rl_load_cell_scale, _config.rl_load_cell_offset); vehicle_state.normalized_corner_load.RR = math::linear_approx(raw_data.raw_load_cell_values.RR, _config.rr_load_cell_scale, _config.rr_load_cell_offset); + + vehicle_state.steering_angle_deg = raw_data.raw_steering_analog; return vehicle_state; } @@ -228,7 +230,7 @@ std::pair StateEstimator::get_latest_state_and_validit 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, current_raw_data); + _vehicle_state = append_state_variables_from_raw_inputs(_vehicle_state, _raw_input_data); current_state = _vehicle_state; current_raw_data = _raw_input_data; } diff --git a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp index bb4bc46..a934777 100644 --- a/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp +++ b/drivebrain_core_impl/drivebrain_logging/src/DrivebrainMCAPLogger.cpp @@ -1,3 +1,4 @@ +#include #include #define MCAP_IMPLEMENTATION #include @@ -10,14 +11,21 @@ #include #include #include -#include +#include + +// #include namespace common { DrivebrainMCAPLogger::DrivebrainMCAPLogger(const std::string &base_dir, std::vector> configurable_components) : _options(mcap::McapWriterOptions("")), _configurable_components(configurable_components) { - auto optional_map = util::generate_name_to_id_map({"hytech_msgs.proto", "hytech.proto"}); + std::vector proto_names = {"hytech_msgs.proto", "hytech.proto"}; + + std::vector 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"); @@ -61,7 +69,12 @@ namespace common } // 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"}); + + std::vector proto_names = {"hytech_msgs.proto", "hytech.proto"}; + + std::vector 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 &descriptors, bool skip_channel) @@ -182,6 +195,7 @@ namespace common _msg_name_id_map["drivebrain_configuration"] = config_channel.id; return true; } else { + spdlog::error("error initializing param schema"); return false; } } @@ -206,6 +220,7 @@ namespace common _input_deque.cv.notify_all(); } } + spdlog::info("attempted param log"); } nlohmann::json DrivebrainMCAPLogger::_get_param_vals() { diff --git a/flake.lock b/flake.lock index 2205d79..5091166 100644 --- a/flake.lock +++ b/flake.lock @@ -6,16 +6,16 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1747196911, - "narHash": "sha256-Y8x8DywKp2/d3+9dSK7c4bh2CtNQOht/tvZhNptKpjU=", + "lastModified": 1747951466, + "narHash": "sha256-lGLuSEPOgqyw8vl7p+KhftzcE9IFwdFcuoTBBQL71Rg=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "cfcf44824c5cbfebe137ce8d330203616f184697", + "rev": "627d3d4b3103eef0d1666685f6d7748458e59259", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "2025-05-14T04_28_50", + "ref": "2025-05-22T22_04_40", "repo": "HT_proto", "type": "github" } @@ -55,11 +55,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1747667698, - "narHash": "sha256-paXLS7OZo3llCuw0OTWlDzxwMZJtAh5AwaUMfZc1RcQ=", + "lastModified": 1747936368, + "narHash": "sha256-bvvhntG3FVRzBeOJ05XG3WTvV3vVK6sTR+Gqzce+nvw=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "b1535620917e54c7a8b022385d78bbf253a51f09", + "rev": "c1839c053ccccd06908ff37b8acb5fedca6efd27", "type": "github" }, "original": { @@ -68,6 +68,19 @@ "type": "github" } }, + "db-simulink-gen-src": { + "flake": false, + "locked": { + "lastModified": 1747968350, + "narHash": "sha256-ZjHrZxPgEWG+rgFvLzwiKe3JNM4mYz0Lm1htxr0sNOQ=", + "type": "tarball", + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-release/gen_rel.tar.gz" + }, + "original": { + "type": "tarball", + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-release/gen_rel.tar.gz" + } + }, "dbcppp-src": { "flake": false, "locked": { @@ -652,6 +665,7 @@ "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", diff --git a/flake.nix b/flake.nix index 2e6d681..e8eae09 100644 --- a/flake.nix +++ b/flake.nix @@ -16,7 +16,7 @@ nix-proto.url = "github:notalltim/nix-proto"; nix-proto.inputs.nixpkgs.follows = "nixpkgs"; - HT_proto.url = "github:hytech-racing/HT_proto/2025-05-14T04_28_50"; + HT_proto.url = "github:hytech-racing/HT_proto/2025-05-22T22_04_40"; foxglove-schemas-src = { url = "github:foxglove/schemas"; @@ -34,12 +34,16 @@ flake = false; }; + db-simulink-gen-src = { + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-release/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, ... }@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 - nix-proto-foxglove-overlays = nix-proto.generateOverlays' { + nix-proto-overlays = nix-proto.generateOverlays' { foxglove-schemas = nix-proto.mkProtoDerivation { name = "foxglove-schemas"; version = "1.0.1"; @@ -54,6 +58,12 @@ 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"; @@ -74,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: { @@ -89,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; } @@ -120,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"; @@ -133,6 +147,7 @@ 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 .." @@ -140,7 +155,7 @@ alias br="cd build && cmake .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON && make -j && cd .." ''; nativeBuildInputs = [ pkgs.drivebrain_core_msgs_proto_cpp ]; - packages = [ pkgs.mcap-cli pkgs.valgrind ]; + packages = [ pkgs.mcap-cli pkgs.valgrind pkgs.grpcui pkgs.python311Packages.cantools pkgs.python311Packages.tkinter ]; inputsFrom = [ pkgs.drivebrain_software ]; @@ -175,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/simulink_automation.nix b/simulink_automation.nix index 72c5ba0..eee81c9 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}/source_code"; 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/test/sender.py b/test/sender.py new file mode 100644 index 0000000..7d36f90 --- /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( + "", + 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("", 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( + "", + 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("", 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 From fd6a8289055a59661cc84384117c46eef327b36e Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 25 May 2025 01:08:55 -0400 Subject: [PATCH 64/87] adjusting to rename and integrating new pid tv with load cell and steering fixed --- .../drivebrain_comms/src/VNComms.cpp | 7 +++++-- .../src/LoadCellVectoringTorqueController.cpp | 18 +++++++++--------- .../src/StateEstimator.cpp | 12 ++++++++---- flake.lock | 18 +++++++++--------- flake.nix | 2 +- unit_test/ControllerTesting.cpp | 4 ++-- 6 files changed, 34 insertions(+), 27 deletions(-) diff --git a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp index 85d552b..4c0d05a 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp @@ -97,7 +97,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, @@ -127,7 +127,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"); @@ -142,6 +142,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 msg_out = std::make_shared(); @@ -183,6 +184,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>(msg_out)); } diff --git a/drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp b/drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp index 12d0efc..118fd75 100644 --- a/drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp +++ b/drivebrain_core_impl/drivebrain_control/src/LoadCellVectoringTorqueController.cpp @@ -60,7 +60,7 @@ core::ControllerOutput control::LoadCellVectoringTorqueController::step_controll speed_out.mcu_recv_millis = in.prev_MCU_recv_millis; // heartbeat TODO this isnt needed any more, prob should remove - float sum_normal = in.normalized_corner_load.FL + in.normalized_corner_load.FR + in.normalized_corner_load.RL+ in.normalized_corner_load.RR; + float sum_normal = in.loadcells.FL + in.loadcells.FR + in.loadcells.RL+ in.loadcells.RR; if (accelRequest >= 0.0) { // Positive torque request @@ -75,10 +75,10 @@ core::ControllerOutput control::LoadCellVectoringTorqueController::step_controll 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.normalized_corner_load.FL / sum_normal) ); - speed_out.torque_lim_nm.FR = ((2.0 - cur_config.rear_torque_scale) * accel_torque_pool * (in.normalized_corner_load.FR / sum_normal) ); - speed_out.torque_lim_nm.RL = (cur_config.rear_torque_scale * accel_torque_pool * (in.normalized_corner_load.RL / sum_normal) ); - speed_out.torque_lim_nm.RR = (cur_config.rear_torque_scale * accel_torque_pool * (in.normalized_corner_load.RR / sum_normal) ); + 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 @@ -94,10 +94,10 @@ core::ControllerOutput control::LoadCellVectoringTorqueController::step_controll if(cur_config.apply_vectoring_in_regen) { - speed_out.torque_lim_nm.FL = (regen_torque_pool * (in.normalized_corner_load.FL / sum_normal) * (2.0 - cur_config.rear_torque_scale)); - speed_out.torque_lim_nm.FR = (regen_torque_pool * (in.normalized_corner_load.FR / sum_normal) * (2.0 - cur_config.rear_torque_scale)); - speed_out.torque_lim_nm.RL = (regen_torque_pool * (in.normalized_corner_load.RL / sum_normal) * cur_config.rear_torque_scale); - speed_out.torque_lim_nm.RR = (regen_torque_pool * (in.normalized_corner_load.RR / sum_normal) * cur_config.rear_torque_scale); + 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; diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 2291c5a..87fbfc6 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -181,10 +181,14 @@ StateEstimator::append_state_variables_from_raw_inputs(core::VehicleState vs, 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.normalized_corner_load.FL = math::linear_approx(raw_data.raw_load_cell_values.FL, _config.fl_load_cell_scale, _config.fl_load_cell_offset); - vehicle_state.normalized_corner_load.FR = math::linear_approx(raw_data.raw_load_cell_values.FR, _config.fr_load_cell_scale, _config.fr_load_cell_offset); - vehicle_state.normalized_corner_load.RL = math::linear_approx(raw_data.raw_load_cell_values.RL, _config.rl_load_cell_scale, _config.rl_load_cell_offset); - vehicle_state.normalized_corner_load.RR = math::linear_approx(raw_data.raw_load_cell_values.RR, _config.rr_load_cell_scale, _config.rr_load_cell_offset); + // vehicle_state.normalized_corner_load.FL = math::linear_approx(raw_data.raw_load_cell_values.FL, _config.fl_load_cell_scale, _config.fl_load_cell_offset); + // vehicle_state.normalized_corner_load.FR = math::linear_approx(raw_data.raw_load_cell_values.FR, _config.fr_load_cell_scale, _config.fr_load_cell_offset); + // vehicle_state.normalized_corner_load.RL = math::linear_approx(raw_data.raw_load_cell_values.RL, _config.rl_load_cell_scale, _config.rl_load_cell_offset); + // vehicle_state.normalized_corner_load.RR = math::linear_approx(raw_data.raw_load_cell_values.RR, _config.rr_load_cell_scale, _config.rr_load_cell_offset); + 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; diff --git a/flake.lock b/flake.lock index 5091166..0158f7c 100644 --- a/flake.lock +++ b/flake.lock @@ -6,16 +6,16 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1747951466, - "narHash": "sha256-lGLuSEPOgqyw8vl7p+KhftzcE9IFwdFcuoTBBQL71Rg=", + "lastModified": 1748032319, + "narHash": "sha256-D5ftWgFLHKa3HwgYDkrJdjC1GDTUgAGecOExuAACavg=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "627d3d4b3103eef0d1666685f6d7748458e59259", + "rev": "68a3af7085dcbac78441c60374a6ead129bbae27", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "2025-05-22T22_04_40", + "ref": "2025-05-23T20_32_14", "repo": "HT_proto", "type": "github" } @@ -55,11 +55,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1747936368, - "narHash": "sha256-bvvhntG3FVRzBeOJ05XG3WTvV3vVK6sTR+Gqzce+nvw=", + "lastModified": 1748149199, + "narHash": "sha256-vZkDo9cp4aTqSTymEMvIfYXnp6anKkxOlzaZFXl6MUQ=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "c1839c053ccccd06908ff37b8acb5fedca6efd27", + "rev": "c5931dddad9179cea3c32dffd92fbfc1be811c7b", "type": "github" }, "original": { @@ -71,8 +71,8 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1747968350, - "narHash": "sha256-ZjHrZxPgEWG+rgFvLzwiKe3JNM4mYz0Lm1htxr0sNOQ=", + "lastModified": 1748149624, + "narHash": "sha256-d+5wosb7YmO9Gcy1T+t9TMMlJya+oOQrGoD/9pA1kLA=", "type": "tarball", "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-release/gen_rel.tar.gz" }, diff --git a/flake.nix b/flake.nix index e8eae09..e196841 100644 --- a/flake.nix +++ b/flake.nix @@ -16,7 +16,7 @@ nix-proto.url = "github:notalltim/nix-proto"; nix-proto.inputs.nixpkgs.follows = "nixpkgs"; - HT_proto.url = "github:hytech-racing/HT_proto/2025-05-22T22_04_40"; + HT_proto.url = "github:hytech-racing/HT_proto/2025-05-23T20_32_14"; foxglove-schemas-src = { url = "github:foxglove/schemas"; diff --git a/unit_test/ControllerTesting.cpp b/unit_test/ControllerTesting.cpp index cc80eb3..f60013e 100644 --- a/unit_test/ControllerTesting.cpp +++ b/unit_test/ControllerTesting.cpp @@ -36,7 +36,7 @@ class ControllerTesting : public testing::Test { TEST_F(ControllerTesting, FullBrakeRequestLCTV) { in.input.requested_brake = 1.0; - in.normalized_corner_load = {1.0, 1.0, 1.0, 1.0}; // simulate balanced weight + in.loadcells = {1.0, 1.0, 1.0, 1.0}; // simulate balanced weight auto cmd = lctv.step_controller(in); auto res = std::get_if(&cmd.out); @@ -61,7 +61,7 @@ TEST_F(ControllerTesting, FullBrakeRequestLCTV) TEST_F(ControllerTesting, FullPositiveAccelProportionalToLoad) { in.input.requested_accel = 1.0; - in.normalized_corner_load = {1.2f, 0.8f, 1.2f, 0.8f}; // FL, FR, RL, RR + 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(&cmd.out); From 77160c82969bffff57d3799fa227c34275c6399b Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 25 May 2025 01:14:32 -0400 Subject: [PATCH 65/87] added outport --- flake.lock | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flake.lock b/flake.lock index 0158f7c..e45464a 100644 --- a/flake.lock +++ b/flake.lock @@ -71,8 +71,8 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1748149624, - "narHash": "sha256-d+5wosb7YmO9Gcy1T+t9TMMlJya+oOQrGoD/9pA1kLA=", + "lastModified": 1748149903, + "narHash": "sha256-KQa4IHgU4S5HVPvt8Kd1UaFr+6UouKeVTjVez18ESpk=", "type": "tarball", "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-release/gen_rel.tar.gz" }, From 58ad863aa61631d175dd7381836d5bb51efa965c Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 28 May 2025 15:24:53 -0400 Subject: [PATCH 66/87] Feature/mm gend estimator integration (#47) * integrating matlab generated estimators and new state variables --- config/drivebrain_config.json | 28 ++++++++++++++++++- drivebrain_app/include/DriveBrainApp.hpp | 6 ++-- drivebrain_app/src/DriveBrainApp.cpp | 12 +++++--- .../src/StateEstimator.cpp | 15 +++++++++- flake.lock | 22 +++++++-------- flake.nix | 4 +-- 6 files changed, 65 insertions(+), 22 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index dfec2a9..4ab9eb7 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -25,7 +25,11 @@ "i": 0.0, "d": 0.0, "use_fake_vx": false, - "fake_vx": 0.0 + "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, @@ -86,6 +90,28 @@ "baud_rate": 9600, "port_name": "/dev/ttyUSB0" }, + "gain_MatlabEstimModel": { + "in":2.0 + }, + "fz_estimator_MatlabEstimModel": + { + "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, + "Q": 0.0, + "R": 0.0 + }, + "vel_estimator_MatlabEstimModel": { + "process_noise_vx": 0.0, + "process_noise_yaw_rate": 0.0, + "process_noise_ax": 0.0, + "percent_wheel_diff_noise_gain": 0.0, + "steer_sensor_offset": 0.0, + "accel_noise_coef": 0.0, + "yaw_rate_noise_coef": 0.0 + }, "use_vectornav": false, "use_fake_vn": true, "use_surrey_aero": false, diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 6d4de12..26f919f 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -84,9 +85,6 @@ class DriveBrainApp { std::shared_ptr controller1; std::shared_ptr _mode1; - // const - // namespace matlab_model_gen { - control::ControllerManager, 2 + matlab_model_gen::num_controllers> _controllerManager; @@ -108,6 +106,8 @@ class DriveBrainApp { std::shared_ptr _scale_comms = nullptr; std::vector> _gend_controllers; + + std::shared_ptr _estim_manager; std::thread _process_thread; std::thread _io_context_thread; std::thread _io_context_secondary_thread; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index c6f60f3..c16504c 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -21,7 +21,8 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d , _config(_param_path) , _settings(settings) , controller1(std::make_shared(_config)) - , _controllerManager(_config, {controller1}) // Initialize correctly + , _controllerManager(_config, {controller1}) + , _estim_manager(std::make_shared(_config)) // Initialize correctly { // spdlog::info("top o"); std::vector> configurable_components; @@ -42,12 +43,12 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d configurable_components.push_back(_mode1); spdlog::info("made mode 1 controller"); - + _estim_manager->handle_inits(configurable_components); // calls throw internally here std::array>, 2 + matlab_model_gen::num_controllers> controllers{}; std::array>, 2> existing_controllers = {controller1, _mode1}; - - _gend_controllers = matlab_model_gen::create_controllers(_config, configurable_components); + + _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!"); @@ -240,6 +241,8 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d controller->set_msg_logger(_message_logger); } + _estim_manager->set_loggers(_message_logger); + _message_logger->start_logging_params(); spdlog::info("constructed app"); } @@ -321,6 +324,7 @@ void DriveBrainApp::_process_loop() { auto state_and_validity = _state_estimator->get_latest_state_and_validity(); + _estim_manager->evaluate_all_estimators(state_and_validity.first); auto out_struct = _controllerManager.step_active_controller(state_and_validity.first); // get current command diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 87fbfc6..191045c 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -40,12 +40,17 @@ void StateEstimator::handle_recv_process(std::shared_ptr 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); _vehicle_state.current_body_vel_ms = body_vel_ms; _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 { _recv_low_level_state(message); @@ -99,7 +104,7 @@ void StateEstimator::_recv_low_level_state(std::shared_ptr(in_msg->actual_torque_nm()); _raw_input_data.raw_inverter_power.set_from_index(in_msg->actual_power_w()); _vehicle_state.current_rpms.set_from_index(in_msg->actual_speed_rpm()); + _vehicle_state.current_torques_nm = _raw_input_data.raw_inverter_torques; } } @@ -259,6 +265,13 @@ std::pair StateEstimator::get_latest_state_and_validit 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(); diff --git a/flake.lock b/flake.lock index e45464a..27a4bea 100644 --- a/flake.lock +++ b/flake.lock @@ -6,16 +6,16 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1748032319, - "narHash": "sha256-D5ftWgFLHKa3HwgYDkrJdjC1GDTUgAGecOExuAACavg=", + "lastModified": 1748325502, + "narHash": "sha256-coKQEqVAb27VqvGnL8PqcAWzq/2wk62fwozUzl5Oq3Y=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "68a3af7085dcbac78441c60374a6ead129bbae27", + "rev": "dd7a4c4f45ce74c05c40a9bcc66e221cceb2835b", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "2025-05-23T20_32_14", + "ref": "2025-05-27T05_58_38", "repo": "HT_proto", "type": "github" } @@ -55,11 +55,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1748149199, - "narHash": "sha256-vZkDo9cp4aTqSTymEMvIfYXnp6anKkxOlzaZFXl6MUQ=", + "lastModified": 1748400693, + "narHash": "sha256-8mJWCZgaSPEiPHPT/k1T2kyVeLf7u3TxVjhyvJOpTvs=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "c5931dddad9179cea3c32dffd92fbfc1be811c7b", + "rev": "e565c5c2fba93d3f181a77c2ab8eeadffffd99b1", "type": "github" }, "original": { @@ -71,14 +71,14 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1748149903, - "narHash": "sha256-KQa4IHgU4S5HVPvt8Kd1UaFr+6UouKeVTjVez18ESpk=", + "lastModified": 1748451572, + "narHash": "sha256-ZrzpfSFvlez2Hy5rBr5I6/U2omy2sgGnjy9vkrMdjkQ=", "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-release/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel2/gen_rel.tar.gz" }, "original": { "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-release/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel2/gen_rel.tar.gz" } }, "dbcppp-src": { diff --git a/flake.nix b/flake.nix index e196841..b2399fa 100644 --- a/flake.nix +++ b/flake.nix @@ -16,7 +16,7 @@ nix-proto.url = "github:notalltim/nix-proto"; nix-proto.inputs.nixpkgs.follows = "nixpkgs"; - HT_proto.url = "github:hytech-racing/HT_proto/2025-05-23T20_32_14"; + HT_proto.url = "github:hytech-racing/HT_proto/2025-05-27T05_58_38"; foxglove-schemas-src = { url = "github:foxglove/schemas"; @@ -35,7 +35,7 @@ }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-release/gen_rel.tar.gz"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel2/gen_rel.tar.gz"; flake = false; }; }; From b43e706f81a0d1cff833a55f079d0ea32ca92144 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 30 May 2025 12:07:05 -0400 Subject: [PATCH 67/87] updating to latest release of the simulink models --- flake.lock | 8 ++++---- flake.nix | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/flake.lock b/flake.lock index 27a4bea..699c813 100644 --- a/flake.lock +++ b/flake.lock @@ -71,14 +71,14 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1748451572, - "narHash": "sha256-ZrzpfSFvlez2Hy5rBr5I6/U2omy2sgGnjy9vkrMdjkQ=", + "lastModified": 1748574232, + "narHash": "sha256-8Y5DPY/YN7XNZDxEta2JJFy/aDtVul6ezbZMZp2cH+M=", "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel2/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz" }, "original": { "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel2/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz" } }, "dbcppp-src": { diff --git a/flake.nix b/flake.nix index b2399fa..e9543d3 100644 --- a/flake.nix +++ b/flake.nix @@ -35,7 +35,7 @@ }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel2/gen_rel.tar.gz"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz"; flake = false; }; }; From a708d1887e9de6060ba5d6fa9d3c116c04b0cbb7 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 30 May 2025 16:52:41 -0400 Subject: [PATCH 68/87] updated to latest version --- flake.lock | 8 ++++---- flake.nix | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/flake.lock b/flake.lock index 27a4bea..699c813 100644 --- a/flake.lock +++ b/flake.lock @@ -71,14 +71,14 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1748451572, - "narHash": "sha256-ZrzpfSFvlez2Hy5rBr5I6/U2omy2sgGnjy9vkrMdjkQ=", + "lastModified": 1748574232, + "narHash": "sha256-8Y5DPY/YN7XNZDxEta2JJFy/aDtVul6ezbZMZp2cH+M=", "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel2/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz" }, "original": { "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel2/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz" } }, "dbcppp-src": { diff --git a/flake.nix b/flake.nix index b2399fa..e9543d3 100644 --- a/flake.nix +++ b/flake.nix @@ -35,7 +35,7 @@ }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel2/gen_rel.tar.gz"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz"; flake = false; }; }; From 5695bed96f087ca194aff5d3689e6b2308e9ace2 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 30 May 2025 16:57:18 -0400 Subject: [PATCH 69/87] cleaning up logging setup, fixing loop time and pulling in latest config structure --- config/drivebrain_config.json | 40 +++- drivebrain_app/include/DriveBrainApp.hpp | 10 +- drivebrain_app/src/DriveBrainApp.cpp | 216 +++++++++--------- .../drivebrain_comms/include/CANComms.hpp | 17 +- .../include/DBServiceImpl.hpp | 6 +- .../drivebrain_comms/include/ETHRecvComms.hpp | 15 +- .../drivebrain_comms/include/ETHRecvComms.tpp | 14 +- .../drivebrain_comms/include/ETHSendComms.hpp | 10 +- .../include/SpeedTechComms.hpp | 10 +- .../drivebrain_comms/include/VNComms.hpp | 15 +- .../drivebrain_comms/src/CANComms.cpp | 14 +- .../drivebrain_comms/src/DBServiceImpl.cpp | 5 +- .../drivebrain_comms/src/ETHSendComms.cpp | 11 +- .../drivebrain_comms/src/SpeedTechComms.cpp | 5 +- .../drivebrain_comms/src/VNComms.cpp | 11 +- .../include/StateEstimator.hpp | 20 +- .../src/StateEstimator.cpp | 7 +- drivebrain_utils/src/MCAPReplay.cpp | 6 +- 18 files changed, 191 insertions(+), 241 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index 4ab9eb7..df66c0a 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -93,25 +93,49 @@ "gain_MatlabEstimModel": { "in":2.0 }, - "fz_estimator_MatlabEstimModel": - { + "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, - "Q": 0.0, - "R": 0.0 - }, - "vel_estimator_MatlabEstimModel": { + "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, - "steer_sensor_offset": 0.0, "accel_noise_coef": 0.0, - "yaw_rate_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 }, + "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": 0.0, + "coast_brake_torq": 0.0 + }, "use_vectornav": false, "use_fake_vn": true, "use_surrey_aero": false, diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 26f919f..3a5bfd1 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -61,6 +61,8 @@ class DriveBrainApp { // Private member functions void _process_loop(); void _signal_handler(int signal); + void _setup_loggers(std::vector>>> logging_components); + private: // Private member variables static std::atomic _stop_signal; @@ -96,10 +98,10 @@ class DriveBrainApp { std::shared_ptr _aero_sensor_driver = nullptr; std::shared_ptr _lap_timer_driver; bool _using_lap_timer = false; - std::unique_ptr> _acu_eth_driver; - std::unique_ptr> _vcr_eth_driver; - std::unique_ptr> _vcf_eth_driver; - std::unique_ptr> _fake_vn = nullptr; + std::shared_ptr> _acu_eth_driver; + std::shared_ptr> _vcr_eth_driver; + std::shared_ptr> _vcf_eth_driver; + std::shared_ptr> _fake_vn = nullptr; std::shared_ptr _vn_driver; std::unique_ptr _db_service; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index c16504c..ba48f64 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -65,7 +65,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d throw std::runtime_error("Failed to initialize controller manager"); } - _state_estimator = std::make_unique(_config, _message_logger); + _state_estimator = std::make_shared(_config); if(!_state_estimator->init()) { throw std::runtime_error("Failed to initialize state estimator"); @@ -78,7 +78,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d // this also calls init() in the constructor _driver_primary_can = std::make_shared( - _config, _message_logger, _primary_can_tx_queue, _io_context, + _config, _primary_can_tx_queue, _io_context, _dbc_path, construction_failed, _state_estimator, "CANDriverPrimary"); if (construction_failed) { @@ -86,7 +86,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d } _driver_secondary_can = std::make_shared( - _config, _message_logger, _secondary_can_tx_queue, _io_context_secondary_can, + _config, _secondary_can_tx_queue, _io_context_secondary_can, _dbc_path, construction_failed, _state_estimator, "CANDriverSecondary"); if (construction_failed) { @@ -97,9 +97,9 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d configurable_components.push_back(_driver_primary_can); configurable_components.push_back(_driver_secondary_can); spdlog::info("made CAN driver"); - _acu_eth_driver = std::make_unique>(_io_context, 7766); - _vcr_eth_driver = std::make_unique>(_io_context, 9999); - _vcf_eth_driver = std::make_unique>(_io_context, 4444); + _acu_eth_driver = std::make_shared>(_io_context, 7766); + _vcr_eth_driver = std::make_shared>(_io_context, 9999); + _vcf_eth_driver = std::make_shared>(_io_context, 4444); spdlog::info("eth drivers"); @@ -107,7 +107,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d [this](size_t mode) -> bool { return _controllerManager.swap_active_controller(mode, _state_estimator->get_latest_state_and_validity().first); }; - _db_service = std::make_unique(_message_logger, switch_modes); + _db_service = std::make_unique(switch_modes); spdlog::info("made db service"); nlohmann::json &config_json = _config.get_config(); @@ -115,14 +115,14 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d { spdlog::info("using vectornav"); // on creation calls init() - _vn_driver = std::make_shared(_config, _logger, _message_logger, _state_estimator, _io_context, construction_failed); + _vn_driver = std::make_shared(_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_unique>( _io_context, 13111, _state_estimator); + _fake_vn = std::make_shared>( _io_context, 13111, _state_estimator); } if(config_json.contains("use_surrey_aero") && config_json["use_surrey_aero"]) @@ -185,136 +185,61 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d std::bind(&common::DrivebrainMCAPLogger::init_param_schema, _mcap_logger), std::bind(&common::DrivebrainMCAPLogger::log_params, _mcap_logger)); - if(_driver_primary_can) - { - _driver_primary_can->update_msg_logger(_message_logger); - } - if(_driver_secondary_can) - { - _driver_secondary_can->update_msg_logger(_message_logger); - } - - if(_vn_driver) - { - _vn_driver->update_msg_logger(_message_logger); - } - if(_fake_vn) - { - _fake_vn->update_msg_logger(_message_logger); - } - if(_state_estimator) - { - _state_estimator->update_msg_logger(_message_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); } - if(_acu_eth_driver) - { - _acu_eth_driver->update_msg_logger(_message_logger); - } - if(_vcr_eth_driver) - { - _vcr_eth_driver->update_msg_logger(_message_logger); - } - if(_vcf_eth_driver) - { - _vcf_eth_driver->update_msg_logger(_message_logger); - } - if(_aero_sensor_driver) - { - _aero_sensor_driver->set_msg_logger(_message_logger); - } - if(_lap_timer_driver) - { - _lap_timer_driver->update_msg_logger(_message_logger); - } - if(_scale_comms) - { - _scale_comms->set_msg_logger(_message_logger); - } - - for(auto controller : _gend_controllers) - { - controller->set_msg_logger(_message_logger); - } + using loggertype = std::shared_ptr>>; + + std::vector logging_components = {_driver_primary_can, + _driver_secondary_can, + _vn_driver, + _fake_vn, + _state_estimator, + _acu_eth_driver, + _vcr_eth_driver, + _vcf_eth_driver, + _aero_sensor_driver, + _scale_comms + }; + // 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"); } -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) +void DriveBrainApp::_setup_loggers(std::vector>>> logging_components) +{ + if(!_message_logger) { - _scale_usb_io_context.stop(); - if(_scale_usb_io_context_thread.joinable()) - { - _scale_usb_io_context_thread.join(); - } + throw std::runtime_error("Failed to set message logger on components, message logger does not exist yet!"); } - - - 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) + for(auto component : logging_components) { - _io_context_speed_tech_serial.stop(); - if(_io_context_speed_tech_serial_thread.joinable()) + if(component) { - _io_context_speed_tech_serial_thread.join(); + component->set_msg_logger(_message_logger); + } else { + spdlog::warn("component does not exist, not setting message logger"); } } - spdlog::info("destructed db app"); } void DriveBrainApp::_process_loop() { auto desired_rpm_msg = std::make_shared(); auto torque_limit_msg = std::make_shared(); auto desired_torque_msg = std::make_shared(); - auto loop_time = 0.005; + auto loop_time = 0.004; auto loop_time_micros = (int)(loop_time * 1000000.0f); std::chrono::microseconds loop_chrono_time(loop_time_micros); @@ -491,3 +416,66 @@ void DriveBrainApp::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_core_impl/drivebrain_comms/include/CANComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp index a9812cf..8a3d33a 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp @@ -1,6 +1,7 @@ #pragma once // drivebrain includes #include +#include #include #include #include @@ -54,20 +55,19 @@ namespace comms { - class CANDriver : public core::common::Configurable + class CANDriver : public core::common::Loggable>, + public core::common::Configurable { public: using FieldVariant = std::variant; using deqtype = core::common::ThreadSafeDeque>; - using loggertype = core::MsgLogger>; /// @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, std::shared_ptr message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator) : + CANDriver(core::JsonFileHandler &json_file_handler, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator) : Configurable(json_file_handler, "CANDriver"), - _message_logger(message_logger), _input_deque_ref(in_deq), _socket(io_context), _dbc_path(dbc_path), @@ -78,9 +78,8 @@ namespace comms construction_failed = !init(); } - CANDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr message_logger, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator, std::string driver_name) : + CANDriver(core::JsonFileHandler &json_file_handler, deqtype &in_deq, boost::asio::io_context& io_context, std::optional dbc_path, bool &construction_failed, std::shared_ptr state_estimator, std::string driver_name) : Configurable(json_file_handler, driver_name), - _message_logger(message_logger), _input_deque_ref(in_deq), _socket(io_context), _dbc_path(dbc_path), @@ -94,11 +93,6 @@ namespace comms bool init(); - void update_msg_logger(std::shared_ptr message_logger) - { - _message_logger = message_logger; - } - void _handle_send_msg_from_queue(); std::shared_ptr pb_msg_recv(const can_frame &in_frame); void set_field_values_of_pb_msg(const std::unordered_map &field_values, std::shared_ptr message); @@ -128,7 +122,6 @@ namespace comms static std::string _to_lowercase(std::string s); private: - std::shared_ptr _message_logger; deqtype &_input_deque_ref; std::condition_variable _cv; diff --git a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp index 8b39009..e6f7d2d 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/DBServiceImpl.hpp @@ -17,16 +17,14 @@ class DBInterfaceImpl final : public db_service::v1::service::DBInterface::Service { - struct InitStruct { - std::shared_ptr>> 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* 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>> logger_inst, std::function mode_switch); void run_server(); + DBInterfaceImpl(std::function mode_switch); + void run_server(); void stop_server(); void update_msg_logger(std::shared_ptr>> logger_inst) { _logger_inst = logger_inst; diff --git a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp index 82ee63e..1c23d73 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.hpp @@ -1,19 +1,18 @@ #ifndef __ETHRECVCOMMS_H__ #define __ETHRECVCOMMS_H__ +#include +#include -#include +#include #include -#include #include #include #include -#include #include #include "hytech_msgs.pb.h" -#include // - [x] boost asio socket for udp port comms // - [x] handle receiving UDP messages on a specific port @@ -27,24 +26,20 @@ namespace comms { template - class ETHRecvComms + class ETHRecvComms : public core::common::Loggable> { public: - using loggertype = core::MsgLogger>; ETHRecvComms() = delete; ~ETHRecvComms(); ETHRecvComms(boost::asio::io_context &io_context, uint16_t recv_port, std::shared_ptr state_estim=nullptr); - void update_msg_logger(std::shared_ptr message_logger) { - _message_logger = message_logger; - } + private: void _handle_receive(const boost::system::error_code &error, std::size_t size); void _start_receive(); private: - std::shared_ptr _message_logger; std::shared_ptr _state_estimator = nullptr; std::array _recv_buffer; boost::asio::ip::udp::socket _socket; diff --git a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp index 63373ff..c522237 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp +++ b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp @@ -10,9 +10,8 @@ namespace comms ETHRecvComms::ETHRecvComms(boost::asio::io_context &io_context, uint16_t port, std::shared_ptr state_estim) - : _message_logger(nullptr), - _state_estimator(state_estim), - _socket(io_context, udp::endpoint(udp::v4(), port)) + : _state_estimator(state_estim), + _socket(io_context, udp::endpoint(udp::v4(), port)) { _eth_msg = std::make_shared(); _start_receive(); @@ -33,12 +32,9 @@ namespace comms { _eth_msg->ParseFromArray(_recv_buffer.data(), size); auto out_msg = static_cast>(_eth_msg); - if (_message_logger) - { - _message_logger->log_msg(out_msg); - } else { - spdlog::info("Message logger not real"); - } + + this->log(out_msg); + if(_state_estimator) { _state_estimator->handle_recv_process(out_msg); diff --git a/drivebrain_core_impl/drivebrain_comms/include/ETHSendComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/ETHSendComms.hpp index c4ad55a..e27fddf 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/ETHSendComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/ETHSendComms.hpp @@ -25,7 +25,7 @@ namespace comms { - class ETHSendComms + class ETHSendComms : public core::common::Loggable> { public: using deqtype = core::common::ThreadSafeDeque>; @@ -33,11 +33,8 @@ namespace comms ETHSendComms() = delete; ~ETHSendComms(); - ETHSendComms(std::shared_ptr message_logger, boost::asio::io_context &io_context, uint16_t send_port, std::string send_ip, bool bind=true); - - void update_msg_logger(std::shared_ptr message_logger) { - _message_logger = message_logger; - } + 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 send_msg); @@ -54,7 +51,6 @@ namespace comms boost::asio::ip::udp::endpoint _endp; - std::shared_ptr _message_logger; std::array _recv_buffer; boost::asio::ip::udp::socket _socket; boost::asio::ip::udp::endpoint _remote_endpoint; diff --git a/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp index 4464ed6..12cb5f2 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/SpeedTechComms.hpp @@ -3,7 +3,7 @@ // drivebrain_core #include #include -#include +#include #include "hytech_msgs.pb.h" @@ -20,7 +20,8 @@ // https://wiki.hytechracing.org/books/software/page/speedtech-lap-timing-protocol-description namespace comms { -class SpeedTechComms : public core::common::Configurable { +class SpeedTechComms : public core::common::Loggable>, + public core::common::Configurable { private: struct config { int baud_rate; @@ -34,10 +35,6 @@ class SpeedTechComms : public core::common::Configurable { bool init() override final; - void update_msg_logger(std::shared_ptr>> message_logger) { - _message_logger = message_logger; - } - void process_buffer(const boost::array &buff, std::size_t length); private: @@ -46,6 +43,5 @@ class SpeedTechComms : public core::common::Configurable { private: boost::array _input_buff; boost::asio::serial_port _serial; - std::shared_ptr>> _message_logger = nullptr; }; } // namespace comms \ 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 2b34b99..5e9f670 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 -#include -#include +#include #include // protobuf @@ -38,11 +37,12 @@ using SerialPort = boost::asio::serial_port; using loggertype = core::MsgLogger>; namespace comms { - class VNDriver : public core::common::Configurable + class VNDriver : public core::common::Loggable>, + public core::common::Configurable { public: - VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, std::shared_ptr state_estimator, boost::asio::io_context &io_context, bool &init_successful); - ~VNDriver(){ + VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_estimator, boost::asio::io_context &io_context, bool &init_successful); + ~VNDriver() { spdlog::info("destructed %s", this->get_name()); } bool init(); @@ -59,15 +59,12 @@ namespace comms { boost::array _output_buff; boost::array _input_buff; SerialPort _serial; - std::shared_ptr _message_logger; config _config; public: // Public methods void log_proto_message(std::shared_ptr msg); - void update_msg_logger(std::shared_ptr message_logger) { - _message_logger = message_logger; - } + 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/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index fe9a5e1..21decd6 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -122,13 +122,8 @@ void comms::CANDriver::_handle_recv_CAN_frame(const struct can_frame &frame) { { _state_estimator->handle_recv_process(msg); } - if(_message_logger) // this may not exist yet as this gets constr - { - _message_logger->log_msg(msg); - }else { - spdlog::warn("can comms message logger not real"); - } + this->log(msg); } } @@ -412,12 +407,7 @@ void comms::CANDriver::_handle_send_msg_from_queue() { if (can_msg) { _send_message(*can_msg); - if(_message_logger) - { - _message_logger->log_msg(msg); - } else { - spdlog::warn("message logger not real"); - } + 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 2014465..e293485 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/DBServiceImpl.cpp @@ -47,9 +47,8 @@ grpc::Status DBInterfaceImpl::RequestControllerChange(grpc::ServerContext *conte return grpc::Status::OK; } -DBInterfaceImpl::DBInterfaceImpl(std::shared_ptr>> logger_inst, std::function mode_switch) - : _logger_inst(logger_inst), _mode_switch(mode_switch){ -} +DBInterfaceImpl::DBInterfaceImpl(std::function mode_switch) : _mode_switch(mode_switch) + { } void DBInterfaceImpl::stop_server() { if (_server) { spdlog::warn("Shutting down the server..."); diff --git a/drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp index 8c0990b..85cddb3 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/ETHSendComms.cpp @@ -2,9 +2,8 @@ namespace comms { using boost::asio::ip::udp; -ETHSendComms::ETHSendComms(std::shared_ptr message_logger, boost::asio::io_context &io_context, uint16_t send_port, std::string send_ip, bool bind) -: _message_logger(message_logger), -_endp(boost::asio::ip::make_address(send_ip.c_str()), send_port), +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()); @@ -54,11 +53,7 @@ void ETHSendComms::_handle_send_msg_from_queue() for (const auto &msg : _queue.deque) { _send_message(msg); - if(_message_logger) - { - _message_logger->log_msg(msg); - } - + this->log(msg); } _queue.deque.clear(); } diff --git a/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp index 08aa2cc..a42a013 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp @@ -43,10 +43,7 @@ void SpeedTechComms::process_buffer(const boost::array &buff, speed_tech_lap_time_msg->set_lapcount(lap_count); spdlog::debug("lapcount {} laptime {}", lap_count, lap_time); - if(_message_logger) - { - _message_logger->log_msg(speed_tech_lap_time_msg); - } + this->log(speed_tech_lap_time_msg); } else { spdlog::warn("speedtech message byte length incorrect"); diff --git a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp index 4c0d05a..c1dd75e 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/VNComms.cpp @@ -54,10 +54,9 @@ namespace comms return true; } - VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, core::Logger &logger, std::shared_ptr message_logger, std::shared_ptr state_estimator, boost::asio::io_context& io, bool &init_not_successful) + VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr 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_not_successful = !init(); @@ -74,13 +73,7 @@ namespace comms void VNDriver::log_proto_message(std::shared_ptr msg) { _state_estimator->handle_recv_process(static_cast>(msg)); - if(_message_logger) - { - _message_logger->log_msg(static_cast>(msg)); - } else { - spdlog::warn("message logger not real"); - } - + this->log(msg); } void VNDriver::_configure_binary_outputs() diff --git a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp index 1951f88..5bae153 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 +#include +#include +#include #include #include @@ -26,12 +30,7 @@ #include #include -#include -#include -#include -#include -#include // 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,7 +51,8 @@ // for now i will just move the state estimator into the estimation impl and call it a day namespace core { - class StateEstimator : public core::common::Configurable + class StateEstimator : public core::common::Loggable>, + public core::common::Configurable { struct config { @@ -86,11 +86,9 @@ namespace core public: using tsq = core::common::ThreadSafeDeque>; - StateEstimator(core::JsonFileHandler &json_file_handler, std::shared_ptr message_logger) : + StateEstimator(core::JsonFileHandler &json_file_handler) : Configurable(json_file_handler, "StateEstimator") - , _message_logger(message_logger) - // _matlab_estimator(matlab_estimator) { _vehicle_state = {}; // initialize to all zeros _raw_input_data = {}; @@ -106,10 +104,6 @@ namespace core std::pair get_latest_state_and_validity(); void set_previous_control_output(ControllerOutput prev_control_output); core::VehicleState append_state_variables_from_raw_inputs(core::VehicleState vs, core::RawInputData raw_inputs); - void update_msg_logger(std::shared_ptr message_logger) - { - _message_logger = message_logger; - } bool init() override final; diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 191045c..9fd8765 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -293,11 +293,8 @@ std::pair StateEstimator::get_latest_state_and_validit } auto log_start = std::chrono::high_resolution_clock::now(); - if (_message_logger) { - _message_logger->log_msg(static_cast>(msg_out)); - } else { - spdlog::warn("message logger not real"); - } + + this->log(msg_out); auto log_end = std::chrono::high_resolution_clock::now(); diff --git a/drivebrain_utils/src/MCAPReplay.cpp b/drivebrain_utils/src/MCAPReplay.cpp index d5d44f1..2a41410 100644 --- a/drivebrain_utils/src/MCAPReplay.cpp +++ b/drivebrain_utils/src/MCAPReplay.cpp @@ -14,17 +14,17 @@ MCAPReplay::MCAPReplay(std::string param_file_path, std::string dbc_file_path) std::cout << param_file_path << std::endl; bool cf = false; _driver_primary_can = - std::make_shared(_config, nullptr, _primary_can_tx_queue, _io_context, + std::make_shared(_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(nullptr, _io_context, 7766, "127.0.0.1", false); + _acu_sender = std::make_shared(_io_context, 7766, "127.0.0.1", false); // _acu_core_sender = std::make_shared(nullptr, _io_context, 7777, "127.0.0.1"); // _vcr_sender = std::make_shared(nullptr, _io_context, 9999, "127.0.0.1"); - _vn_sender = std::make_shared(nullptr, _io_context, 13111, "127.0.0.1", false); // fake VN + _vn_sender = std::make_shared(_io_context, 13111, "127.0.0.1", false); // fake VN } From 824f471a9c0e8c817713114dbc6a8381d98811c2 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 30 May 2025 17:34:17 -0400 Subject: [PATCH 70/87] pulling in changes to core and HT_proto for energy meter data --- drivebrain_app/src/DriveBrainApp.cpp | 2 +- .../src/StateEstimator.cpp | 64 ++++++++++--------- drivebrain_utils/src/MCAPReplay.cpp | 2 +- flake.lock | 14 ++-- flake.nix | 2 +- 5 files changed, 45 insertions(+), 39 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index ba48f64..772fa91 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -98,7 +98,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d configurable_components.push_back(_driver_secondary_can); spdlog::info("made CAN driver"); _acu_eth_driver = std::make_shared>(_io_context, 7766); - _vcr_eth_driver = std::make_shared>(_io_context, 9999); + _vcr_eth_driver = std::make_shared>(_io_context, 9999, _state_estimator); _vcf_eth_driver = std::make_shared>(_io_context, 4444); spdlog::info("eth drivers"); diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 9fd8765..658f5dc 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -23,6 +23,21 @@ StateEstimator::~StateEstimator() { spdlog::info("destructed StateEstimator"); } + +// INV1_STATUS INV1_TEMPS INV1_DYNAMICS INV1_POWER INV1_FEEDBACK +void StateEstimator::_recv_inverter_states(std::shared_ptr 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); + } +} + void StateEstimator::handle_recv_process(std::shared_ptr message) { if (message->GetTypeName() == "hytech_msgs.VNData") { auto in_msg = std::static_pointer_cast(message); @@ -52,7 +67,15 @@ void StateEstimator::handle_recv_process(std::shared_ptrGetTypeName() == "hytech_msgs.VCRData_s") { + auto in_msg = std::static_pointer_cast(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 { _recv_low_level_state(message); } } @@ -99,35 +122,20 @@ void StateEstimator::_recv_low_level_state(std::shared_ptrsteering_digital_raw(); } } else if (message->GetTypeName() == "hytech.em_measurement") { - // auto in_msg = std::static_pointer_cast(message); - // { - // std::unique_lock lk(_state_mutex); - // // TODO - // } + auto in_msg = std::static_pointer_cast(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 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 void StateEstimator::_handle_set_inverter_dynamics(std::shared_ptr msg) { @@ -187,10 +195,6 @@ StateEstimator::append_state_variables_from_raw_inputs(core::VehicleState vs, 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.normalized_corner_load.FL = math::linear_approx(raw_data.raw_load_cell_values.FL, _config.fl_load_cell_scale, _config.fl_load_cell_offset); - // vehicle_state.normalized_corner_load.FR = math::linear_approx(raw_data.raw_load_cell_values.FR, _config.fr_load_cell_scale, _config.fr_load_cell_offset); - // vehicle_state.normalized_corner_load.RL = math::linear_approx(raw_data.raw_load_cell_values.RL, _config.rl_load_cell_scale, _config.rl_load_cell_offset); - // vehicle_state.normalized_corner_load.RR = math::linear_approx(raw_data.raw_load_cell_values.RR, _config.rr_load_cell_scale, _config.rr_load_cell_offset); 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; @@ -251,8 +255,8 @@ std::pair StateEstimator::get_latest_state_and_validit std::shared_ptr msg_out = std::make_shared(); - msg_out->set_is_ready_to_drive(true); - + 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); @@ -292,6 +296,8 @@ std::pair StateEstimator::get_latest_state_and_validit _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(); this->log(msg_out); diff --git a/drivebrain_utils/src/MCAPReplay.cpp b/drivebrain_utils/src/MCAPReplay.cpp index 2a41410..8a53984 100644 --- a/drivebrain_utils/src/MCAPReplay.cpp +++ b/drivebrain_utils/src/MCAPReplay.cpp @@ -23,7 +23,7 @@ MCAPReplay::MCAPReplay(std::string param_file_path, std::string dbc_file_path) _acu_sender = std::make_shared(_io_context, 7766, "127.0.0.1", false); // _acu_core_sender = std::make_shared(nullptr, _io_context, 7777, "127.0.0.1"); - // _vcr_sender = std::make_shared(nullptr, _io_context, 9999, "127.0.0.1"); + _vcr_sender = std::make_shared(nullptr, _io_context, 9999, "127.0.0.1"); _vn_sender = std::make_shared(_io_context, 13111, "127.0.0.1", false); // fake VN diff --git a/flake.lock b/flake.lock index 699c813..1ee259a 100644 --- a/flake.lock +++ b/flake.lock @@ -6,16 +6,16 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1748325502, - "narHash": "sha256-coKQEqVAb27VqvGnL8PqcAWzq/2wk62fwozUzl5Oq3Y=", + "lastModified": 1748639873, + "narHash": "sha256-7KC4DEH/hqrnjRaOEXqvvQJdyBjUFjTGFYM2HVflrYk=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "dd7a4c4f45ce74c05c40a9bcc66e221cceb2835b", + "rev": "98d4b72640844f87ae2d2cef88a13bb6acd8226f", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "2025-05-27T05_58_38", + "ref": "2025-05-30T21_18_06", "repo": "HT_proto", "type": "github" } @@ -55,11 +55,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1748400693, - "narHash": "sha256-8mJWCZgaSPEiPHPT/k1T2kyVeLf7u3TxVjhyvJOpTvs=", + "lastModified": 1748639958, + "narHash": "sha256-qRfdJxKy/O1L2pkGX3Xe78m+K/pLNBQFk6s5HvK6YVI=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "e565c5c2fba93d3f181a77c2ab8eeadffffd99b1", + "rev": "a3faebc4b772dd26314697d43f4409bf4f5c3e1d", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index e9543d3..1e20a9b 100644 --- a/flake.nix +++ b/flake.nix @@ -16,7 +16,7 @@ nix-proto.url = "github:notalltim/nix-proto"; nix-proto.inputs.nixpkgs.follows = "nixpkgs"; - HT_proto.url = "github:hytech-racing/HT_proto/2025-05-27T05_58_38"; + HT_proto.url = "github:hytech-racing/HT_proto/2025-05-30T21_18_06"; foxglove-schemas-src = { url = "github:foxglove/schemas"; From 596b217671129c591b300146e0409429aac2cdd7 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 30 May 2025 17:34:51 -0400 Subject: [PATCH 71/87] adding in vcr sender to replay util --- drivebrain_utils/src/MCAPReplay.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/drivebrain_utils/src/MCAPReplay.cpp b/drivebrain_utils/src/MCAPReplay.cpp index 8a53984..266ca75 100644 --- a/drivebrain_utils/src/MCAPReplay.cpp +++ b/drivebrain_utils/src/MCAPReplay.cpp @@ -23,7 +23,7 @@ MCAPReplay::MCAPReplay(std::string param_file_path, std::string dbc_file_path) _acu_sender = std::make_shared(_io_context, 7766, "127.0.0.1", false); // _acu_core_sender = std::make_shared(nullptr, _io_context, 7777, "127.0.0.1"); - _vcr_sender = std::make_shared(nullptr, _io_context, 9999, "127.0.0.1"); + _vcr_sender = std::make_shared(nullptr, _io_context, 9999, "127.0.0.1", false); _vn_sender = std::make_shared(_io_context, 13111, "127.0.0.1", false); // fake VN @@ -148,6 +148,9 @@ void MCAPReplay::start(std::string filename) { } 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; From 971ea2fad529bcaaf298f84235f2d2d28983781e Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 30 May 2025 20:43:05 -0400 Subject: [PATCH 72/87] fixing mcapreplay util to be up to current ethsender --- drivebrain_utils/src/MCAPReplay.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivebrain_utils/src/MCAPReplay.cpp b/drivebrain_utils/src/MCAPReplay.cpp index 266ca75..14705a5 100644 --- a/drivebrain_utils/src/MCAPReplay.cpp +++ b/drivebrain_utils/src/MCAPReplay.cpp @@ -23,7 +23,7 @@ MCAPReplay::MCAPReplay(std::string param_file_path, std::string dbc_file_path) _acu_sender = std::make_shared(_io_context, 7766, "127.0.0.1", false); // _acu_core_sender = std::make_shared(nullptr, _io_context, 7777, "127.0.0.1"); - _vcr_sender = std::make_shared(nullptr, _io_context, 9999, "127.0.0.1", false); + _vcr_sender = std::make_shared(_io_context, 9999, "127.0.0.1", false); _vn_sender = std::make_shared(_io_context, 13111, "127.0.0.1", false); // fake VN From 1dba68bb21b285458c791ba7aba6423ca267e7b4 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 30 May 2025 20:46:54 -0400 Subject: [PATCH 73/87] oops, wrong release --- flake.lock | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flake.lock b/flake.lock index 1ee259a..5e37a92 100644 --- a/flake.lock +++ b/flake.lock @@ -71,8 +71,8 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1748574232, - "narHash": "sha256-8Y5DPY/YN7XNZDxEta2JJFy/aDtVul6ezbZMZp2cH+M=", + "lastModified": 1748650061, + "narHash": "sha256-duttC/lVRWkGrBM/hLUYYjHcAkqE/LppWYJpq4au6+8=", "type": "tarball", "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz" }, From d150e85a48093ec7fa6731cb04febe075fa438ff Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 5 Jun 2025 14:42:12 -0400 Subject: [PATCH 74/87] fixing logging for speed tech comms --- drivebrain_app/src/DriveBrainApp.cpp | 3 ++- drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 772fa91..2483850 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -202,7 +202,8 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d _vcr_eth_driver, _vcf_eth_driver, _aero_sensor_driver, - _scale_comms + _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()); diff --git a/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp index a42a013..3ad51d3 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/SpeedTechComms.cpp @@ -42,7 +42,7 @@ void SpeedTechComms::process_buffer(const boost::array &buff, speed_tech_lap_time_msg->set_laptime(lap_time); speed_tech_lap_time_msg->set_lapcount(lap_count); - spdlog::debug("lapcount {} laptime {}", lap_count, lap_time); + spdlog::info("lapcount {} laptime {}", lap_count, lap_time); this->log(speed_tech_lap_time_msg); } else { From d449824189a28f7c0bbc389617adec19236c63c2 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 6 Jun 2025 04:35:21 -0400 Subject: [PATCH 75/87] erm, just updating json file --- config/drivebrain_config.json | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index df66c0a..86868fd 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -93,7 +93,7 @@ "gain_MatlabEstimModel": { "in":2.0 }, - "estimator_MatlabEstimModel": { + "estimator_MatlabEstimModel": { "steer_sensor_offset": 0.0, "static_fz_N_fl": 0.0, "static_fz_N_fr": 0.0, @@ -120,7 +120,11 @@ "elec_p_lim_kW": 0.0, "motor_rpm_lim": 0.0, "brake_deadzone": 0.0, - "accel_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 }, "qp_torq_allocator_MatlabModel": { "mux": 0.0, @@ -134,12 +138,16 @@ "torq_side_delta": 0.0, "torq_long_delta": 0.0, "w": 0.0, - "coast_brake_torq": 0.0 + "coast_brake_torq": 0.0, + "w_starting": 0.0, + "w_ending": 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": false + "use_laptimer": true } From 0aa2b7fea3a59fab381fc6c715ee7c60ce4ddda1 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 7 Jun 2025 11:38:10 -0400 Subject: [PATCH 76/87] updating to latest simulink gend release --- config/drivebrain_config.json | 27 ++++++++++++++++++++++++--- flake.lock | 4 ++-- 2 files changed, 26 insertions(+), 5 deletions(-) diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index df66c0a..f353b72 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -120,7 +120,16 @@ "elec_p_lim_kW": 0.0, "motor_rpm_lim": 0.0, "brake_deadzone": 0.0, - "accel_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, @@ -133,8 +142,10 @@ "k_opt": 0.0, "torq_side_delta": 0.0, "torq_long_delta": 0.0, - "w": 0.0, - "coast_brake_torq": 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, @@ -143,3 +154,13 @@ "use_laptimer": false } + + +[2025-06-07 11:35:21.181] [warning] config file does not contain config: vel_saturation for component: estimator_MatlabEstimModel +[2025-06-07 11:35:21.181] [warning] config file does not contain config: per_motor_elec_p_lim_kW for component: estimator_MatlabEstimModel +[2025-06-07 11:35:21.181] [warning] config file does not contain config: use_steer_deadzone for component: estimator_MatlabEstimModel +[2025-06-07 11:35:21.181] [warning] config file does not contain config: use_intent_w_derate for component: estimator_MatlabEstimModel +[2025-06-07 11:35:21.181] [warning] config file does not contain config: amk_eff_modifier_fr for component: estimator_MatlabEstimModel +[2025-06-07 11:35:21.181] [warning] config file does not contain config: amk_eff_modifier_rl for component: estimator_MatlabEstimModel +[2025-06-07 11:35:21.181] [warning] config file does not contain config: amk_eff_modifier_rr for component: estimator_MatlabEstimModel +[2025-06-07 11:35:21.181] [warning] config file does not contain config: global_eff_modifier for component: estimator_MatlabEstimModel diff --git a/flake.lock b/flake.lock index 5e37a92..ada2038 100644 --- a/flake.lock +++ b/flake.lock @@ -71,8 +71,8 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1748650061, - "narHash": "sha256-duttC/lVRWkGrBM/hLUYYjHcAkqE/LppWYJpq4au6+8=", + "lastModified": 1749202877, + "narHash": "sha256-qDi85WEtUvEKOu6obN+RPagp9sV/EWlsu9MWJEGHGz0=", "type": "tarball", "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz" }, From a38fd7a80bf0185171826a247119c60c6afd80ff Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 7 Jun 2025 11:48:59 -0400 Subject: [PATCH 77/87] adding in debug statements --- .../drivebrain_comms/include/CANComms.hpp | 3 ++ .../drivebrain_comms/include/ETHRecvComms.tpp | 1 + .../drivebrain_comms/src/CANComms.cpp | 16 ++++++++++ .../src/StateEstimator.cpp | 29 +++++++------------ 4 files changed, 31 insertions(+), 18 deletions(-) diff --git a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp index 8a3d33a..8f02a1d 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp +++ b/drivebrain_core_impl/drivebrain_comms/include/CANComms.hpp @@ -137,5 +137,8 @@ namespace comms int _CAN_socket; // can socket bound to bool _running = false; std::shared_ptr _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/ETHRecvComms.tpp b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp index c522237..544113a 100644 --- a/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp +++ b/drivebrain_core_impl/drivebrain_comms/include/ETHRecvComms.tpp @@ -37,6 +37,7 @@ namespace comms if(_state_estimator) { + _state_estimator->handle_recv_process(out_msg); } diff --git a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp index 21decd6..709e57b 100644 --- a/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp +++ b/drivebrain_core_impl/drivebrain_comms/src/CANComms.cpp @@ -120,7 +120,23 @@ void comms::CANDriver::_handle_recv_CAN_frame(const struct can_frame &frame) { if (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(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); diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 658f5dc..3aa41ec 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -309,25 +309,18 @@ std::pair StateEstimator::get_latest_state_and_validit auto elapsed = std::chrono::duration_cast(state_estim_end - state_estim_start); - constexpr bool debug = false; - if ((elapsed > std::chrono::microseconds(6000)) && debug) // 6ms + constexpr bool debug = true; + if ((elapsed > std::chrono::microseconds(4000)) && debug) // 4ms { - std::cout << "WARNING: timing" << std::endl; - std::cout << "total: " - << (static_cast( - std::chrono::duration_cast(elapsed).count())) - << " us\n"; - std::cout << "state mutex: " - << (static_cast(std::chrono::duration_cast( - state_mutex_end - state_mutex_start) - .count())) - << " us\n"; - std::cout << "log time: " - << (static_cast( - std::chrono::duration_cast(log_end - log_start) - .count())) - << " us\n"; - } + auto total_us = std::chrono::duration_cast(elapsed).count(); + auto state_mutex_us = std::chrono::duration_cast(state_mutex_end - state_mutex_start).count(); + auto log_time_us = std::chrono::duration_cast(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}; } From 493233968c5e05cafa7cdab9605aa530ded99572 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 11 Jun 2025 01:33:21 -0400 Subject: [PATCH 78/87] adding in inverter temps data --- .../include/StateEstimator.hpp | 7 +++ .../src/StateEstimator.cpp | 63 +++++++++++++++++++ flake.lock | 14 ++--- flake.nix | 2 +- 4 files changed, 78 insertions(+), 8 deletions(-) diff --git a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp index 5bae153..34196c1 100644 --- a/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp +++ b/drivebrain_core_impl/drivebrain_estimation/include/StateEstimator.hpp @@ -114,6 +114,9 @@ namespace core template void _handle_set_inverter_dynamics(std::shared_ptr msg); + template + void _handle_set_inverter_temps(std::shared_ptr msg); + std::shared_ptr _set_ins_state_data(core::VehicleState current_state, std::shared_ptr msg_out); template @@ -128,8 +131,12 @@ namespace core core::VehicleState _vehicle_state; core::RawInputData _raw_input_data; std::array _timestamp_array; + std::shared_ptr _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 3aa41ec..8b7c25e 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -35,6 +35,14 @@ void StateEstimator::_recv_inverter_states(std::shared_ptr(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); } } @@ -149,6 +157,21 @@ void StateEstimator::_handle_set_inverter_dynamics(std::shared_ptr +void StateEstimator::_handle_set_inverter_temps(std::shared_ptr msg) { + + + auto in_msg = std::static_pointer_cast(msg); + core::DrivetrainData dt_data = {}; + dt_data.inverter_igbt_temps_c.set_from_index(in_msg->igbt_temp()); + dt_data.inverter_temps_c.set_from_index(in_msg->inverter_temp()); + dt_data.inverter_motor_temps_c.set_from_index(in_msg->motor_temp()); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data = dt_data; + } +} + // TODO parameterize the timeout threshold template bool StateEstimator::_validate_stamps( @@ -158,6 +181,8 @@ bool StateEstimator::_validate_stamps( std::unique_lock lk(_state_mutex); timestamp_array_to_sort = timestamp_arr; } + + auto debug_copy = timestamp_array_to_sort; const std::chrono::microseconds threshold(30000); // 30 milliseconds in microseconds // Sort the array @@ -171,10 +196,48 @@ bool StateEstimator::_validate_stamps( auto curr_time = std::chrono::duration_cast( std::chrono::high_resolution_clock::now().time_since_epoch()); + + constexpr std::chrono::seconds debug_print_period(1); + bool all_members_received = min_stamp.count() > 0; // count here is the count in microseconds bool last_update_recent_enough = (std::chrono::duration_cast(curr_time - max_stamp)) < threshold; + if(std::chrono::duration_cast(curr_time - max_stamp) < _debug_maxtime_diff || (_debug_maxtime_diff.count() < 0)) + { + _debug_maxtime_diff = std::chrono::duration_cast(curr_time - max_stamp); + } + if(std::chrono::duration_cast(max_stamp - min_stamp) < _debug_maxjitter_diff || (_debug_maxjitter_diff.count() < 0)) + { + _debug_maxjitter_diff = std::chrono::duration_cast(max_stamp - min_stamp); + } + + if((curr_time - _last_debug_veh_state_print) > debug_print_period) + { + _last_debug_veh_state_print = std::chrono::duration_cast(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; } diff --git a/flake.lock b/flake.lock index ada2038..705f9d3 100644 --- a/flake.lock +++ b/flake.lock @@ -55,11 +55,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1748639958, - "narHash": "sha256-qRfdJxKy/O1L2pkGX3Xe78m+K/pLNBQFk6s5HvK6YVI=", + "lastModified": 1749617325, + "narHash": "sha256-ccwgioLZGw4nFrlleC+q+bCHKO9YZBdExRPvrcgG6ek=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "a3faebc4b772dd26314697d43f4409bf4f5c3e1d", + "rev": "7ac98d6a4b4d17e2c0ded4051b5aedb58fcdecb3", "type": "github" }, "original": { @@ -71,14 +71,14 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1749202877, - "narHash": "sha256-qDi85WEtUvEKOu6obN+RPagp9sV/EWlsu9MWJEGHGz0=", + "lastModified": 1749619821, + "narHash": "sha256-XT04hxSvIrIaGLRs8F//x/drQczET34rF7wtkSo/1BE=", "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel4/gen_rel.tar.gz" }, "original": { "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel4/gen_rel.tar.gz" } }, "dbcppp-src": { diff --git a/flake.nix b/flake.nix index 1e20a9b..9f5ba59 100644 --- a/flake.nix +++ b/flake.nix @@ -35,7 +35,7 @@ }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/test-rel3/gen_rel.tar.gz"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel4/gen_rel.tar.gz"; flake = false; }; }; From 2a1dd9c837bde3cd71c9bcff19af90a51006f8e3 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 11 Jun 2025 21:16:32 -0400 Subject: [PATCH 79/87] 500ms timeout for CAN msgs --- .../drivebrain_estimation/src/StateEstimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 8b7c25e..e8ce1de 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -183,7 +183,7 @@ bool StateEstimator::_validate_stamps( } auto debug_copy = timestamp_array_to_sort; - const std::chrono::microseconds threshold(30000); // 30 milliseconds in microseconds + 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()); From 773e9f726173887af1ff11b0cab5494238ad06d5 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 12 Jun 2025 10:33:06 -0400 Subject: [PATCH 80/87] adding in min cell and latest release of controller with de-rating --- .../drivebrain_estimation/src/StateEstimator.cpp | 6 ++++++ flake.lock | 14 +++++++------- flake.nix | 2 +- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index e8ce1de..4c97dbb 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -82,6 +82,12 @@ void StateEstimator::handle_recv_process(std::shared_ptrGetTypeName() == "hytech_msgs.ACUAllData") { + auto in_msg = std::static_pointer_cast(message); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.acc_data.min_cell_voltage = in_msg->core_data().min_cell_voltage(); + } } else { _recv_low_level_state(message); diff --git a/flake.lock b/flake.lock index 705f9d3..ac65e9c 100644 --- a/flake.lock +++ b/flake.lock @@ -55,11 +55,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1749617325, - "narHash": "sha256-ccwgioLZGw4nFrlleC+q+bCHKO9YZBdExRPvrcgG6ek=", + "lastModified": 1749737491, + "narHash": "sha256-RXqEbUEr5+Oe75hfA+BXYvw++2Nog1wbO86qdn9dEu8=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "7ac98d6a4b4d17e2c0ded4051b5aedb58fcdecb3", + "rev": "8d16f458f7a730c92940dea0552fffc1ef02890e", "type": "github" }, "original": { @@ -71,14 +71,14 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1749619821, - "narHash": "sha256-XT04hxSvIrIaGLRs8F//x/drQczET34rF7wtkSo/1BE=", + "lastModified": 1749738469, + "narHash": "sha256-gUvhSvbgWqHbt6jA+yGecq+ikcuY0HFd/m/+mpKdwAk=", "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel4/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel6/gen_rel.tar.gz" }, "original": { "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel4/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel6/gen_rel.tar.gz" } }, "dbcppp-src": { diff --git a/flake.nix b/flake.nix index 9f5ba59..8bf13dc 100644 --- a/flake.nix +++ b/flake.nix @@ -35,7 +35,7 @@ }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel4/gen_rel.tar.gz"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel6/gen_rel.tar.gz"; flake = false; }; }; From bc3f61fd1a07a07992768fd8fbeea523ee122b45 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Jun 2025 02:29:08 -0400 Subject: [PATCH 81/87] adding in pack voltage measurement to state estimator for propagation --- .../drivebrain_estimation/src/StateEstimator.cpp | 8 ++++++++ flake.lock | 6 +++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 4c97dbb..3f33222 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -43,6 +43,14 @@ void StateEstimator::_recv_inverter_states(std::shared_ptr(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(msg); + auto voltage = in_msg->dc_bus_voltage(); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.acc_data.pack_voltage = static_cast(voltage); + } } } diff --git a/flake.lock b/flake.lock index ac65e9c..9c50a28 100644 --- a/flake.lock +++ b/flake.lock @@ -55,11 +55,11 @@ "db-core-src": { "flake": false, "locked": { - "lastModified": 1749737491, - "narHash": "sha256-RXqEbUEr5+Oe75hfA+BXYvw++2Nog1wbO86qdn9dEu8=", + "lastModified": 1749880473, + "narHash": "sha256-OCbbt3Mt7E96bqQ/uyg3yno79ooLr6vXo+bkD9Z+us8=", "owner": "hytech-racing", "repo": "drivebrain_core", - "rev": "8d16f458f7a730c92940dea0552fffc1ef02890e", + "rev": "40f2c209639924b9ecee2bcf3492564d49479939", "type": "github" }, "original": { From 67c485f2ce3bf69cc5888db3af7b4c1a0b179de6 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Jun 2025 02:36:40 -0400 Subject: [PATCH 82/87] updated to latest hT_PROTO and HT_CAN --- flake.lock | 14 +++++++------- flake.nix | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/flake.lock b/flake.lock index 9c50a28..cce4540 100644 --- a/flake.lock +++ b/flake.lock @@ -6,16 +6,16 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1748639873, - "narHash": "sha256-7KC4DEH/hqrnjRaOEXqvvQJdyBjUFjTGFYM2HVflrYk=", + "lastModified": 1749876080, + "narHash": "sha256-ByaYj2GciK9rYQ/t3RzkBbsxGe7iJft2gKkuLszN2ZA=", "owner": "hytech-racing", "repo": "HT_proto", - "rev": "98d4b72640844f87ae2d2cef88a13bb6acd8226f", + "rev": "e1cb57ba1cb4634f86d1ae29b805ea73ea90f4ff", "type": "github" }, "original": { "owner": "hytech-racing", - "ref": "2025-05-30T21_18_06", + "ref": "2025-06-14T04_41_35", "repo": "HT_proto", "type": "github" } @@ -373,11 +373,11 @@ "utils": "utils_2" }, "locked": { - "lastModified": 1745452764, - "narHash": "sha256-kWgeQEj1HIrIUdGsZQqGqs/k6NgM6lGJsJ1o8atXnnE=", + "lastModified": 1749841515, + "narHash": "sha256-hXVCFS4ixI5bHNnU0ik3fjzi0fUnV26AQ7zIlCq7Rgk=", "owner": "hytech-racing", "repo": "ht_can", - "rev": "b9f6fa362d0853d94b4f1c50306a4535d071856e", + "rev": "e2d22ac11fbf9a1b69680bdab4a277a53e827757", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index 8bf13dc..a4498d0 100644 --- a/flake.nix +++ b/flake.nix @@ -16,7 +16,7 @@ nix-proto.url = "github:notalltim/nix-proto"; nix-proto.inputs.nixpkgs.follows = "nixpkgs"; - HT_proto.url = "github:hytech-racing/HT_proto/2025-05-30T21_18_06"; + HT_proto.url = "github:hytech-racing/HT_proto/2025-06-14T04_41_35"; foxglove-schemas-src = { url = "github:foxglove/schemas"; From e09e9ce2958be0b11dd7a85aaca48801ea8766ce Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Jun 2025 11:33:39 -0400 Subject: [PATCH 83/87] updating to latest rel of db simulink --- flake.lock | 8 ++++---- flake.nix | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/flake.lock b/flake.lock index cce4540..40650dc 100644 --- a/flake.lock +++ b/flake.lock @@ -71,14 +71,14 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1749738469, - "narHash": "sha256-gUvhSvbgWqHbt6jA+yGecq+ikcuY0HFd/m/+mpKdwAk=", + "lastModified": 1749914780, + "narHash": "sha256-u4CDvtmDn1D7VfoD/QITc7BI3leZFZDcsbVsbxsxrdo=", "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel6/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel7/gen_rel.tar.gz" }, "original": { "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel6/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel7/gen_rel.tar.gz" } }, "dbcppp-src": { diff --git a/flake.nix b/flake.nix index a4498d0..314357d 100644 --- a/flake.nix +++ b/flake.nix @@ -35,7 +35,7 @@ }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel6/gen_rel.tar.gz"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel7/gen_rel.tar.gz"; flake = false; }; }; From aaf09fd890a865ce048f4f978c21ee4e1e651ffb Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Jun 2025 12:20:46 -0400 Subject: [PATCH 84/87] removing unnessecary logging of drivebrain command and added in 1hz can message for vn status to be recvd by dashboard --- drivebrain_app/include/DriveBrainApp.hpp | 3 +++ drivebrain_app/src/DriveBrainApp.cpp | 26 ++++++++++++++++-------- flake.lock | 6 +++--- 3 files changed, 23 insertions(+), 12 deletions(-) diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 3a5bfd1..47e686f 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -119,4 +119,7 @@ class DriveBrainApp { 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/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 2483850..77c4924 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -253,6 +253,23 @@ void DriveBrainApp::_process_loop() { _estim_manager->evaluate_all_estimators(state_and_validity.first); auto out_struct = _controllerManager.step_active_controller(state_and_validity.first); + auto now = std::chrono::steady_clock::now(); + + if (now - _last_send_time >= _send_period) { + _last_send_time = now; + + auto drivebrain_state_data_msg = std::make_shared(); + hytech::vn_gps_status status = static_cast(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 + // get current command std::variant cmd_out = out_struct.out; @@ -282,11 +299,6 @@ void DriveBrainApp::_process_loop() { 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)); - if(_message_logger) - { - _message_logger->log_msg(static_cast>(torque_limit_msg)); - } - { std::unique_lock lk(_primary_can_tx_queue.mtx); _primary_can_tx_queue.deque.push_back(desired_rpm_msg); @@ -303,10 +315,6 @@ void DriveBrainApp::_process_loop() { desired_torque_msg->set_drivebrain_torque_rl(torqueControl->desired_torques_nm.RL); desired_torque_msg->set_drivebrain_torque_rr(torqueControl->desired_torques_nm.RR); - if(_message_logger) - { - _message_logger->log_msg(static_cast>(desired_torque_msg)); - } { std::unique_lock lk(_primary_can_tx_queue.mtx); diff --git a/flake.lock b/flake.lock index 40650dc..c274c32 100644 --- a/flake.lock +++ b/flake.lock @@ -373,11 +373,11 @@ "utils": "utils_2" }, "locked": { - "lastModified": 1749841515, - "narHash": "sha256-hXVCFS4ixI5bHNnU0ik3fjzi0fUnV26AQ7zIlCq7Rgk=", + "lastModified": 1749916491, + "narHash": "sha256-YKHet4wW71yYoah2AYlHIC5n930HI0lWFkeuxzlSd54=", "owner": "hytech-racing", "repo": "ht_can", - "rev": "e2d22ac11fbf9a1b69680bdab4a277a53e827757", + "rev": "ef8559488f5c020ecf83175e1cd12afc84e15167", "type": "github" }, "original": { From 602925fb7e00e45a17afae4f4a00be80e2859100 Mon Sep 17 00:00:00 2001 From: Krish Kittur Date: Wed, 10 Sep 2025 21:55:19 -0700 Subject: [PATCH 85/87] added core data --- drivebrain_app/include/DriveBrainApp.hpp | 1 + drivebrain_app/src/DriveBrainApp.cpp | 2 ++ flake.lock | 8 ++++---- flake.nix | 2 +- simulink_automation.nix | 2 +- 5 files changed, 9 insertions(+), 6 deletions(-) diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 47e686f..de6be71 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -99,6 +99,7 @@ class DriveBrainApp { std::shared_ptr _lap_timer_driver; bool _using_lap_timer = false; std::shared_ptr> _acu_eth_driver; + std::shared_ptr> _acu_eth_driver_core; std::shared_ptr> _vcr_eth_driver; std::shared_ptr> _vcf_eth_driver; std::shared_ptr> _fake_vn = nullptr; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 77c4924..6067f70 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -97,6 +97,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d 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>(_io_context, 7777); _acu_eth_driver = std::make_shared>(_io_context, 7766); _vcr_eth_driver = std::make_shared>(_io_context, 9999, _state_estimator); _vcf_eth_driver = std::make_shared>(_io_context, 4444); @@ -198,6 +199,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d _vn_driver, _fake_vn, _state_estimator, + _acu_eth_driver_core, _acu_eth_driver, _vcr_eth_driver, _vcf_eth_driver, diff --git a/flake.lock b/flake.lock index c274c32..2d1d350 100644 --- a/flake.lock +++ b/flake.lock @@ -71,14 +71,14 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1749914780, - "narHash": "sha256-u4CDvtmDn1D7VfoD/QITc7BI3leZFZDcsbVsbxsxrdo=", + "lastModified": 1757551410, + "narHash": "sha256-/G9qLyVhUpcuJMQpI3Aval3Gr5Brw59XuwtxXE5+DKc=", "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel7/gen_rel.tar.gz" + "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/rel7/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel13/gen_rel.tar" } }, "dbcppp-src": { diff --git a/flake.nix b/flake.nix index 314357d..3d87c75 100644 --- a/flake.nix +++ b/flake.nix @@ -35,7 +35,7 @@ }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel7/gen_rel.tar.gz"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel13/gen_rel.tar"; flake = false; }; }; diff --git a/simulink_automation.nix b/simulink_automation.nix index eee81c9..ae16c35 100644 --- a/simulink_automation.nix +++ b/simulink_automation.nix @@ -2,7 +2,7 @@ stdenv.mkDerivation { name = "matlab-math"; - src = "${db-simulink-gen-src}/source_code"; + src = "${db-simulink-gen-src}/matlab_math.tar.gz"; version = "1.0.0"; nativeBuildInputs = [ cmake ]; propagatedBuildInputs = [ drivebrain_core simulink_automation_msgs_proto_cpp]; From 759603582d3e571779f2cc88e19cb52e8f6416ea Mon Sep 17 00:00:00 2001 From: Krish Kittur Date: Sun, 21 Sep 2025 00:32:55 -0400 Subject: [PATCH 86/87] update --- flake.nix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flake.nix b/flake.nix index 3d87c75..37f31af 100644 --- a/flake.nix +++ b/flake.nix @@ -35,7 +35,7 @@ }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel13/gen_rel.tar"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel14/gen_rel.tar"; flake = false; }; }; From 8b021e8964813d9a64fbc0466b82645ad3b5e247 Mon Sep 17 00:00:00 2001 From: Krish Kittur Date: Sun, 21 Sep 2025 00:33:59 -0400 Subject: [PATCH 87/87] missing .gz --- flake.nix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flake.nix b/flake.nix index 37f31af..68cd76b 100644 --- a/flake.nix +++ b/flake.nix @@ -35,7 +35,7 @@ }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel14/gen_rel.tar"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel14/gen_rel.tar.gz"; flake = false; }; };