diff --git a/src/adaptation/adapt_map.cpp b/src/adaptation/adapt_map.cpp index db5a3057..9a6e892d 100644 --- a/src/adaptation/adapt_map.cpp +++ b/src/adaptation/adapt_map.cpp @@ -300,18 +300,18 @@ void AdaptationMap::perform_adaptation(SensorData* sensors, ProfileGearChange ch bool accel_shift = sensors->d_output_rpm > 0; bool idle_shift = sensors->static_torque > 0; - if (response.spc_map_start - response.spc_change_start > 20) { // SPC is taking too long to bite so reduce it || Gearbox flared so needs more SPC + if (response.spc_map_start - response.spc_change_start > 50) { // SPC is taking too long to bite so reduce it || Gearbox flared so needs more SPC if (idle_shift) { if (accel_shift) { - this->adapt_data[adaptation_idx].offset_accel_idle -= 10; // +1% pressure + this->adapt_data[adaptation_idx].offset_accel_idle -= 5; // +1% pressure } else { - this->adapt_data[adaptation_idx].offset_decel_idle -= 10; // +1% pressure + this->adapt_data[adaptation_idx].offset_decel_idle -= 5; // +1% pressure } } else { if (accel_shift) { - this->adapt_data[adaptation_idx].offset_accel_load -= 5; // +1% pressure + this->adapt_data[adaptation_idx].offset_accel_load -= 10; // +1% pressure } else { - this->adapt_data[adaptation_idx].offset_decel_load -= 5; // +1% pressure + this->adapt_data[adaptation_idx].offset_decel_load -= 7; // +1% pressure } } } else if (response.spc_map_start - response.spc_change_start < 10) { // SPC tolorance is too tight, decrease initial SPC pressure diff --git a/src/canbus/can_egs52.cpp b/src/canbus/can_egs52.cpp index e4eb625b..0c294d39 100644 --- a/src/canbus/can_egs52.cpp +++ b/src/canbus/can_egs52.cpp @@ -270,11 +270,13 @@ PaddlePosition Egs52Can::get_paddle_position(uint64_t now, uint64_t expire_time_ return PaddlePosition::Plus; case SBW_232h_LRT_PM3::MINUS: return PaddlePosition::Minus; + case SBW_232h_LRT_PM3::SNV: + return PaddlePosition::SNV; default: return PaddlePosition::None; } } else { - return PaddlePosition::None; + return PaddlePosition::SNV; } } diff --git a/src/canbus/can_hal.h b/src/canbus/can_hal.h index d9c6a28f..497db82a 100644 --- a/src/canbus/can_hal.h +++ b/src/canbus/can_hal.h @@ -17,7 +17,7 @@ enum class WheelDirection { Forward, // Wheel going forwards Reverse, // Wheel going backwards Stationary, // Stationary (Not forward or backwards) - SignalNotAvaliable // SNV + SignalNotAvaliable = 0xFF // SNV }; struct WheelData { @@ -44,7 +44,7 @@ enum class SystemStatusCheck { enum class EngineType { Diesel, Petrol, - Unknown + Unknown = 0xFF }; enum class TorqueRequest { @@ -63,14 +63,15 @@ enum class GearboxGear { Neutral = 9, Reverse_First = 10, Reverse_Second = 11, - SignalNotAvaliable = 12, + SignalNotAvaliable = 0xFF, }; enum class PaddlePosition { None, Plus, Minus, - PlusAndMinus + PlusAndMinus, + SNV = 0xFF }; enum class ClutchStatus { @@ -106,7 +107,7 @@ enum class ShifterPosition { D, PLUS, MINUS, - SignalNotAvaliable // SNV + SignalNotAvaliable = 0xFF // SNV }; enum class SolenoidName { @@ -130,7 +131,7 @@ enum class GearboxDisplayGear { Four, Five, Failure, - SNA, + SNA = 0xFF, }; enum class GearboxMessage { diff --git a/src/common_structs.h b/src/common_structs.h index 00ce2f42..e76fb166 100644 --- a/src/common_structs.h +++ b/src/common_structs.h @@ -101,13 +101,16 @@ enum class ProfileGearChange { typedef struct { /// The initial SPC PWM value to start the shift off. This should be the 'biting point' of the clutch packs uint16_t initial_spc_pwm; - /// The intial MPC PWM value to start the shift off. This should allow for easy SPC movement, but not - /// too little otherwise the gearbox will slip too much - uint16_t mpc_pwm; - /// Target time in milliseconds for the shift to complete - uint16_t targ_d_rpm; - // Shift speed factor. Valid range = 1 - 10 (Auto clamped if value is outside this range) - Higher = faster shift - float shift_speed; + /// The intial MPC PWM value to start the shift off. + /// This must ALWAYS be more or equal to initial_spc_pwm, otherwise the box will hydralically + /// perform an ABORT shift + uint16_t initial_mpc_pwm; + /// SPC Ramp speed, denotes the speed of which SPC pressure will increase during the shift + float spc_dec_speed; + /// MPC Ramp speed, denotes the speed of which MPC pressure will increase during the shift + /// IMPORTANT: This value must ALWAYS be less than spc_dec_speed, otherwise the box will + /// fail to shift! + float mpc_dec_speed; /// The shift solenoid required to change gears Solenoid* shift_solenoid; /// Current gear the gearbox is in as an integer @@ -117,7 +120,7 @@ typedef struct { } ShiftData; #pragma GCC diagnostic ignored "-Wmissing-field-initializers" // This is ALWAYS correctly initialized in pressure_manager.cpp -const ShiftData DEFAULT_SHIFT_DATA = { .initial_spc_pwm = 100, .mpc_pwm = 100, .targ_d_rpm = 50, .shift_speed = 5.0}; +const ShiftData DEFAULT_SHIFT_DATA = { .initial_spc_pwm = 100, .initial_mpc_pwm = 100, .spc_dec_speed = 5.0, .mpc_dec_speed = 3.0}; typedef struct { /** diff --git a/src/diag/diag_data.cpp b/src/diag/diag_data.cpp index 28d86ce1..144505c5 100644 --- a/src/diag/diag_data.cpp +++ b/src/diag/diag_data.cpp @@ -1,5 +1,6 @@ #include "diag_data.h" #include "sensors.h" +#include "solenoids/solenoids.h" DATA_GEARBOX_SENSORS get_gearbox_sensors() { DATA_GEARBOX_SENSORS ret = {}; @@ -8,11 +9,11 @@ DATA_GEARBOX_SENSORS get_gearbox_sensors() { if (!Sensors::read_input_rpm(&d, false)) { ret.n2_rpm = 0xFFFF; ret.n3_rpm = 0xFFFF; - ret.calc_rpm = 0xFFFF; + ret.calculated_rpm = 0xFFFF; } else { ret.n2_rpm = d.n2_raw; ret.n3_rpm = d.n3_raw; - ret.calc_rpm = d.calc_rpm; + ret.calculated_rpm = d.calc_rpm; } if(!Sensors::read_atf_temp(&ret.atf_temp_c)) { ret.atf_temp_c = 0xFFFF; @@ -25,6 +26,49 @@ DATA_GEARBOX_SENSORS get_gearbox_sensors() { } else { ret.parking_lock = 0xFF; } - ret.rli = RLI_GEARBOX_SENSORS; + return ret; +} + +DATA_SOLENOIDS get_solenoid_data() { + DATA_SOLENOIDS ret = {}; + + ret.mpc_current = sol_mpc->get_current_estimate(); + ret.spc_current = sol_spc->get_current_estimate(); + ret.tcc_current = sol_tcc->get_current_estimate(); + ret.y3_current = sol_y3->get_current_estimate(); + ret.y4_current = sol_y4->get_current_estimate(); + ret.y5_current = sol_y5->get_current_estimate(); + + ret.mpc_pwm = sol_mpc->get_pwm(); + ret.spc_pwm = sol_spc->get_pwm(); + ret.tcc_pwm = sol_tcc->get_pwm(); + ret.y3_pwm = sol_y3->get_pwm(); + ret.y4_pwm = sol_y4->get_pwm(); + ret.y5_pwm = sol_y5->get_pwm(); + + return ret; +} + +DATA_CANBUS_RX get_rx_can_data(AbstractCan* can_layer) { + DATA_CANBUS_RX ret = {}; + uint64_t now = esp_timer_get_time() / 1000; + + WheelData t = can_layer->get_rear_left_wheel(now, 250); + ret.left_rear_rpm = t.current_dir == WheelDirection::SignalNotAvaliable ? 0xFFFF : t.double_rpm; + t = can_layer->get_rear_right_wheel(now, 250); + ret.right_rear_rpm = t.current_dir == WheelDirection::SignalNotAvaliable ? 0xFFFF : t.double_rpm; + + ret.paddle_position = can_layer->get_paddle_position(now, 250); + ret.pedal_pos = can_layer->get_pedal_value(now, 250); + + int torque = 0xFFFF; + torque = can_layer->get_maximum_engine_torque(now, 250); + ret.max_torque = torque == INT_MAX ? 0xFFFF : (torque + 500)*4; + torque = can_layer->get_minimum_engine_torque(now, 250); + ret.min_torque = torque == INT_MAX ? 0xFFFF : (torque + 500)*4; + torque = can_layer->get_static_engine_torque(now, 250); + ret.static_torque = torque == INT_MAX ? 0xFFFF : (torque + 500)*4; + ret.shift_button_pressed = can_layer->get_profile_btn_press(now, 250); + ret.shifter_position = can_layer->get_shifter_position_ewm(now, 250); return ret; } \ No newline at end of file diff --git a/src/diag/diag_data.h b/src/diag/diag_data.h index ec1730f4..29b9501a 100644 --- a/src/diag/diag_data.h +++ b/src/diag/diag_data.h @@ -2,6 +2,7 @@ #define __DIAG_DATA_H__ #include +#include "canbus/can_hal.h" // Diagnostic data IDs and data structures // used by the KWP2000 server on the TCM @@ -12,19 +13,52 @@ // The following IDs are already taken by the OEM EGS52/53 module // 0F,30-60,7A,A0,B0,B1,C0-C4,D1 -#define RLI_GEARBOX_SENSORS 0x20 +#define RLI_GEARBOX_SENSORS 0x20 // Sensor data status +#define RLI_SOLENOID_STATUS 0x21 // Solenoid data status +#define RLI_CAN_DATA_DUMP 0x22 // Gearbox brain logic status // Gearbox sensor struct typedef struct { - uint8_t rli; // MUST FOR KWP DEF uint16_t n2_rpm; // Raw N2 RPM uint16_t n3_rpm; // Raw N3 RPM - uint16_t calc_rpm; // Calculated input RPM (From N2 and N3) + uint16_t calculated_rpm; // Calculated input RPM (From N2 and N3) uint16_t v_batt; // Battery voltage (mV) int atf_temp_c; // ATF Temp (Celcius) uint8_t parking_lock; // Parking lock (1 for Engaged, 0 for disengaged) } __attribute__ ((packed)) DATA_GEARBOX_SENSORS; +// Solenoid command struct +typedef struct { + uint16_t spc_pwm; + uint16_t mpc_pwm; + uint16_t tcc_pwm; + uint16_t y3_pwm; + uint16_t y4_pwm; + uint16_t y5_pwm; + uint16_t spc_current; + uint16_t mpc_current; + uint16_t tcc_current; + uint16_t y3_current; + uint16_t y4_current; + uint16_t y5_current; +} __attribute__ ((packed)) DATA_SOLENOIDS; + +// Solenoid command struct +typedef struct { + uint8_t pedal_pos; + uint16_t min_torque; + uint16_t max_torque; + uint16_t static_torque; + uint16_t left_rear_rpm; + uint16_t right_rear_rpm; + uint8_t shift_button_pressed; + ShifterPosition shifter_position; + PaddlePosition paddle_position; +} __attribute__ ((packed)) DATA_CANBUS_RX; + DATA_GEARBOX_SENSORS get_gearbox_sensors(); +DATA_SOLENOIDS get_solenoid_data(); +DATA_CANBUS_RX get_rx_can_data(AbstractCan* can_layer); + #endif // __DIAG_DATA_H__ \ No newline at end of file diff --git a/src/diag/kwp2000.cpp b/src/diag/kwp2000.cpp index b92c7805..255c0c85 100644 --- a/src/diag/kwp2000.cpp +++ b/src/diag/kwp2000.cpp @@ -137,7 +137,7 @@ void Kwp2000_server::make_diag_neg_msg(uint8_t sid, uint8_t nrc) { this->send_resp = true; } -void Kwp2000_server::make_diag_pos_msg(uint8_t sid, uint8_t* resp, uint16_t len) { +void Kwp2000_server::make_diag_pos_msg(uint8_t sid, const uint8_t* resp, uint16_t len) { this->tx_msg.id = KWP_ECU_TX_ID; this->tx_msg.data_size = len+1; this->tx_msg.data[0] = sid+0x40; @@ -145,6 +145,20 @@ void Kwp2000_server::make_diag_pos_msg(uint8_t sid, uint8_t* resp, uint16_t len) this->send_resp = true; } +void Kwp2000_server::make_diag_pos_msg(uint8_t sid, uint8_t pid, const uint8_t* resp, uint16_t len) { + this->tx_msg.id = KWP_ECU_TX_ID; + this->tx_msg.data_size = len+2; + this->tx_msg.data[0] = sid+0x40; + this->tx_msg.data[1] = pid; + memcpy(&this->tx_msg.data[2], resp, len); + this->send_resp = true; +} + +int Kwp2000_server::allocate_routine_args(uint8_t* src, uint8_t arg_len) { + free(this->running_routine_args); + return 0; +} + void Kwp2000_server::server_loop() { this->send_resp = false; while(1) { @@ -296,35 +310,61 @@ void Kwp2000_server::process_read_ecu_ident(uint8_t* args, uint16_t arg_len) { esp_ota_get_partition_description(running, &running_info); if (args[0] == 0x86) { ECU_Date date = fw_date_to_bcd(running_info.date); - uint8_t daimler_ident_data[17]; - memset(daimler_ident_data, 0x00, 17); + uint8_t daimler_ident_data[16]; + memset(daimler_ident_data, 0x00, 16); // Part number - daimler_ident_data[1] = 0x01; - daimler_ident_data[2] = 0x23; - daimler_ident_data[3] = 0x45; - daimler_ident_data[4] = 0x67; - daimler_ident_data[5] = 0x89; + daimler_ident_data[0] = 0x01; + daimler_ident_data[1] = 0x23; + daimler_ident_data[2] = 0x45; + daimler_ident_data[3] = 0x67; + daimler_ident_data[4] = 0x89; // ECU Hardware date - daimler_ident_data[6] = date.week; - daimler_ident_data[7] = date.year; + daimler_ident_data[5] = date.week; + daimler_ident_data[6] = date.year; // ECU Software date - daimler_ident_data[8] = date.week; - daimler_ident_data[9] = date.year; - daimler_ident_data[10] = SUPPLIER_ID; - daimler_ident_data[11] = DIAG_VARIANT_CODE >> 8; - daimler_ident_data[12] = DIAG_VARIANT_CODE & 0xFF; - daimler_ident_data[14] = date.year; - daimler_ident_data[15] = date.month; - daimler_ident_data[16] = date.day; - make_diag_pos_msg(SID_READ_ECU_IDENT, daimler_ident_data, 17); - return; - } else if (args[0] == 0x88) { - // VIN (Original) - Let this be partition name - make_diag_pos_msg(SID_READ_ECU_IDENT, (uint8_t*)running->label, 17); - return; + daimler_ident_data[7] = date.week; + daimler_ident_data[8] = date.year; + daimler_ident_data[9] = SUPPLIER_ID; + daimler_ident_data[10] = DIAG_VARIANT_CODE >> 8; + daimler_ident_data[11] = DIAG_VARIANT_CODE & 0xFF; + daimler_ident_data[13] = date.year; + daimler_ident_data[14] = date.month; + daimler_ident_data[15] = date.day; + make_diag_pos_msg(SID_READ_ECU_IDENT, 0x86, daimler_ident_data, 16); + } else if (args[0] == 0x87) { // Daimler and Mitsubishi compatible identification + ECU_Date date = fw_date_to_bcd(running_info.date); + uint8_t ident_data[19]; + memset(ident_data, 0x00, 19); + ident_data[0] = 0x00; // TODO ECU origin + ident_data[1] = SUPPLIER_ID; + ident_data[2] = DIAG_VARIANT_CODE >> 8; + ident_data[3] = DIAG_VARIANT_CODE & 0xFF; + ident_data[5] = 0x00;// HW version + ident_data[6] = 0x00;// HW version + ident_data[7] = date.day;// SW version + ident_data[8] = date.month;// SW version + ident_data[9] = date.year;// SW version + ident_data[10] = '0'; // Part number to end + ident_data[11] = '1'; // Part number to end + ident_data[12] = '2'; // Part number to end + ident_data[13] = '3'; // Part number to end + ident_data[14] = '4'; // Part number to end + ident_data[15] = '5'; // Part number to end + ident_data[16] = '6'; // Part number to end + ident_data[17] = '7'; // Part number to end + ident_data[18] = '8'; // Part number to end + ident_data[19] = '9'; // Part number to end + make_diag_pos_msg(SID_READ_ECU_IDENT, 0x87, ident_data, 19); + } else if (args[0] == 0x88) { // VIN original + make_diag_pos_msg(SID_READ_ECU_IDENT, 0x88, (const uint8_t*)"ULTIMATENAG52ESP0", 17); + } else if (args[0] == 0x89) { // Diagnostic variant code + int d = DIAG_VARIANT_CODE; + make_diag_pos_msg(SID_READ_ECU_IDENT, 0x89, (const uint8_t*)d, 4); + } else if (args[0] == 0x90) { // VIN current + make_diag_pos_msg(SID_READ_ECU_IDENT, 0x90, (const uint8_t*)"ULTIMATENAG52ESP0", 17); + } else { + make_diag_neg_msg(SID_READ_ECU_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT); } - - make_diag_neg_msg(SID_READ_ECU_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT); } void Kwp2000_server::process_read_data_local_ident(uint8_t* args, uint16_t arg_len) { @@ -332,20 +372,27 @@ void Kwp2000_server::process_read_data_local_ident(uint8_t* args, uint16_t arg_l make_diag_neg_msg(SID_READ_DATA_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT); return; } - if (args[0] >= 0x80 && args[1] <= 0x9F) { // ECU Ident + if (args[0] >= 0x80 && args[0] <= 0x9F) { // ECU Ident this->process_read_ecu_ident(args, arg_len); // Modify the SID byte in pos/neg response to be SID_READ_DATA_LOCAL_IDENT if(this->tx_msg.data[0] == 0x7F) { this->tx_msg.data[1] = SID_READ_DATA_LOCAL_IDENT; } else { this->tx_msg.data[0] = SID_READ_DATA_LOCAL_IDENT+0x40; } - } - if (args[0] == RLI_GEARBOX_SENSORS) { + } else if (args[0] == 0xE1) { // ECU Serial number + make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, 0xE1, (const uint8_t*)"ULTIMATENAG52", 14); + } else if (args[0] == RLI_GEARBOX_SENSORS) { DATA_GEARBOX_SENSORS r = get_gearbox_sensors(); - make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, (uint8_t*)&r, sizeof(DATA_GEARBOX_SENSORS)); - return; + make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_GEARBOX_SENSORS, (uint8_t*)&r, sizeof(DATA_GEARBOX_SENSORS)); + } else if (args[0] == RLI_SOLENOID_STATUS) { + DATA_SOLENOIDS r = get_solenoid_data(); + make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_SOLENOID_STATUS, (uint8_t*)&r, sizeof(DATA_SOLENOIDS)); + } else if (args[0] == RLI_CAN_DATA_DUMP) { + DATA_CANBUS_RX r = get_rx_can_data(egs_can_hal); + make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_CAN_DATA_DUMP, (uint8_t*)&r, sizeof(DATA_CANBUS_RX)); + } else { + make_diag_neg_msg(SID_READ_DATA_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT); } - make_diag_neg_msg(SID_READ_DATA_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT); } void Kwp2000_server::process_read_data_ident(uint8_t* args, uint16_t arg_len) { @@ -415,6 +462,13 @@ void Kwp2000_server::process_start_routine_by_local_ident(uint8_t* args, uint16_ } else { make_diag_neg_msg(SID_START_ROUTINE_BY_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT); } + } else if (arg_len == 3) { + if (args[0] == ROUTINE_SOLENOID_TEST) { + // Args[1] -> Freq/10 + // Args[2] -> Time/10 + } else { + make_diag_neg_msg(SID_START_ROUTINE_BY_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT); + } } else { make_diag_neg_msg(SID_START_ROUTINE_BY_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT); } @@ -482,7 +536,6 @@ void Kwp2000_server::process_response_on_event(uint8_t* args, uint16_t arg_len) } - void Kwp2000_server::run_solenoid_test() { this->gearbox_ptr->diag_inhibit_control(); ESP_LOGI("RT_SOL_TEST", "Starting solenoid test"); diff --git a/src/diag/kwp2000.h b/src/diag/kwp2000.h index 304d2a27..27d9b8af 100644 --- a/src/diag/kwp2000.h +++ b/src/diag/kwp2000.h @@ -23,7 +23,17 @@ #define DIAG_VARIANT_CODE 0x0251 // DiagVersion51_EGS52 #endif +#define PROCESSOR_TYPE +#define COMM_MATRIX_VERSION 0x0101 // 01.01 +#define CAN_DRIVER_VERSION 0x0101 // 01.01 +#define NM_VERSION 0x0101 // 01.01 +#define KWP_MOD_VERSION 0x0001 // 00.01 +#define TP_LAYER_VERSION 0x0001 // 00.01 +#define DBKOM_VERSION 0x0001 // 00.01 +#define FLEXER_VERSION 0x9999 // 99.99 (unsupported) + #define ROUTINE_SOLENOID_TEST 0xDE +#define ROUTINE_SPEAKER_TEST 0xDF class Kwp2000_server { public: @@ -46,11 +56,15 @@ class Kwp2000_server { TaskHandle_t routine_task; uint8_t routine_id = 0x00; uint8_t routine_result[255]; + uint8_t* running_routine_args; uint8_t routine_results_len = 0; + CpuStats cpu_usage; bool send_resp; bool reboot_pending; + int allocate_routine_args(uint8_t* src, uint8_t arg_len); + void process_start_diag_session(uint8_t* args, uint16_t arg_len); void process_ecu_reset(uint8_t* args, uint16_t arg_len); void process_clear_diag_info(uint8_t* args, uint16_t arg_len); @@ -79,16 +93,22 @@ class Kwp2000_server { void process_response_on_event(uint8_t* args, uint16_t arg_len); void make_diag_neg_msg(uint8_t sid, uint8_t nrc); - void make_diag_pos_msg(uint8_t sid, uint8_t* resp, uint16_t len); + void make_diag_pos_msg(uint8_t sid, const uint8_t* resp, uint16_t len); + void make_diag_pos_msg(uint8_t sid, uint8_t pid, const uint8_t* resp, uint16_t len); static void launch_solenoid_test(void *_this) { static_cast(_this)->run_solenoid_test(); } + static void launch_speaker_test(void *_this) { + static_cast(_this)->run_solenoid_test(); + } + void run_solenoid_test(); Gearbox* gearbox_ptr; AbstractCan* can_layer; + xTaskHandle* running_routine; }; #endif //_KWP_H__ \ No newline at end of file diff --git a/src/gearbox.cpp b/src/gearbox.cpp index 95a24a02..55760f6d 100644 --- a/src/gearbox.cpp +++ b/src/gearbox.cpp @@ -234,19 +234,24 @@ ShiftResponse Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfil SensorData pre_shift = this->sensor_data; ESP_LOGI("ELAPSE_SHIFT", "Shift started!"); ShiftData sd = pressure_mgr->get_shift_data(&this->sensor_data, req_lookup, profile->get_shift_characteristics(req_lookup, &this->sensor_data)); - CLAMP(sd.shift_speed, 1, 10); // Ensure shift speed is a valid amount + CLAMP(sd.spc_dec_speed, 1, 10); // Ensure shift speed is a valid amount + CLAMP(sd.mpc_dec_speed, 1, 10); // Ensure shift speed is a valid amount + if (sd.mpc_dec_speed < sd.spc_dec_speed) { + sd.mpc_dec_speed = sd.spc_dec_speed * 0.9; + } float curr_spc_pwm = sd.initial_spc_pwm; + float curr_mpc_pwm = sd.initial_mpc_pwm; + int start_torque = sensor_data.static_torque; // Save this for later + int limited_torque = start_torque; + if (sensor_data.static_torque > 0 && is_upshift) { + egs_can_hal->set_torque_request(TorqueRequest::Minimum); + limited_torque = max(0, (int)(sensor_data.static_torque*sd.torque_cut_multiplier)); + egs_can_hal->set_requested_torque(limited_torque); + } this->tcc->on_shift_start(sensor_data.current_timestamp_ms, !is_upshift, &this->sensor_data); - sol_mpc->write_pwm_percent_with_voltage(sd.mpc_pwm, this->sensor_data.voltage); + sol_mpc->write_pwm_percent_with_voltage(curr_mpc_pwm, this->sensor_data.voltage); sd.shift_solenoid->write_pwm_percent_with_voltage(1000, this->sensor_data.voltage); // Start shifting sol_spc->write_pwm_percent_with_voltage(curr_spc_pwm, this->sensor_data.voltage); // Open SPC - if (sensor_data.static_torque > 50 && is_upshift) { - egs_can_hal->set_torque_request(TorqueRequest::Minimum); - egs_can_hal->set_requested_torque(max(55, (int)(sensor_data.static_torque*0.6))); - } else if (sensor_data.static_torque > MAX_TORQUE_RATING_NM/2 && !is_upshift) { - egs_can_hal->set_torque_request(TorqueRequest::Minimum); - egs_can_hal->set_requested_torque(MAX_TORQUE_RATING_NM/2); - } uint32_t elapsed = 0; // Counter for shift timing // Init counters for Input RPM measuring and ratio measuring @@ -273,14 +278,14 @@ ShiftResponse Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfil if (ratio_now > start_ratio+20) { // Upshift - Ratio should get smaller so inverse means flaring) this->flaring = true; flared = true; - } else if (ratio_now < start_ratio-25){ + } else if (ratio_now < sd.sip_threshold){ shift_in_progress = true; } } else { if (ratio_now < start_ratio-20) { // Downshift - Ratio should get larger so inverse means flaring this->flaring = true; flared = true; - } else if (ratio_now > start_ratio+25) { + } else if (ratio_now > sd.sip_threshold) { shift_in_progress = true; } } @@ -298,10 +303,20 @@ ShiftResponse Gearbox::elapse_shift(ProfileGearChange req_lookup, AbstractProfil // Cannot monitor, so set flare flag to false this->flaring = false; } - sol_mpc->write_pwm_percent_with_voltage(sd.mpc_pwm, this->sensor_data.voltage); - if (curr_spc_pwm > sd.shift_speed) { - curr_spc_pwm -= sd.shift_speed; + // Try increasing MPC PWM too to stop flaring + if (!shift_in_progress) { + if (curr_mpc_pwm > sd.mpc_dec_speed) { + curr_mpc_pwm -= sd.mpc_dec_speed; + } + if (curr_spc_pwm > sd.spc_dec_speed) { + curr_spc_pwm -= sd.spc_dec_speed; + } + } else { + if (curr_spc_pwm > sd.spc_dec_speed*0.75) { + curr_spc_pwm -= sd.spc_dec_speed*0.75; + } } + sol_mpc->write_pwm_percent_with_voltage(curr_mpc_pwm, this->sensor_data.voltage); sol_spc->write_pwm_percent_with_voltage(curr_spc_pwm, this->sensor_data.voltage); // Open SPC // Unlock torque reqest once box has started to change if (shift_in_progress) { diff --git a/src/pressure_manager.cpp b/src/pressure_manager.cpp index 245b180d..6d5bedd8 100644 --- a/src/pressure_manager.cpp +++ b/src/pressure_manager.cpp @@ -38,7 +38,6 @@ inline uint16_t locate_pressure_map_value(const pressure_map map, int percent) { else if (percent >= 100) { return map[10]; } else { int min = percent/10; - int max = min+1; float dy = map[max] - map[min]; float dx = (max-min)*10; @@ -53,8 +52,9 @@ uint16_t find_spc_pressure(const pressure_map map, SensorData* sensors) { uint16_t find_mpc_pressure(const pressure_map map, SensorData* sensors) { // MPC reacts to Torque (Also sets pressure for SPC. Shift firmness can be increased) - int load = sensors->static_torque*100/MAX_TORQUE_RATING_NM; - if (load < 0) { load *= -0.25; } // Pulling engine + //int load = sensors->static_torque*100/MAX_TORQUE_RATING_NM; + //if (load < 0) { load *= -0.25; } // Pulling engine + int load = (sensors->pedal_pos*100/250); // Try same as SPC // For now forget about shift_firmness (TODO work on shift_firmness 1-10 to 1.1-0.9) return locate_pressure_map_value(map, load); } @@ -68,57 +68,76 @@ ShiftData PressureManager::get_shift_data(SensorData* sensors, ProfileGearChange switch (shift_request) { case ProfileGearChange::ONE_TWO: sd.initial_spc_pwm = find_spc_pressure(spc_1_2, this->sensor_data); - sd.mpc_pwm = find_mpc_pressure(mpc_1_2, this->sensor_data); + sd.initial_mpc_pwm = find_mpc_pressure(mpc_1_2, this->sensor_data); sd.targ_g = 2; sd.curr_g = 1; sd.shift_solenoid = sol_y3; + sd.torque_cut_multiplier = TC_1_2; + sd.sip_threshold = SIP_1_2; break; case ProfileGearChange::TWO_THREE: sd.initial_spc_pwm = find_spc_pressure(spc_2_3, this->sensor_data); - sd.mpc_pwm = find_mpc_pressure(mpc_2_3, this->sensor_data); + sd.initial_mpc_pwm = find_mpc_pressure(mpc_2_3, this->sensor_data); sd.targ_g = 3; sd.curr_g = 2; sd.shift_solenoid = sol_y5; + sd.torque_cut_multiplier = TC_2_3; + sd.sip_threshold = SIP_2_3; break; case ProfileGearChange::THREE_FOUR: sd.initial_spc_pwm = find_spc_pressure(spc_3_4, this->sensor_data); - sd.mpc_pwm = find_mpc_pressure(mpc_3_4, this->sensor_data); + sd.initial_mpc_pwm = find_mpc_pressure(mpc_3_4, this->sensor_data); sd.targ_g = 4; sd.curr_g = 3; sd.shift_solenoid = sol_y4; + sd.torque_cut_multiplier = TC_3_4; + sd.sip_threshold = SIP_3_4; break; case ProfileGearChange::FOUR_FIVE: sd.initial_spc_pwm = find_spc_pressure(spc_4_5, this->sensor_data); - sd.mpc_pwm = find_mpc_pressure(mpc_4_5, this->sensor_data); + sd.initial_mpc_pwm = find_mpc_pressure(mpc_4_5, this->sensor_data); sd.targ_g = 5; sd.curr_g = 4; sd.shift_solenoid = sol_y3; + sd.torque_cut_multiplier = TC_4_5; + sd.sip_threshold = SIP_4_5; break; case ProfileGearChange::FIVE_FOUR: sd.initial_spc_pwm = find_spc_pressure(spc_5_4, this->sensor_data); - sd.mpc_pwm = find_mpc_pressure(mpc_5_4, this->sensor_data); + sd.initial_mpc_pwm = find_mpc_pressure(mpc_5_4, this->sensor_data); sd.targ_g = 4; sd.curr_g = 5; sd.shift_solenoid = sol_y3; + sd.torque_cut_multiplier = 1.00; + sd.sip_threshold = SIP_5_4; break; case ProfileGearChange::FOUR_THREE: sd.initial_spc_pwm = find_spc_pressure(spc_4_3, this->sensor_data); - sd.mpc_pwm = find_mpc_pressure(mpc_4_3, this->sensor_data); + sd.initial_mpc_pwm = find_mpc_pressure(mpc_4_3, this->sensor_data); sd.shift_solenoid = sol_y4; + sd.torque_cut_multiplier = 1.00; + sd.sip_threshold = SIP_4_3; break; case ProfileGearChange::THREE_TWO: sd.initial_spc_pwm = find_spc_pressure(spc_3_2, this->sensor_data); - sd.mpc_pwm = find_mpc_pressure(mpc_3_2, this->sensor_data); + sd.initial_mpc_pwm = find_mpc_pressure(mpc_3_2, this->sensor_data); sd.targ_g = 2; sd.curr_g = 3; sd.shift_solenoid = sol_y5; + sd.torque_cut_multiplier = 1.00; + sd.sip_threshold = SIP_3_2; break; case ProfileGearChange::TWO_ONE: sd.initial_spc_pwm = find_spc_pressure(spc_2_1, this->sensor_data); - sd.mpc_pwm = find_mpc_pressure(mpc_2_1, this->sensor_data); + sd.initial_mpc_pwm = find_mpc_pressure(mpc_2_1, this->sensor_data); sd.targ_g = 1; sd.curr_g = 2; sd.shift_solenoid = sol_y3; + sd.torque_cut_multiplier = 1.00; + sd.sip_threshold = SIP_2_1; break; } - sd.targ_d_rpm = chars.target_d_rpm; - sd.shift_speed = chars.shift_speed * find_temp_ramp_speed_multiplier(sensors->atf_temp); + sd.spc_dec_speed = chars.shift_speed * find_temp_ramp_speed_multiplier(sensors->atf_temp); if (this->adapt_map != nullptr) { sd.initial_spc_pwm += adapt_map->get_adaptation_offset(sensors, shift_request); } + if (sd.targ_g < sd.curr_g) { + sd.spc_dec_speed *= 0.75; // Make downshifting a little smoother + } + sd.mpc_dec_speed = sd.spc_dec_speed/2; // For now return sd; } diff --git a/src/pressure_manager.h b/src/pressure_manager.h index 0710e80b..69c39886 100644 --- a/src/pressure_manager.h +++ b/src/pressure_manager.h @@ -89,6 +89,26 @@ const pressure_map mpc_5_4 = {500, 480, 460, 440, 420, 400, 360, 360, 350, 340, #endif +// These values are autogenerated by the compiler. Please do NOT modify! +// The gearbox uses these values to work out when to stop torque cutting and monitoring shift progress + +#define SHIFT_IN_PROGRESS_THRESH 25 // 25% + +const int SIP_1_2 = ((RAT_1*(100+SHIFT_IN_PROGRESS_THRESH))+(RAT_2*(100-SHIFT_IN_PROGRESS_THRESH)))/2; +const int SIP_2_3 = ((RAT_2*(100+SHIFT_IN_PROGRESS_THRESH))+(RAT_3*(100-SHIFT_IN_PROGRESS_THRESH)))/2; +const int SIP_3_4 = ((RAT_3*(100+SHIFT_IN_PROGRESS_THRESH))+(RAT_4*(100-SHIFT_IN_PROGRESS_THRESH)))/2; +const int SIP_4_5 = ((RAT_4*(100+SHIFT_IN_PROGRESS_THRESH))+(RAT_5*(100-SHIFT_IN_PROGRESS_THRESH)))/2; + +const int SIP_2_1 = ((RAT_1*(100-SHIFT_IN_PROGRESS_THRESH))+(RAT_2*(100+SHIFT_IN_PROGRESS_THRESH)))/2; +const int SIP_3_2 = ((RAT_2*(100-SHIFT_IN_PROGRESS_THRESH))+(RAT_3*(100+SHIFT_IN_PROGRESS_THRESH)))/2; +const int SIP_4_3 = ((RAT_3*(100-SHIFT_IN_PROGRESS_THRESH))+(RAT_4*(100+SHIFT_IN_PROGRESS_THRESH)))/2; +const int SIP_5_4 = ((RAT_4*(100-SHIFT_IN_PROGRESS_THRESH))+(RAT_5*(100+SHIFT_IN_PROGRESS_THRESH)))/2; + +const float TC_1_2 = (RAT_2/RAT_1); +const float TC_2_3 = (RAT_3/RAT_2); +const float TC_3_4 = (RAT_4/RAT_3); +const float TC_4_5 = (RAT_5/RAT_4); + const float pressure_temp_normalizer[17] = { 0.7, 0.72, 0.75, 0.78, 0.84, // -40-0C (0-40) 0.89, 0.93, 0.96, 0.98, 0.99, // 10-50C (50-90) diff --git a/src/sensors.cpp b/src/sensors.cpp index 24d4da05..8bd7be30 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -9,8 +9,7 @@ #include "esp_log.h" #include "pins.h" -#define PULSES_PER_REV 60 // N2 and N3 are 60 pulses per revolution -#define RPM_AVERAGE_SAMPLES 5 +#define PULSES_PER_REV 120 // N2 and N3 are 60 pulses per revolution #define PCNT_H_LIM PULSES_PER_REV * 10 #define LOG_TAG "SENSORS" @@ -32,7 +31,7 @@ const pcnt_config_t pcnt_cfg_n2 { .lctrl_mode = PCNT_MODE_KEEP, .hctrl_mode = PCNT_MODE_KEEP, .pos_mode = PCNT_COUNT_INC, - .neg_mode = PCNT_COUNT_DIS, + .neg_mode = PCNT_COUNT_INC, .counter_h_lim = PCNT_H_LIM, .counter_l_lim = 0, .unit = PCNT_N2_RPM, @@ -45,26 +44,13 @@ const pcnt_config_t pcnt_cfg_n3 { .lctrl_mode = PCNT_MODE_KEEP, .hctrl_mode = PCNT_MODE_KEEP, .pos_mode = PCNT_COUNT_INC, - .neg_mode = PCNT_COUNT_DIS, + .neg_mode = PCNT_COUNT_INC, .counter_h_lim = PCNT_H_LIM, .counter_l_lim = 0, .unit = PCNT_N3_RPM, .channel = PCNT_CHANNEL_0 }; -struct RpmSampleData { - uint64_t readings[RPM_AVERAGE_SAMPLES]; - uint8_t sample_id; - uint64_t sum; -}; - -void IRAM_ATTR add_value_to_rpm(volatile RpmSampleData* sample, uint64_t reading) { - sample->sum -= sample->readings[sample->sample_id]; - sample->readings[sample->sample_id] = reading; - sample->sum += reading; - sample->sample_id = (sample->sample_id+1) % RPM_AVERAGE_SAMPLES; -} - typedef struct { // Voltage in mV uint16_t v; @@ -77,28 +63,28 @@ const static temp_reading_t atf_temp_lookup[NUM_TEMP_POINTS] = { // mV, Temp(x10) // mV Values are calibrated on 3.45V rail // as that is how much the ATF sensor power gets - {495, -400}, - {513, -300}, - {526, -200}, - {537, -100}, - {553, 0}, - {569, 100}, - {585, 200}, - {608, 300}, - {625, 400}, - {643, 500}, - {663, 600}, - {683, 700}, - {707, 800}, - {731, 900}, - {755, 1000}, - {779, 1100}, - {803, 1200}, - {827, 1300}, - {851, 1400}, - {875, 1500}, - {899, 1600}, - {923, 1700} + {446, -400}, + {461, -300}, + {476, -200}, + {491, -100}, + {507, 0}, + {523, 100}, + {540, 200}, + {557, 300}, + {574, 400}, + {592, 500}, + {610, 600}, + {629, 700}, + {648, 800}, + {669, 900}, + {690, 1000}, + {711, 1100}, + {732, 1200}, + {755, 1300}, + {778, 1400}, + {802, 1500}, + {814, 1600}, + {851, 1700} }; #define ADC_CHANNEL_VBATT adc2_channel_t::ADC2_CHANNEL_8 @@ -117,16 +103,8 @@ portMUX_TYPE n3_mux = portMUX_INITIALIZER_UNLOCKED; uint64_t n2_overflow = 0; uint64_t n3_overflow = 0; -volatile RpmSampleData n2_rpm = { - .readings = {0}, - .sample_id = 0, - .sum = 0 -}; -volatile RpmSampleData n3_rpm = { - .readings = {0}, - .sample_id = 0, - .sum = 0 -}; +volatile uint64_t n2_rpm = 0; +volatile uint64_t n3_rpm = 0; static intr_handle_t rpm_timer_handle; static void IRAM_ATTR RPM_TIMER_ISR(void* arg) { @@ -138,7 +116,7 @@ static void IRAM_ATTR RPM_TIMER_ISR(void* arg) { pcnt_counter_clear(PCNT_N3_RPM); t += (n3_overflow * PCNT_H_LIM); n3_overflow = 0; - add_value_to_rpm(&n3_rpm, t); + n3_rpm = t; portEXIT_CRITICAL_ISR(&n3_mux); portENTER_CRITICAL_ISR(&n2_mux); @@ -146,7 +124,7 @@ static void IRAM_ATTR RPM_TIMER_ISR(void* arg) { pcnt_counter_clear(PCNT_N2_RPM); t += (n2_overflow * PCNT_H_LIM); n2_overflow = 0; - add_value_to_rpm(&n2_rpm, t); + n2_rpm = t; portEXIT_CRITICAL_ISR(&n2_mux); } @@ -232,8 +210,8 @@ bool Sensors::init_sensors(){ } bool Sensors::read_input_rpm(RpmReading* dest, bool check_sanity) { - dest->n2_raw = ((float)n2_rpm.sum * (float)PULSE_MULTIPLIER / (float)RPM_AVERAGE_SAMPLES); - dest->n3_raw = ((float)n3_rpm.sum * (float)PULSE_MULTIPLIER / (float)RPM_AVERAGE_SAMPLES); + dest->n2_raw = ((float)n2_rpm * (float)PULSE_MULTIPLIER * 0.5); // *0.5 as counting both Inc and Dec pulse dir + dest->n3_raw = ((float)n3_rpm * (float)PULSE_MULTIPLIER * 0.5); // *0.5 as counting both Inc and Dec pulse dir //ESP_LOGI("RPM", "N2 %d, N3 %d", dest->n2_raw, dest->n3_raw); if (dest->n2_raw < 10 && dest->n3_raw < 10) { // Stationary, break here to avoid divideBy0Ex dest->calc_rpm = 0; diff --git a/src/torque_converter.cpp b/src/torque_converter.cpp index 38fc9e83..67b54895 100644 --- a/src/torque_converter.cpp +++ b/src/torque_converter.cpp @@ -37,7 +37,7 @@ void TorqueConverter::update(GearboxGear curr_gear, PressureManager* pm, Abstrac if (this->was_idle && this->curr_tcc_pwm > 50) { this->was_idle = false; - this->curr_tcc_pwm -= 50; // Faster response for TCC + this->curr_tcc_pwm *= 0.5; // Faster response for TCC } if (!is_shifting) { @@ -55,6 +55,10 @@ void TorqueConverter::update(GearboxGear curr_gear, PressureManager* pm, Abstrac TccLockupBounds bounds = profile->get_tcc_lockup_bounds(sensors, curr_gear); max_allowed_slip = bounds.max_slip_rpm; min_allowed_slip = bounds.min_slip_rpm; + if (sensors->output_rpm > 1900) { + max_allowed_slip /= 2; // Always + min_allowed_slip /= 2; // Always + } } if (sensors->tcc_slip_rpm > max_allowed_slip) { @@ -62,12 +66,12 @@ void TorqueConverter::update(GearboxGear curr_gear, PressureManager* pm, Abstrac this->curr_tcc_pwm += MAX(1, (sensors->pedal_pos/11)) * pm->get_tcc_temp_multiplier(sensors->atf_temp); } else if (sensors->tcc_slip_rpm < min_allowed_slip && this->curr_tcc_pwm >= 2 && sensors->pedal_pos > 10) { // Decrease pressure, but only if we have pedal input - this->curr_tcc_pwm -= 1 * pm->get_tcc_temp_multiplier(sensors->atf_temp); + this->curr_tcc_pwm -= 0.5 * pm->get_tcc_temp_multiplier(sensors->atf_temp); } } int tcc_offset = 0; if (this->curr_tcc_pwm != 0 ) { - tcc_offset = 300; + tcc_offset = 250; } if (is_shifting && tcc_offset != 0 && !this->inhibit_increase) { tcc_offset += sol_mpc->get_pwm() / 20;