From fb13a216b42863e6339b2aec344bcf55b1dcd6b1 Mon Sep 17 00:00:00 2001 From: Krish Kittur Date: Mon, 19 Jan 2026 22:17:38 -0500 Subject: [PATCH 1/2] lap tracker outline --- CMakeLists.txt | 7 ++-- drivebrain_core/CMakeLists.txt | 1 + drivebrain_core/include/LapTracker.hpp | 47 +++++++++++++++++++++++ drivebrain_core/include/StateTracker.hpp | 30 ++++++++++++--- drivebrain_core/src/LapTracker.cpp | 30 +++++++++++++++ drivebrain_core/src/StateTracker.cpp | 27 +++++++++++-- test_1.mcap | Bin 0 -> 809 bytes 7 files changed, 130 insertions(+), 12 deletions(-) create mode 100644 drivebrain_core/include/LapTracker.hpp create mode 100644 drivebrain_core/src/LapTracker.cpp create mode 100644 test_1.mcap diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b85247..6b8dc36 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -# cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.10) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) project(drivebrain_software_2026) @@ -10,7 +10,7 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_SOURCE_DIR}/cmake") # Custom libs ################################# add_subdirectory(drivebrain_core) -add_subdirectory(drivebrain_comms) +# add_subdirectory(drivebrain_comms) ################################# # Find packages @@ -28,7 +28,7 @@ find_package(GTest REQUIRED) FetchContent_Declare( HT_Proto GIT_REPOSITORY https://github.com/hytech-racing/HT_proto.git - GIT_TAG 5f61368 + GIT_TAG f7967e90831271f9704a406a8f777d43c03d0d0b ) FetchContent_MakeAvailable(HT_Proto) @@ -62,7 +62,6 @@ target_link_libraries(drivebrain PUBLIC hytech_msgs_cpp_lib hytech_can_msgs_cpp_lib drivebrain_core - drivebrain_comms ) ################################# diff --git a/drivebrain_core/CMakeLists.txt b/drivebrain_core/CMakeLists.txt index 355ccc4..5e64ad6 100644 --- a/drivebrain_core/CMakeLists.txt +++ b/drivebrain_core/CMakeLists.txt @@ -11,6 +11,7 @@ add_library(drivebrain_core src/FoxgloveServer.cpp src/MCAPLogger.cpp src/StateTracker.cpp + src/LapTracker.cpp ) target_include_directories(drivebrain_core PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") diff --git a/drivebrain_core/include/LapTracker.hpp b/drivebrain_core/include/LapTracker.hpp new file mode 100644 index 0000000..319f8b6 --- /dev/null +++ b/drivebrain_core/include/LapTracker.hpp @@ -0,0 +1,47 @@ +#include +#include +#include + +// TODO Add any imports you might think are neccesary here + +namespace core { + + class LapTracker { + + public: + + /* Singleton move semantics */ + LapTracker(const LapTracker&) = delete; + LapTracker& operator=(const LapTracker&) = delete; + + /** + * Steps the lap tracker + * + * @param latest_state the latest state (will be updated in the main loop after get_latest_state_and_validity + * is called by the state tracker) + */ + void step_tracker(core::VehicleState& latest_state); + + /** + * Creates a new instance of the lap tracker + */ + static void create(); + + /** + * Returns the lap tracker instance + */ + static LapTracker& instance(); + + + private: + + LapTracker(); + + /** Internal State */ + inline static std::atomic _s_instance; + + // TODO put variables here to keep track of the lap tracker's internal state + + }; + +} \ No newline at end of file diff --git a/drivebrain_core/include/StateTracker.hpp b/drivebrain_core/include/StateTracker.hpp index 6565030..bb24a71 100644 --- a/drivebrain_core/include/StateTracker.hpp +++ b/drivebrain_core/include/StateTracker.hpp @@ -209,6 +209,13 @@ namespace core { float min_cell_voltage; float pack_voltage; }; + /** + * @struct Laptime info + */ + struct LaptimeInfo { + float laptime_seconds; + int lapcount; + }; /** * @struct Contains a wealth of data @@ -239,6 +246,7 @@ namespace core { float old_energy_meter_kw; DrivetrainData dt_data; AccumulatorData acc_data; + LaptimeInfo laptime_info; }; /** @@ -249,10 +257,9 @@ namespace core { public: - /** - * Constructs a new state tracker. - */ - StateTracker() = default; + /* Singleton move semantics */ + StateTracker(const StateTracker&) = delete; + StateTracker& operator=(const StateTracker&) = delete; /** * Receives a protobuf message and adds any useful information to the internal @@ -276,9 +283,20 @@ namespace core { */ std::pair get_latest_state_and_validity(); - + /** + * Creates a new instance of the state tracker + */ + static void create(); + + /** + * Returns the state tracker instance + */ + static StateTracker& instance(); + private: + StateTracker(); + template void _handle_set_inverter_dynamics(std::shared_ptr msg); @@ -297,6 +315,8 @@ namespace core { std::mutex _state_mutex; std::array _timestamp_array; + inline static std::atomic _s_instance; + }; } diff --git a/drivebrain_core/src/LapTracker.cpp b/drivebrain_core/src/LapTracker.cpp new file mode 100644 index 0000000..70c9b36 --- /dev/null +++ b/drivebrain_core/src/LapTracker.cpp @@ -0,0 +1,30 @@ +#include + +void core::LapTracker::step_tracker(core::VehicleState& latest_state) { + std::shared_ptr laptime_information = std::make_shared(); // TODO you need to fill in the fields of this protobuf message + /** + * TODO it is your responsibility to fill in this method. Look at the VehicleState + * struct to see all the things it gives you (you should see vn position from there). Using this + * and the private variables you added in the header, complete this method. It should use all of the information + * in the latest state to update it's local variables, create a LapTime protobuf, and invoke handle_receive_protobuf_message + * on the state tracker. Some of this is completed for you. Good luck! + */ + core::StateTracker::instance().handle_receive_protobuf_message(laptime_information); // What "records" the information + +} + +void core::LapTracker::create() { + LapTracker* expected = nullptr; + LapTracker* local = new LapTracker(); + if(!_s_instance.compare_exchange_strong(expected, local, std::memory_order_release, std::memory_order_relaxed)) { + // Already initialized, delete local instance + delete local; + } +} + +core::LapTracker& core::LapTracker::instance() { + LapTracker* instance = _s_instance.load(std::memory_order_acquire); + assert(instance != nullptr && "LapTracker has not been initialized"); + return *instance; +} + diff --git a/drivebrain_core/src/StateTracker.cpp b/drivebrain_core/src/StateTracker.cpp index c4461c4..e414667 100644 --- a/drivebrain_core/src/StateTracker.cpp +++ b/drivebrain_core/src/StateTracker.cpp @@ -52,8 +52,14 @@ void StateTracker::handle_receive_protobuf_message(std::shared_ptrcore_data().min_cell_voltage(); } - } - else { + } else if (msg->GetDescriptor() == hytech_msgs::LapTime::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.laptime_info.laptime_seconds = in_msg->laptime_seconds(); + _vehicle_state.laptime_info.lapcount = in_msg->lapcount(); + } + } else { _receive_low_level_state(msg); } } @@ -263,4 +269,19 @@ bool StateTracker::_validate_timestamps(const std::array(curr_time - max_stamp)) < threshold; return within_threshold && all_members_received && last_update_recent_enough; -} \ No newline at end of file +} + +void core::StateTracker::create() { + StateTracker* expected = nullptr; + StateTracker* local = new StateTracker(); + if(!_s_instance.compare_exchange_strong(expected, local, std::memory_order_release, std::memory_order_relaxed)) { + // Already initialized, delete local instance + delete local; + } +} + +core::StateTracker& core::StateTracker::instance() { + StateTracker* instance = _s_instance.load(std::memory_order_acquire); + assert(instance != nullptr && "StateTracker has not been initialized"); + return *instance; +} diff --git a/test_1.mcap b/test_1.mcap new file mode 100644 index 0000000000000000000000000000000000000000..b4ffd545595d2e4485a8548b26bdc156f4890a3b GIT binary patch literal 809 zcmeD5b#@Fe;N@ZzWk3L4ATuX3DK|N>K*31QK+lMci4h{}R2`b7$k!tQqi;bq75#pq z3Sz?e%(+mBs|*rAR!UK3S!z;IVrE`^a(-S~W_oE+Vo7Fx9v4t9t2jTeI5{IVH_;Zz zt5zx~$}dPQD#=VORL+#ky34~j#3FkBtI!DHMvBo zmPH3{D}H-fp!R~4v#P;0Nk=d+z4r5dg%7c@;*3BJV0(6^9p8HB1^t!-AI^Y92(JA(SVNumhfWV2OxN5Gu(W y3sqeP6@bgIT!zY+qRVh|FhPVI(PcP1p)$cx872`Z17^(}sDjYl%4<8J83X|N1)ALe literal 0 HcmV?d00001 From fc5808d3be5db0aa39fd61e4790214aae535083d Mon Sep 17 00:00:00 2001 From: nm121212 <127545866+nm121212@users.noreply.github.com> Date: Mon, 23 Feb 2026 16:13:50 -0500 Subject: [PATCH 2/2] Add max lap speed to lap tracker and enable drivebrain_comms for tests Co-authored-by: Cursor --- CMakeLists.txt | 5 +- drivebrain_core/include/LapTracker.hpp | 40 +++++++------ drivebrain_core/include/StateTracker.hpp | 1 + drivebrain_core/src/LapTracker.cpp | 71 ++++++++++++++++++++---- drivebrain_core/src/StateTracker.cpp | 11 +++- 5 files changed, 98 insertions(+), 30 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b8dc36..0fb9095 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,6 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_SOURCE_DIR}/cmake") # Custom libs ################################# add_subdirectory(drivebrain_core) -# add_subdirectory(drivebrain_comms) ################################# # Find packages @@ -28,7 +27,7 @@ find_package(GTest REQUIRED) FetchContent_Declare( HT_Proto GIT_REPOSITORY https://github.com/hytech-racing/HT_proto.git - GIT_TAG f7967e90831271f9704a406a8f777d43c03d0d0b + GIT_TAG 7cdd552 ) FetchContent_MakeAvailable(HT_Proto) @@ -45,6 +44,8 @@ FetchContent_Declare( ) FetchContent_MakeAvailable(HT_CAN) +add_subdirectory(drivebrain_comms) + ################################# # Linking to main app ################################# diff --git a/drivebrain_core/include/LapTracker.hpp b/drivebrain_core/include/LapTracker.hpp index 319f8b6..707ab97 100644 --- a/drivebrain_core/include/LapTracker.hpp +++ b/drivebrain_core/include/LapTracker.hpp @@ -1,23 +1,22 @@ -#include -#include +#include +#include +#include #include -// TODO Add any imports you might think are neccesary here - namespace core { class LapTracker { - public: + public: /* Singleton move semantics */ LapTracker(const LapTracker&) = delete; LapTracker& operator=(const LapTracker&) = delete; /** - * Steps the lap tracker - * - * @param latest_state the latest state (will be updated in the main loop after get_latest_state_and_validity + * Steps the lap tracker + * + * @param latest_state the latest state (will be updated in the main loop after get_latest_state_and_validity * is called by the state tracker) */ void step_tracker(core::VehicleState& latest_state); @@ -25,23 +24,32 @@ namespace core { /** * Creates a new instance of the lap tracker */ - static void create(); - + static void create(); /** * Returns the lap tracker instance */ - static LapTracker& instance(); - + static LapTracker& instance(); + + void set_start_finish_line(double start_lat, double start_lon, + double end_lat, double end_lon); private: LapTracker(); - + /** Internal State */ inline static std::atomic _s_instance; - - // TODO put variables here to keep track of the lap tracker's internal state - + + std::chrono::steady_clock::time_point _lap_start_time; + int _lap_count{0}; + float _max_lap_speed_mps{0.f}; + + double _line_start_lat{0.}; + double _line_start_lon{0.}; + double _line_end_lat{0.}; + double _line_end_lon{0.}; + double _prev_side{0.}; + bool _has_prev_position{false}; }; } \ No newline at end of file diff --git a/drivebrain_core/include/StateTracker.hpp b/drivebrain_core/include/StateTracker.hpp index bb24a71..7c5a1b5 100644 --- a/drivebrain_core/include/StateTracker.hpp +++ b/drivebrain_core/include/StateTracker.hpp @@ -215,6 +215,7 @@ namespace core { struct LaptimeInfo { float laptime_seconds; int lapcount; + float max_lap_speed_mps; }; /** diff --git a/drivebrain_core/src/LapTracker.cpp b/drivebrain_core/src/LapTracker.cpp index 70c9b36..611c8d5 100644 --- a/drivebrain_core/src/LapTracker.cpp +++ b/drivebrain_core/src/LapTracker.cpp @@ -1,16 +1,56 @@ -#include +#include +#include + +namespace { + +double line_side(double line_ax, double line_ay, double line_bx, double line_by, + double px, double py) { + double line_dx = line_bx - line_ax; + double line_dy = line_by - line_ay; + double pt_dx = px - line_ax; + double pt_dy = py - line_ay; + return pt_dx * line_dy - pt_dy * line_dx; +} + +} // namespace void core::LapTracker::step_tracker(core::VehicleState& latest_state) { - std::shared_ptr laptime_information = std::make_shared(); // TODO you need to fill in the fields of this protobuf message - /** - * TODO it is your responsibility to fill in this method. Look at the VehicleState - * struct to see all the things it gives you (you should see vn position from there). Using this - * and the private variables you added in the header, complete this method. It should use all of the information - * in the latest state to update it's local variables, create a LapTime protobuf, and invoke handle_receive_protobuf_message - * on the state tracker. Some of this is completed for you. Good luck! - */ - core::StateTracker::instance().handle_receive_protobuf_message(laptime_information); // What "records" the information + std::shared_ptr laptime_information = std::make_shared(); + float current_speed_mps = std::sqrt( + latest_state.current_body_vel_ms.x * latest_state.current_body_vel_ms.x + + latest_state.current_body_vel_ms.y * latest_state.current_body_vel_ms.y + + latest_state.current_body_vel_ms.z * latest_state.current_body_vel_ms.z); + _max_lap_speed_mps = std::max(_max_lap_speed_mps, current_speed_mps); + + bool line_configured = (_line_start_lat != _line_end_lat) || (_line_start_lon != _line_end_lon); + if (latest_state.vehicle_position.valid && line_configured) { + double curr_lat = latest_state.vehicle_position.lat; + double curr_lon = latest_state.vehicle_position.lon; + double curr_side = line_side(_line_start_lat, _line_start_lon, + _line_end_lat, _line_end_lon, + curr_lat, curr_lon); + + if (_has_prev_position) { + if ((_prev_side > 0 && curr_side < 0) || (_prev_side < 0 && curr_side > 0)) { + _lap_count++; + _lap_start_time = std::chrono::steady_clock::now(); + _max_lap_speed_mps = 0.f; + } + } else { + _has_prev_position = true; + } + _prev_side = curr_side; + } + + auto now = std::chrono::steady_clock::now(); + float laptime_seconds = std::chrono::duration(now - _lap_start_time).count(); + + laptime_information->set_laptime_seconds(laptime_seconds); + laptime_information->set_lapcount(static_cast(_lap_count)); + laptime_information->set_max_lap_speed_ms(_max_lap_speed_mps); + + core::StateTracker::instance().handle_receive_protobuf_message(laptime_information); } void core::LapTracker::create() { @@ -22,6 +62,17 @@ void core::LapTracker::create() { } } +core::LapTracker::LapTracker() + : _lap_start_time(std::chrono::steady_clock::now()) {} + +void core::LapTracker::set_start_finish_line(double start_lat, double start_lon, + double end_lat, double end_lon) { + _line_start_lat = start_lat; + _line_start_lon = start_lon; + _line_end_lat = end_lat; + _line_end_lon = end_lon; +} + core::LapTracker& core::LapTracker::instance() { LapTracker* instance = _s_instance.load(std::memory_order_acquire); assert(instance != nullptr && "LapTracker has not been initialized"); diff --git a/drivebrain_core/src/StateTracker.cpp b/drivebrain_core/src/StateTracker.cpp index e414667..8ec7c40 100644 --- a/drivebrain_core/src/StateTracker.cpp +++ b/drivebrain_core/src/StateTracker.cpp @@ -29,9 +29,15 @@ void StateTracker::handle_receive_protobuf_message(std::shared_ptrstatus().ins_mode_int(); auto vel_u = in_msg->status().ins_vel_u(); + core::Position pos = { + static_cast(in_msg->vn_gps().lat()), + static_cast(in_msg->vn_gps().lon()), + true + }; { std::unique_lock lk(_state_mutex); + _vehicle_state.vehicle_position = pos; _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; @@ -55,9 +61,10 @@ void StateTracker::handle_receive_protobuf_message(std::shared_ptrGetDescriptor() == hytech_msgs::LapTime::descriptor()) { auto in_msg = std::static_pointer_cast(msg); { - std::unique_lock lk(_state_mutex); + std::unique_lock lk(_state_mutex); _vehicle_state.laptime_info.laptime_seconds = in_msg->laptime_seconds(); - _vehicle_state.laptime_info.lapcount = in_msg->lapcount(); + _vehicle_state.laptime_info.lapcount = static_cast(in_msg->lapcount()); + _vehicle_state.laptime_info.max_lap_speed_mps = in_msg->max_lap_speed_ms(); } } else { _receive_low_level_state(msg);