From de1757d7af2be2529a4c11dd651271273249739e Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sat, 17 Jun 2023 21:43:59 +0200
Subject: [PATCH] add option to disable gt safety angle slowdown

---
 hoverbrettctrl/platformio.ini |  3 +--
 hoverbrettctrl/src/main.cpp   | 43 +++++++++++++++++++++++------------
 2 files changed, 29 insertions(+), 17 deletions(-)

diff --git a/hoverbrettctrl/platformio.ini b/hoverbrettctrl/platformio.ini
index 9e86b7a..2973d44 100644
--- a/hoverbrettctrl/platformio.ini
+++ b/hoverbrettctrl/platformio.ini
@@ -27,5 +27,4 @@ lib_deps =
     adafruit/Adafruit BusIO@^1.11.3
     https://github.com/adafruit/Adafruit-GFX-Library
     arduino-libraries/SD@^1.2.4
-    mathertel/OneButton@^2.0.3
-    arduino-libraries/SD@^1.2.4
\ No newline at end of file
+    mathertel/OneButton@^2.0.3
\ No newline at end of file
diff --git a/hoverbrettctrl/src/main.cpp b/hoverbrettctrl/src/main.cpp
index fc7e50d..53a15e6 100644
--- a/hoverbrettctrl/src/main.cpp
+++ b/hoverbrettctrl/src/main.cpp
@@ -73,6 +73,13 @@ float steer_coefficient_nrf=0.5; // higher value == stronger
 float speed_coefficient_gt=1.0;  // higher value == stronger
 float steer_coefficient_gt=0.5;  // higher value == stronger
 
+#define GT_SAFETY_THRESHOLD_H 120  //above which value (abs) safety slowdown should start
+#define GT_SAFETY_THRESHOLD_V 120
+#define SAFETYMULTIPLIER_UPDATE_INTERVAL 100
+#define SAFETYMULTIPLIER_CHANGE_TIME_DOWN 2.0 //Time how long it should take to go from 100% to 0% when in safety slowdown
+#define SAFETYMULTIPLIER_CHANGE_TIME_UP 1.0
+bool gt_safety_enable=true;
+
 unsigned long watchdogtimer=0; //set to current millis everytime new good control input is calculated
 
 long last_adcupdated=0;
@@ -359,7 +366,6 @@ void loop() {
 
   if (loopmillis - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings
     
-      
     int raw_length_a=analogRead(PIN_GAMETRAK_LENGTH_A);
     int raw_length_b=analogRead(PIN_GAMETRAK_LENGTH_B);
     
@@ -367,7 +373,6 @@ void loop() {
 
     int raw_length=(raw_length_a+raw_length_b)/2;
 
-    
     uint16_t gt_length_1 = GT_LENGTH_1_OFFSET+raw_length*GT_LENGTH_1_SCALE;
     uint16_t gt_length_2 = GT_LENGTH_2_OFFSET+raw_length*GT_LENGTH_2_SCALE;
 
@@ -591,23 +596,23 @@ void loop() {
       }
     }
     
+    
     static float safetymultiplier=1.0; //value to reduce output speed if gametrack is pointing too far down or up (string might be behind vehicle)
-    #define GT_SAFETY_THRESHOLD_H 120  //above which value (abs) safety slowdown should start
-    #define GT_SAFETY_THRESHOLD_V 120
-    #define SAFETYMULTIPLIER_UPDATE_INTERVAL 100
-    #define SAFETYMULTIPLIER_CHANGE_TIME_DOWN 2.0 //Time how long it should take to go from 100% to 0% when in safety slowdown
-    #define SAFETYMULTIPLIER_CHANGE_TIME_UP 1.0
     static unsigned long last_safetymultiplier_update=0;
 
     
-    if (loopmillis-last_safetymultiplier_update > SAFETYMULTIPLIER_UPDATE_INTERVAL) {
-      if (abs(gt_vertical)>GT_SAFETY_THRESHOLD_V || abs(gt_horizontal)>GT_SAFETY_THRESHOLD_H) {
-        safetymultiplier-=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_DOWN;
-      }else{
-        safetymultiplier+=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_UP;
+    if (gt_safety_enable) {        
+      if (loopmillis-last_safetymultiplier_update > SAFETYMULTIPLIER_UPDATE_INTERVAL) {
+        if (abs(gt_vertical)>GT_SAFETY_THRESHOLD_V || abs(gt_horizontal)>GT_SAFETY_THRESHOLD_H) {
+          safetymultiplier-=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_DOWN;
+        }else{
+          safetymultiplier+=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_UP;
+        }
+        safetymultiplier=constrain(safetymultiplier,0.0,1.0);
+        last_safetymultiplier_update=loopmillis;
       }
-      safetymultiplier=constrain(safetymultiplier,0.0,1.0);
-      last_safetymultiplier_update=loopmillis;
+    }else{
+      safetymultiplier=1.0; //always 100% when disabled
     }
 
     //calculate speed l and r from speed and steer
@@ -891,6 +896,14 @@ void updateInputs(unsigned long loopmillis) {
               steer_coefficient_gt = constrain(steer_coefficient_gt, 0.05, 1.0);
             }
             break;
+          case 3: // gt_safety_enable
+            if (button_inc_click) {
+              gt_safety_enable=true;
+            }
+            if (button_dec_click) {
+              gt_safety_enable=false;
+            }
+            break;
 
         }
 
@@ -1109,7 +1122,7 @@ void display_show_menu() {
           display.print(F("steer_coeff_gt=")); display.println(steer_coefficient_gt,2);
           break;
         case 3:
-          
+          display.print(F("gt_safety_enable=")); display.println(gt_safety_enable);
           break;
         case 4: