Merge pull request #170 from dschiedsch/master

Added doubletap feature. Thanks for the support
dlktdr · May 14, 2024 · 43c0c37 · 43c0c37
2 parents 246068c + 3482fed
commit 43c0c37
Show file tree

Hide file tree

Showing 7 changed files with 426 additions and 172 deletions.
diff --git a/firmware/src/src/basetrackersettings.h b/firmware/src/src/basetrackersettings.h
@@ -773,6 +773,40 @@ class BaseTrackerSettings {
   inline const bool& getRstOnTlt() {return rstontlt;}
   void setRstOnTlt(bool val=false) { rstontlt = val; }
 
+  // Reset on a double tap
+  inline const bool& getRstOnDbltTap() {return rstondblttap;}
+  void setRstOnDbltTap(bool val=false) { rstondblttap = val; }
+
+  // Double Tap Threshold
+  inline const float& getRstOnDblTapThres() {return rstondbltapthres;}
+  bool setRstOnDblTapThres(float val=80) {
+    if(val >= 50 && val <= 200) {
+      rstondbltapthres = val;
+      return true;
+    }
+    return false;
+  }
+
+  // Double Tap Min Time
+  inline const float& getRstOnDblTapMin() {return rstondbltapmin;}
+  bool setRstOnDblTapMin(float val=100) {
+    if(val >= 50 && val <= 400) {
+      rstondbltapmin = val;
+      return true;
+    }
+    return false;
+  }
+
+  // Double Tap Max Time
+  inline const float& getRstOnDblTapMax() {return rstondbltapmax;}
+  bool setRstOnDblTapMax(float val=400) {
+    if(val >= 20 && val <= 1000) {
+      rstondbltapmax = val;
+      return true;
+    }
+    return false;
+  }
+
   // Invert PPM Output
   inline const bool& getPpmOutInvert() {return ppmoutinvert;}
   void setPpmOutInvert(bool val=false) { ppmoutinvert = val; }
@@ -1022,6 +1056,10 @@ class BaseTrackerSettings {
     json["rstonwave"] = rstonwave;
     json["butlngps"] = butlngps;
     json["rstontlt"] = rstontlt;
+    json["rstondblttap"] = rstondblttap;
+    json["rstondbltapthres"] = rstondbltapthres;
+    json["rstondbltapmin"] = rstondbltapmin;
+    json["rstondbltapmax"] = rstondbltapmax;
     json["ppmoutinvert"] = ppmoutinvert;
     json["ppmininvert"] = ppmininvert;
     json["ppmframe"] = ppmframe;
@@ -1106,6 +1144,10 @@ class BaseTrackerSettings {
     v = json["rstonwave"]; if(!v.isNull()) {setRstOnWave(v);}
     v = json["butlngps"]; if(!v.isNull()) {setButLngPs(v);}
     v = json["rstontlt"]; if(!v.isNull()) {setRstOnTlt(v);}
+    v = json["rstondblttap"]; if(!v.isNull()) {setRstOnDbltTap(v);}
+    v = json["rstondbltapthres"]; if(!v.isNull()) {setRstOnDblTapThres(v);}
+    v = json["rstondbltapmin"]; if(!v.isNull()) {setRstOnDblTapMin(v);}
+    v = json["rstondbltapmax"]; if(!v.isNull()) {setRstOnDblTapMax(v);}
     v = json["ppmoutinvert"]; if(!v.isNull()) {setPpmOutInvert(v);}
     v = json["ppmininvert"]; if(!v.isNull()) {setPpmInInvert(v);}
     v = json["ppmframe"]; if(!v.isNull()) {setPpmFrame(v);}
@@ -1514,6 +1556,10 @@ class BaseTrackerSettings {
   bool rstonwave = false; // Reset on Proximity Sense
   bool butlngps = false; // Long Press on the Button to Enable/Disable Tilt Roll and Pan
   bool rstontlt = false; // Reset Center on a Head Tilt
+  bool rstondblttap = false; // Reset on a double tap
+  float rstondbltapthres = 80; // Double Tap Threshold
+  float rstondbltapmin = 100; // Double Tap Min Time
+  float rstondbltapmax = 400; // Double Tap Max Time
   bool ppmoutinvert = false; // Invert PPM Output
   bool ppmininvert = false; // Invert PPM Output
   uint16_t ppmframe = 22500; // PPM Frame Length (us)

diff --git a/firmware/src/src/include/defines.h b/firmware/src/src/include/defines.h
@@ -59,7 +59,7 @@
 #define BT_PERIOD 12500        // (us) Bluetooth update rate
 #define SERIAL_PERIOD 30       // (ms) Serial processing
 #define DATA_PERIOD 2          // Multiplier of Serial Period (Live Data Transmission Speed)
-#define SENSOR_PERIOD 4000     // (us) Sensor Reads
+#define SENSOR_PERIOD 6666     // (us) Sensor Reads 150Hz
 #define CALCULATE_PERIOD 7000  // (us) Channel Calculations
 #define UART_PERIOD 4000       // (us) Update rate of UART
 #define PWM_FREQUENCY 50       // (ms) PWM Period

diff --git a/firmware/src/src/sense.cpp b/firmware/src/src/sense.cpp
@@ -69,6 +69,7 @@
 // #define DEBUG_SENSOR_RATES
 
 void gyroCalibrate();
+void detectDoubleTap();
 
 static float auxdata[10];
 static float raccx = 0, raccy = 0, raccz = 0;
@@ -1042,7 +1043,13 @@ void sensor_Thread()
     }
 
     // Run Gyro Calibration, only on good gyro data
-    if (gyrValid) gyroCalibrate();
+    if (gyrValid)
+    {
+      gyroCalibrate();
+      // If double tap detection is enabled, check for it
+      if(trkset.getRstOnDbltTap())
+        detectDoubleTap();
+    }
 
     // Only do this update after the first mag and accel data have been read.
     if (madgreads == 0) {
@@ -1106,7 +1113,7 @@ void sensor_Thread()
     // Adjust sleep for a more accurate period
     senseUsDuration = micros64() - senseUsDuration;
     if (SENSOR_PERIOD - senseUsDuration <
-        SENSOR_PERIOD * 0.7) {  // Took a long time. Will crash if sleep is too short
+        SENSOR_PERIOD * 0.5) {  // Took a long time. Will crash if sleep is too short
       LOG_ERR("Sensor Thread Overrun %lld", senseUsDuration);
       k_usleep(SENSOR_PERIOD);
     } else {
@@ -1126,6 +1133,39 @@ void sensor_Thread()
   }  // END THREAD
 }
 
+void detectDoubleTap()
+{
+
+  static float last_acc_mag = 0;
+  static uint64_t lasttaptime = 0;
+  static uint64_t lasttime = 0;
+  uint64_t time = millis64();
+  uint64_t timediff;
+
+  float deltatime = (float)(time - lasttime) / 1000.0f;
+  if (deltatime == 0.0f) return;
+  lasttime = time;
+
+  float acc_magnitude = sqrtf(raccx * raccx + raccy * raccy + raccz * raccz);
+  float acc_dif = (acc_magnitude - last_acc_mag) / deltatime;
+  last_acc_mag = acc_magnitude;
+
+  if (acc_dif > trkset.getRstOnDblTapThres())
+  {
+      timediff = time - lasttaptime;
+      LOG_INF("Tap detected, mag=%.1f, time=%lld, timediff=%lld", (double)acc_dif, time, timediff);
+
+      if (timediff > trkset.getRstOnDblTapMin() && timediff < trkset.getRstOnDblTapMin() + trkset.getRstOnDblTapMax())
+      {
+        LOG_INF("Double Tap detected !!!");
+        pressButton();
+      }
+
+      lasttaptime = time;
+  }
+
+}
+
 void gyroCalibrate()
 {
   static float last_gyro_mag = 0;

diff --git a/gui/src/basetrackersettings.h b/gui/src/basetrackersettings.h
@@ -148,6 +148,10 @@ class BaseTrackerSettings : public QObject
     _setting["rstonwave"] = false;
     _setting["butlngps"] = false;
     _setting["rstontlt"] = false;
+    _setting["rstondblttap"] = false;
+    _setting["rstondbltapthres"] = 80;
+    _setting["rstondbltapmin"] = 100;
+    _setting["rstondbltapmax"] = 400;
     _setting["ppmoutinvert"] = false;
     _setting["ppmininvert"] = false;
     _setting["ppmframe"] = 22500;
@@ -258,6 +262,10 @@ class BaseTrackerSettings : public QObject
     descriptions["rstonwave"] = "Reset on Proximity Sense";
     descriptions["butlngps"] = "Long Press on the Button to Enable/Disable Tilt Roll and Pan";
     descriptions["rstontlt"] = "Reset Center on a Head Tilt";
+    descriptions["rstondblttap"] = "Reset on a double tap";
+    descriptions["rstondbltapthres"] = "Double Tap Threshold";
+    descriptions["rstondbltapmin"] = "Double Tap Min Time";
+    descriptions["rstondbltapmax"] = "Double Tap Max Time";
     descriptions["ppmoutinvert"] = "Invert PPM Output";
     descriptions["ppmininvert"] = "Invert PPM Output";
     descriptions["ppmframe"] = "PPM Frame Length (us)";
@@ -1156,6 +1164,49 @@ class BaseTrackerSettings : public QObject
   bool getRstOnTlt() {return _setting["rstontlt"].toBool();}
   void setRstOnTlt(bool val=false) { _setting["rstontlt"] = val; }
 
+  // Reset on a double tap
+  bool getRstOnDbltTap() {return _setting["rstondblttap"].toBool();}
+  void setRstOnDbltTap(bool val=false) { _setting["rstondblttap"] = val; }
+
+  // Double Tap Threshold
+  float getRstOnDblTapThres() {
+    return _setting["rstondbltapthres"].toFloat();
+  }
+  bool setRstOnDblTapThres(float val=80) {
+    if(val >= 50 && val <= 200) {
+      _setting["rstondbltapthres"] = QString::number(val,'g',4);
+      return true;
+    }
+    return false;
+  }
+
+
+  // Double Tap Min Time
+  float getRstOnDblTapMin() {
+    return _setting["rstondbltapmin"].toFloat();
+  }
+  bool setRstOnDblTapMin(float val=100) {
+    if(val >= 50 && val <= 400) {
+      _setting["rstondbltapmin"] = QString::number(val,'g',4);
+      return true;
+    }
+    return false;
+  }
+
+
+  // Double Tap Max Time
+  float getRstOnDblTapMax() {
+    return _setting["rstondbltapmax"].toFloat();
+  }
+  bool setRstOnDblTapMax(float val=400) {
+    if(val >= 20 && val <= 1000) {
+      _setting["rstondbltapmax"] = QString::number(val,'g',4);
+      return true;
+    }
+    return false;
+  }
+
+
   // Invert PPM Output
   bool getPpmOutInvert() {return _setting["ppmoutinvert"].toBool();}
   void setPpmOutInvert(bool val=false) { _setting["ppmoutinvert"] = val; }

diff --git a/gui/src/mainwindow.cpp b/gui/src/mainwindow.cpp
@@ -113,6 +113,7 @@ MainWindow::MainWindow(QWidget *parent)
     connect(ui->chkRstOnTlt, &QPushButton::clicked, this, &MainWindow::updateFromUI);
     connect(ui->chkCh5Arm, &QPushButton::clicked, this ,&MainWindow::updateFromUI);
     connect(ui->chkCRSFInv, &QPushButton::clicked, this, &MainWindow::updateFromUI);
+    connect(ui->chkResetDblTap, &QPushButton::clicked, this, &MainWindow::updateFromUI);
 
     //connect(ui->chkRawData,&QPushButton::clicked,this, &MainWindow::setDataMode(bool)));
 
@@ -132,6 +133,9 @@ MainWindow::MainWindow(QWidget *parent)
     connect(ui->spnRotZ, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI);
     connect(ui->spnSBUSRate, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI);
     connect(ui->spnCRSFRate, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI);
+    connect(ui->spnRstDblTapMax, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI);
+    connect(ui->spnRstDblTapMin, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI);
+    connect(ui->spnRstDblTapThres, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI);
 
     // Gain Sliders
     connect(ui->til_gain, &GainSlider::valueChanged, this, &MainWindow::updateFromUI);
@@ -564,6 +568,22 @@ void MainWindow::updateToUI()
     ui->spnSBUSRate->setValue(trkset.getSbusTxRate());
     ui->spnCRSFRate->setValue(trkset.getCrsfTxRate());
 
+    // Reset on double tap
+    if(trkset.getRstOnDbltTap()) {
+        ui->spnRstDblTapMax->setEnabled(true);
+        ui->spnRstDblTapMin->setEnabled(true);
+        ui->spnRstDblTapThres->setEnabled(true);
+        ui->chkResetDblTap->setChecked(true);
+    } else {
+        ui->spnRstDblTapMax->setEnabled(false);
+        ui->spnRstDblTapMin->setEnabled(false);
+        ui->spnRstDblTapThres->setEnabled(false);
+        ui->chkResetDblTap->setChecked(false);
+    }
+    ui->spnRstDblTapMax->setValue(trkset.getRstOnDblTapMax());
+    ui->spnRstDblTapMin->setValue(trkset.getRstOnDblTapMin());
+    ui->spnRstDblTapThres->setValue(trkset.getRstOnDblTapThres());
+
     int panCh = trkset.getPanCh();
     int rllCh = trkset.getRllCh();
     int tltCh = trkset.getTltCh();
@@ -747,6 +767,21 @@ void MainWindow::updateFromUI()
     trkset.setButLngPs(ui->chkLngBttnPress->isChecked());
     trkset.setRstOnTlt(ui->chkRstOnTlt->isChecked());
 
+    // Reset on Double Tap
+    trkset.setRstOnDblTapMax(ui->spnRstDblTapMax->value());
+    trkset.setRstOnDblTapMin(ui->spnRstDblTapMin->value());
+    trkset.setRstOnDblTapThres(ui->spnRstDblTapThres->value());
+    trkset.setRstOnDbltTap(ui->chkResetDblTap->isChecked());
+    if(ui->chkResetDblTap->isChecked()) {
+        ui->spnRstDblTapMax->setEnabled(true);
+        ui->spnRstDblTapMin->setEnabled(true);
+        ui->spnRstDblTapThres->setEnabled(true);
+    } else {
+        ui->spnRstDblTapMax->setEnabled(false);
+        ui->spnRstDblTapMin->setEnabled(false);
+        ui->spnRstDblTapThres->setEnabled(false);
+    }
+
     trkset.setSbOutInv(ui->chkSbusOutInv->isChecked());
     trkset.setSbusTxRate(ui->spnSBUSRate->value());
     trkset.setCrsfTxRate(ui->spnCRSFRate->value());