From c129106ff4029b3d3a9cd129e86c534bff8451e2 Mon Sep 17 00:00:00 2001
From: CommanderRedYT <ccommanderred@gmail.com>
Date: Sat, 17 Sep 2022 17:38:53 +0200
Subject: [PATCH] Added iq and id

---
 main/can.cpp                                 | 12 ++++++++++++
 main/debugtexthelpers.h                      |  2 ++
 main/displays/menus/motorfeedbackdebugmenu.h |  2 ++
 3 files changed, 16 insertions(+)

diff --git a/main/can.cpp b/main/can.cpp
index bfc0fa7..b86e148 100644
--- a/main/can.cpp
+++ b/main/can.cpp
@@ -161,6 +161,18 @@ bool parseMotorControllerCanMessage(const twai_message_t &message, Controller &c
     case MotorController<isBack, true>::Feedback::Temp:
         controller.feedback.boardTemp = *((int16_t*)message.data);
         return true;
+    case MotorController<isBack, false>::Feedback::Id:
+        controller.feedback.left.id = *((int16_t*)message.data);
+        return true;
+    case MotorController<isBack, true>::Feedback::Id:
+        controller.feedback.right.id = *((int16_t*)message.data);
+        return true;
+    case MotorController<isBack, false>::Feedback::Iq:
+        controller.feedback.left.iq = *((int16_t*)message.data);
+        return true;
+    case MotorController<isBack, true>::Feedback::Iq:
+        controller.feedback.right.iq = *((int16_t*)message.data);
+        return true;
     }
 
     return false;
diff --git a/main/debugtexthelpers.h b/main/debugtexthelpers.h
index bdaa1a2..a57f9d4 100644
--- a/main/debugtexthelpers.h
+++ b/main/debugtexthelpers.h
@@ -77,6 +77,8 @@ private:
         struct DcPhaCFixedText : public virtual TextInterface { public: std::string text() const override { std::string line{"dcPhaC: "}; if (controller::get().feedbackValid) line += fmt::format("{:.2f}A", fixCurrent(MotorFeedbackGetter::get().dcPhaC)); return line; } };
         struct ChopsText : public virtual TextInterface { public: std::string text() const override { std::string line{"chops: "}; if (controller::get().feedbackValid) line += std::to_string(MotorFeedbackGetter::get().chops); return line; } };
         struct HallText : public virtual TextInterface { public: std::string text() const override { std::string line{"hall: "}; if (controller::get().feedbackValid) line += hallString(MotorFeedbackGetter::get()); return line; } };
+        struct IdText : public virtual TextInterface { public: std::string text() const override { std::string line{"id: "}; if (controller::get().feedbackValid) line += fmt::format("{:.2f}A", fixCurrent(MotorFeedbackGetter::get().id)); return line; } };
+        struct IqText : public virtual TextInterface { public: std::string text() const override { std::string line{"iq: "}; if (controller::get().feedbackValid) line += fmt::format("{:.2f}A", fixCurrent(MotorFeedbackGetter::get().iq)); return line; } };
     };
 
 public:
diff --git a/main/displays/menus/motorfeedbackdebugmenu.h b/main/displays/menus/motorfeedbackdebugmenu.h
index 303ebdd..9b67f35 100644
--- a/main/displays/menus/motorfeedbackdebugmenu.h
+++ b/main/displays/menus/motorfeedbackdebugmenu.h
@@ -35,6 +35,8 @@ public:
         constructMenuItem<makeComponent<MenuItem, typename Ttexts::DcPhaCFixedText,  ColorInterface<TFT_DARKGREY>, DummyAction>>();
         constructMenuItem<makeComponent<MenuItem, typename Ttexts::ChopsText,        ColorInterface<TFT_DARKGREY>, DummyAction>>();
         constructMenuItem<makeComponent<MenuItem, typename Ttexts::HallText,         ColorInterface<TFT_DARKGREY>, DummyAction>>();
+        constructMenuItem<makeComponent<MenuItem, typename Ttexts::IqText,           ColorInterface<TFT_DARKGREY>, DummyAction>>();
+        constructMenuItem<makeComponent<MenuItem, typename Ttexts::IdText,           ColorInterface<TFT_DARKGREY>, DummyAction>>();
         constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_BACK>,             PopScreenAction, StaticMenuItemIcon<&espgui::icons::back>>>();
     }