4 files changed, 91 insertions, 6 deletions

diff --git a/drivers/haptic/DRV2605L.c b/drivers/haptic/DRV2605L.c
index f74c965253..728554b01e 100644
--- a/drivers/haptic/DRV2605L.c
+++ b/drivers/haptic/DRV2605L.c
@@ -114,6 +114,17 @@ void DRV_init(void) {
     DRV_write(DRV_GO, 0x01);
 }
 
+void DRV_rtp_init(void) {
+  DRV_write(DRV_GO, 0x00);
+  DRV_write(DRV_RTP_INPUT, 20); //20 is the lowest value I've found where haptics can still be felt. 
+  DRV_write(DRV_MODE, 0x05); 
+  DRV_write(DRV_GO, 0x01);
+}
+
+void DRV_amplitude(uint8_t amplitude) {
+  DRV_write(DRV_RTP_INPUT, amplitude);
+}
+
 void DRV_pulse(uint8_t sequence) {
     DRV_write(DRV_GO, 0x00);
     DRV_write(DRV_WAVEFORM_SEQ_1, sequence);
diff --git a/drivers/haptic/DRV2605L.h b/drivers/haptic/DRV2605L.h
index f550b859f7..535c777658 100644
--- a/drivers/haptic/DRV2605L.h
+++ b/drivers/haptic/DRV2605L.h
@@ -170,6 +170,8 @@
 void    DRV_init(void);
 void    DRV_write(const uint8_t drv_register, const uint8_t settings);
 uint8_t DRV_read(const uint8_t regaddress);
+void    DRV_rtp_init(void);
+void    DRV_amplitude(const uint8_t amplitude);
 void    DRV_pulse(const uint8_t sequence);
 
 typedef enum DRV_EFFECT {
diff --git a/drivers/haptic/haptic.c b/drivers/haptic/haptic.c
index ded6d8a44a..989970beee 100644
--- a/drivers/haptic/haptic.c
+++ b/drivers/haptic/haptic.c
@@ -168,6 +168,15 @@ void haptic_set_mode(uint8_t mode) {
     xprintf("haptic_config.mode = %u\n", haptic_config.mode);
 }
 
+void haptic_set_amplitude(uint8_t amp) {
+  haptic_config.amplitude = amp;
+  eeconfig_update_haptic(haptic_config.raw);
+  xprintf("haptic_config.amplitude = %u\n", haptic_config.amplitude);
+  #ifdef DRV2605L
+  DRV_amplitude(amp);
+  #endif
+}
+
 void haptic_set_buzz(uint8_t buzz) {
     haptic_config.buzz = buzz;
     eeconfig_update_haptic(haptic_config.raw);
@@ -201,6 +210,53 @@ uint8_t haptic_get_dwell(void) {
     return haptic_config.dwell;
 }
 
+void haptic_enable_continuous(void) {
+  haptic_config.cont        = 1;
+  xprintf("haptic_config.cont = %u\n", haptic_config.cont);
+  eeconfig_update_haptic(haptic_config.raw);
+  #ifdef DRV2605L
+  DRV_rtp_init();
+  #endif
+}
+
+void haptic_disable_continuous(void) {
+  haptic_config.cont        = 0;
+  xprintf("haptic_config.cont = %u\n", haptic_config.cont);
+  eeconfig_update_haptic(haptic_config.raw);
+  #ifdef DRV2605L
+  DRV_write(DRV_MODE,0x00); 
+  #endif
+}
+
+void haptic_toggle_continuous(void) {
+#ifdef DRV2605L
+if (haptic_config.cont) {
+  haptic_disable_continuous();
+  } else {
+    haptic_enable_continuous();
+  }
+  eeconfig_update_haptic(haptic_config.raw);
+#endif
+}
+
+
+void haptic_cont_increase(void) {
+  uint8_t amp = haptic_config.amplitude + 10;
+  if (haptic_config.amplitude >= 120) {
+    amp = 120;
+  }
+  haptic_set_amplitude(amp);
+}
+
+void haptic_cont_decrease(void) {
+  uint8_t amp = haptic_config.amplitude - 10;
+  if (haptic_config.amplitude < 20) {
+    amp = 20;
+  }
+  haptic_set_amplitude(amp);
+}
+
+
 void haptic_play(void) {
 #ifdef DRV2605L
     uint8_t play_eff = 0;
@@ -213,6 +269,7 @@ void haptic_play(void) {
 }
 
 bool process_haptic(uint16_t keycode, keyrecord_t *record) {
+
     if (keycode == HPT_ON && record->event.pressed) {
         haptic_enable();
     }
@@ -243,6 +300,16 @@ bool process_haptic(uint16_t keycode, keyrecord_t *record) {
     if (keycode == HPT_DWLD && record->event.pressed) {
         haptic_dwell_decrease();
     }
+    if (keycode == HPT_CONT && record->event.pressed) { 
+        haptic_toggle_continuous(); 
+    }
+    if (keycode == HPT_CONI && record->event.pressed) { 
+        haptic_cont_increase(); 
+    }
+    if (keycode == HPT_COND && record->event.pressed) { 
+        haptic_cont_decrease(); 
+    }
+      
     if (haptic_config.enable) {
         if (record->event.pressed) {
             // keypress
diff --git a/drivers/haptic/haptic.h b/drivers/haptic/haptic.h
index 8135d0d439..2f6eb31fc2 100644
--- a/drivers/haptic/haptic.h
+++ b/drivers/haptic/haptic.h
@@ -34,12 +34,14 @@
 typedef union {
     uint32_t raw;
     struct {
-        bool     enable : 1;
-        uint8_t  feedback : 2;
-        uint8_t  mode : 7;
-        bool     buzz : 1;
-        uint8_t  dwell : 7;
-        uint16_t reserved : 16;
+        bool     enable    :1;
+        uint8_t  feedback  :2;
+        uint8_t  mode      :7;
+        bool     buzz      :1;
+        uint8_t  dwell     :7;
+        bool     cont      :1;
+        uint8_t  amplitude :8;
+        uint16_t reserved  :7; 
     };
 } haptic_config_t;
 
@@ -71,6 +73,9 @@ uint8_t haptic_get_mode(void);
 uint8_t haptic_get_feedback(void);
 void    haptic_dwell_increase(void);
 void    haptic_dwell_decrease(void);
+void    haptic_toggle_continuous(void);
+void    haptic_cont_increase(void);
+void    haptic_cont_decrease(void);
 
 void haptic_play(void);
 void haptic_shutdown(void);