From 684f5f393f120835c44e1094144c0426982b6cd2 Mon Sep 17 00:00:00 2001
From: Ashcon Mohseninia <ashconm@outlook.com>
Date: Fri, 16 Aug 2024 08:51:00 +0100
Subject: [PATCH] EGS51. Freeze driver torque before request

---
 src/canbus/can_egs51.cpp | 11 ++++++++++-
 src/canbus/can_egs51.h   |  1 +
 2 files changed, 11 insertions(+), 1 deletion(-)

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();