Nimay/feature/lap tracker

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,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 5f61368
+    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
 #################################
@@ -62,7 +63,6 @@ target_link_libraries(drivebrain PUBLIC
     hytech_msgs_cpp_lib
     hytech_can_msgs_cpp_lib
     drivebrain_core
-    drivebrain_comms
 )
 
 #################################

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")

drivebrain_core/include/LapTracker.hpp

@@ -0,0 +1,55 @@
+#include <StateTracker.hpp>
+#include <hytech_msgs.pb.h>
+#include <chrono>
+#include <memory>
+
+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();
+
+            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<LapTracker*> _s_instance;
+
+            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};
+    };
+
+}

drivebrain_core/include/StateTracker.hpp

@@ -209,6 +209,14 @@ namespace core {
         float min_cell_voltage;
         float pack_voltage;
     };
+     /**
+     * @struct Laptime info
+     */
+    struct LaptimeInfo {
+        float laptime_seconds;
+        int lapcount;
+        float max_lap_speed_mps;
+    };
 
     /**
      * @struct Contains a wealth of data
@@ -239,6 +247,7 @@ namespace core {
         float old_energy_meter_kw;
         DrivetrainData dt_data;
         AccumulatorData acc_data;
+        LaptimeInfo laptime_info;
     };
 
     /**
@@ -249,10 +258,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 +284,20 @@ namespace core {
              */
             std::pair<core::VehicleState, bool> 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 <size_t index, typename inverter_dynamics_message>
             void _handle_set_inverter_dynamics(std::shared_ptr<google::protobuf::Message> msg);
 
@@ -297,6 +316,8 @@ namespace core {
             std::mutex _state_mutex;
             std::array<std::chrono::microseconds, 4> _timestamp_array; 
 
+            inline static std::atomic<StateTracker*> _s_instance;
+
 
     };
 }

drivebrain_core/src/LapTracker.cpp

@@ -0,0 +1,81 @@
+#include <LapTracker.hpp>
+#include <cmath>
+
+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<hytech_msgs::LapTime> laptime_information = std::make_shared<hytech_msgs::LapTime>();
+
+    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<float>(now - _lap_start_time).count();
+
+    laptime_information->set_laptime_seconds(laptime_seconds);
+    laptime_information->set_lapcount(static_cast<int64_t>(_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() {
+    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::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");
+    return *instance;
+}
+

drivebrain_core/src/StateTracker.cpp

@@ -29,9 +29,15 @@ void StateTracker::handle_receive_protobuf_message(std::shared_ptr<google::proto
 
         auto ins_mode_int = in_msg->status().ins_mode_int();
         auto vel_u = in_msg->status().ins_vel_u();
+        core::Position pos = {
+            static_cast<double>(in_msg->vn_gps().lat()),
+            static_cast<double>(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;
@@ -52,8 +58,15 @@ void StateTracker::handle_receive_protobuf_message(std::shared_ptr<google::proto
             std::unique_lock lk(_state_mutex);
             _vehicle_state.acc_data.min_cell_voltage = in_msg->core_data().min_cell_voltage();
         }
-    }
-    else {
+    } else if (msg->GetDescriptor() == hytech_msgs::LapTime::descriptor()) {
+        auto in_msg = std::static_pointer_cast<hytech_msgs::LapTime>(msg);
+        {
+            std::unique_lock lk(_state_mutex);
+            _vehicle_state.laptime_info.laptime_seconds = in_msg->laptime_seconds();
+            _vehicle_state.laptime_info.lapcount = static_cast<int>(in_msg->lapcount());
+            _vehicle_state.laptime_info.max_lap_speed_mps = in_msg->max_lap_speed_ms();
+        }
+    } else {
         _receive_low_level_state(msg);
     }
 }
@@ -263,4 +276,19 @@ bool StateTracker::_validate_timestamps(const std::array<std::chrono::microsecon
         (std::chrono::duration_cast<std::chrono::microseconds>(curr_time - max_stamp)) < threshold;
 
     return within_threshold && all_members_received && last_update_recent_enough;
-}
+}
+
+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;
+}