ATR: Account for ATR Thresholds in Boost margin

Fix: High ATR Thresholds no longer make Transition Boost harder to trigger

Author: NicoAleman

src/atr.c

@@ -26,6 +26,8 @@ void atr_init(ATR *atr) {
     atr->on_step_size = 0.0f;
     atr->off_step_size = 0.0f;
     atr->speed_boost_mult = 0.0f;
+    atr->tt_boost_uphill_margin = 0.0f;
+    atr->tt_boost_downhill_threshold = 0.0f;
     atr_reset(atr);
 }
 
@@ -48,6 +50,12 @@ void atr_configure(ATR *atr, const RefloatConfig *config) {
         // most +6000 for 100% speed boost
         atr->speed_boost_mult = 1.0f / ((fabsf(config->atr_speed_boost) - 0.4f) * 5000 + 3000.0f);
     }
+
+    // If ATR Thresholds are in place, they should be negated from the Transition Boost margin
+    // for the effective condition (change in accel_diff needed) to be the same
+    atr->tt_boost_uphill_margin =
+        fmaxf(0, 2 - config->atr_threshold_down - config->atr_threshold_up);
+    atr->tt_boost_downhill_threshold = fmaxf(0, 3 - config->atr_threshold_down);
 }
 
 void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config) {
@@ -140,20 +148,18 @@ void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config) {
     // When Transition Boost condition is met, use the highest allowed step size
     float atr_transition_step_size = fmaxf(atr_tb_step_size, atr_on_step_size);
 
-    const float TT_BOOST_UPHILL_MARGIN = 2;
-    const float TT_BOOST_DOWNHILL_THRESHOLD = 3;
-
     bool should_transition_boost;
     if (atr->target * atr->setpoint >= 0) {
         // If Target and Setpoint are of same sign or 0, we are not transitioning
         should_transition_boost = false;
     } else if (forward ? (atr->setpoint < 0 && atr->target > 0)
                        : (atr->setpoint > 0 && atr->target < 0)) {
         // Transitioning Downhill to Uphill: check margin between target and setpoint
-        should_transition_boost = fabsf(atr->target - atr->setpoint) >= TT_BOOST_UPHILL_MARGIN;
+        should_transition_boost =
+            fabsf(atr->target - atr->setpoint) >= atr->tt_boost_uphill_margin;
     } else {
         // Transitioning Uphill to Downhill: check target magnitude (stricter condition)
-        should_transition_boost = fabsf(atr->target) >= TT_BOOST_DOWNHILL_THRESHOLD;
+        should_transition_boost = fabsf(atr->target) >= atr->tt_boost_downhill_threshold;
     }
 
     // Winding Down: Setpoint is moving toward 0

src/atr.h

@@ -25,6 +25,8 @@ typedef struct {
     float on_step_size;
     float off_step_size;
     float speed_boost_mult;
+    float tt_boost_uphill_margin;
+    float tt_boost_downhill_threshold;
 
     float accel_diff;
     float speed_boost;