diff --git a/firmware/controllers/algo/rusefi_enums.h b/firmware/controllers/algo/rusefi_enums.h index 262a42fffd..9d8a24280d 100644 --- a/firmware/controllers/algo/rusefi_enums.h +++ b/firmware/controllers/algo/rusefi_enums.h @@ -699,7 +699,8 @@ enum class TsCalMode : uint8_t { enum class GearControllerMode : uint8_t { None = 0, ButtonShift = 1, - Generic = 2, + Automatic = 2, + Generic = 3, }; enum class TransmissionControllerMode : uint8_t { diff --git a/firmware/controllers/controllers.mk b/firmware/controllers/controllers.mk index f7c9dab069..a01b351c0b 100644 --- a/firmware/controllers/controllers.mk +++ b/firmware/controllers/controllers.mk @@ -63,6 +63,7 @@ CONTROLLERS_SRC_CPP = \ $(CONTROLLERS_DIR)/tcu/tcu.cpp \ $(CONTROLLERS_DIR)/tcu/gear_controller.cpp \ $(CONTROLLERS_DIR)/tcu/gc_generic.cpp \ + $(CONTROLLERS_DIR)/tcu/gc_auto.cpp \ $(CONTROLLERS_DIR)/tcu/simple_tcu.cpp \ $(CONTROLLERS_DIR)/tcu/tc_4l6x.cpp \ $(CONTROLLERS_DIR)/shutdown_controller.cpp \ diff --git a/firmware/controllers/tcu/gc_auto.cpp b/firmware/controllers/tcu/gc_auto.cpp new file mode 100644 index 0000000000..a798357103 --- /dev/null +++ b/firmware/controllers/tcu/gc_auto.cpp @@ -0,0 +1,68 @@ +#include "pch.h" + +#include "gc_auto.h" + +#if EFI_TCU +AutomaticGearController automaticGearController; + +AutomaticGearController::AutomaticGearController() { +} + +void AutomaticGearController::init() { + for (size_t i = 0; i < efi::size(engineConfiguration->tcu_rangeInput); i++) { + if (isBrainPinValid(engineConfiguration->tcu_rangeInput[i])) { + efiSetPadMode("Range Input", engineConfiguration->tcu_rangeInput[i], getInputMode(engineConfiguration->tcu_rangeInputMode[i])); + } + } + + GearControllerBase::init(); +} + +void AutomaticGearController::update() { + auto tps = Sensor::get(SensorType::DriverThrottleIntent); + auto vss = Sensor::get(SensorType::VehicleSpeed); + + if (getDesiredGear() == NEUTRAL) { + setDesiredGear(GEAR_1); + } + + if (tps.Valid && vss.Valid) { + switch (getDesiredGear()) { + case GEAR_1 : + shift(vss.Value, tps.Value, &config->tcu_shiftSpeed12, GEAR_2); + break; + case GEAR_2 : + shift(vss.Value, tps.Value, &config->tcu_shiftSpeed21, GEAR_1, true); + shift(vss.Value, tps.Value, &config->tcu_shiftSpeed23, GEAR_3); + break; + case GEAR_3 : + shift(vss.Value, tps.Value, &config->tcu_shiftSpeed32, GEAR_2, true); + shift(vss.Value, tps.Value, &config->tcu_shiftSpeed34, GEAR_4); + break; + case GEAR_4 : + shift(vss.Value, tps.Value, &config->tcu_shiftSpeed43, GEAR_3, true); + break; + default : + break; + } + } + + GearControllerBase::update(); +} + +void AutomaticGearController::shift(float speed, float throttle, uint8_t (*curve)[TCU_TABLE_WIDTH], gear_e gear) { + shift(speed, throttle, curve, gear, false); +} + +void AutomaticGearController::shift(float speed, float throttle, uint8_t (*curve)[TCU_TABLE_WIDTH], gear_e gear, bool down) { + int curveSpeed = interpolate2d(throttle, config->tcu_shiftTpsBins, *curve); + + if ((down && speed < curveSpeed) || (!down && speed > curveSpeed)) { + setDesiredGear(gear); + } +} + +AutomaticGearController* getAutomaticGearController() { + return &automaticGearController; +} +#endif // EFI_TCU diff --git a/firmware/controllers/tcu/gc_auto.h b/firmware/controllers/tcu/gc_auto.h new file mode 100644 index 0000000000..4b755023b9 --- /dev/null +++ b/firmware/controllers/tcu/gc_auto.h @@ -0,0 +1,22 @@ +#pragma once + +#include "gear_controller.h" + +#if EFI_TCU +class AutomaticGearController: public GearControllerBase { +public: + AutomaticGearController(); + + void update(); + void init(); + void findDesiredGear(); + GearControllerMode getMode() const { + return GearControllerMode::Automatic; + } +private: + void shift(float speed, float throttle, uint8_t (*curve)[TCU_TABLE_WIDTH], gear_e gear); + void shift(float speed, float throttle, uint8_t (*curve)[TCU_TABLE_WIDTH], gear_e gear, bool down); +}; + +AutomaticGearController* getAutomaticGearController(); +#endif // EFI_TCU diff --git a/firmware/controllers/tcu/gc_generic.cpp b/firmware/controllers/tcu/gc_generic.cpp index 55fdee1bf3..edc12b07b9 100644 --- a/firmware/controllers/tcu/gc_generic.cpp +++ b/firmware/controllers/tcu/gc_generic.cpp @@ -126,9 +126,8 @@ void GenericGearController::update() { } break; case SelectedGear::Drive : - // TODO - // for Drive, need shift tables etc. - break; + AutomaticGearController::update(); + return; default: break; } diff --git a/firmware/controllers/tcu/gc_generic.h b/firmware/controllers/tcu/gc_generic.h index 4e5aabf82f..0a39f68347 100644 --- a/firmware/controllers/tcu/gc_generic.h +++ b/firmware/controllers/tcu/gc_generic.h @@ -1,9 +1,10 @@ #pragma once #include "gear_controller.h" +#include "gc_auto.h" #if EFI_TCU -class GenericGearController: public GearControllerBase { +class GenericGearController: public AutomaticGearController { public: GenericGearController(); diff --git a/firmware/integration/rusefi_config.txt b/firmware/integration/rusefi_config.txt index a2045b8012..52edfd348a 100644 --- a/firmware/integration/rusefi_config.txt +++ b/firmware/integration/rusefi_config.txt @@ -1494,7 +1494,7 @@ uint8_t[TORQUE_CURVE_SIZE x TORQUE_CURVE_SIZE] autoscale torqueTable;;"Nm", 10, uint16_t[TORQUE_CURVE_SIZE] torqueRpmBins;;"RPM", 1, 0, 0, 65000, 0 uint16_t[TORQUE_CURVE_SIZE] torqueLoadBins;;"Load", 1, 0, 0, 65000, 0 -#define gear_controller_e_enum "None", "Button Shift", "Generic" +#define gear_controller_e_enum "None", "Button Shift", "Automatic", "Generic" custom GearControllerMode 1 bits, U08, @OFFSET@, [0:1], @@gear_controller_e_enum@@ GearControllerMode gearControllerMode