From 3479b5eb8dc5983a62c9d7b6d06342401441b674 Mon Sep 17 00:00:00 2001
From: 3djc <lesitewebdejc@hotmail.com>
Date: Sat, 30 Jan 2021 12:57:04 +0100
Subject: [PATCH] Add trims autoswitch

---
 radio/src/datastructs.h |  5 +++++
 radio/src/switches.cpp  | 11 +++++++++++
 2 files changed, 16 insertions(+)

diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h
index b37dd3e55..400cbcec7 100644
--- a/radio/src/datastructs.h
+++ b/radio/src/datastructs.h
@@ -502,6 +502,11 @@ PACK(struct ModelHeader {
   MODEL_HEADER_BITMAP_FIELD
 });
 
+#if NUM_TRIMS_KEYS <= 8
+typedef uint8_t trimstate_t;
+#else
+typedef uint16_t trimstate_t;
+#endif
 #if defined(COLORLCD)
 typedef uint32_t swconfig_t;
 typedef uint32_t swarnstate_t;
diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp
index 9a7fbb2f8..56324b2fa 100644
--- a/radio/src/switches.cpp
+++ b/radio/src/switches.cpp
@@ -514,6 +514,7 @@ void evalLogicalSwitches(bool isCurrentFlightmode)
 }
 
 swarnstate_t switches_states = 0;
+trimstate_t trims_states = 0;
 swsrc_t getMovedSwitch()
 {
   static tmr10ms_t s_move_last_time = 0;
@@ -532,6 +533,16 @@ swsrc_t getMovedSwitch()
       }
     }
   }
+  // Trim switches
+  for (int i = 0; i < NUM_TRIMS_KEYS; i++) {
+    trimstate_t mask = ((trimstate_t) 0x01 << i);
+    uint8_t prev = (trims_states & mask) >> i;
+    uint8_t next = getSwitch(SWSRC_FIRST_TRIM + i);
+    if (prev != next) {
+      trims_states = (trims_states & (~mask)) | ((trimstate_t) next << i);
+      result = SWSRC_FIRST_TRIM + i;
+    }
+  }
   // Multipos
   for (int i = 0; i < NUM_XPOTS; i++) {
     if (IS_POT_MULTIPOS(POT1 + i)) {