From 7569498bcbf9b8e4b4ce203b08130fc9d437df67 Mon Sep 17 00:00:00 2001
From: Dado Mista <dadomista@gmail.com>
Date: Sun, 8 Dec 2024 19:27:14 -0800
Subject: [PATCH] Haptic feedback for high voltage intensifies after 3 seconds

Also, HV tiltback now delayed by 5 seconds instead of just 0.5
---
 src/haptic_feedback.c | 22 ++++++++++++++++------
 src/haptic_feedback.h |  3 ++-
 src/main.c            |  6 +++---
 3 files changed, 21 insertions(+), 10 deletions(-)

diff --git a/src/haptic_feedback.c b/src/haptic_feedback.c
index 745cfbe..ecd634a 100644
--- a/src/haptic_feedback.c
+++ b/src/haptic_feedback.c
@@ -46,8 +46,9 @@ static HapticFeedbackType state_to_haptic_type(const State *state) {
     case SAT_PB_TEMPERATURE:
         return HAPTIC_FEEDBACK_ERROR_TEMPERATURE;
     case SAT_PB_LOW_VOLTAGE:
+        return HAPTIC_FEEDBACK_ERROR_LO_VOLTAGE;
     case SAT_PB_HIGH_VOLTAGE:
-        return HAPTIC_FEEDBACK_ERROR_VOLTAGE;
+        return HAPTIC_FEEDBACK_ERROR_HI_VOLTAGE;
     default:
         return HAPTIC_FEEDBACK_NONE;
     }
@@ -64,7 +65,8 @@ static uint8_t get_beats(HapticFeedbackType type) {
         return 0;
     case HAPTIC_FEEDBACK_ERROR_TEMPERATURE:
         return 6;
-    case HAPTIC_FEEDBACK_ERROR_VOLTAGE:
+    case HAPTIC_FEEDBACK_ERROR_LO_VOLTAGE:
+    case HAPTIC_FEEDBACK_ERROR_HI_VOLTAGE:
         return 8;
     case HAPTIC_FEEDBACK_NONE:
         break;
@@ -79,7 +81,8 @@ static const CfgHapticTone *get_haptic_tone(const HapticFeedback *hf) {
     case HAPTIC_FEEDBACK_DUTY_CONTINUOUS:
         return &hf->cfg->duty;
     case HAPTIC_FEEDBACK_ERROR_TEMPERATURE:
-    case HAPTIC_FEEDBACK_ERROR_VOLTAGE:
+    case HAPTIC_FEEDBACK_ERROR_LO_VOLTAGE:
+    case HAPTIC_FEEDBACK_ERROR_HI_VOLTAGE:
         return &hf->cfg->error;
     case HAPTIC_FEEDBACK_NONE:
         break;
@@ -145,9 +148,16 @@ void haptic_feedback_update(
         float speed = VESC_IF->mc_get_speed();
         const CfgHapticTone *tone = get_haptic_tone(hf);
         if (tone->strength > 0.0f) {
-            VESC_IF->foc_play_tone(
-                0, tone->frequency, scale_strength(tone->strength, hf->cfg, speed)
-            );
+            float strength = scale_strength(tone->strength, hf->cfg, speed);
+            if (type_to_play == HAPTIC_FEEDBACK_ERROR_HI_VOLTAGE) {
+                float duration = current_time - hf->start_time;
+                if (duration > 3) {
+                    // more than 3 seconds in HV? Start scaling up for next 2 seconds
+                    // up to 50% louder
+                    strength = strength * fminf(1.5, duration * 0.25 + 0.25);
+                }
+            }
+            VESC_IF->foc_play_tone(0, tone->frequency, strength);
         }
 
         if (hf->cfg->vibrate.strength > 0.0f) {
diff --git a/src/haptic_feedback.h b/src/haptic_feedback.h
index c8c6d77..7d32b21 100644
--- a/src/haptic_feedback.h
+++ b/src/haptic_feedback.h
@@ -27,7 +27,8 @@ typedef enum {
     HAPTIC_FEEDBACK_DUTY,
     HAPTIC_FEEDBACK_DUTY_CONTINUOUS,
     HAPTIC_FEEDBACK_ERROR_TEMPERATURE,
-    HAPTIC_FEEDBACK_ERROR_VOLTAGE,
+    HAPTIC_FEEDBACK_ERROR_LO_VOLTAGE,
+    HAPTIC_FEEDBACK_ERROR_HI_VOLTAGE,
 } HapticFeedbackType;
 
 typedef struct {
diff --git a/src/main.c b/src/main.c
index 5bd9b8a..813bd36 100644
--- a/src/main.c
+++ b/src/main.c
@@ -755,9 +755,9 @@ static void calculate_setpoint_target(data *d) {
     } else if (d->motor.duty_cycle > 0.05 && input_voltage > d->float_conf.tiltback_hv) {
         d->beep_reason = BEEP_HV;
         beep_alert(d, 3, false);
-        if (((d->current_time - d->tb_highvoltage_timer) > .5) ||
-            (input_voltage > d->float_conf.tiltback_hv + 1)) {
-            // 500ms have passed or voltage is another volt higher, time for some tiltback
+        if ((d->current_time - d->tb_highvoltage_timer) > 5) {
+            // It is assumed that haptic feedback is enabled!
+            // 5s have passed or voltage is another volt higher, time for some tiltback
             if (d->motor.erpm > 0) {
                 d->setpoint_target = d->float_conf.tiltback_hv_angle;
             } else {