add SR04 ultrasonic sensor

Author: qqqlab

src/madflight.h

@@ -188,6 +188,8 @@ void madflight_setup() {
   rdr.config.gizmo = (Cfg::rdr_gizmo_enum)cfg.rdr_gizmo; //the gizmo to use
   rdr.config.ser_bus_id = cfg.rdr_ser_bus; //serial bus
   rdr.config.baud = cfg.rdr_baud; //baud rate
+  rdr.config.pin_trig = cfg.pin_rdr_trig;
+  rdr.config.pin_echo = cfg.pin_rdr_echo;
   rdr.setup();
 
   // GPS

src/madflight/cfg/cfg.h

@@ -229,9 +229,13 @@ SOFTWARE.
   MF_PARAM( rcl_deadband,      0, int32_t, 'i') \
 \
   /*RDR - Radar*/ \
-  MF_PARAM( rdr_gizmo,         0, int32_t, 'e', mf_NONE,mf_LD2411S,mf_LD2413,mf_USD1) \
+  MF_PARAM( rdr_gizmo,         0, int32_t, 'e', mf_NONE,mf_LD2411S,mf_LD2413,mf_USD1,mf_SR04) \
   MF_PARAM( rdr_ser_bus,      -1, int32_t, 'i') \
   MF_PARAM( rdr_baud,          0, int32_t, 'i') \
+\
+  /*v2.0.1 additions */ \
+  MF_PARAM( pin_rdr_trig,     -1, int32_t, 'p') \
+  MF_PARAM( pin_rdr_echo,     -1, int32_t, 'p') \
 //end MF_PARAM_LIST
 
 

src/madflight/rdr/RdrGizmoSR04.h

@@ -0,0 +1,92 @@
+//https://uglyduck.vajn.icu/ep/archive/2014/01/Making_a_better_HC_SR04_Echo_Locator.html
+
+// HC-SR04 driver (single instance only)
+// range: 2-6m (depending on variant)
+// reporting rate: 100ms
+// trigger pin high pulse >= 8us --> echo high pulse 
+
+#pragma once
+
+#include "rdr.h"
+#include "../hal/hal.h"
+#include <Arduino.h> //micros, pinMode, digitalWrite, attachInterrupt
+
+#define SR04_TRIG_US 100      // minimum length of trigger pulse
+#define SR04_TIMEOUT_US 60000 // echo timeout 60 ms ~= 10 meter, this is also the appoximate sample rate
+
+static uint8_t _RdrGizmoSR04_irq_pin_echo = 0;
+static volatile uint32_t _RdrGizmoSR04_irq_echo_ts1 = 0;
+static volatile uint32_t _RdrGizmoSR04_irq_echo_ts2 = 0;
+
+static void _RdrGizmoSR04_irq() {
+  if(digitalRead(_RdrGizmoSR04_irq_pin_echo) == HIGH) {
+    _RdrGizmoSR04_irq_echo_ts1 = micros();
+  }else{
+    _RdrGizmoSR04_irq_echo_ts2 = micros();
+  }
+}
+
+
+class RdrGizmoSR04: public RdrGizmo {
+protected:
+  enum class state_enum { TRIG_START, TRIG_END, ECHO_WAIT, INTERVAL_WAIT};
+  state_enum state = state_enum::TRIG_START;
+  uint8_t pin_trig = 0;
+  uint32_t state_ts = 0;
+  int *dist = nullptr;
+
+  RdrGizmoSR04() {} //private constructor
+
+public:
+  static RdrGizmoSR04* create(int* dist, int pin_trig, int pin_echo) {
+      if(!dist || pin_trig < 0 || pin_echo < 0) return nullptr;
+      pinMode(pin_trig, OUTPUT);
+      digitalWrite(pin_trig, LOW);
+      pinMode(pin_echo, INPUT);
+      attachInterrupt(digitalPinToInterrupt(pin_echo), _RdrGizmoSR04_irq, CHANGE);
+
+      //setup gizmo
+      auto gizmo = new RdrGizmoSR04();
+      _RdrGizmoSR04_irq_pin_echo = pin_echo;
+      gizmo->pin_trig = pin_trig;
+      gizmo->dist = dist;
+      return gizmo;
+    }
+
+  bool update() override {
+    switch(state) {
+      case state_enum::TRIG_START:
+        //note: send trigger pulse regardless of echo pin state, is this wise???
+        digitalWrite(pin_trig, HIGH);
+        state_ts = micros();
+        state = state_enum::TRIG_END;
+        break;
+      case state_enum::TRIG_END:
+        if(micros() - state_ts > SR04_TRIG_US) {
+          _RdrGizmoSR04_irq_echo_ts1 = 0;
+          _RdrGizmoSR04_irq_echo_ts2 = 0;
+          digitalWrite(pin_trig, LOW);
+          state_ts = micros();
+          state = state_enum::ECHO_WAIT;
+        }
+        break;
+      case state_enum::ECHO_WAIT:
+        if(_RdrGizmoSR04_irq_echo_ts2 && _RdrGizmoSR04_irq_echo_ts1) {
+          //got echo
+          *dist = (_RdrGizmoSR04_irq_echo_ts2 - _RdrGizmoSR04_irq_echo_ts1) * 171 / 1000; // integer math echo_us -> dist_mm with speed of sound 342 m/s
+          state = state_enum::INTERVAL_WAIT;
+          //Serial.printf("ECHO %d\n",*dist);
+          return true;
+        }
+        //fall thru
+      case state_enum::INTERVAL_WAIT:
+        if(micros() - state_ts > SR04_TIMEOUT_US) {
+          //if(state == state_enum::ECHO_WAIT) Serial.printf("SR04 TIMEOUT\n");
+          state = state_enum::TRIG_START;
+        }
+        break;
+    }
+    return false;
+  }
+
+};

src/madflight/rdr/rdr.cpp

@@ -31,6 +31,7 @@ SOFTWARE.
 #include "RdrGizmoLD2411S.h"
 #include "RdrGizmoLD2413.h"
 #include "RdrGizmoUSD1.h"
+#include "RdrGizmoSR04.h"
 
 //create global module instance
 Rdr rdr;
@@ -53,6 +54,9 @@ int Rdr::setup() {
     case Cfg::rdr_gizmo_enum::mf_USD1 :
       gizmo = RdrGizmoUSD1::create(&dist, config.ser_bus_id, config.baud);
       break;
+    case Cfg::rdr_gizmo_enum::mf_SR04 :
+      gizmo = RdrGizmoSR04::create(&dist, config.pin_trig, config.pin_echo);
+      break;
   }
 
   //check gizmo

src/madflight/rdr/rdr.h

@@ -36,6 +36,8 @@ struct RdrConfig {
     Cfg::rdr_gizmo_enum gizmo = Cfg::rdr_gizmo_enum::mf_NONE;
     int ser_bus_id = -1; //Serial bus id
     int baud = 0; //baud rate. 0=autobaud
+    int pin_trig = -1; //trigger pulse output pin
+    int pin_echo = -1; //echo pulse input pin
 };
 
 class RdrGizmo {