From 796fa8b4843df5a9b8e2a236264f695aae5d6652 Mon Sep 17 00:00:00 2001 From: Ashcon Mohseninia Date: Sun, 23 Jun 2024 22:30:36 +0100 Subject: [PATCH] Split shifting code into new classes per algo --- src/adaptation/shift_adaptation.cpp | 4 +- src/adaptation/shift_adaptation.h | 5 +- src/common_structs.h | 31 +- src/common_structs_ops.cpp | 13 - src/common_structs_ops.h | 1 - src/diag/diag_data.cpp | 2 +- src/diag/kwp2000.cpp | 15 +- src/gearbox.cpp | 542 +++----------------- src/pressure_manager.cpp | 10 +- src/pressure_manager.h | 4 +- src/profiles.cpp | 16 +- src/shifting_algo/s_algo.h | 74 +++ src/shifting_algo/shift_crossover.cpp | 309 +++++++++++ src/shifting_algo/shift_crossover.h | 29 ++ src/shifting_algo/shift_release.h | 5 + src/shifting_algo/shifting_algo_helpers.cpp | 12 + 16 files changed, 532 insertions(+), 540 deletions(-) create mode 100644 src/shifting_algo/s_algo.h create mode 100644 src/shifting_algo/shift_crossover.cpp create mode 100644 src/shifting_algo/shift_crossover.h create mode 100644 src/shifting_algo/shift_release.h create mode 100644 src/shifting_algo/shifting_algo_helpers.cpp diff --git a/src/adaptation/shift_adaptation.cpp b/src/adaptation/shift_adaptation.cpp index cd1c7aea..1f3e83b0 100644 --- a/src/adaptation/shift_adaptation.cpp +++ b/src/adaptation/shift_adaptation.cpp @@ -120,6 +120,7 @@ void ShiftAdaptationSystem::record_shift_start(uint64_t time_into_shift, int ove } } +/* void ShiftAdaptationSystem::record_shift_end(ShiftStage c_stage, uint64_t time_into_phase, uint16_t mpc, uint16_t spc) { ESP_LOGI("ADAPT", "Shift ended. %d mBar on MPC, %d mBar on SPC", mpc, spc); } @@ -129,6 +130,7 @@ void ShiftAdaptationSystem::record_flare(ShiftStage when, uint64_t elapsed){ this->flare_location = when; this->flare_time = elapsed; } +*/ uint16_t ShiftAdaptationSystem::get_overlap_end_shift_pressure(ProfileGearChange change, uint16_t selected_prefill_pressure) { uint16_t ret = selected_prefill_pressure*2; @@ -142,7 +144,7 @@ uint32_t ShiftAdaptationSystem::check_prefill_adapt_conditions_start(SensorData* uint32_t ret = (int)AdaptCancelFlag::ADAPTABLE; this->current_change = change; this->flared = false; - this->flare_location = ShiftStage::Bleed; + //this->flare_location = ShiftStage::Bleed; this->flare_time = 0; if (sensors->input_rpm < ADP_CURRENT_SETTINGS.min_input_rpm) { diff --git a/src/adaptation/shift_adaptation.h b/src/adaptation/shift_adaptation.h index d42d6c4b..00c728b9 100644 --- a/src/adaptation/shift_adaptation.h +++ b/src/adaptation/shift_adaptation.h @@ -43,9 +43,9 @@ class ShiftAdaptationSystem { uint32_t check_prefill_adapt_conditions_start(SensorData* sensors, ProfileGearChange change); void record_shift_start(uint64_t time_into_shift, int overlap_start_ts, uint16_t mpc, uint16_t spc, ShiftClutchVelocity vel, uint16_t delta_rpm); - void record_shift_end(ShiftStage c_stage, uint64_t time_into_phase, uint16_t mpc, uint16_t spc); + //void record_shift_end(ShiftStage c_stage, uint64_t time_into_phase, uint16_t mpc, uint16_t spc); - void record_flare(ShiftStage when, uint64_t elapsed); + //void record_flare(ShiftStage when, uint64_t elapsed); uint16_t get_overlap_end_shift_pressure(ProfileGearChange change, uint16_t selected_prefill_pressure); AdaptPrefillData get_prefill_adapt_data(ProfileGearChange change); @@ -67,7 +67,6 @@ class ShiftAdaptationSystem { ProfileGearChange current_change; bool flared = false; - ShiftStage flare_location = ShiftStage::Bleed; uint64_t flare_time = 0; }; diff --git a/src/common_structs.h b/src/common_structs.h index a88cde0c..ccba8c11 100644 --- a/src/common_structs.h +++ b/src/common_structs.h @@ -10,35 +10,6 @@ typedef int16_t pressure_map[11]; typedef float rpm_modifier_map[9]; -/** - * @brief Shifting state - */ -enum class ShiftStage { - /** - * @brief Hydralic bleed phase. - * In this phase, Shift pressure fluid is bled in order to not over pressurise the overlap valve - * The shift solenoid is not active - */ - Bleed = 1, - /** - * @brief Clutch filling phase. - * In this phase, the shift solenoid is engaged, and Shift pressure is ramped in order to clear - * the tolorance between the engaging clutch pack, without actually applying it. - */ - Fill = 2, - /** - * @brief Clutch overlap phase. - * In this phase, the engaging clutch's pressure is raised, whilst the disengaging clutch's pressure - * is reduced, causing (Via the overlap hydralic valve), the gearbox to transition clutch packs. - */ - Overlap = 3, - /** - * @brief Shift pressure is increased to its maximum via a ramp in order to lock the engaging clutch in place. - * Then, the shift solenoid turns off, completing the gear change. - */ - MaxPressure = 4 -}; - enum class Clutch { K1 = 0, K2 = 1, @@ -169,7 +140,7 @@ enum class ShiftCircuit { * @brief Shift data request structure * */ -struct ShiftData{ +struct CircuitInfo{ ShiftCircuit shift_circuit; uint8_t targ_g; uint8_t curr_g; diff --git a/src/common_structs_ops.cpp b/src/common_structs_ops.cpp index 8ccd37e2..aacf1be7 100644 --- a/src/common_structs_ops.cpp +++ b/src/common_structs_ops.cpp @@ -1,18 +1,5 @@ #include "common_structs_ops.h" -ShiftStage next_shift_stage(ShiftStage now) { - switch(now) { - case ShiftStage::Bleed: - return ShiftStage::Fill; - case ShiftStage::Fill: - return ShiftStage::Overlap; - case ShiftStage::Overlap: - case ShiftStage::MaxPressure: - default: - return ShiftStage::MaxPressure; - } -} - Clutch get_clutch_to_apply(ProfileGearChange change) { switch(change) { case ProfileGearChange::ONE_TWO: diff --git a/src/common_structs_ops.h b/src/common_structs_ops.h index 4f952bed..6de860e4 100644 --- a/src/common_structs_ops.h +++ b/src/common_structs_ops.h @@ -4,7 +4,6 @@ #include "common_structs.h" #include "shifter/shifter.h" -ShiftStage next_shift_stage(ShiftStage now); Clutch get_clutch_to_apply(ProfileGearChange change); Clutch get_clutch_to_release(ProfileGearChange change); diff --git a/src/diag/diag_data.cpp b/src/diag/diag_data.cpp index 95e304c9..02202b6f 100644 --- a/src/diag/diag_data.cpp +++ b/src/diag/diag_data.cpp @@ -51,7 +51,7 @@ DATA_GEARBOX_SENSORS get_gearbox_sensors(Gearbox* g) { DATA_SOLENOIDS get_solenoid_data(Gearbox* gb_ptr) { DATA_SOLENOIDS ret = {}; - if (gb_ptr == nullptr) { + if (sol_mpc == nullptr) { memset(&ret, 0xFF, sizeof(ret)); return ret; } diff --git a/src/diag/kwp2000.cpp b/src/diag/kwp2000.cpp index 68b33af4..336e40a2 100644 --- a/src/diag/kwp2000.cpp +++ b/src/diag/kwp2000.cpp @@ -1276,9 +1276,14 @@ void Kwp2000_server::run_solenoid_test() { SolRtRes res{}; res.lid = this->routine_id; - res.atf_temp = this->gearbox_ptr->sensor_data.atf_temp; - - this->gearbox_ptr->diag_inhibit_control(); + int16_t tmp = 0; + if (Sensors::read_atf_temp(&tmp) != ESP_OK) { + return; + } + res.atf_temp = tmp; + if (nullptr != this->gearbox_ptr) { + this->gearbox_ptr->diag_inhibit_control(); + } Solenoids::notify_diag_test_start(); PwmSolenoid* order[6] = {sol_mpc, sol_spc, sol_tcc, sol_y3, sol_y4, sol_y5}; @@ -1297,7 +1302,9 @@ void Kwp2000_server::run_solenoid_test() { } this->routine_running = false; Solenoids::notify_diag_test_end(); - this->gearbox_ptr->diag_regain_control(); + if (nullptr != this->gearbox_ptr) { + this->gearbox_ptr->diag_regain_control(); + } memcpy(this->routine_result, &res, sizeof(SolRtRes)); vTaskDelete(nullptr); } \ No newline at end of file diff --git a/src/gearbox.cpp b/src/gearbox.cpp index 1b23199c..c371d395 100644 --- a/src/gearbox.cpp +++ b/src/gearbox.cpp @@ -8,6 +8,8 @@ #include "nvs/device_mode.h" #include "egs_calibration/calibration_structs.h" #include "firstorder_average.h" +#include "shifting_algo/s_algo.h" +#include "shifting_algo/shift_crossover.h" #define SBS SBS_CURRENT_SETTINGS // ONLY FOR FORWARD GEARS! @@ -344,8 +346,6 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil if (nullptr != profile) { - GearboxGear t_gear = this->target_gear; - GearboxGear a_gear = this->actual_gear; ShiftReport sr = ShiftReport{}; sr.profile = profile->get_profile_id(); sr.change = req_lookup; @@ -354,14 +354,13 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil uint8_t overlap_report_size = 0; ShiftCharacteristics chars = profile->get_shift_characteristics(req_lookup, &this->sensor_data); chars.target_shift_time = MAX(100, chars.target_shift_time); - ShiftData sd = pressure_mgr->get_basic_shift_data(&this->gearboxConfig, req_lookup, chars); + CircuitInfo sd = pressure_mgr->get_basic_shift_data(&this->gearboxConfig, req_lookup, chars); if (this->last_shift_circuit == sd.shift_circuit) { // Same shift solenoid while (GET_CLOCK_TIME() - sensor_data.last_shift_time < 500) { vTaskDelay(10); } } this->last_shift_circuit = sd.shift_circuit; - ShiftStage current_stage = ShiftStage::Bleed; bool process_shift = true; sr.start_reading = this->collect_report_segment(shift_start_time); @@ -369,14 +368,7 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil // calculate prefill data now, as we need this to set MPC offset PrefillData prefill_data = pressure_mgr->make_fill_data(req_lookup); - //AdaptShiftRequest adaptation_req; - float phase_total_time = 100; - - // Current Y values ShiftPressures p_now = {}; - - - // Previous stage Y values (Set at end of stage) ShiftPressures p_prev = {}; memset(&p_now, 0, sizeof(ShiftPressures)); @@ -386,18 +378,7 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil uint32_t phase_elapsed = 0; uint32_t prefill_adapt_flags = this->shift_adapter->check_prefill_adapt_conditions_start(&this->sensor_data, req_lookup); - //AdaptPrefillData adapt_prefill = this->shift_adapter->get_prefill_adapt_data(req_lookup); - //prefill_data.fill_pressure_on_clutch += adapt_prefill.pressure_offset_on_clutch; - //prefill_data.fill_time += adapt_prefill.timing_offset; pressure_manager->register_shift_pressure_data(&p_now); - /** - * Precomputed variables - */ - - // Max input shaft torque for MPC fast off / adaptation - // int torque_lim_adapt = adapt_prefill.torque_lim; - // Min input torque to perform shift overlap torque ramp - //int torque_lim_ramp_shift = torque_lim_adapt/2; if (prefill_adapt_flags != 0) { ESP_LOGI("SHIFT", "Prefill adapting is not allowed. Reason flag is 0x%08X", (int)prefill_adapt_flags); @@ -412,34 +393,10 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil int spring_pressure_on_clutch = this->pressure_mgr->get_spring_pressure(get_clutch_to_apply(req_lookup)); int spring_pressure_off_clutch = this->pressure_mgr->get_spring_pressure(get_clutch_to_release(req_lookup)); - - - bool detected_shift_done_clutch_progress = false; - float target_c_on = pre_cs.on_clutch_speed / MAX(100, chars.target_shift_time); - bool overlap_record_start_done = false; - bool flare_notified = false; - bool recordable_shift = true; - - int delta_rpm = 0; - float pre_overlap_torque = 0; - int rpm_to_overlap = 0; - // In the event we are doing a low torque shift, - // then suddenly the user slams the pedal down (During fill phase 2 or 3), creating a load spike, - // we activate this flag and torque limit is applied to torque_lim_adapt. - bool prefill_protection_active = false; - bool mpc_released = false; - int current_torque_req = 0; - float torque_adder = 0; - int trq_up_time = 0; PressureStageTiming maxp = pressure_manager->get_max_pressure_timing(); - pressure_manager->set_shift_stage(current_stage); Clutch applying = get_clutch_to_apply(req_lookup); Clutch releasing = get_clutch_to_release(req_lookup); - int clutch_merge_rpm = 0; bool enable_torque_request = true; - int trq_req_start_time = 100 + prefill_data.fill_time; - int overlap_phase_2_time = INT_MAX; - int overlap_phase_2_total_time = INT_MAX; switch (req_lookup) { case ProfileGearChange::ONE_TWO: enable_torque_request = SBS_CURRENT_SETTINGS.trq_req_1_2_enable; @@ -453,7 +410,6 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil case ProfileGearChange::FOUR_FIVE: enable_torque_request = SBS_CURRENT_SETTINGS.trq_req_4_5_enable; break; - case ProfileGearChange::FIVE_FOUR: enable_torque_request = SBS_CURRENT_SETTINGS.trq_req_5_4_enable; break; @@ -469,14 +425,11 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil default: break; }; - int shifting_torque_delta = 0; float SPC_GAIN = 1.0; if (req_lookup == ProfileGearChange::ONE_TWO || req_lookup == ProfileGearChange::TWO_ONE) { SPC_GAIN = 1.993; } int MOD_MAX = this->pressure_mgr->get_max_solenoid_pressure(); - - int fill_p_torque_max_on_clutch = this->pressure_mgr->calc_max_torque_for_clutch(target_gear, applying, prefill_data.fill_pressure_on_clutch); #define FILL_RAMP_TIME 60 #define FILL_LP_HOLD_TIME 100 int release_time_min = 100 + prefill_data.fill_time + FILL_LP_HOLD_TIME + FILL_RAMP_TIME; @@ -485,72 +438,71 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil float release_clutch_vel = 0; int pre_cs_time = 0; - int clutch_max_spc = (MOD_MAX*SPC_GAIN) - spring_pressure_on_clutch; - int clutch_max_mpc = (MOD_MAX) - spring_pressure_off_clutch; - int max_torque_on = pressure_manager->calc_max_torque_for_clutch(this->target_gear, applying, clutch_max_spc); - int max_torque_off = pressure_manager->calc_max_torque_for_clutch(this->actual_gear, releasing, clutch_max_mpc); FirstOrderAverage on_velocity_avg = FirstOrderAverage(100/SHIFT_DELAY_MS); FirstOrderAverage off_velocity_avg = FirstOrderAverage(100/SHIFT_DELAY_MS); - - // Precomputed - int fill_start_time = 100; - int overlap_start_time = 100 + prefill_data.fill_time + FILL_LP_HOLD_TIME + FILL_RAMP_TIME; - float current_torque_reduction = 0; - float target_torque_reduction = 0; - bool inc_ramp = false; - float torque_at_ramp = 0; - + bool freeze_torque = false; + int t_delta = 0; - int downshift_time_req_start = 0; - int filling_torque = 0; + TorqueRequstData trd = { + .ty = TorqueRequestControlType::None, + .bounds = TorqueRequestBounds::LessThan, + .amount = 0 + }; - bool clutch_crossover = false; - int clutch_crossover_rpm = 0; + ShiftInterfaceData sid = { + .MOD_MAX = MOD_MAX, + .SPC_GAIN = SPC_GAIN, + .change = req_lookup, + .applying = applying, + .releasing = releasing, + .curr_g = this->actual_gear, + .targ_g = this->target_gear, + .inf = sd, + .spring_on_clutch = pressure_manager->get_spring_pressure(applying), + .spring_off_clutch = pressure_manager->get_spring_pressure(releasing), + .prefill_info = prefill_data, + .chars = chars, + .ptr_r_clutch_speeds = &now_cs, + .ptr_prev_pressures = &p_prev, + .ptr_w_pressures = &p_now, + .ptr_w_trq_req = &trd, + .maxp_info = maxp + }; - bool do_dynamic_shift = !is_upshift && chars.target_shift_time < 500; - bool overlap_trigger_torque_request = false; + ShiftingAlgorithm* algo = new CrossoverShift(&sid); + uint8_t algo_phase_id = 0; + uint8_t algo_max_phase = algo->max_shift_stage_id(); + const float multi_vel = 100.0/SHIFT_DELAY_MS; while(process_shift) { bool stationary_shift = this->is_stationary(); - bool skip_phase = false; // Shifter moved mid shift! if (!is_shifter_in_valid_drive_pos(this->shifter_pos)) { process_shift = false; result = false; break; } + + int model_torque = sensor_data.static_torque; + if (!freeze_torque) { + t_delta = sensor_data.driver_requested_torque - sensor_data.static_torque; + } else { + // Torque freeze + model_torque = MAX(sensor_data.driver_requested_torque-t_delta, sensor_data.static_torque); + } int abs_input_torque = abs(InputTorqueModel::get_input_torque( sensor_data.engine_rpm, sensor_data.input_rpm, - sensor_data.static_torque, + model_torque, egs_can_hal->get_ac_torque_loss(500) )); - int centrifugal_force_on_clutch = 0; - int centrifugal_force_off_clutch = 0; - int SPC_MAX = SPC_GAIN * (this->pressure_mgr->get_max_solenoid_pressure() - this->pressure_mgr->get_shift_regulator_pressure()); - if (current_stage == ShiftStage::MaxPressure || current_stage == ShiftStage::Bleed) { - // No gain - SPC_MAX = (this->pressure_mgr->get_max_solenoid_pressure() - this->pressure_mgr->get_shift_regulator_pressure()); - } - // Grab ratio informations - bool coasting_shift = 0 > sensor_data.static_torque; - float multi_vel = 100.0/SHIFT_DELAY_MS; // Shift reporting if (!stationary_shift) { - int input_rpm_old_gear = calc_input_rpm_from_req_gear(sensor_data.output_rpm, a_gear, &this->gearboxConfig); - delta_rpm = abs(sensor_data.input_rpm - input_rpm_old_gear); now_cs = ClutchSpeedModel::get_shifting_clutch_speeds(sensor_data.output_rpm, this->rpm_reading, req_lookup, this->gearboxConfig.bounds); - // Only calculate if speed > 0, to avoid weirdness with negative speed values in the event of a flare - // This only applies for K2 and K3. Stock EGS has K1 at 0, meaning its not used (Maybe they found an issue whilst testing?) - centrifugal_force_on_clutch = pressure_manager->calculate_centrifugal_force_for_clutch(applying, sensor_data.input_rpm, MAX(0, now_cs.rear_sun_speed)); - centrifugal_force_off_clutch = pressure_manager->calculate_centrifugal_force_for_clutch(releasing, sensor_data.input_rpm, MAX(0, now_cs.rear_sun_speed)); if (now_cs.off_clutch_speed < -50 || now_cs.on_clutch_speed < -50) { flaring = true; - if (prefill_adapt_flags == 0 && !flare_notified) { - this->shift_adapter->record_flare(current_stage, phase_elapsed); - } if (sr.detect_flare_ts == 0) { sr.detect_flare_ts = (uint16_t)(GET_CLOCK_TIME() - shift_start_time); } @@ -560,411 +512,62 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil if (now_cs.off_clutch_speed < 25) { pre_cs = now_cs; pre_cs_time = total_elapsed; - target_c_on = abs((float)pre_cs.on_clutch_speed / ((float)MAX(100, chars.target_shift_time) / SHIFT_DELAY_MS)); } else if (now_cs.off_clutch_speed >= 100 && release_time_recorded == 0) { release_time_recorded = total_elapsed; float time = MAX(SHIFT_DELAY_MS, total_elapsed - pre_cs_time); release_clutch_vel = (float)(pre_cs.on_clutch_speed - now_cs.on_clutch_speed) / (time/100.0); // Calculate shifting velosity. Time to go from 50RPM -> 100RPM ESP_LOGI("Shift", "Detect shift release at %d ms. Vel is %.1f RPM/100ms", release_time_recorded, release_clutch_vel); } - if (!clutch_crossover && now_cs.on_clutch_speed < now_cs.off_clutch_speed) { - clutch_crossover = true; - clutch_crossover_rpm = now_cs.on_clutch_speed; - } on_velocity_avg.add_sample((old_cs.on_clutch_speed - now_cs.on_clutch_speed)*multi_vel); off_velocity_avg.add_sample((old_cs.off_clutch_speed - now_cs.off_clutch_speed)*multi_vel); this->shifting_velocity.on_clutch_vel = on_velocity_avg.get_average(); this->shifting_velocity.off_clutch_vel = off_velocity_avg.get_average(); old_cs = now_cs; - - if (now_cs.off_clutch_speed >= now_cs.on_clutch_speed && now_cs.on_clutch_speed < 10 && on_velocity_avg.get_average() < 10) { - detected_shift_done_clutch_progress = true; - } - - // Protect gearbox if MPC has already released, and there is a load - // spike on input torque - //if (mpc_released && sensor_data.input_torque > torque_lim_adapt && !prefill_protection_active) { - // ESP_LOGW("SHIFT", "Activating prefill shift protection. Prefill adaptation has been cancelled"); - // prefill_protection_active = true; - //} - - int torque_req_upper_torque = sensor_data.driver_requested_torque; - //if (prefill_protection_active && current_stage < ShiftStage::Overlap) { - // // Use torque lim (Activate protection) - // this->set_torque_request(TorqueRequestControlType::NormalSpeed, TorqueRequestBounds::LessThan, torque_lim_adapt); - // torque_req_upper_torque = MIN(torque_lim_adapt, torque_req_upper_torque); - //} - // for Max Pressure or overlap - if (enable_torque_request) { - bool goto_torque_ramp = false; - if (output_data.ctrl_type != TorqueRequestControlType::None) { - goto_torque_ramp = true; - } - if (sensor_data.driver_requested_torque >= fill_p_torque_max_on_clutch) { // Torque above bounds - if (output_data.ctrl_type == TorqueRequestControlType::None) { // No current trq request - goto_torque_ramp = true; - } - } - - if (goto_torque_ramp) { - // Default torque - Torque target before and after request - int default_torque = sensor_data.driver_requested_torque; - if (sensor_data.max_torque < sensor_data.driver_requested_torque) { - default_torque = sensor_data.max_torque; - } - - if (is_upshift) { - // Upshift request - // Start at begining of shift - // End at max pressure phase - // This allows for reduced pressures during the overlap phase, creating a smoother shift - if (overlap_trigger_torque_request) { - if (!inc_ramp) { - int time = total_elapsed - (overlap_phase_2_total_time); - if (target_torque_reduction == 0) { - target_torque_reduction = sensor_data.driver_requested_torque-(float)sensor_data.driver_requested_torque*calc_torque_reduction_factor(req_lookup, chars.target_shift_time); - } - current_torque_reduction = interpolate_float(time, MAX(sensor_data.static_torque, sensor_data.driver_requested_torque), target_torque_reduction, 0, chars.target_shift_time/2, InterpType::Linear); - this->set_torque_request(TorqueRequestControlType::NormalSpeed, TorqueRequestBounds::LessThan, current_torque_reduction); - if (now_cs.on_clutch_speed < now_cs.off_clutch_speed) { - downshift_time_req_start = total_elapsed; - inc_ramp = true; - } - } else { - int time = total_elapsed - downshift_time_req_start; - current_torque_reduction = interpolate_float(time, target_torque_reduction, sensor_data.driver_requested_torque, 0, 300, InterpType::Linear); - this->set_torque_request(TorqueRequestControlType::BackToDemandTorque, TorqueRequestBounds::LessThan, current_torque_reduction); - if (abs(current_torque_reduction - sensor_data.driver_requested_torque) < 1) { - this->set_torque_request(TorqueRequestControlType::None, TorqueRequestBounds::LessThan, 0); - enable_torque_request = false; - } - } - } - } else { - // Downshift request - // Start at clutch switchover - // End at end of max pressure phase - // This protects the gearbox from slamming the clutches at the end of the shift - if (now_cs.off_clutch_speed > now_cs.on_clutch_speed) { - if (downshift_time_req_start == 0) { - // First call - downshift_time_req_start = total_elapsed; - target_torque_reduction = (float)sensor_data.driver_requested_torque*calc_torque_reduction_factor(req_lookup, chars.target_shift_time); - } - if (current_stage == ShiftStage::Overlap) { - int time = total_elapsed - downshift_time_req_start; - // Switchover period, start the request! - current_torque_reduction = interpolate_float(time, 0, target_torque_reduction, 0, 100, InterpType::Linear); - this->set_torque_request(TorqueRequestControlType::FastAsPossible, TorqueRequestBounds::LessThan, default_torque-current_torque_reduction); - } else if (current_stage == ShiftStage::MaxPressure) { - if (phase_elapsed < 300) { - this->set_torque_request(TorqueRequestControlType::NormalSpeed, TorqueRequestBounds::LessThan, default_torque-current_torque_reduction); - } else { - current_torque_reduction = interpolate_float(phase_elapsed, target_torque_reduction, 0, 0, phase_total_time-300, InterpType::Linear); - this->set_torque_request(TorqueRequestControlType::BackToDemandTorque, TorqueRequestBounds::LessThan, default_torque-current_torque_reduction); - if (current_torque_reduction < 1) { - this->set_torque_request(TorqueRequestControlType::None, TorqueRequestBounds::LessThan, 0); - enable_torque_request = false; - } - } - } - - } - } - } + this->set_torque_request(trd.ty, trd.bounds, trd.amount); } } else { // If input speed is too low, use the overlap time as a way of measuring shift progress shifting_velocity = {0, 0}; - recordable_shift = false; this->flaring = false; this->set_torque_request(TorqueRequestControlType::None, TorqueRequestBounds::LessThan, 0); // And also torque requests } + // Algorithm has control + uint8_t step_result = algo->step( + algo_phase_id, + abs_input_torque, + stationary_shift, + is_upshift, + phase_elapsed, + total_elapsed, + this->pressure_mgr, + &this->sensor_data + ); - // Check next stage (Only when in bleeding, filling) - // Overlap phase self cancels as it has to monitor shift progress - if (phase_elapsed >= phase_total_time && current_stage <= ShiftStage::Overlap) { - current_stage = next_shift_stage(current_stage); - pressure_manager->set_shift_stage(current_stage); - p_prev = p_now; - // New stage - phase_elapsed = 0; - // One time phase initial code - if (current_stage == ShiftStage::Fill) { - pressure_manager->set_shift_circuit(sd.shift_circuit, true); - phase_total_time = prefill_data.fill_time + FILL_RAMP_TIME + FILL_LP_HOLD_TIME; // TODO make these constants - filling_torque = abs_input_torque; - } else if (current_stage == ShiftStage::Overlap) { - if (!do_dynamic_shift) { - this->tcc->set_shift_target_state(InternalTccState::Open); - } - phase_total_time = chars.target_shift_time+SBS.shift_timeout_coasting; //(No ramping) (Worse case time) - } else if (current_stage == ShiftStage::MaxPressure) { - if (!result) { - ESP_LOGE("SHIFT", "Max Pressure stage not running due to failed shift"); - break; - } - maxp = pressure_manager->get_max_pressure_timing(); - phase_total_time = maxp.hold_time + maxp.ramp_time; - } - } else if (phase_elapsed > phase_total_time && current_stage == ShiftStage::MaxPressure) { - process_shift = false; - break; - } - - if (current_stage == ShiftStage::Bleed) { - int wp_old_clutch = pressure_manager->find_working_pressure_for_clutch(a_gear, releasing, abs_input_torque, false); - p_now.on_clutch = 0; - p_now.off_clutch = wp_old_clutch; - p_now.overlap_shift = prefill_data.fill_pressure_on_clutch + spring_pressure_on_clutch; - p_now.overlap_mod = wp_old_clutch + spring_pressure_off_clutch; - p_now.shift_sol_req = (p_now.overlap_shift-centrifugal_force_on_clutch)/SPC_GAIN; - p_now.mod_sol_req = - ((p_now.overlap_shift - centrifugal_force_on_clutch)*sd.pressure_multi_spc / SPC_GAIN)+ - ((p_now.overlap_mod - centrifugal_force_off_clutch)*sd.pressure_multi_mpc)+ - sd.mpc_pressure_spring_reduction; - } else if (current_stage == ShiftStage::Fill) { - int wp_old_clutch = pressure_manager->find_working_pressure_for_clutch(a_gear, releasing, abs_input_torque, false); - bool was_adapting = prefill_adapt_flags == 0; - if (was_adapting && prefill_adapt_flags != 0) { - ESP_LOGW("SHIFT", "Adapting was cancelled. Reason flag: 0x%08X", (int)prefill_adapt_flags); - } - if (phase_elapsed < prefill_data.fill_time) { // Hold 1 (Same for all shift types) - p_now.off_clutch = wp_old_clutch; - p_now.on_clutch = prefill_data.fill_pressure_on_clutch; - filling_torque = abs_input_torque; // Update filling torque variable as shift types diverge after this - } else if (phase_elapsed < prefill_data.fill_time + FILL_RAMP_TIME) { // Ramp phase (Hold 1 -> Hold 2) - if (do_dynamic_shift) { // Dynamic mode (Trq req starts here) - p_now.on_clutch = interpolate_float( - phase_elapsed - prefill_data.fill_time, - prefill_data.fill_pressure_on_clutch, - prefill_data.low_fill_pressure_on_clutch, - 0, - FILL_RAMP_TIME, - InterpType::Linear - ); - p_now.off_clutch = wp_old_clutch; - } else { // Comfort shift - p_now.on_clutch = interpolate_float( - phase_elapsed - prefill_data.fill_time, - prefill_data.fill_pressure_on_clutch, - 0, - 0, - FILL_RAMP_TIME, - InterpType::Linear - ); - p_now.off_clutch = wp_old_clutch; - } - } else { // Hold 2 - if (do_dynamic_shift) { // Dynamic - p_now.on_clutch = prefill_data.low_fill_pressure_on_clutch; - p_now.off_clutch = wp_old_clutch/2; - } else { // Comfort - p_now.on_clutch = 0; - p_now.off_clutch = wp_old_clutch; - } - } - pre_overlap_torque = sensor_data.input_torque; - - p_now.overlap_mod = MAX(p_now.off_clutch + spring_pressure_off_clutch, 0); - p_now.overlap_shift = MAX(p_now.on_clutch + spring_pressure_on_clutch, 0); - p_now.shift_sol_req = MAX((p_now.overlap_shift - centrifugal_force_on_clutch)/SPC_GAIN, 0); - p_now.mod_sol_req = MAX( - ((p_now.overlap_shift - centrifugal_force_on_clutch)*sd.pressure_multi_spc/SPC_GAIN)+ - ((p_now.overlap_mod - centrifugal_force_off_clutch)*sd.pressure_multi_mpc)+ - sd.mpc_pressure_spring_reduction - , 0); - - if (mpc_released && !prefill_protection_active && prefill_adapt_flags == 0x0000) { - // Do adapting for prefill - prefill_adapt_flags = 0x1000; - } - } else if (current_stage == ShiftStage::Overlap) { - if (phase_elapsed > 500 || now_cs.off_clutch_speed > 100) { - - // Stronger overlap phase (Clutch has released) - if (INT_MAX == overlap_phase_2_time) { - // Set p_prev to be memory pressures at the end of the releasing overlap phase - p_prev = p_now; - overlap_phase_2_time = phase_elapsed; - overlap_phase_2_total_time = total_elapsed; - int target_vel = (float)pre_cs.on_clutch_speed / (100.0/SHIFT_DELAY_MS); // Per 20ms div - if (overlap_phase_2_time >= 500) { - ESP_LOGI("SHIFT", "Disconnect late!"); - } else { - ESP_LOGI("SHIFT", "Disconnect in valid time window (%d ms)", (int)phase_elapsed); - } - ESP_LOGI("SHIFT", "Target vel is %d RPM/100ms", target_vel); - if (!stationary_shift) { - overlap_trigger_torque_request = true; - } - } - int into_phase = phase_elapsed - overlap_phase_2_time; - // Phase 2 is harder, we have to monitor the clutch release and apply speed, and adjust pressures to get the desired - // ramping speeds from both clutches - - // Inertia of the engine / pump - float inertia = calcualte_abs_engine_inertia(sd.map_idx, sensor_data.engine_rpm, sensor_data.input_rpm); - int wp_new_clutch_start = pressure_manager->find_working_pressure_for_clutch(t_gear, applying, abs_input_torque, false); - - int targ_torque = abs_input_torque + inertia; - //if (this->output_data.torque_req_amount != 0) { - // // Active request - // targ_torque = abs(sensor_data.driver_requested_torque) + inertia; - //} - if (sensor_data.pedal_pos == 0 && !is_upshift) { - targ_torque += abs_input_torque; // For engine braking coast shifts - } - targ_torque += torque_adder; - if (is_upshift && sensor_data.pedal_pos < 10) { - targ_torque = abs_input_torque; // Don't count inertia - } - // For all shifts, when clutches start merging, double pressure - if (clutch_crossover) { - targ_torque += interpolate_float(now_cs.on_clutch_speed, 0, abs_input_torque, clutch_crossover_rpm, 0, InterpType::Linear); - } - int wp_new_clutch_end = pressure_manager->find_working_pressure_for_clutch(t_gear, applying, targ_torque, false); - p_now.on_clutch = interpolate_float(into_phase, wp_new_clutch_start, wp_new_clutch_end, 0, chars.target_shift_time/2, InterpType::Linear); - - //if (into_phase > chars.target_shift_time && now_cs.on_clutch_speed > now_cs.off_clutch_speed) { - // spc_delta += 10; - //} - p_now.overlap_shift = p_now.on_clutch + spring_pressure_on_clutch; - p_now.shift_sol_req = MAX((p_now.overlap_shift - centrifugal_force_on_clutch)/SPC_GAIN, 0); - - if (do_dynamic_shift) { - p_now.off_clutch = 0; - p_now.overlap_mod = p_now.off_clutch + spring_pressure_off_clutch; - } else { - // Fade shift - Pressure on new clutch ramps up slowly - // Amount of torque the new clutch can hold based on its current pressure during the ramp - int torque_held_by_new_clutch = pressure_manager->calc_max_torque_for_clutch(target_gear, applying, p_now.on_clutch); - // Pressure of old clutch is = target_pressure(Input torque - Torque held by new clutch) - int mpc_required_holding = pressure_manager->find_working_pressure_for_clutch(actual_gear, releasing, MAX(0, abs_input_torque - torque_held_by_new_clutch), false); - p_now.off_clutch = MAX(0, mpc_required_holding); - p_now.overlap_mod = p_now.off_clutch + spring_pressure_off_clutch; - } - p_now.mod_sol_req = ( - ((p_now.overlap_shift - centrifugal_force_on_clutch) * sd.pressure_multi_spc / SPC_GAIN) + - ((p_now.overlap_mod - centrifugal_force_off_clutch) * sd.centrifugal_factor_off_clutch * sd.pressure_multi_mpc) + - (-(sd.pressure_multi_mpc*150)) + // 150 comes from calibration pointer on E55 coding (TODO - Locate in calibration and assign in CAL structure) - sd.mpc_pressure_spring_reduction - ); - } else { - // Phase 1. Release phase. - // This part of the overlap phase should start the disengagement of the off clutch. - int wp_new_clutch = pressure_manager->find_working_pressure_for_clutch(t_gear, applying, abs_input_torque, false); - if (do_dynamic_shift) { - p_now.on_clutch = interpolate_float(phase_elapsed, p_prev.on_clutch, wp_new_clutch, 0, 500, InterpType::Linear); - p_now.overlap_shift = spring_pressure_on_clutch + p_now.on_clutch; - p_now.shift_sol_req = MAX((p_now.overlap_shift - centrifugal_force_on_clutch)/SPC_GAIN, 0); - // Drop all of modulating clutch pressure quickly (Release the old clutch fast) - // DYN bits are set to start the torque request - p_now.off_clutch = 0; - p_now.overlap_mod = p_now.off_clutch + spring_pressure_off_clutch; - p_now.mod_sol_req = ( - ((p_now.overlap_shift - centrifugal_force_on_clutch) * sd.pressure_multi_spc / SPC_GAIN) + - ((p_now.overlap_mod - centrifugal_force_off_clutch) * sd.centrifugal_factor_off_clutch * sd.pressure_multi_mpc) + - sd.mpc_pressure_spring_reduction - ); - } else { - // Fade shift - Pressure on new clutch ramps up slowly - p_now.on_clutch = MAX(interpolate_float(phase_elapsed, p_prev.on_clutch, wp_new_clutch, 0, 500, InterpType::Linear), p_prev.on_clutch); - int overlap_end_spc_max = spring_pressure_on_clutch + p_prev.on_clutch; - p_now.overlap_shift = spring_pressure_on_clutch + p_now.on_clutch; - p_now.shift_sol_req = MAX((p_now.overlap_shift - centrifugal_force_on_clutch)/SPC_GAIN, 0); - - // Pressure on old clutch is released such that combined, both clutches can hold the torque of the box - int torque_held_by_new_clutch = pressure_manager->calc_max_torque_for_clutch(target_gear, applying, p_now.on_clutch); - // Back calculate pressure to hold the remainder of the torque - int mpc_required_holding = pressure_manager->find_working_pressure_for_clutch(actual_gear, releasing, MAX(0, abs(sensor_data.input_torque) - torque_held_by_new_clutch), false); - //printf("%d %d %d\n", torque_held_by_new_clutch, MAX(0, abs(sensor_data.input_torque) - torque_held_by_new_clutch), mpc_required_holding); - if (is_upshift && sensor_data.pedal_pos < 10) { - // MPC is resisting shift now - int wp_old_clutch = pressure_manager->find_working_pressure_for_clutch(a_gear, releasing, abs_input_torque, false); - p_now.off_clutch = wp_old_clutch; - } else { - p_now.off_clutch = MAX(0, mpc_required_holding); - } - p_now.overlap_mod = p_now.off_clutch + spring_pressure_off_clutch; - // Pressure based on torque transfer - int mod_off_torque_pressure = ( - ((p_now.overlap_shift - centrifugal_force_on_clutch) * sd.pressure_multi_spc / SPC_GAIN) + - ((p_now.overlap_mod - centrifugal_force_off_clutch) * sd.centrifugal_factor_off_clutch * sd.pressure_multi_mpc) + - sd.mpc_pressure_spring_reduction - ); - // Pressure at the end of the phase (Minimum) - int mod_off_end_pressure = ( - ((overlap_end_spc_max - centrifugal_force_on_clutch) * sd.pressure_multi_spc / SPC_GAIN) + - ((spring_pressure_off_clutch - centrifugal_force_off_clutch) * sd.centrifugal_factor_off_clutch * sd.pressure_multi_mpc) + - sd.mpc_pressure_spring_reduction - ); - p_now.mod_sol_req = MAX(mod_off_torque_pressure, mod_off_end_pressure); - } - } - } else if (current_stage == ShiftStage::MaxPressure) { - // Ramp time is always 250ms - int wp_new_gear = pressure_manager->find_working_mpc_pressure(this->target_gear); - int wp_new_clutch = pressure_manager->find_working_pressure_for_clutch(t_gear, applying, abs_input_torque, false); - if (phase_elapsed < maxp.ramp_time) { - p_now.on_clutch = interpolate_float(phase_elapsed, p_prev.on_clutch, wp_new_clutch, 0, maxp.ramp_time, InterpType::Linear); - p_now.overlap_shift = interpolate_float(phase_elapsed, p_prev.overlap_shift, MOD_MAX, 0, maxp.ramp_time, InterpType::Linear); - p_now.shift_sol_req = (p_now.overlap_shift) / SPC_GAIN; - } else { - // Hold phase. Mod at 0, Shift at full - p_now.on_clutch = wp_new_clutch; - p_now.overlap_shift = MOD_MAX; - p_now.shift_sol_req = MOD_MAX/SPC_GAIN; - } - // Merge working pressure slowly - p_now.off_clutch = 0; - p_now.overlap_mod = 0; - p_now.mod_sol_req = interpolate_float(phase_elapsed, p_prev.mod_sol_req, wp_new_gear, 0, maxp.ramp_time, InterpType::Linear); - } + // Update pressures pressure_mgr->set_target_modulating_pressure(p_now.mod_sol_req); pressure_mgr->set_target_shift_pressure(p_now.shift_sol_req); - pressure_mgr->update_pressures(current_stage == ShiftStage::MaxPressure ? this->target_gear : this->actual_gear); - - // Timeout checking (Only in overlap) - if (ShiftStage::Overlap == current_stage) { - if (!stationary_shift) { // Confirmed shift! - if (detected_shift_done_clutch_progress) { - result = true; - skip_phase = true; - sr.detect_shift_end_ts = (uint16_t)(GET_CLOCK_TIME() - shift_start_time); - if (prefill_adapt_flags == 0) { - this->shift_adapter->record_shift_end(current_stage, phase_elapsed, p_now.off_clutch, p_now.on_clutch); - } - } - } else if (stationary_shift && phase_elapsed > 1000) { - result = true; - skip_phase = true; - } else if (!coasting_shift && MAX(SBS.shift_timeout_pulling, phase_total_time*2) < phase_elapsed) { // TIMEOUT - result = false; - skip_phase = true; - } else if (coasting_shift && MAX(SBS.shift_timeout_coasting, phase_total_time*2) < phase_elapsed) { // TIMEOUT - result = false; - skip_phase = true; - } - } - // Shift reporting - if (recordable_shift && prefill_adapt_flags == 0) { - if (sr.detect_shift_start_ts == 0 && shifting_velocity.on_clutch_vel < -20) { - sr.detect_shift_start_ts = (uint16_t)(GET_CLOCK_TIME() - shift_start_time); - } - } - if (detected_shift_done_clutch_progress && sr.detect_shift_end_ts == 0) { - sr.detect_shift_end_ts = (uint16_t)(GET_CLOCK_TIME() - shift_start_time); - } - if (skip_phase) { - phase_elapsed = phase_total_time; + pressure_mgr->update_pressures(algo_phase_id != algo_max_phase ? this->target_gear : this->actual_gear); + + if (step_result == 0) { + // Continue + phase_elapsed += SHIFT_DELAY_MS; + } else if (step_result == STEP_RES_END_SHIFT) { + // Shift completed OK! + result = true; + break; + } else if (step_result == STEP_RES_FAILURE) { + // Shift failure! + break; + } else { + // Phase has completed, update our data + p_prev = p_now; + phase_elapsed = 0; + algo_phase_id = step_result; } - vTaskDelay(SHIFT_DELAY_MS / portTICK_PERIOD_MS); - phase_elapsed += SHIFT_DELAY_MS; total_elapsed += SHIFT_DELAY_MS; } //this->shift_adapter->debug_print_prefill_data(); @@ -1002,6 +605,7 @@ bool Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfile *profil this->sensor_data.last_shift_time = GET_CLOCK_TIME(); this->flaring = false; shifting_velocity = {0, 0}; + delete algo; } return result; } diff --git a/src/pressure_manager.cpp b/src/pressure_manager.cpp index 8fcf239c..464ebeb9 100644 --- a/src/pressure_manager.cpp +++ b/src/pressure_manager.cpp @@ -134,10 +134,6 @@ uint16_t PressureManager::get_shift_regulator_pressure(void) { return HYDR_PTR->shift_reg_spring_pressure; } -void PressureManager::set_shift_stage(ShiftStage s) { - this->shift_stage = s; -} - /* MERCEDES-BENZS PRESSURE NAMES (WIS) @@ -171,7 +167,7 @@ void PressureManager::update_pressures(GearboxGear current_gear) { } else { float amplifier = 1.0; - if ((this->shift_circuit_flag == (uint8_t)ShiftCircuit::sc_1_2) && this->shift_stage != ShiftStage::MaxPressure) { + if ((this->shift_circuit_flag == (uint8_t)ShiftCircuit::sc_1_2)) { amplifier = 1.993; } @@ -337,8 +333,8 @@ void PressureManager::notify_shift_end() { // TODO pull this from calibration tables const float C_C_FACTOR[8] = {1.0, 1.0, 1.0, 1.0, 0.8, 0.8, 1.0, 1.0}; -ShiftData PressureManager::get_basic_shift_data(GearboxConfiguration* cfg, ProfileGearChange shift_request, ShiftCharacteristics chars) { - ShiftData sd; +CircuitInfo PressureManager::get_basic_shift_data(GearboxConfiguration* cfg, ProfileGearChange shift_request, ShiftCharacteristics chars) { + CircuitInfo sd; uint8_t lookup_valve_info = 0; switch (shift_request) { case ProfileGearChange::ONE_TWO: diff --git a/src/pressure_manager.h b/src/pressure_manager.h index e448263f..21339431 100644 --- a/src/pressure_manager.h +++ b/src/pressure_manager.h @@ -47,7 +47,6 @@ class PressureManager { * @param enable Set circuit state to on */ void set_shift_circuit(ShiftCircuit ss, bool enable); - void set_shift_stage(ShiftStage s); /** * @brief Set the target modulating pressure of the gearbox. @@ -104,7 +103,7 @@ class PressureManager { * @return ShiftData */ void notify_shift_end(); - ShiftData get_basic_shift_data(GearboxConfiguration* cfg, ProfileGearChange shift_request, ShiftCharacteristics chars); + CircuitInfo get_basic_shift_data(GearboxConfiguration* cfg, ProfileGearChange shift_request, ShiftCharacteristics chars); uint16_t find_working_mpc_pressure(GearboxGear curr_g); uint16_t find_working_pressure_for_clutch(GearboxGear gear, Clutch clutch, uint16_t abs_torque_nm, bool clamp_to_min_mpc = true); uint16_t calc_max_torque_for_clutch(GearboxGear gear, Clutch clutch, uint16_t pressure); @@ -177,7 +176,6 @@ class PressureManager { uint8_t c_gear = 0; uint8_t t_gear = 0; uint16_t solenoid_max_pressure = 0; - ShiftStage shift_stage; bool init_ss_recovery = false; uint64_t last_ss_on_time = 0; ShiftPressures* ptr_shift_pressures = nullptr; diff --git a/src/profiles.cpp b/src/profiles.cpp index 32176a6f..678d4adc 100644 --- a/src/profiles.cpp +++ b/src/profiles.cpp @@ -339,14 +339,14 @@ bool StandardProfile::should_upshift(GearboxGear current_gear, SensorData* senso // can_upshift = false; //} // Load check - //if (can_upshift) { - // if (sensors->max_torque != 0) { - // float demanded_load = (MAX(sensors->driver_requested_torque, 0) * 100) / sensors->max_torque; - // if (demanded_load > 30) { - // can_upshift = false; - // } - // } - //} + if (can_upshift) { + if (sensors->max_torque != 0) { + float demanded_load = (MAX(sensors->driver_requested_torque, 0) * 100) / sensors->max_torque; + if (demanded_load > 30) { + can_upshift = false; + } + } + } return can_upshift; } else { return false; diff --git a/src/shifting_algo/s_algo.h b/src/shifting_algo/s_algo.h new file mode 100644 index 00000000..902fb25f --- /dev/null +++ b/src/shifting_algo/s_algo.h @@ -0,0 +1,74 @@ +#ifndef __S_ALGO_H__ +#define __S_ALGO_H__ + +#include "../src/common_structs.h" +#include "../pressure_manager.h" + +struct TorqueRequstData { + TorqueRequestControlType ty; + TorqueRequestBounds bounds; + float amount; +}; + +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; + + +typedef struct { + int MOD_MAX; + float SPC_GAIN; + ProfileGearChange change; + Clutch applying; + Clutch releasing; + GearboxGear curr_g; + GearboxGear targ_g; + CircuitInfo inf; + uint16_t spring_on_clutch; + uint16_t spring_off_clutch; + PrefillData prefill_info; + ShiftCharacteristics chars; + ShiftClutchData* ptr_r_clutch_speeds; + ShiftPressures* ptr_prev_pressures; + ShiftPressures* ptr_w_pressures; + TorqueRequstData* ptr_w_trq_req; + PressureStageTiming maxp_info; +} ShiftInterfaceData; + +class ShiftingAlgorithm { +public: + ShiftingAlgorithm(ShiftInterfaceData* data) { + this->sid = data; + } + virtual ~ShiftingAlgorithm() = default; + + /** + * SPECIAL RETURN VALUES + * STEP_RES_CONTINUE - Continue in the same phase + * STEP_RES_END_SHIFT - Finish shift + * 1..254 - The next phase ID to run + */ + virtual uint8_t step( + uint8_t phase_id, + uint16_t abs_input_torque, + bool stationary, + bool is_upshift, + uint16_t phase_elapsed, + uint16_t total_elapsed, + PressureManager* pm, + SensorData* sd + ) = 0; + + virtual uint8_t max_shift_stage_id() = 0; + + protected: + ShiftInterfaceData* sid; +}; + +// Helper functions + +namespace ShiftHelpers { + float calcualte_abs_engine_inertia(uint8_t shift_idx, uint16_t engine_rpm, uint16_t input_rpm); +} + +#endif \ No newline at end of file diff --git a/src/shifting_algo/shift_crossover.cpp b/src/shifting_algo/shift_crossover.cpp new file mode 100644 index 00000000..b8870719 --- /dev/null +++ b/src/shifting_algo/shift_crossover.cpp @@ -0,0 +1,309 @@ +#include "shift_crossover.h" + +const uint8_t PHASE_BLEED = 0; +const uint8_t PHASE_PREFILL_1 = 1; +const uint8_t PHASE_PREFILL_2 = 2; +const uint8_t PHASE_PREFILL_3 = 3; + +const uint8_t PHASE_OVERLAP = 4; + +const uint8_t PHASE_MOMENTUM_RAMP = 5; +const uint8_t PHASE_MOMENTUM_HOLD = 6; +const uint8_t PHASE_MAX_PRESSURE = 7; +const uint8_t PHASE_END_CONTROL = 8; + +const uint8_t FILL_RAMP_TIME = 60; +const uint8_t FILL_HOLD_TIME = 100; + +CrossoverShift::CrossoverShift(ShiftInterfaceData* data) : ShiftingAlgorithm(data) {} + +CrossoverShift::~CrossoverShift() { + +} + +uint8_t CrossoverShift::step( + uint8_t phase_id, + uint16_t abs_input_torque, + bool stationary, + bool is_upshift, + uint16_t phase_elapsed, + uint16_t total_elapsed, + PressureManager* pm, + SensorData* sd +) { + uint8_t ret = STEP_RES_CONTINUE; + int centrifugal_force_on_clutch = pm->calculate_centrifugal_force_for_clutch(sid->applying, sd->input_rpm, MAX(0, sid->ptr_r_clutch_speeds->rear_sun_speed)); + int centrifugal_force_off_clutch = pm->calculate_centrifugal_force_for_clutch(sid->releasing, sd->input_rpm, MAX(0, sid->ptr_r_clutch_speeds->rear_sun_speed)); + ShiftPressures* p_now = sid->ptr_w_pressures; + if (phase_id == PHASE_BLEED) { + int wp_old_clutch = pm->find_working_pressure_for_clutch(sid->curr_g, sid->releasing, MAX(abs_input_torque, 30), false); + // Clutches + p_now->on_clutch = sid->prefill_info.fill_pressure_on_clutch; + p_now->off_clutch = wp_old_clutch; + // Overlap valves + p_now->overlap_mod = p_now->off_clutch + sid->spring_off_clutch; + p_now->overlap_shift = p_now->on_clutch + sid->spring_on_clutch; + // Solenoids + p_now->shift_sol_req = MAX((p_now->overlap_shift - centrifugal_force_on_clutch)/sid->SPC_GAIN, 0); + p_now->mod_sol_req = MAX( + ((p_now->overlap_shift - centrifugal_force_on_clutch) * sid->inf.pressure_multi_spc/sid->SPC_GAIN)+ + ((p_now->overlap_mod - centrifugal_force_off_clutch) * sid->inf.pressure_multi_mpc)+ + sid->inf.mpc_pressure_spring_reduction + , 0); + if (phase_elapsed >= 100) { + // Turn on the switching valve! + pressure_manager->set_shift_circuit(sid->inf.shift_circuit, true); + ret = PHASE_PREFILL_1; + } + } else if (phase_id == PHASE_PREFILL_1) { + int wp_old_clutch = pm->find_working_pressure_for_clutch(sid->curr_g, sid->releasing, MAX(abs_input_torque, 30), false); + // Clutches + p_now->on_clutch = sid->prefill_info.fill_pressure_on_clutch; + p_now->off_clutch = wp_old_clutch; + // Overlap valves + p_now->overlap_mod = p_now->off_clutch + sid->spring_off_clutch; + p_now->overlap_shift = p_now->on_clutch + sid->spring_on_clutch; + // Solenoids + p_now->shift_sol_req = MAX((p_now->overlap_shift - centrifugal_force_on_clutch)/sid->SPC_GAIN, 0); + p_now->mod_sol_req = MAX( + ((p_now->overlap_shift - centrifugal_force_on_clutch) * sid->inf.pressure_multi_spc/sid->SPC_GAIN)+ + ((p_now->overlap_mod - centrifugal_force_off_clutch) * sid->inf.pressure_multi_mpc)+ + sid->inf.mpc_pressure_spring_reduction + , 0); + // Return condition + if (phase_elapsed > sid->prefill_info.fill_time) { + if (abs_input_torque > 150) { + this->do_high_filling = true; + } + ret = PHASE_PREFILL_2; + } + } else if (phase_id == PHASE_PREFILL_2) { + int wp_old_clutch = pm->find_working_pressure_for_clutch(sid->curr_g, sid->releasing, MAX(abs_input_torque, 30), false); + int targ_p = 0; + if (this->do_high_filling) { + targ_p = sid->prefill_info.low_fill_pressure_on_clutch; + } + // Clutches + p_now->on_clutch = interpolate_float(phase_elapsed, sid->prefill_info.fill_pressure_on_clutch, targ_p, 0, FILL_RAMP_TIME, InterpType::Linear); + p_now->off_clutch = wp_old_clutch; + // Overlap valves + p_now->overlap_mod = p_now->off_clutch + sid->spring_off_clutch; + p_now->overlap_shift = p_now->on_clutch + sid->spring_on_clutch; + // Solenoids + p_now->shift_sol_req = MAX((p_now->overlap_shift - centrifugal_force_on_clutch)/sid->SPC_GAIN, 0); + p_now->mod_sol_req = MAX( + ((p_now->overlap_shift - centrifugal_force_on_clutch) * sid->inf.pressure_multi_spc/sid->SPC_GAIN)+ + ((p_now->overlap_mod - centrifugal_force_off_clutch) * sid->inf.pressure_multi_mpc)+ + sid->inf.mpc_pressure_spring_reduction + , 0); + // Return condition + if (sid->ptr_r_clutch_speeds->off_clutch_speed > 100) { + // Jump to the overlap phase as filling has finished early! + ret = PHASE_OVERLAP; + } + if (phase_elapsed >= FILL_RAMP_TIME) { + ret = PHASE_PREFILL_3; + } + } else if (phase_id == PHASE_PREFILL_3) { + int wp_old_clutch = pm->find_working_pressure_for_clutch(sid->curr_g, sid->releasing, MAX(abs_input_torque, 30), false); + int targ_p = 0; + if (this->do_high_filling) { + targ_p = sid->prefill_info.low_fill_pressure_on_clutch; + } + // Clutches + p_now->on_clutch = targ_p; + p_now->off_clutch = wp_old_clutch; + // Overlap valves + p_now->overlap_mod = p_now->off_clutch + sid->spring_off_clutch; + p_now->overlap_shift = p_now->on_clutch + sid->spring_on_clutch; + // Solenoids + p_now->shift_sol_req = MAX((p_now->overlap_shift - centrifugal_force_on_clutch)/sid->SPC_GAIN, 0); + p_now->mod_sol_req = MAX( + ((p_now->overlap_shift - centrifugal_force_on_clutch) * sid->inf.pressure_multi_spc/sid->SPC_GAIN)+ + ((p_now->overlap_mod - centrifugal_force_off_clutch) * sid->inf.pressure_multi_mpc)+ + sid->inf.mpc_pressure_spring_reduction + , 0); + // Return condition + if (sid->ptr_r_clutch_speeds->off_clutch_speed > 100) { + // Jump to the overlap phase as filling has finished early! + ret = PHASE_OVERLAP; + } + if (phase_elapsed >= FILL_HOLD_TIME) { + ret = PHASE_OVERLAP; + } + } else if (phase_id == PHASE_OVERLAP) { + int wp_new_clutch = pm->find_working_pressure_for_clutch(sid->targ_g, sid->applying, abs_input_torque, false); + // Clutches + p_now->on_clutch = interpolate_float(phase_elapsed, sid->ptr_prev_pressures->on_clutch, wp_new_clutch, 0, 500, InterpType::Linear); + int torque_held_new_clutch = pm->calc_max_torque_for_clutch(sid->targ_g, sid->applying, p_now->on_clutch); + int torque_left = MAX(0, MAX(30, abs_input_torque) - torque_held_new_clutch); + p_now->off_clutch = pm->find_working_pressure_for_clutch(sid->curr_g, sid->releasing, torque_left, false); + // Overlap valves + p_now->overlap_mod = p_now->off_clutch + sid->spring_off_clutch; + p_now->overlap_shift = p_now->on_clutch + sid->spring_on_clutch; + // Solenoids + p_now->shift_sol_req = MAX((p_now->overlap_shift - centrifugal_force_on_clutch)/sid->SPC_GAIN, 0); + p_now->mod_sol_req = MAX( + ((p_now->overlap_shift - centrifugal_force_on_clutch) * sid->inf.pressure_multi_spc/sid->SPC_GAIN)+ + ((p_now->overlap_mod - centrifugal_force_off_clutch) * sid->inf.pressure_multi_mpc)+ + sid->inf.mpc_pressure_spring_reduction + , 0); + // Next phase once clutch is moving, or 500ms + if (phase_elapsed >= 500 || sid->ptr_r_clutch_speeds->off_clutch_speed > 100) { + ret = PHASE_MOMENTUM_RAMP; + } + } else if (phase_id == PHASE_MOMENTUM_RAMP) { + float inertia = ShiftHelpers::calcualte_abs_engine_inertia(sid->inf.map_idx, sd->engine_rpm, sd->input_rpm); + int targ_torque = abs_input_torque + inertia; + if (sd->pedal_pos < 10) { + if (is_upshift) { + targ_torque = MIN(inertia, abs_input_torque); + } else { + targ_torque += abs_input_torque; + } + } + int start_p = pm->find_working_pressure_for_clutch(sid->targ_g, sid->applying, abs_input_torque, false); + int wp_new_clutch = pm->find_working_pressure_for_clutch(sid->targ_g, sid->applying, targ_torque, false); + // Clutches + sid->ptr_w_pressures->on_clutch = interpolate_float(phase_elapsed, start_p, wp_new_clutch, 0, sid->chars.target_shift_time/4, InterpType::Linear); + sid->ptr_w_pressures->off_clutch = interpolate_float(phase_elapsed, sid->ptr_prev_pressures->off_clutch, 0, 0, sid->chars.target_shift_time/2, InterpType::Linear); + // Overlap valves + p_now->overlap_mod = p_now->off_clutch + sid->spring_off_clutch; + p_now->overlap_shift = p_now->on_clutch + sid->spring_on_clutch; + // Solenoids + p_now->shift_sol_req = MAX((p_now->overlap_shift - centrifugal_force_on_clutch)/sid->SPC_GAIN, 0); + p_now->mod_sol_req = ( + ((p_now->overlap_shift - centrifugal_force_on_clutch) * sid->inf.pressure_multi_spc / sid->SPC_GAIN) + + ((p_now->overlap_mod - centrifugal_force_off_clutch) * sid->inf.centrifugal_factor_off_clutch * sid->inf.pressure_multi_mpc) + + (-(sid->inf.pressure_multi_mpc*150)) + // 150 comes from calibration pointer on E55 coding (TODO - Locate in calibration and assign in CAL structure) + sid->inf.mpc_pressure_spring_reduction + ); + if (is_upshift && sd->static_torque > 100) { + if (this->torque_req_start_time == 0) { + this->torque_req_start_time = total_elapsed; + this->torque_at_req_start = sd->driver_requested_torque; + } + } + if (this->torque_req_start_time != 0) { + // Do down ramp + int into_req = total_elapsed- this->torque_req_start_time; + int reduction = interpolate_float(into_req, 0, inertia, 0, sid->chars.target_shift_time/2, InterpType::Linear); + sid->ptr_w_trq_req->amount = this->torque_at_req_start - reduction; + sid->ptr_w_trq_req->bounds = TorqueRequestBounds::LessThan; + sid->ptr_w_trq_req->ty = TorqueRequestControlType::NormalSpeed; + } + if (phase_elapsed >= sid->chars.target_shift_time/2 || sid->ptr_r_clutch_speeds->off_clutch_speed > sid->ptr_r_clutch_speeds->on_clutch_speed) { + ret = PHASE_MOMENTUM_HOLD; + } + } else if (phase_id == PHASE_MOMENTUM_HOLD) { + // TODO (here we should control the decent of the clutch) + // For now, just ramp up pressures if target has not been achieved + float inertia = ShiftHelpers::calcualte_abs_engine_inertia(sid->inf.map_idx, sd->engine_rpm, sd->input_rpm); + int targ_torque = abs_input_torque + inertia; + if (sd->pedal_pos < 10) { + if (is_upshift) { + targ_torque = MIN(inertia, abs_input_torque); + } else { + targ_torque += abs_input_torque; + } + } + float adder = 0; + if (phase_elapsed > sid->chars.target_shift_time/2) { + adder = interpolate_float(phase_elapsed-sid->chars.target_shift_time/2, 0, inertia, 0, sid->chars.target_shift_time/2, InterpType::Linear); + } + int wp_new_clutch = pm->find_working_pressure_for_clutch(sid->targ_g, sid->applying, targ_torque + adder, false); + // Clutches + sid->ptr_w_pressures->on_clutch = wp_new_clutch; + sid->ptr_w_pressures->off_clutch = 0; + // Overlap valves + p_now->overlap_mod = p_now->off_clutch + sid->spring_off_clutch; + p_now->overlap_shift = p_now->on_clutch + sid->spring_on_clutch; + // Solenoids + p_now->shift_sol_req = MAX((p_now->overlap_shift - centrifugal_force_on_clutch)/sid->SPC_GAIN, 0); + p_now->mod_sol_req = ( + ((p_now->overlap_shift - centrifugal_force_on_clutch) * sid->inf.pressure_multi_spc / sid->SPC_GAIN) + + ((p_now->overlap_mod - centrifugal_force_off_clutch) * sid->inf.centrifugal_factor_off_clutch * sid->inf.pressure_multi_mpc) + + (-(sid->inf.pressure_multi_mpc*150)) + // 150 comes from calibration pointer on E55 coding (TODO - Locate in calibration and assign in CAL structure) + sid->inf.mpc_pressure_spring_reduction + ); + + if (this->torque_req_start_time != 0) { + // Torque request hold + int into_req = total_elapsed - this->torque_req_start_time; + int reduction = interpolate_float(into_req, 0, inertia, 0, sid->chars.target_shift_time/2, InterpType::Linear); + sid->ptr_w_trq_req->amount = this->torque_at_req_start - reduction; + trq_req_amount_at_end = sid->ptr_w_trq_req->amount; + sid->ptr_w_trq_req->bounds = TorqueRequestBounds::LessThan; + sid->ptr_w_trq_req->ty = TorqueRequestControlType::NormalSpeed; + } + // Exit conditions + if (stationary && phase_elapsed > 1000) { // 1. Stationary shift (can't measure clutch speed) + ret = PHASE_MAX_PRESSURE; + } else if (sid->ptr_r_clutch_speeds->on_clutch_speed < 100) { + ret = PHASE_MAX_PRESSURE; + } + // SHIFT FAILURE (NO MOVEMENT!) + if (!stationary && phase_elapsed > 3000 && sid->ptr_r_clutch_speeds->off_clutch_speed < 100) { + ret = STEP_RES_FAILURE; + } + } else if (phase_id == PHASE_MAX_PRESSURE) { + // Max pressure phase. Pressures on the applied clutch are ramped up to ensure locking in 2 ramps. + int wp_new_clutch = pm->find_working_pressure_for_clutch(sid->targ_g, sid->applying, abs_input_torque, false); + ShiftPressures* p_prev = sid->ptr_prev_pressures; + // Clutches + p_now->on_clutch = interpolate_float(phase_elapsed, p_prev->on_clutch, wp_new_clutch, 0, sid->maxp_info.ramp_time, InterpType::Linear); + p_now->off_clutch = 0; + // Overlap valves + p_now->overlap_mod = p_now->off_clutch + sid->spring_off_clutch; + p_now->overlap_shift = interpolate_float(phase_elapsed, p_prev->overlap_shift, sid->MOD_MAX, 0, sid->maxp_info.ramp_time, InterpType::Linear); + // DIFFERS! - For mod_sol_req, use a different value for overlap_shift + int overlap_shift_mod_sol = MIN(p_now->overlap_shift, wp_new_clutch + sid->spring_on_clutch); + // Solenoids (important!) + p_now->shift_sol_req = p_now->overlap_shift / sid->SPC_GAIN; + p_now->mod_sol_req = ( + ((overlap_shift_mod_sol - centrifugal_force_on_clutch) * sid->inf.pressure_multi_spc / sid->SPC_GAIN) + + ((p_now->overlap_mod - centrifugal_force_off_clutch) * sid->inf.centrifugal_factor_off_clutch * sid->inf.pressure_multi_mpc) + + (-(sid->inf.pressure_multi_mpc*150)) + // 150 comes from calibration pointer on E55 coding (TODO - Locate in calibration and assign in CAL structure) + sid->inf.mpc_pressure_spring_reduction + ); + if (this->torque_req_start_time != 0) { + // Torque request ramp up + int duration = MIN(sid->chars.target_shift_time/2, 300); + int reduction = interpolate_float(phase_elapsed, this->trq_req_amount_at_end, sd->driver_requested_torque, 0, duration, InterpType::Linear); + sid->ptr_w_trq_req->amount = reduction; + sid->ptr_w_trq_req->bounds = TorqueRequestBounds::LessThan; + sid->ptr_w_trq_req->ty = TorqueRequestControlType::BackToDemandTorque; + if (sd->driver_requested_torque-10 < sid->ptr_w_trq_req->amount || phase_elapsed >= duration) { + // End request + sid->ptr_w_trq_req->ty = TorqueRequestControlType::None; + sid->ptr_w_trq_req->amount = 0; + this->torque_req_start_time = 0; + } + } + + if (phase_elapsed > sid->maxp_info.hold_time + sid->maxp_info.ramp_time && sid->ptr_w_trq_req->ty == TorqueRequestControlType::None) { + // Turn off the shift circuit! + pm->set_shift_circuit(sid->inf.shift_circuit, false); + ret = PHASE_END_CONTROL; + } + } else if (phase_id == PHASE_END_CONTROL) { + // Shift solenoid is off + int wp_gear = pm->find_working_mpc_pressure(sid->targ_g); + p_now->on_clutch = 0; + p_now->off_clutch = 0; + p_now->overlap_mod = 0; + p_now->overlap_shift = 0; + p_now->shift_sol_req = sid->MOD_MAX / sid->SPC_GAIN; + p_now->overlap_mod = interpolate_float(phase_elapsed, sid->ptr_prev_pressures->mod_sol_req, wp_gear, 0, 250, InterpType::Linear); + if (phase_elapsed > 250) { + ret = STEP_RES_END_SHIFT; + } + } else { + ret = STEP_RES_END_SHIFT; // WTF? Should never happen + } + return ret; +} + +uint8_t CrossoverShift::max_shift_stage_id() { + return PHASE_MAX_PRESSURE; +} \ No newline at end of file diff --git a/src/shifting_algo/shift_crossover.h b/src/shifting_algo/shift_crossover.h new file mode 100644 index 00000000..efcb09e5 --- /dev/null +++ b/src/shifting_algo/shift_crossover.h @@ -0,0 +1,29 @@ +#ifndef __SHIFT_CROSSOVER_H__ +#define __SHIFT_CROSSOVER_H__ + +#include "s_algo.h" + +class CrossoverShift : public ShiftingAlgorithm { +public: + CrossoverShift(ShiftInterfaceData* data); + ~CrossoverShift() override; + uint8_t step( + uint8_t phase_id, + uint16_t abs_input_torque, + bool stationary, + bool is_upshift, + uint16_t phase_elapsed, + uint16_t total_elapsed, + PressureManager* pm, + SensorData* sd + ) override; + + uint8_t max_shift_stage_id() override; +private: + bool do_high_filling = false; + uint16_t torque_req_start_time = 0; + int16_t torque_at_req_start = 0; + int16_t trq_req_amount_at_end = 0; +}; + +#endif \ No newline at end of file diff --git a/src/shifting_algo/shift_release.h b/src/shifting_algo/shift_release.h new file mode 100644 index 00000000..7ea98999 --- /dev/null +++ b/src/shifting_algo/shift_release.h @@ -0,0 +1,5 @@ +#ifndef __SHIFT_RELEASE_H__ +#define __SHIFT_RELEASE_H__ + + +#endif \ No newline at end of file diff --git a/src/shifting_algo/shifting_algo_helpers.cpp b/src/shifting_algo/shifting_algo_helpers.cpp new file mode 100644 index 00000000..cfe4a260 --- /dev/null +++ b/src/shifting_algo/shifting_algo_helpers.cpp @@ -0,0 +1,12 @@ +#include "s_algo.h" +#include "egs_calibration/calibration_structs.h" +#include "nvs/eeprom_config.h" + +float ShiftHelpers::calcualte_abs_engine_inertia(uint8_t shift_idx, uint16_t engine_rpm, uint16_t input_rpm) { + float min_factor = 1.0 / ((float)(MECH_PTR->intertia_factor[shift_idx])/1000.0); + float turbine_factor = (float)input_rpm / (float)engine_rpm; + float engine_inertia = (float)(VEHICLE_CONFIG.engine_drag_torque)/10.0; + 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); +} \ No newline at end of file