Implement a unified torque request model system

src/shifting_algo/s_algo.cpp

@@ -38,4 +38,51 @@ ShiftAlgoFeedback ShiftingAlgorithm::get_diag_feedback(uint8_t phase_id) {
         .s_off = (int16_t)this->sid->ptr_r_clutch_speeds->off_clutch_speed,
         .s_on = (int16_t)this->sid->ptr_r_clutch_speeds->on_clutch_speed,
     };
+}
+
+void ShiftingAlgorithm::trq_req_set_val(uint16_t max_req) {
+    if (!this->trq_mdl.up_triggered) { // Only do this if we are not 'up' ramping
+        this->trq_mdl.targ = max_req;
+    }
+}
+
+void ShiftingAlgorithm::trq_req_start_ramp(uint16_t total_elapsed) {
+    if (!this->trq_mdl.down_triggered) { // One time latch
+        this->trq_mdl.ramp_down_start_ms = total_elapsed;
+        this->trq_mdl.down_triggered = true;
+    }
+}
+
+void ShiftingAlgorithm::trq_req_end_ramp(uint16_t total_elapsed) {
+    if (!this->trq_mdl.up_triggered) { // One time latch
+        this->trq_mdl.targ = this->trq_req_get_val(total_elapsed); // In case we didn't reach our initial target, get the value of the ramp and hold as our target
+        this->trq_mdl.ramp_down_start_ms = 0; // Stop ramping down
+        this->trq_mdl.ramp_up_start_ms = total_elapsed;
+        this->trq_mdl.up_triggered = true;
+    }
+}
+
+uint16_t ShiftingAlgorithm::trq_req_get_val(uint16_t total_elapsed) {
+    uint16_t ret = 0;
+    // Check up ramp first, as this would be triggered second
+    if (this->trq_mdl.ramp_up_start_ms != 0) {
+        // Ramping back up to torque
+        int into = total_elapsed - this->trq_mdl.ramp_up_start_ms;
+        ret = interpolate_float(into, this->trq_mdl.targ, 0, 0, this->trq_mdl.ramp_up_ms, InterpType::Linear);
+        if (into >= this->trq_mdl.ramp_up_ms) {
+            this->trq_mdl.ramp_up_start_ms = 0; // Disable the entire sytem once we come out of this ramp
+            this->trq_mdl.ramp_down_start_ms = 0;
+        }
+    } 
+    // No up ramp, then check if we should be ramping down or holding torque
+    else if (this->trq_mdl.ramp_down_start_ms != 0) {
+        // We are ramping down
+        int into = total_elapsed - this->trq_mdl.ramp_down_start_ms;
+        ret = interpolate_float(into, 0, this->trq_mdl.targ, 0, this->trq_mdl.ramp_down_ms, InterpType::Linear);
+    } 
+    return ret;
+}
+
+bool ShiftingAlgorithm::trq_req_is_end_ramp() {
+    return (this->trq_mdl.ramp_up_start_ms != 0);
 }

src/shifting_algo/s_algo.h

@@ -11,6 +11,23 @@ struct TorqueRequstData {
     float amount;
 };
 
+struct TorqueRequestModel {
+    // TARGET time to ramp down to request level
+    uint16_t ramp_down_ms;
+    // RAGET time to ramp up to 0 (No EGS request)
+    uint16_t ramp_up_ms;
+    // Recorded timestamp of the start of the down ramp
+    uint16_t ramp_down_start_ms;
+    // Recorded timestamp of the start of the up ramp
+    uint16_t ramp_up_start_ms;
+    // Momentum reduction target
+    uint16_t targ;
+    // Latch for if the down ramp has started
+    bool down_triggered;
+    // Latch for if the up ramp has started
+    bool up_triggered;
+};
+
 const uint8_t STEP_RES_CONTINUE = 0;
 const uint8_t STEP_RES_FAILURE = 0xFE; // Shift failed. Abort!!
 const uint8_t STEP_RES_END_SHIFT = 0xFF;
@@ -81,14 +98,29 @@ class ShiftingAlgorithm {
         uint16_t elapsed_subphase_shift(uint16_t phase_elapsed_now);
         uint16_t elapsed_subphase_mod(uint16_t phase_elapsed_now);
 
+        void trq_req_set_val(uint16_t max_req);
+        void trq_req_start_ramp(uint16_t total_elapsed);
+        void trq_req_end_ramp(uint16_t total_elapsed);
+        /**
+         * Returns the current value of the active torque request (Motor torque reduction amount from indicated torque)
+         * If this value is 0, then the request is not active.
+         */
+        uint16_t trq_req_get_val(uint16_t total_elapsed);
+        /**
+         * @brief Returns if the torque request is on its end ramp (Going from target to 0). This is needed for ignition
+         * angle control on petrol engines.
+         */
+        bool trq_req_is_end_ramp();
+
         uint16_t threshold_rpm = 0;
         float inertia = 0;
+        TorqueRequestModel trq_mdl;
 };
 
 // Helper functions
-
 namespace ShiftHelpers {
     float calcualte_abs_engine_inertia(uint8_t shift_idx, uint16_t engine_rpm, uint16_t input_rpm);
+    TorqueRequestModel trq_req_init_model(uint16_t ramp_down_ms, uint16_t ramp_up_ms);
 }
 
 #endif