diff --git a/src/canbus/can_egs51.cpp b/src/canbus/can_egs51.cpp index 11ba3216..89a644ba 100644 --- a/src/canbus/can_egs51.cpp +++ b/src/canbus/can_egs51.cpp @@ -126,7 +126,16 @@ int Egs51Can::get_static_engine_torque(const uint32_t expire_time_ms) { int Egs51Can::get_driver_engine_torque(const uint32_t expire_time_ms) { // Don't think EGS51 supports this so just run with static torque for now - return this->get_static_engine_torque(expire_time_ms); + int tmp; + if (this->gs218.TORQUE_REQ_EN) { // Active torque request (Frozen value for now) + tmp = this->torque_before_request; + } else { // No torque request (Unfreeze the torque value) + tmp = this->get_static_engine_torque(expire_time_ms); + this->torque_before_request = tmp; + } + + + return tmp; } int Egs51Can::get_maximum_engine_torque(const uint32_t expire_time_ms) { diff --git a/src/canbus/can_egs51.h b/src/canbus/can_egs51.h index 86c94fde..41ee5d56 100644 --- a/src/canbus/can_egs51.h +++ b/src/canbus/can_egs51.h @@ -98,6 +98,7 @@ class Egs51Can: public EgsBaseCan { void tx_frames() override; void on_rx_frame(uint32_t id, uint8_t dlc, uint64_t data, uint32_t timestamp) override; private: + int16_t torque_before_request=0; // CAN Frames to Tx GS_218_EGS51 gs218 = {0}; ECU_MS51 ms51 = ECU_MS51();