Kevin/lap tracker

drivebrain_core/include/LapTracker.hpp

@@ -2,8 +2,6 @@
 #include <hytech_msgs.pb.h> 
 #include <memory>
 
-// TODO Add any imports you might think are neccesary here
-
 namespace core {
 
     class LapTracker {
@@ -40,7 +38,12 @@ namespace core {
             /** Internal State */
             inline static std::atomic<LapTracker*> _s_instance;
 
-            // TODO put variables here to keep track of the lap tracker's internal state
+            int _lapcount = 0;
+            float _laptime = 0.0f;
+            double _start_lat;
+            double _start_lon;
+            bool _started = false;
+            std::chrono::steady_clock::time_point _last_timestamp;
 
     };
 

drivebrain_core/include/StateTracker.hpp

@@ -14,6 +14,16 @@
 #include "hytech_msgs.pb.h"
 #include "hytech.pb.h"
 
+// Tolerance for crossing finish line. Equal to ~3 meters of tolerance in latitude and longitude.
+#define FINISH_LINE_POSITION_TOLERANCE 0.000027
+// Error for wheels to be considered stationary
+#define STATIONARY_WHEEL_ERROR 0.001
+// Minimum rpm for car to be considered "racing"
+#define MINIMUM_WHEEL_ROTATION 10
+// Minimum velocity m/s for car to be considered "racing"
+#define MINIMUM_CAR_VELOCITY 0.2
+
+
 /**
  * The state tracker acts 
  * as a thread-safe translation unit
@@ -225,21 +235,21 @@ namespace core {
     struct VehicleState {
         bool is_ready_to_drive; 
         DriverInput input;
-        xyz_vec<float> current_body_vel_ms;
-        xyz_vec<float> current_body_accel_mss;
-        xyz_vec<float> current_angular_rate_rads;
-        ypr_vec<float> current_ypr_rad;
-        veh_vec<float> current_rpms;
+        xyz_vec<float> current_body_vel_ms; // velocity
+        xyz_vec<float> current_body_accel_mss; // accel
+        xyz_vec<float> current_angular_rate_rads; // spin speed
+        ypr_vec<float> current_ypr_rad; // orientation
+        veh_vec<float> current_rpms; // wheel rotation speed
         veh_vec<float> motor_overload_percentages;
         bool state_is_valid;
-        int prev_MCU_recv_millis;
+        int prev_MCU_recv_millis; // watchdog timer
         float steering_angle_deg;
         ControllerOutput prev_controller_output;
         TireDynamics tire_dynamics;
         veh_vec<float> driver_torque;
         ControllerTorqueOut matlab_math_temp_out;
         veh_vec<float> suspension_potentiometers_mm;
-        Position vehicle_position;
+        Position vehicle_position; // coords relative to start?
         veh_vec<float> loadcells;
         veh_vec<float> current_torques_nm;
         INSStatus ins_status;

drivebrain_core/src/LapTracker.cpp

@@ -1,16 +1,56 @@
 #include <LapTracker.hpp> 
 
 void core::LapTracker::step_tracker(core::VehicleState& latest_state) {
-    std::shared_ptr<hytech_msgs::LapTime> laptime_information = std::make_shared<hytech_msgs::LapTime>(); // 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<hytech_msgs::LapTime> laptime_information = std::make_shared<hytech_msgs::LapTime>();
+
+    // Assuming 1 rpms correponds to 1 real life RPM
+
+    auto now = std::chrono::steady_clock::now();
+    float time_differential = std::chrono::duration<float>(now - _last_timestamp).count();
+    _last_timestamp = now;
+
+    if (_lapcount == 0
+        && latest_state.is_ready_to_drive
+        && std::abs(latest_state.current_rpms.FL) < STATIONARY_WHEEL_ERROR
+        && std::abs(latest_state.current_rpms.FR) < STATIONARY_WHEEL_ERROR
+        && std::abs(latest_state.current_rpms.RL) < STATIONARY_WHEEL_ERROR
+        && std::abs(latest_state.current_rpms.RR) < STATIONARY_WHEEL_ERROR
+        && std::abs(latest_state.current_body_vel_ms.x) < STATIONARY_WHEEL_ERROR
+        && std::abs(latest_state.current_body_vel_ms.y) < STATIONARY_WHEEL_ERROR)
+    {
+        // Assume car is in start box and stationary
+        _start_lat = latest_state.vehicle_position.lat;
+        _start_lon = latest_state.vehicle_position.lon;
+    }
+    else if (!_started
+        && std::abs(latest_state.current_rpms.FL) > MINIMUM_WHEEL_ROTATION
+        && std::abs(latest_state.current_rpms.FR) > MINIMUM_WHEEL_ROTATION
+        && std::abs(latest_state.current_rpms.RL) > MINIMUM_WHEEL_ROTATION
+        && std::abs(latest_state.current_rpms.RR) > MINIMUM_WHEEL_ROTATION
+        && (std::abs(latest_state.current_body_vel_ms.x) > MINIMUM_CAR_VELOCITY
+        || std::abs(latest_state.current_body_vel_ms.y) > MINIMUM_CAR_VELOCITY))
+    {
+        // Assume car has started
+        _started = true;
+        _last_timestamp = std::chrono::steady_clock::now();
+    }
+    else if (_started && std::abs(latest_state.vehicle_position.lat - _start_lat) < FINISH_LINE_POSITION_TOLERANCE
+            && std::abs(latest_state.vehicle_position.lon - _start_lon) < FINISH_LINE_POSITION_TOLERANCE
+            && _laptime > 10.0f)
+    {
+        // Assume track width ~3m
+        // Assume car crossed start line during race
+        _lapcount++;
+        _laptime = 0.0f;
+    }
+    else if (_started)
+    {
+        _laptime += time_differential;
+    }
+    laptime_information->set_laptime_seconds(_laptime);
+    laptime_information->set_lapcount(_lapcount);
 
+    core::StateTracker::instance().handle_receive_protobuf_message(laptime_information); // What "records" the information
 }
 
 void core::LapTracker::create() {