Implement the Simple Vehicle Dynamics Interface in C++

firmware/projects/VD_Interface/CMakeLists.txt

@@ -0,0 +1,12 @@
+# Teghveer Singh Ateliey
+# January 4, 2025
+
+# The target executable 'main' is created in the master CMakeLists. Do not change its name.
+# We only need to add the source code files and include directories.
+
+# include("${CMAKE_SOURCE_DIR}/cmake/cangen.cmake")
+
+target_sources(main PRIVATE main.cc)
+
+# Notice that we don't include any mcal/ subdirectory in this CMake file.
+# The master CMakeLists handles platform selection and library linking.

firmware/projects/VD_Interface/inc/simp_vd_interface.cc

@@ -0,0 +1,49 @@
+/// @author Teghveer Singh Ateliey
+/// @date 2024-11-23
+
+#include "simp_vd_interface.hpp"
+
+using namespace ctrl;
+
+SimpVdInterface::SimpVdInterface(
+    const shared::util::Mapper<float>& pedal_to_torque, float target_slip)
+    : pedal_to_torque(pedal_to_torque), target_slip(target_slip) {}
+
+VdOutput SimpVdInterface::update(const VdInput& input, int time_ms) {
+    VdOutput output{
+        .lm_torque_limit_positive = 0.0f,
+        .rm_torque_limit_positive = 0.0f,
+        .lm_torque_limit_negative = 0.0f,  // negative limit fields fixed at
+        .rm_torque_limit_negative = 0.0f,  // 0 in simulink model
+        .left_motor_speed_request = 1000,
+        .right_motor_speed_request = 1000,
+    };
+
+    float actual_slip =
+        CalculateActualSlip(input.wheel_speed_lr, input.wheel_speed_rr,
+                            input.wheel_speed_lf, input.wheel_speed_rf);
+    float tc_scale_factor =
+        CalculateTCScaleFactor(actual_slip, target_slip, time_ms);
+
+    TorqueVector<float> torque_vector;
+    if (input.tv_enable) {
+        torque_vector = AdjustTorqueVectoring(input.steering_angle);
+    } else {
+        torque_vector.left = 1.0f;
+        torque_vector.right = 1.0f;
+    }
+
+    float motor_torque_request = ComputeTorqueRequest(
+        input.driver_torque_request, input.brake_pedal_postion);
+
+    running_average.LoadValue(
+        pedal_to_torque.Evaluate(motor_torque_request * tc_scale_factor));
+    float running_average_value = running_average.GetValue();
+
+    output.lm_torque_limit_positive =
+        running_average_value * torque_vector.left;
+    output.rm_torque_limit_positive =
+        running_average_value * torque_vector.right;
+
+    return output;
+}

firmware/projects/VD_Interface/inc/simp_vd_interface.hpp

@@ -0,0 +1,45 @@
+/// @author Teghveer Singh Ateliey
+/// @date 2024-11-23
+
+#pragma once
+
+#include <cstdint>
+
+#include "shared/controls/motor_torque.h"
+#include "shared/controls/tc_scale_factor.h"
+#include "shared/controls/tvFactor.h"
+#include "shared/util/mappers/lookup_table.hpp"
+#include "shared/util/mappers/mapper.hpp"
+
+struct VdInput {
+    float driver_torque_request;
+    float brake_pedal_postion;
+    float steering_angle;
+    float wheel_speed_lr;
+    float wheel_speed_rr;
+    float wheel_speed_lf;
+    float wheel_speed_rf;
+    bool tv_enable;
+};
+
+struct VdOutput {
+    float lm_torque_limit_positive;
+    float rm_torque_limit_positive;
+    float lm_torque_limit_negative;
+    float rm_torque_limit_negative;
+    uint16_t left_motor_speed_request;
+    uint16_t right_motor_speed_request;
+};
+
+class SimpVdInterface {
+public:
+    SimpVdInterface(
+        const shared::util::Mapper<float>& pedal_to_torque,
+        float target_slip = 0.2f);  // default target slip is float 0.2
+    VdOutput update(const VdInput& input, int time_ms);
+
+private:
+    const shared::util::Mapper<float>& pedal_to_torque;
+    shared::util::MovingAverage<float, 10> running_average;
+    float target_slip;
+};