refactoring for torque based driving (#1339)

### Changelist 
- changed power based driving to torque based

### Testing Done
<!-- Outline the testing that was done to demonstrate the changes are
solid. This could be unit tests, integration tests, testing on the car,
etc. Include relevant code snippets, screenshots, etc as needed. -->
- [x] Compiles
- [x] Runs
- [x] Tested on track

---------

Co-authored-by: Shayan Ajmal <[email protected]>
Co-authored-by: Phi Lam <[email protected]>

firmware/quadruna/VC/src/app/states/app_driveState.c

@@ -112,10 +112,8 @@ static void driveStateRunOnTick100Hz(void)
     bool       exit_drive_to_init        = bms_not_in_drive;
     bool       exit_drive_to_inverter_on = !all_states_ok || start_switch_off;
     bool       prev_regen_switch_val     = regen_switch_is_on;
-    torque_vectoring_switch_is_on        = app_canRx_CRIT_TorqueVecSwitch_get() == SWITCH_ON;
     regen_switch_is_on                   = app_canRx_CRIT_RegenSwitch_get() == SWITCH_ON;
     bool turn_regen_led                  = regen_switch_is_on && !prev_regen_switch_val;
-    bool turn_tv_led                     = torque_vectoring_switch_is_on;
 
     // Regen + TV LEDs and update warnings
     if (turn_regen_led)
@@ -130,8 +128,6 @@ static void driveStateRunOnTick100Hz(void)
         app_canTx_VC_Warning_RegenNotAvailable_set(true);
     }
 
-    app_canTx_VC_TorqueVectoringEnabled_set(turn_tv_led);
-
     if (exit_drive_to_init)
     {
         app_stateMachine_setNextState(app_initState_get());

firmware/quadruna/VC/src/app/vehicle_dynamics/app_activeDifferential.c

@@ -2,6 +2,7 @@
 #include "app_vehicleDynamicsConstants.h"
 #include <math.h>
 #include "app_units.h"
+#include "app_utils.h"
 #include <stdlib.h>
 
 void app_activeDifferential_computeTorque(ActiveDifferential_Inputs *inputs, ActiveDifferential_Outputs *outputs)
@@ -13,14 +14,14 @@ void app_activeDifferential_computeTorque(ActiveDifferential_Inputs *inputs, Act
     float torque_lim_Nm = app_activeDifferential_powerToTorque(
         inputs->power_max_kW, inputs->motor_speed_left_rpm, inputs->motor_speed_right_rpm, cl, cr);
 
-    float torque_left_Nm  = torque_lim_Nm * (1 + Delta);
-    float torque_right_Nm = torque_lim_Nm * (1 - Delta);
+    float torque_left_Nm  = inputs->requested_torque * (1 + Delta);
+    float torque_right_Nm = inputs->requested_torque * (1 - Delta);
     float torque_max_Nm   = fmaxf(torque_left_Nm, torque_right_Nm);
 
     float scale = 1.0f;
-    if (torque_max_Nm > MAX_TORQUE_REQUEST_NM)
+    if (torque_max_Nm > torque_lim_Nm)
     {
-        scale = MAX_TORQUE_REQUEST_NM / torque_max_Nm;
+        scale = torque_lim_Nm / torque_max_Nm;
     }
 
     outputs->torque_left_Nm  = torque_left_Nm * scale;

firmware/quadruna/VC/src/app/vehicle_dynamics/app_torqueVectoring.c

@@ -93,7 +93,6 @@ void app_torqueVectoring_handleAcceleration(void)
     power_limiting_inputs.accelerator_pedal_percent = accelerator_pedal_percent;
     float estimated_power_limit;
     estimated_power_limit = app_powerLimiting_computeMaxPower(&power_limiting_inputs);
-
     // Power limit correction
     float power_limit = estimated_power_limit * (1.0f + pid_power_correction_factor);
 
@@ -102,6 +101,7 @@ void app_torqueVectoring_handleAcceleration(void)
     active_differential_inputs.motor_speed_left_rpm  = motor_speed_left_rpm;
     active_differential_inputs.motor_speed_right_rpm = motor_speed_right_rpm;
     active_differential_inputs.wheel_angle_deg       = steering_angle_deg * APPROX_STEERING_TO_WHEEL_ANGLE;
+    active_differential_inputs.requested_torque      = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
     app_activeDifferential_computeTorque(&active_differential_inputs, &active_differential_outputs);
     app_canTx_VC_ActiveDiffTorqueLeft_set(active_differential_outputs.torque_left_Nm);
     app_canTx_VC_ActiveDiffTorqueRight_set(active_differential_outputs.torque_right_Nm);

firmware/quadruna/VC/src/app/vehicle_dynamics/app_vehicleDynamics.h

@@ -35,6 +35,7 @@ typedef struct ActiveDifferential_Inputs
     float motor_speed_right_rpm;
     float power_max_kW;
     float accelerator_pedal_percentage;
+    float requested_torque;
 } ActiveDifferential_Inputs;
 
 typedef struct ActiveDifferential_Outputs

firmware/quadruna/VC/test/vehicle_dynamics/test_activeDifferential.cpp

@@ -23,36 +23,56 @@ class ActiveDifferentialTest : public testing::Test
 {
 };
 
-TEST_F(ActiveDifferentialTest, torques_do_not_exceed_motor_torque_limit_while_turning_at_max_power)
+TEST_F(ActiveDifferentialTest, torques_do_not_exceed_motor_torque_limit_while_turning_at_max_torque_request)
 {
     struct ActiveDifferential_Inputs  active_diff_inputs_t1;
     struct ActiveDifferential_Outputs actual_active_diff_outputs_t1;
     float                             wheel_angle_t1          = 30.0;
     float                             wheel_speed_left_rpm_t1 = 135, wheel_speed_right_rpm_t1 = 135;
-    active_diff_inputs_t1 = { wheel_angle_t1, wheel_speed_left_rpm_t1, wheel_speed_right_rpm_t1, POWER_LIMIT_CAR_kW };
+    float                             pedal_percentage = 1.0;
+    float                             scale            = 1.0;
+    // our torque based driving requires a requested torque into so we pass in torque_t1 as are requested torque
+    float delta_t1      = TRACK_WIDTH_mm * tanf(DEG_TO_RAD(wheel_angle_t1)) / (2 * WHEELBASE_mm);
+    float cl_t1         = 1 + delta_t1;
+    float cr_t1         = 1 - delta_t1;
+    float torque_t1     = pedal_percentage * MAX_TORQUE_REQUEST_NM;
+    float torque_lim_t1 = (POWER_TO_TORQUE_CONVERSION_FACTOR * POWER_LIMIT_CAR_kW) /
+                          (wheel_speed_left_rpm_t1 * cl_t1 + wheel_speed_right_rpm_t1 * cr_t1 + SMALL_EPSILON);
+
+    active_diff_inputs_t1 = {
+        wheel_angle_t1, wheel_speed_left_rpm_t1, wheel_speed_right_rpm_t1, POWER_LIMIT_CAR_kW, 0.0, torque_t1
+    };
+
     app_activeDifferential_computeTorque(&active_diff_inputs_t1, &actual_active_diff_outputs_t1);
-    float delta_t1  = TRACK_WIDTH_mm * tanf(DEG_TO_RAD(wheel_angle_t1)) / (2 * WHEELBASE_mm);
-    float cl_t1     = 1 + delta_t1;
-    float cr_t1     = 1 - delta_t1;
-    float torque_t1 = (POWER_TO_TORQUE_CONVERSION_FACTOR * POWER_LIMIT_CAR_kW) /
-                      (wheel_speed_left_rpm_t1 * cl_t1 + wheel_speed_right_rpm_t1 * cr_t1 + SMALL_EPSILON);
     float expected_torque_left_t1  = cl_t1 * torque_t1;
     float expected_torque_right_t1 = cr_t1 * torque_t1;
     float max_torque_t1            = fmaxf(expected_torque_left_t1, expected_torque_right_t1);
-    if (max_torque_t1 > MAX_TORQUE_REQUEST_NM)
+
+    if (max_torque_t1 > torque_lim_t1)
     {
-        float sigma_t1           = MAX_TORQUE_REQUEST_NM / max_torque_t1;
-        expected_torque_right_t1 = sigma_t1 * expected_torque_right_t1;
-        expected_torque_left_t1  = sigma_t1 * expected_torque_left_t1;
+        scale = torque_lim_t1 / max_torque_t1;
     }
+
+    expected_torque_left_t1 *= scale;
+    expected_torque_right_t1 *= scale;
+
     float expected_ratio_t1   = cl_t1 / cr_t1;
     float calculated_ratio_t1 = expected_torque_left_t1 / expected_torque_right_t1;
     float actual_ratio_t1 =
         actual_active_diff_outputs_t1.torque_left_Nm / actual_active_diff_outputs_t1.torque_right_Nm;
     ASSERT_FLOAT_EQ(expected_torque_left_t1, actual_active_diff_outputs_t1.torque_left_Nm);
     ASSERT_FLOAT_EQ(expected_torque_right_t1, actual_active_diff_outputs_t1.torque_right_Nm);
-    ASSERT_TRUE(actual_active_diff_outputs_t1.torque_right_Nm <= MAX_TORQUE_REQUEST_NM);
-    ASSERT_TRUE(actual_active_diff_outputs_t1.torque_left_Nm <= MAX_TORQUE_REQUEST_NM);
+
+    // We clamp in torque vectoring.c as a final clamp before sending torque to motors.
+    // I believe it is best to keep it like that rather than clamping anything that leaves
+    // active diff because we still might want to manipulate the number when it leaves active diff
+    //  and clamping it would mess with the algo. These tests are commented out because they assume that
+    //  the output of active diff is clamped. Previously this worked because our scale was with max torque
+    // not limit based on power limit
+
+    // ASSERT_TRUE(actual_active_diff_outputs_t1.torque_right_Nm <= MAX_TORQUE_REQUEST_NM);
+    // ASSERT_TRUE(actual_active_diff_outputs_t1.torque_left_Nm <= MAX_TORQUE_REQUEST_NM);
+
     ASSERT_FLOAT_EQ(expected_ratio_t1, calculated_ratio_t1);
     ASSERT_FLOAT_EQ(expected_ratio_t1, actual_ratio_t1);
 };
@@ -63,7 +83,10 @@ TEST_F(ActiveDifferentialTest, no_torques_while_turning_at_zero_power_limit)
     struct ActiveDifferential_Outputs actual_active_diff_outputs_t2;
     float                             wheel_angle_t2          = 30.0;
     float                             wheel_speed_left_rpm_t2 = 135, wheel_speed_right_rpm_t2 = 135;
-    active_diff_inputs_t2 = { wheel_angle_t2, wheel_speed_left_rpm_t2, wheel_speed_right_rpm_t2, 0.0 };
+    float                             pedal_percentage = 0.6;
+    float                             torque_t2        = pedal_percentage * MAX_TORQUE_REQUEST_NM;
+
+    active_diff_inputs_t2 = { wheel_angle_t2, wheel_speed_left_rpm_t2, wheel_speed_right_rpm_t2, 0.0, 0.0, torque_t2 };
     app_activeDifferential_computeTorque(&active_diff_inputs_t2, &actual_active_diff_outputs_t2);
     ASSERT_FLOAT_EQ(0.0, actual_active_diff_outputs_t2.torque_left_Nm);
     ASSERT_FLOAT_EQ(0.0, actual_active_diff_outputs_t2.torque_right_Nm);
@@ -75,27 +98,41 @@ TEST_F(ActiveDifferentialTest, torques_follow_expected_ratio_while_turning_right
     struct ActiveDifferential_Outputs actual_active_diff_outputs_t3;
     float                             wheel_angle_t3          = 30.0;
     float                             wheel_speed_left_rpm_t3 = 135, wheel_speed_right_rpm_t3 = 135;
-    float                             power_lim_t3 = 60.0;
-    active_diff_inputs_t3 = { wheel_angle_t3, wheel_speed_left_rpm_t3, wheel_speed_right_rpm_t3, power_lim_t3 };
+    float                             power_lim_t3     = 60.0;
+    float                             pedal_percentage = 0.42;
+    float                             scale            = 1.0f;
+    // our torque based driving requires a requested torque into so we pass in torque_t3 as are requested torque
+    float delta_t3      = TRACK_WIDTH_mm * tanf(DEG_TO_RAD(wheel_angle_t3)) / (2 * WHEELBASE_mm);
+    float cl_t3         = 1 + delta_t3;
+    float cr_t3         = 1 - delta_t3;
+    float torque_t3     = pedal_percentage * MAX_TORQUE_REQUEST_NM;
+    float torque_lim_t3 = (POWER_TO_TORQUE_CONVERSION_FACTOR * power_lim_t3) /
+                          (wheel_speed_left_rpm_t3 * cl_t3 + wheel_speed_right_rpm_t3 * cr_t3 + SMALL_EPSILON);
+
+    active_diff_inputs_t3 = { wheel_angle_t3, wheel_speed_left_rpm_t3, wheel_speed_right_rpm_t3, power_lim_t3, 0.0,
+                              torque_t3 };
+
     app_activeDifferential_computeTorque(&active_diff_inputs_t3, &actual_active_diff_outputs_t3);
-    float delta_t3  = TRACK_WIDTH_mm * tanf(DEG_TO_RAD(wheel_angle_t3)) / (2 * WHEELBASE_mm);
-    float cl_t3     = 1 + delta_t3;
-    float cr_t3     = 1 - delta_t3;
-    float torque_t3 = (POWER_TO_TORQUE_CONVERSION_FACTOR * power_lim_t3) /
-                      (wheel_speed_left_rpm_t3 * cl_t3 + wheel_speed_right_rpm_t3 * cr_t3 + SMALL_EPSILON);
     float expected_torque_left_t3  = cl_t3 * torque_t3;
     float expected_torque_right_t3 = cr_t3 * torque_t3;
     float max_torque_t3            = fmaxf(expected_torque_left_t3, expected_torque_right_t3);
-    if (max_torque_t3 > MAX_TORQUE_REQUEST_NM)
+
+    if (max_torque_t3 > torque_lim_t3)
     {
-        float sigma_t3           = MAX_TORQUE_REQUEST_NM / max_torque_t3;
-        expected_torque_right_t3 = sigma_t3 * expected_torque_right_t3;
-        expected_torque_left_t3  = sigma_t3 * expected_torque_left_t3;
+        scale = torque_lim_t3 / max_torque_t3;
     }
+
+    expected_torque_left_t3 *= scale;
+    expected_torque_right_t3 *= scale;
+
     ASSERT_FLOAT_EQ(expected_torque_left_t3, actual_active_diff_outputs_t3.torque_left_Nm);
     ASSERT_FLOAT_EQ(expected_torque_right_t3, actual_active_diff_outputs_t3.torque_right_Nm);
-    ASSERT_TRUE(actual_active_diff_outputs_t3.torque_right_Nm <= MAX_TORQUE_REQUEST_NM);
-    ASSERT_TRUE(actual_active_diff_outputs_t3.torque_left_Nm <= MAX_TORQUE_REQUEST_NM);
+
+    // see first test comment
+
+    // ASSERT_TRUE(actual_active_diff_outputs_t3.torque_right_Nm <= MAX_TORQUE_REQUEST_NM);
+    // ASSERT_TRUE(actual_active_diff_outputs_t3.torque_left_Nm <= MAX_TORQUE_REQUEST_NM);
+
     ASSERT_TRUE(actual_active_diff_outputs_t3.torque_left_Nm > actual_active_diff_outputs_t3.torque_right_Nm);
 };
 
@@ -105,26 +142,40 @@ TEST_F(ActiveDifferentialTest, torques_follow_expected_ratio_while_turning_left_
     struct ActiveDifferential_Outputs actual_active_diff_outputs_t4;
     float                             wheel_angle_t4          = -30.0;
     float                             wheel_speed_left_rpm_t4 = 135, wheel_speed_right_rpm_t4 = 135;
-    float                             power_lim_t4 = 60.0;
-    active_diff_inputs_t4 = { wheel_angle_t4, wheel_speed_left_rpm_t4, wheel_speed_right_rpm_t4, power_lim_t4 };
+    float                             power_lim_t4     = 60.0;
+    float                             pedal_percentage = 0.73;
+    float                             scale            = 1.0;
+    // our torque based driving requires a requested torque into so we pass in torque_t4 as are requested torque
+    float delta_t4      = TRACK_WIDTH_mm * tanf(DEG_TO_RAD(wheel_angle_t4)) / (2 * WHEELBASE_mm);
+    float cl_t4         = 1 + delta_t4;
+    float cr_t4         = 1 - delta_t4;
+    float torque_t4     = pedal_percentage * MAX_TORQUE_REQUEST_NM;
+    float torque_lim_t4 = (POWER_TO_TORQUE_CONVERSION_FACTOR * power_lim_t4) /
+                          (wheel_speed_left_rpm_t4 * cl_t4 + wheel_speed_right_rpm_t4 * cr_t4 + SMALL_EPSILON);
+
+    active_diff_inputs_t4 = { wheel_angle_t4, wheel_speed_left_rpm_t4, wheel_speed_right_rpm_t4, power_lim_t4, 0.0,
+                              torque_t4 };
+
     app_activeDifferential_computeTorque(&active_diff_inputs_t4, &actual_active_diff_outputs_t4);
-    float delta_t4  = TRACK_WIDTH_mm * tanf(DEG_TO_RAD(wheel_angle_t4)) / (2 * WHEELBASE_mm);
-    float cl_t4     = 1 + delta_t4;
-    float cr_t4     = 1 - delta_t4;
-    float torque_t4 = (POWER_TO_TORQUE_CONVERSION_FACTOR * power_lim_t4) /
-                      (wheel_speed_left_rpm_t4 * cl_t4 + wheel_speed_right_rpm_t4 * cr_t4 + SMALL_EPSILON);
     float expected_torque_left_t4  = cl_t4 * torque_t4;
     float expected_torque_right_t4 = cr_t4 * torque_t4;
     float max_torque_t4            = fmaxf(expected_torque_left_t4, expected_torque_right_t4);
-    if (max_torque_t4 > MAX_TORQUE_REQUEST_NM)
+
+    if (max_torque_t4 > torque_lim_t4)
     {
-        float sigma_t4           = MAX_TORQUE_REQUEST_NM / max_torque_t4;
-        expected_torque_right_t4 = sigma_t4 * expected_torque_right_t4;
-        expected_torque_left_t4  = sigma_t4 * expected_torque_left_t4;
+        scale = torque_lim_t4 / max_torque_t4;
     }
+
+    expected_torque_left_t4 *= scale;
+    expected_torque_right_t4 *= scale;
+
     ASSERT_FLOAT_EQ(expected_torque_left_t4, actual_active_diff_outputs_t4.torque_left_Nm);
     ASSERT_FLOAT_EQ(expected_torque_right_t4, actual_active_diff_outputs_t4.torque_right_Nm);
-    ASSERT_TRUE(actual_active_diff_outputs_t4.torque_right_Nm <= MAX_TORQUE_REQUEST_NM);
-    ASSERT_TRUE(actual_active_diff_outputs_t4.torque_left_Nm <= MAX_TORQUE_REQUEST_NM);
+
+    // see first test comment
+
+    // ASSERT_TRUE(actual_active_diff_outputs_t4.torque_right_Nm <= MAX_TORQUE_REQUEST_NM);
+    // ASSERT_TRUE(actual_active_diff_outputs_t4.torque_left_Nm <= MAX_TORQUE_REQUEST_NM);
+
     ASSERT_TRUE(actual_active_diff_outputs_t4.torque_left_Nm < actual_active_diff_outputs_t4.torque_right_Nm);
 };