Improve torque request handling

src/shifting_algo/s_algo.cpp

@@ -40,49 +40,39 @@ ShiftAlgoFeedback ShiftingAlgorithm::get_diag_feedback(uint8_t phase_id) {
     };
 }
 
-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::set_trq_request_val(uint16_t v) {
+    this->torque_request_targ = v;
 }
 
-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::disable_trq_request(uint16_t total_elapsed) {
+    if (0 == this->end_timestamp) {
+        this->end_timestamp = total_elapsed;
     }
 }
 
-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;
+void ShiftingAlgorithm::trigger_trq_request(uint16_t total_elapsed) {
+    if (0 == this->start_timestamp) {
+        this->start_timestamp = total_elapsed;
     }
 }
 
-uint16_t ShiftingAlgorithm::trq_req_get_val(uint16_t total_elapsed) {
+uint16_t ShiftingAlgorithm::get_trq_req_ramp_val(uint16_t total_elapsed, uint16_t ramp_down_time, uint16_t ramp_up_time) {
     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;
+    if (0 != this->start_timestamp) {
+        // Request has been triggered at some point
+        if (0 != this->end_timestamp) {
+            // We are ramping up (Finishing)
+            int into_up_ramp = total_elapsed - this->end_timestamp;
+            ret = interpolate_float(into_up_ramp, this->torque_request_targ, 0, 0, ramp_up_time, InterpType::Linear);
+        } else {
+            // We are ramping down or holding
+            int into_down_ramp = total_elapsed - this->start_timestamp;
+            ret = interpolate_float(into_down_ramp, 0, this->torque_request_targ, 0, ramp_down_time, InterpType::Linear);
         }
-    } 
-    // 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);
+bool ShiftingAlgorithm::trq_request_is_end_ramp() {
+    return 0 != this->end_timestamp;
 }

src/shifting_algo/s_algo.h

@@ -11,23 +11,6 @@ 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;
@@ -98,29 +81,26 @@ 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();
-
+        void set_trq_request_val(uint16_t v);
+        void disable_trq_request(uint16_t total_elapsed);
+        void trigger_trq_request(uint16_t total_elapsed);
+        uint16_t get_trq_req_ramp_val(uint16_t total_elapsed, uint16_t ramp_down_time, uint16_t ramp_up_time);
+        bool trq_request_is_end_ramp();
         uint16_t threshold_rpm = 0;
         float inertia = 0;
-        TorqueRequestModel trq_mdl;
+
+        // ------ TORQUE REQUEST STUFF ------
+        bool request_trigger = false;
+        uint16_t torque_request_targ = 0;
+        uint16_t start_timestamp = 0;
+        uint16_t end_timestamp = 0;
+
+
 };
 
 // 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

src/shifting_algo/shift_crossover.cpp

@@ -13,7 +13,6 @@ const uint8_t FILL_RAMP_TIME = 100;
 const uint8_t FILL_HOLD_TIME = 100;
 
 CrossoverShift::CrossoverShift(ShiftInterfaceData* data) : ShiftingAlgorithm(data) {
-    ShiftHelpers::trq_req_init_model(160, 140);
 }
 CrossoverShift::~CrossoverShift() {}
 
@@ -124,6 +123,7 @@ uint8_t CrossoverShift::step(
             }
         } else if (1 == this->subphase_shift) { // Filling ramp
             if (this->do_high_filling) {
+                // Decrease pressure but not by too much!
                 p_now->on_clutch = interpolate_float(subphase_shift, sid->ptr_prev_pressures->on_clutch, sid->prefill_info.low_fill_pressure_on_clutch, 0, FILL_RAMP_TIME, InterpType::Linear);
             } else {
                 // Drop to 0 pressure when filling with little torque
@@ -135,7 +135,7 @@ uint8_t CrossoverShift::step(
                 this->inc_subphase_shift(phase_elapsed);
             }
         } else if (2 == this->subphase_shift) { // Lower to 0 pressure
-            if (this->do_high_filling) {
+            if (this->do_high_filling) { // High filling - we keep the pressure quite high, the off clutch is held to prevent accidental engagmenet (See below)
                 p_now->on_clutch = sid->prefill_info.low_fill_pressure_on_clutch;
                 p_now->overlap_shift = p_now->on_clutch + sid->spring_on_clutch;
                 p_now->shift_sol_req = p_now->overlap_shift - centrifugal_force_on_clutch;
@@ -144,11 +144,11 @@ uint8_t CrossoverShift::step(
                     this->min_spc_clutch_allowed = p_now->on_clutch;
                     ret = PHASE_OVERLAP;
                 }
-            } else {
-                // 2 stage ramp (Hacky way of doing it on 1 phase)
-#define FILL_RAMP_P_1 500 // FROM CAL
+            } else { // Low pressure filling - Ramp up from no pressure with 2 small ramps. The off clutch has been released here so we can do this as there is no torque
+                // 2 stage ramp (Hacky way of doing it in 1 phase)
+#define FILL_RAMP_P_1 500 // Pressure points // FROM EGS Calibrations for my E55 - TODO - Pull these into the general calibrations and extract from flash :)
 #define FILL_RAMP_P_2 700
-#define FILL_RAMP_T_1 500
+#define FILL_RAMP_T_1 500 // Duration points
 #define FILL_RAMP_T_2 240
                 if (elapsed_shift <= FILL_RAMP_T_1) {
                     p_now->on_clutch = interpolate_float(subphase_shift, 0, FILL_RAMP_P_1, 0, FILL_RAMP_T_1, InterpType::Linear);
@@ -166,8 +166,8 @@ uint8_t CrossoverShift::step(
         } else {}
 
 
-        // LOW MOD MODE (LOW PRESSURE FILLING)
-        if (0 == this->subphase_mod) { // Low mode phase 1
+        // OFF CLUTCH CONTROL FOR FILLING PHASE
+        if (0 == this->subphase_mod) { // Low pressure mode phase 1
             int min_torque = MAX(50, abs_input_torque);
             int wp_old_clutch = pm->find_releasing_pressure_for_clutch(sid->curr_g, sid->releasing, min_torque);
             p_now->off_clutch = wp_old_clutch;
@@ -177,23 +177,20 @@ uint8_t CrossoverShift::step(
                 ((p_now->overlap_mod -centrifugal_force_off_clutch) * sid->inf.centrifugal_factor_off_clutch * sid->inf.pressure_multi_mpc) +
                 sid->inf.mpc_pressure_spring_reduction
             );
-
             // Now we have to calc the pressure for overlap phase so mod pressure doesn't drop below it
             // Floor target SPC pressure to low filling pressure
             int spc_target = MAX(
                 MAX(pm->find_working_pressure_for_clutch(sid->targ_g, sid->applying, abs_input_torque, false), sid->prefill_info.low_fill_pressure_on_clutch),
                 sid->prefill_info.low_fill_pressure_on_clutch
             ) + sid->spring_on_clutch;
-
             int mod_min = ( // Assumes p_mod->off_clutch = 0 (No mod pressure at the clutch)
                 ((spc_target - centrifugal_force_on_clutch) * sid->inf.pressure_multi_spc) +
                 ((sid->spring_off_clutch - centrifugal_force_off_clutch) * sid->inf.centrifugal_factor_off_clutch * sid->inf.pressure_multi_mpc) +
                 sid->inf.mpc_pressure_spring_reduction
             );
             p_now->mod_sol_req = MAX(p_now->mod_sol_req, mod_min);
-
             if (elapsed_mod > sid->prefill_info.fill_time) {
-                // Next phase
+                // Next step of the phase
                 this->inc_subphase_mod(phase_elapsed);
             }
         } else if (1 == this->subphase_mod) { // Low mode phase 2
@@ -205,9 +202,7 @@ uint8_t CrossoverShift::step(
                 ((p_now->overlap_mod -centrifugal_force_off_clutch) * sid->inf.centrifugal_factor_off_clutch * sid->inf.pressure_multi_mpc) +
                 sid->inf.mpc_pressure_spring_reduction
             );
-        } 
-        // HIGH MOD MODE (HIGH PRESSURE FILLING)
-        else if (2 == this->subphase_mod) {
+        } else if (2 == this->subphase_mod) { // high pressure filling
             // Does not end, waits for the end of shift pressure ramp
             int min_torque = MAX(50, abs_input_torque);
             int wp_old_clutch = pm->find_releasing_pressure_for_clutch(sid->curr_g, sid->releasing, min_torque);
@@ -218,7 +213,6 @@ uint8_t CrossoverShift::step(
                 ((p_now->overlap_mod -centrifugal_force_off_clutch) * sid->inf.centrifugal_factor_off_clutch * sid->inf.pressure_multi_mpc) +
                 sid->inf.mpc_pressure_spring_reduction
             );
-
             // Now we have to calc the pressure for overlap phase so mod pressure doesn't drop below it
             int spc_target = MAX(
                 pm->find_working_pressure_for_clutch(sid->targ_g, sid->applying, abs_input_torque, false),
@@ -302,7 +296,7 @@ uint8_t CrossoverShift::step(
                 // Next phase
                 this->inc_subphase_shift(phase_elapsed);
             }
-            this->adder_torque += 1;
+            this->adder_torque += interpolate_float(sid->targ_time, 1.0, 3.0, 100, 500, InterpType::Linear);
         } else if (2 == this->subphase_shift) { // Reduce pressure slightly to avoid a clunk at the end of the clutch movement
             int trq = abs_input_torque + this->adder_torque + this->decent_adder_torque + sid->ptr_w_trq_req->amount;
             int trq_end = abs_input_torque + this->adder_torque + this->decent_adder_torque + sid->ptr_w_trq_req->amount;
@@ -389,17 +383,17 @@ uint8_t CrossoverShift::step(
     int trq_req_raw = interpolate_float(abs_input_torque, 0, abs_input_torque/2, 0, max, InterpType::Linear);
     trq_req_raw *= interpolate_float(sd->engine_rpm, 1.0, 1.5, 1000, 4000, InterpType::Linear);
 
-    this->trq_req_set_val(trq_req_raw);
+    this->set_trq_request_val(MAX(trq_req_raw, decent_adder_torque));
 
     // Now check if the model is active or not (Check up ramp first)
     if (sid->ptr_r_clutch_speeds->on_clutch_speed < 150) {
-        this->trq_req_end_ramp(total_elapsed);
+        this->disable_trq_request(total_elapsed);
     } else if ((phase_id == PHASE_MOMENTUM_CONTROL && subphase_shift == 0) || abs(sid->ptr_r_clutch_speeds->off_clutch_speed) > 100) {
-        this->trq_req_start_ramp(total_elapsed);
+        this->trigger_trq_request(total_elapsed);
     }
 
     int request_val = 0;
-    request_val = this->trq_req_get_val(total_elapsed);
+    request_val = this->get_trq_req_ramp_val(total_elapsed, 140, 160);
 
     // Disable torque request if conditions are not met (Override)
     if (sd->indicated_torque <= sd->min_torque || sd->converted_torque <= sd->min_torque || sd->input_rpm < 1000) {
@@ -408,7 +402,7 @@ uint8_t CrossoverShift::step(
     request_val = MAX(0, MIN(request_val, sd->indicated_torque)); // Ensure not out of bounds :)
 
     if (0 != request_val) {
-        bool up_ramp = this->trq_req_is_end_ramp();
+        bool up_ramp = this->trq_request_is_end_ramp();
         sid->ptr_w_trq_req->amount = sd->indicated_torque - request_val;
         sid->ptr_w_trq_req->bounds = TorqueRequestBounds::LessThan;
         sid->ptr_w_trq_req->ty =  up_ramp ? TorqueRequestControlType::BackToDemandTorque : TorqueRequestControlType::NormalSpeed;

src/shifting_algo/shift_crossover.h

@@ -25,7 +25,7 @@ class CrossoverShift : public ShiftingAlgorithm {
 
     int momentum_adder = 0;
     bool filling_mode_check = false;
-    int adder_torque = 0;
+    float adder_torque = 0;
     int decent_adder_torque = 0;
     int min_spc_clutch_allowed = 0;
     int torque_request_calc = 0;

src/shifting_algo/shift_release.cpp

@@ -9,7 +9,6 @@ const uint8_t FILL_RAMP_TIME = 60;
 const uint8_t FILL_HOLD_TIME = 100;
 
 ReleasingShift::ReleasingShift(ShiftInterfaceData* data) : ShiftingAlgorithm(data) {
-    ShiftHelpers::trq_req_init_model(160, 140);
 }
 ReleasingShift::~ReleasingShift() {}
 
@@ -332,21 +331,21 @@ uint8_t ReleasingShift::step(
     }
 
     if (trq_req_protection != 0) {
-        this->trq_req_set_val(trq_req_protection);
-        this->trq_req_start_ramp(total_elapsed);
+        this->set_trq_request_val(trq_req_protection);
+        this->trigger_trq_request(total_elapsed);
     } else {
-        this->trq_req_set_val(trq_request_raw);
+        this->set_trq_request_val(trq_request_raw);
     }
 
     // Now check if the model is active or not (Check up ramp first)
     if (sid->ptr_r_clutch_speeds->on_clutch_speed < 100 || phase_id == PHASE_MAX_PRESSURE) {
-        this->trq_req_end_ramp(total_elapsed);
+        this->disable_trq_request(total_elapsed);
     } else if (time_for_trq_req) {
-        this->trq_req_start_ramp(total_elapsed);
+        this->trigger_trq_request(total_elapsed);
     }
 
     int request_val = 0;
-    request_val = this->trq_req_get_val(total_elapsed);
+    request_val = this->get_trq_req_ramp_val(total_elapsed, 160, 140);
 
     if (sd->indicated_torque <= sd->min_torque || sd->converted_torque <= sd->min_torque || sd->input_rpm < 1000) {
         request_val = 0;
@@ -355,7 +354,7 @@ uint8_t ReleasingShift::step(
     request_val = MAX(0, MIN(request_val, sd->indicated_torque)); // Ensure not out of bounds :)
 
     if (0 != request_val) {
-        bool up_ramp = this->trq_req_is_end_ramp();
+        bool up_ramp = this->trq_request_is_end_ramp();
         sid->ptr_w_trq_req->amount = sd->indicated_torque - request_val;
         sid->ptr_w_trq_req->bounds = TorqueRequestBounds::LessThan;
         sid->ptr_w_trq_req->ty =  up_ramp ? TorqueRequestControlType::BackToDemandTorque : TorqueRequestControlType::NormalSpeed;

src/shifting_algo/shifting_algo_helpers.cpp

@@ -9,12 +9,4 @@ float ShiftHelpers::calcualte_abs_engine_inertia(uint8_t shift_idx, uint16_t eng
     float pump_inertia = MECH_PTR->intertia_torque[shift_idx];
     float ret = interpolate_float(turbine_factor, pump_inertia, engine_inertia, min_factor, 1, InterpType::Linear);
     return abs(ret);
-}
-
-TorqueRequestModel ShiftHelpers::trq_req_init_model(uint16_t ramp_down_ms, uint16_t ramp_up_ms) {
-    TorqueRequestModel mdl;
-    memset(&mdl, 0x00, sizeof(TorqueRequestModel));
-    mdl.ramp_up_ms = ramp_up_ms;
-    mdl.ramp_down_ms = ramp_down_ms;
-    return mdl;
 }