Merge pull request #4166 from TD-er/bugfix/Ultrasonic_newping_ESP32

[NewPing] Use Direct GPIO access to reduce jitter on ultrasonic sensors

lib/GPIO_DirectAccess/GPIO_Direct_Access.cpp

@@ -0,0 +1,56 @@
+#include "GPIO_Direct_Access.h"
+
+#include <Arduino.h>
+
+#if defined(ARDUINO_ARCH_ESP8266)
+  # ifndef CORE_POST_3_0_0
+    #  define IRAM_ATTR ICACHE_RAM_ATTR
+  # endif // ifndef CORE_POST_3_0_0
+#endif // if defined(ARDUINO_ARCH_ESP8266)
+
+
+#if defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32)
+
+IO_REG_TYPE DIRECT_pinRead(IO_REG_TYPE pin)
+{
+  return DIRECT_READ(reg, PIN_TO_BITMASK(pin));
+}
+
+void DIRECT_pinWrite(IO_REG_TYPE pin, bool pinstate)
+{
+  if (pinstate) { DIRECT_WRITE_HIGH(reg, PIN_TO_BITMASK(pin)); }
+  else { DIRECT_WRITE_LOW(reg, PIN_TO_BITMASK(pin)); }
+}
+
+void  DIRECT_PINMODE_OUTPUT(IO_REG_TYPE pin)
+{
+  DIRECT_MODE_OUTPUT(reg, PIN_TO_BITMASK(pin));
+}
+
+void  DIRECT_PINMODE_INPUT(IO_REG_TYPE pin)
+{
+  DIRECT_MODE_INPUT(reg, PIN_TO_BITMASK(pin));
+}
+
+IO_REG_TYPE IRAM_ATTR DIRECT_pinRead_ISR(IO_REG_TYPE pin)
+{
+  return DIRECT_READ(reg, PIN_TO_BITMASK(pin));
+}
+
+void IRAM_ATTR DIRECT_pinWrite_ISR(IO_REG_TYPE pin, bool pinstate)
+{
+  if (pinstate) { DIRECT_WRITE_HIGH(reg, PIN_TO_BITMASK(pin)); }
+  else { DIRECT_WRITE_LOW(reg, PIN_TO_BITMASK(pin)); }
+}
+
+void  IRAM_ATTR DIRECT_PINMODE_OUTPUT_ISR(IO_REG_TYPE pin)
+{
+  DIRECT_MODE_OUTPUT(reg, PIN_TO_BITMASK(pin));
+}
+
+void  IRAM_ATTR DIRECT_PINMODE_INPUT_ISR(IO_REG_TYPE pin)
+{
+  DIRECT_MODE_INPUT(reg, PIN_TO_BITMASK(pin));
+}
+
+#endif // if defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32)

src/src/DataTypes/GPIO_Direct_gpio.h → lib/GPIO_DirectAccess/GPIO_Direct_Access.h

@@ -4,9 +4,9 @@
 // Original code from Paul Stoffregen
 // See: https://github.com/PaulStoffregen/OneWire/blob/master/util/OneWire_direct_gpio.h
 
-// This header should ONLY be included by GPIO_Helper.cpp.  These defines are
-// meant to be private, used within GPIO_Helper.cpp, but not exposed to Arduino
-// sketches or other libraries which may include GPIO_Helper.h.
+// This header should ONLY be included by .cpp files.  These defines are
+// meant to be private, used within this .cpp file, but not exposed to Arduino
+// sketches or other libraries which may include a .h file.
 
 #include <stdint.h>
 
@@ -119,7 +119,19 @@
 #define DIRECT_WRITE_LOW(base, mask)    (GPOC = (mask))             //GPIO_OUT_W1TC_ADDRESS
 #define DIRECT_WRITE_HIGH(base, mask)   (GPOS = (mask))             //GPIO_OUT_W1TS_ADDRESS
 
+IO_REG_TYPE DIRECT_pinRead(IO_REG_TYPE pin);
+void DIRECT_pinWrite(IO_REG_TYPE pin, bool pinstate);
+void DIRECT_PINMODE_OUTPUT(IO_REG_TYPE pin);
+void DIRECT_PINMODE_INPUT(IO_REG_TYPE pin);
+
+IO_REG_TYPE DIRECT_pinRead_ISR(IO_REG_TYPE pin);
+void DIRECT_pinWrite_ISR(IO_REG_TYPE pin, bool pinstate);
+void DIRECT_PINMODE_OUTPUT_ISR(IO_REG_TYPE pin);
+void DIRECT_PINMODE_INPUT_ISR(IO_REG_TYPE pin);
+
 #elif defined(ARDUINO_ARCH_ESP32)
+
+#include <esp32-hal-gpio.h>
 #include <driver/rtc_io.h>
 #define PIN_TO_BASEREG(pin)             (0)
 #define PIN_TO_BITMASK(pin)             (pin)
@@ -225,6 +237,19 @@ void directModeOutput(IO_REG_TYPE pin)
 #define DIRECT_WRITE_HIGH(base, pin)    directWriteHigh(pin)
 #define DIRECT_MODE_INPUT(base, pin)    directModeInput(pin)
 #define DIRECT_MODE_OUTPUT(base, pin)   directModeOutput(pin)
+
+
+IO_REG_TYPE DIRECT_pinRead(IO_REG_TYPE pin);
+void DIRECT_pinWrite(IO_REG_TYPE pin, bool pinstate);
+void DIRECT_PINMODE_OUTPUT(IO_REG_TYPE pin);
+void DIRECT_PINMODE_INPUT(IO_REG_TYPE pin);
+
+IO_REG_TYPE DIRECT_pinRead_ISR(IO_REG_TYPE pin) IRAM_ATTR;
+void DIRECT_pinWrite_ISR(IO_REG_TYPE pin, bool pinstate) IRAM_ATTR;
+void DIRECT_PINMODE_OUTPUT_ISR(IO_REG_TYPE pin) IRAM_ATTR;
+void DIRECT_PINMODE_INPUT_ISR(IO_REG_TYPE pin) IRAM_ATTR;
+
+/*
 // https://github.com/PaulStoffregen/OneWire/pull/47
 // https://github.com/stickbreaker/OneWire/commit/6eb7fc1c11a15b6ac8c60e5671cf36eb6829f82c
 #ifdef  interrupts
@@ -236,6 +261,7 @@ void directModeOutput(IO_REG_TYPE pin)
 #define noInterrupts() {portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;portENTER_CRITICAL(&mux)
 #define interrupts() portEXIT_CRITICAL(&mux);}
 //#warning "ESP32 OneWire testing"
+*/
 
 #elif defined(ARDUINO_ARCH_STM32)
 #define PIN_TO_BASEREG(pin)             (0)

lib/NewPing/README.md

@@ -0,0 +1,3 @@
+# NewPing Arduino Library for Arduino
+
+## See the [NewPing Wiki](https://bitbucket.org/teckel12/arduino-new-ping/wiki/Home) for documentation

lib/NewPing/examples/NewPing15SensorsTimer/NewPing15SensorsTimer.pde

@@ -0,0 +1,78 @@
+// ---------------------------------------------------------------------------
+// Before attempting to use this sketch, please read the "Help with 15 Sensors Example Sketch":
+// https://bitbucket.org/teckel12/arduino-new-ping/wiki/Help%20with%2015%20Sensors%20Example%20Sketch
+//
+// This example code was used to successfully communicate with 15 ultrasonic sensors. You can adjust
+// the number of sensors in your project by changing SONAR_NUM and the number of NewPing objects in the
+// "sonar" array. You also need to change the pins for each sensor for the NewPing objects. Each sensor
+// is pinged at 33ms intervals. So, one cycle of all sensors takes 495ms (33 * 15 = 495ms). The results
+// are sent to the "oneSensorCycle" function which currently just displays the distance data. Your project
+// would normally process the sensor results in this function (for example, decide if a robot needs to
+// turn and call the turn function). Keep in mind this example is event-driven. Your complete sketch needs
+// to be written so there's no "delay" commands and the loop() cycles at faster than a 33ms rate. If other
+// processes take longer than 33ms, you'll need to increase PING_INTERVAL so it doesn't get behind.
+// ---------------------------------------------------------------------------
+#include <NewPing.h>
+
+#define SONAR_NUM     15 // Number of sensors.
+#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
+#define PING_INTERVAL 33 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).
+
+unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
+unsigned int cm[SONAR_NUM];         // Where the ping distances are stored.
+uint8_t currentSensor = 0;          // Keeps track of which sensor is active.
+
+NewPing sonar[SONAR_NUM] = {     // Sensor object array.
+  NewPing(41, 42, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
+  NewPing(43, 44, MAX_DISTANCE),
+  NewPing(45, 20, MAX_DISTANCE),
+  NewPing(21, 22, MAX_DISTANCE),
+  NewPing(23, 24, MAX_DISTANCE),
+  NewPing(25, 26, MAX_DISTANCE),
+  NewPing(27, 28, MAX_DISTANCE),
+  NewPing(29, 30, MAX_DISTANCE),
+  NewPing(31, 32, MAX_DISTANCE),
+  NewPing(34, 33, MAX_DISTANCE),
+  NewPing(35, 36, MAX_DISTANCE),
+  NewPing(37, 38, MAX_DISTANCE),
+  NewPing(39, 40, MAX_DISTANCE),
+  NewPing(50, 51, MAX_DISTANCE),
+  NewPing(52, 53, MAX_DISTANCE)
+};
+
+void setup() {
+  Serial.begin(115200);
+  pingTimer[0] = millis() + 75;           // First ping starts at 75ms, gives time for the Arduino to chill before starting.
+  for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
+    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
+}
+
+void loop() {
+  for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
+    if (millis() >= pingTimer[i]) {         // Is it this sensor's time to ping?
+      pingTimer[i] += PING_INTERVAL * SONAR_NUM;  // Set next time this sensor will be pinged.
+      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
+      sonar[currentSensor].timer_stop();          // Make sure previous timer is canceled before starting a new ping (insurance).
+      currentSensor = i;                          // Sensor being accessed.
+      cm[currentSensor] = 0;                      // Make distance zero in case there's no ping echo for this sensor.
+      sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
+    }
+  }
+  // Other code that *DOESN'T* analyze ping results can go here.
+}
+
+void echoCheck() { // If ping received, set the sensor distance to array.
+  if (sonar[currentSensor].check_timer())
+    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
+}
+
+void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
+  // The following code would be replaced with your code that does something with the ping results.
+  for (uint8_t i = 0; i < SONAR_NUM; i++) {
+    Serial.print(i);
+    Serial.print("=");
+    Serial.print(cm[i]);
+    Serial.print("cm ");
+  }
+  Serial.println();
+}

lib/NewPing/examples/NewPing3Sensors/NewPing3Sensors.pde

@@ -0,0 +1,29 @@
+// ---------------------------------------------------------------------------
+// Example NewPing library sketch that pings 3 sensors 20 times a second.
+// ---------------------------------------------------------------------------
+
+#include <NewPing.h>
+
+#define SONAR_NUM 3      // Number of sensors.
+#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
+
+NewPing sonar[SONAR_NUM] = {   // Sensor object array.
+  NewPing(4, 5, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping. 
+  NewPing(6, 7, MAX_DISTANCE), 
+  NewPing(8, 9, MAX_DISTANCE)
+};
+
+void setup() {
+  Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
+}
+
+void loop() { 
+  for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through each sensor and display results.
+    delay(50); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
+    Serial.print(i);
+    Serial.print("=");
+    Serial.print(sonar[i].ping_cm());
+    Serial.print("cm ");
+  }
+  Serial.println();
+}

lib/NewPing/examples/NewPingEventTimer/NewPingEventTimer.pde

@@ -0,0 +1,46 @@
+// ---------------------------------------------------------------------------
+// This example shows how to use NewPing's ping_timer method which uses the Timer2 interrupt to get the
+// ping time. The advantage of using this method over the standard ping method is that it permits a more
+// event-driven sketch which allows you to appear to do two things at once. An example would be to ping
+// an ultrasonic sensor for a possible collision while at the same time navigating. This allows a
+// properly developed sketch to multitask. Be aware that because the ping_timer method uses Timer2,
+// other features or libraries that also use Timer2 would be effected. For example, the PWM function on
+// pins 3 & 11 on Arduino Uno (pins 9 and 10 on Arduino Mega) and the Tone library. Note, only the PWM
+// functionality of the pins is lost (as they use Timer2 to do PWM), the pins are still available to use.
+// NOTE: For Teensy/Leonardo (ATmega32U4) the library uses Timer4 instead of Timer2.
+// ---------------------------------------------------------------------------
+#include <NewPing.h>
+
+#define TRIGGER_PIN   12 // Arduino pin tied to trigger pin on ping sensor.
+#define ECHO_PIN      11 // Arduino pin tied to echo pin on ping sensor.
+#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
+
+NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
+
+unsigned int pingSpeed = 50; // How frequently are we going to send out a ping (in milliseconds). 50ms would be 20 times a second.
+unsigned long pingTimer;     // Holds the next ping time.
+
+void setup() {
+  Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
+  pingTimer = millis(); // Start now.
+}
+
+void loop() {
+  // Notice how there's no delays in this sketch to allow you to do other processing in-line while doing distance pings.
+  if (millis() >= pingTimer) {   // pingSpeed milliseconds since last ping, do another ping.
+    pingTimer += pingSpeed;      // Set the next ping time.
+    sonar.ping_timer(echoCheck); // Send out the ping, calls "echoCheck" function every 24uS where you can check the ping status.
+  }
+  // Do other stuff here, really. Think of it as multi-tasking.
+}
+
+void echoCheck() { // Timer2 interrupt calls this function every 24uS where you can check the ping status.
+  // Don't do anything here!
+  if (sonar.check_timer()) { // This is how you check to see if the ping was received.
+    // Here's where you can add code.
+    Serial.print("Ping: ");
+    Serial.print(sonar.ping_result / US_ROUNDTRIP_CM); // Ping returned, uS result in ping_result, convert to cm with US_ROUNDTRIP_CM.
+    Serial.println("cm");
+  }
+  // Don't do anything here!
+}

lib/NewPingESP8266/examples/NewPingESP8266Example/NewPingESP8266Example.pde → lib/NewPing/examples/NewPingExample/NewPingExample.pde

@@ -1,14 +1,14 @@
 // ---------------------------------------------------------------------------
-// Example NewPingESP8266 library sketch that does a ping about 20 times per second.
+// Example NewPing library sketch that does a ping about 20 times per second.
 // ---------------------------------------------------------------------------
 
-#include <NewPingESP8266.h>
+#include <NewPing.h>
 
 #define TRIGGER_PIN  12  // Arduino pin tied to trigger pin on the ultrasonic sensor.
 #define ECHO_PIN     11  // Arduino pin tied to echo pin on the ultrasonic sensor.
 #define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
 
-NewPingESP8266 sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPingESP8266 setup of pins and maximum distance.
+NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
 
 void setup() {
   Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.

lib/NewPing/examples/NewPingTimerMedian/NewPingTimerMedian.pde

@@ -0,0 +1,60 @@
+// ---------------------------------------------------------------------------
+// Calculate a ping median using the ping_timer() method.
+// ---------------------------------------------------------------------------
+
+#include <NewPing.h>
+
+#define ITERATIONS     5 // Number of iterations.
+#define TRIGGER_PIN   12 // Arduino pin tied to trigger pin on ping sensor.
+#define ECHO_PIN      11 // Arduino pin tied to echo pin on ping sensor.
+#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
+#define PING_INTERVAL 33 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).
+
+unsigned long pingTimer[ITERATIONS]; // Holds the times when the next ping should happen for each iteration.
+unsigned int cm[ITERATIONS];         // Where the ping distances are stored.
+uint8_t currentIteration = 0;        // Keeps track of iteration step.
+
+NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
+
+void setup() {
+  Serial.begin(115200);
+  pingTimer[0] = millis() + 75;            // First ping starts at 75ms, gives time for the Arduino to chill before starting.
+  for (uint8_t i = 1; i < ITERATIONS; i++) // Set the starting time for each iteration.
+    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
+}
+
+void loop() {
+  for (uint8_t i = 0; i < ITERATIONS; i++) { // Loop through all the iterations.
+    if (millis() >= pingTimer[i]) {          // Is it this iteration's time to ping?
+      pingTimer[i] += PING_INTERVAL * ITERATIONS; // Set next time this sensor will be pinged.
+      if (i == 0 && currentIteration == ITERATIONS - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
+      sonar.timer_stop();          // Make sure previous timer is canceled before starting a new ping (insurance).
+      currentIteration = i;        // Sensor being accessed.
+      cm[currentIteration] = 0;    // Make distance zero in case there's no ping echo for this iteration.
+      sonar.ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
+    }
+  }
+  // Other code that *DOESN'T* analyze ping results can go here.
+}
+
+void echoCheck() { // If ping received, set the sensor distance to array.
+  if (sonar.check_timer())
+    cm[currentIteration] = sonar.ping_result / US_ROUNDTRIP_CM;
+}
+
+void oneSensorCycle() { // All iterations complete, calculate the median.
+  unsigned int uS[ITERATIONS];
+  uint8_t j, it = ITERATIONS;
+  uS[0] = NO_ECHO;
+  for (uint8_t i = 0; i < it; i++) { // Loop through iteration results.
+    if (cm[i] != NO_ECHO) { // Ping in range, include as part of median.
+      if (i > 0) {          // Don't start sort till second ping.
+        for (j = i; j > 0 && uS[j - 1] < cm[i]; j--) // Insertion sort loop.
+          uS[j] = uS[j - 1];                         // Shift ping array to correct position for sort insertion.
+      } else j = 0;         // First ping is sort starting point.
+      uS[j] = cm[i];        // Add last ping to array in sorted position.
+    } else it--;            // Ping out of range, skip and don't include as part of median.
+  }
+  Serial.print(uS[it >> 1]);
+  Serial.println("cm");
+}

lib/NewPing/examples/TimerExample/TimerExample.pde

@@ -0,0 +1,25 @@
+// ---------------------------------------------------------------------------
+// While the NewPing library's primary goal is to interface with ultrasonic sensors, interfacing with
+// the Timer2 interrupt was a result of creating an interrupt-based ping method. Since these Timer2
+// interrupt methods were built, the library may as well provide the functionality to use these methods
+// in your sketches.  This shows how simple it is (no ultrasonic sensor required).  Keep in mind that
+// these methods use Timer2, as does NewPing's ping_timer method for using ultrasonic sensors.  You
+// can't use ping_timer at the same time you're using timer_ms or timer_us as all use the same timer.
+// ---------------------------------------------------------------------------
+
+#include <NewPing.h>
+
+#define LED_PIN 13 // Pin with LED attached.
+
+void setup() {
+  pinMode(LED_PIN, OUTPUT);
+  NewPing::timer_ms(500, toggleLED); // Create a Timer2 interrupt that calls toggleLED in your sketch once every 500 milliseconds.
+}
+
+void loop() {
+  // Do anything here, the Timer2 interrupt will take care of the flashing LED without your intervention.
+}
+
+void toggleLED() {
+  digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Toggle the LED.
+}