AP_RangeFinder: add support for HC-SR04 rangefinder

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -39,6 +39,7 @@
 #include "AP_RangeFinder_Benewake_TFMini.h"
 #include "AP_RangeFinder_Benewake_TFMiniPlus.h"
 #include "AP_RangeFinder_PWM.h"
+#include "AP_RangeFinder_HC_SR04.h"
 #include "AP_RangeFinder_BLPing.h"
 #include "AP_RangeFinder_UAVCAN.h"
 #include "AP_RangeFinder_Lanbao.h"
@@ -457,6 +458,14 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         if (AP_RangeFinder_analog::detect(params[instance])) {
             drivers[instance] = new AP_RangeFinder_analog(state[instance], params[instance]);
         }
+#endif
+        break;
+    case Type::HC_SR04:
+#ifndef HAL_BUILD_AP_PERIPH
+        // note that this will always come back as present if the pin is valid
+        if (AP_RangeFinder_HC_SR04::detect(params[instance])) {
+            drivers[instance] = new AP_RangeFinder_HC_SR04(state[instance], params[instance]);
+        }
 #endif
         break;
     case Type::NMEA:

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -79,6 +79,7 @@ class RangeFinder
         BenewakeTF03 = 27,
         VL53L1X_Short = 28,
         LeddarVu8_Serial = 29,
+        HC_SR04 = 30,
     };
 
     enum class Function {

libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp

@@ -0,0 +1,195 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *   AP_RangeFinder_HC_SR04.cpp - rangefinder for HC_SR04 source
+ *
+ *   https://cdn.sparkfun.com/datasheets/Sensors/Proximity/HCSR04.pdf
+ *
+ *   There are two pins involved - one we attach an interrupt handler
+ *   to and use for measuring the supplied interval which is
+ *   proportional to distance.
+ *
+ *   The second pin we use for triggering the ultransonic pulse
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Params.h"
+#include "AP_RangeFinder_HC_SR04.h"
+
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+AP_RangeFinder_HC_SR04::AP_RangeFinder_HC_SR04(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
+    AP_RangeFinder_Backend(_state, _params)
+{
+    set_status(RangeFinder::Status::NoData);
+}
+
+void AP_RangeFinder_HC_SR04::check_pins()
+{
+    check_echo_pin();
+    check_trigger_pin();
+}
+
+void AP_RangeFinder_HC_SR04::check_trigger_pin()
+{
+    if (params.stop_pin == trigger_pin) {
+        // no change
+        return;
+    }
+    trigger_pin = params.stop_pin;
+}
+
+void AP_RangeFinder_HC_SR04::check_echo_pin()
+{
+    if (params.pin == echo_pin) {
+        // no change
+        return;
+    }
+
+    // detach last one
+    if (echo_pin) {
+        if (!hal.gpio->detach_interrupt(echo_pin)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
+                          "HC_SR04: Failed to detach from pin %u",
+                          echo_pin);
+            // ignore this failure or the user may be stuck
+        }
+    }
+
+    echo_pin = params.pin;
+
+    if (!params.pin) {
+        // don't need to install handler
+        return;
+    }
+
+    // install interrupt handler on rising and falling edge
+    hal.gpio->pinMode(params.pin, HAL_GPIO_INPUT);
+    if (!hal.gpio->attach_interrupt(
+            params.pin,
+            FUNCTOR_BIND_MEMBER(&AP_RangeFinder_HC_SR04::irq_handler,
+                                void,
+                                uint8_t,
+                                bool,
+                                uint32_t),
+            AP_HAL::GPIO::INTERRUPT_BOTH)) {
+        // failed to attach interrupt
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
+                      "HC_SR04: Failed to attach to pin %u",
+                      (unsigned int)params.pin);
+        return;
+    }
+}
+
+
+
+/*
+   detect if an HC_SR04 rangefinder is connected. The only thing we
+   can do is check if the pin number is valid. If it is, then assume
+   that the device is connected
+*/
+bool AP_RangeFinder_HC_SR04::detect(AP_RangeFinder_Params &_params)
+{
+    if (_params.pin == -1) {
+        return false;
+    }
+    if (_params.stop_pin == -1) {
+        return false;
+    }
+    return true;
+}
+
+
+// interrupt handler for reading distance-proportional time interval
+void AP_RangeFinder_HC_SR04::irq_handler(uint8_t pin, bool pin_high, uint32_t timestamp_us)
+{
+    if (pin_high) {
+        pulse_start_us = timestamp_us;
+    } else {
+        irq_value_us = timestamp_us - pulse_start_us;
+    }
+}
+
+/*
+  update distance_cm
+ */
+void AP_RangeFinder_HC_SR04::update(void)
+{
+    // check if pin has changed and configure interrupt handlers if required:
+    check_pins();
+
+    if (!echo_pin || ! trigger_pin || echo_pin == -1 || trigger_pin == -1) {
+        // disabled (either by configuration or failure to attach interrupt)
+        state.distance_cm = 0.0f;
+        return;
+    }
+
+    // disable interrupts and grab state
+    void *irqstate = hal.scheduler->disable_interrupts_save();
+    const uint32_t value_us = irq_value_us;
+    irq_value_us = 0;
+    hal.scheduler->restore_interrupts(irqstate);
+
+    const uint32_t now = AP_HAL::millis();
+    if (value_us == 0) {
+        // no reading; check for timeout:
+        if (now - last_reading_ms > 1000) {
+            // no reading for a second - something is broken
+            state.distance_cm = 0.0f;
+        }
+    } else {
+        // gcs().send_text(MAV_SEVERITY_WARNING, "Pong!");
+        // a new reading - convert time to distance
+        state.distance_cm = value_us * (1.0/58.0f);  // 58 is from datasheet, mult for performance
+
+        // glitch remover: measurement is greater than .5m from last.
+        // the SR-04 seeems to suffer from single-measurement glitches
+        // which can be removed by a simple filter.
+        if (labs(state.distance_cm - last_distance_cm) > 50) {
+            // if greater for 5 readings then pass it as new height,
+            // otherwise use last reading
+            if (glitch_count++ > 4) {
+                 last_distance_cm = state.distance_cm;
+            } else {
+                 state.distance_cm = last_distance_cm;
+            }
+        } else {
+            // is not greater 0.5m, pass on and reset glitch counter
+            last_distance_cm = state.distance_cm;
+            glitch_count = 0;
+        }
+
+        last_reading_ms = now;
+    }
+
+    // update range_valid state based on distance measured
+    update_status();
+
+    // consider sending new ping
+    if (now - last_ping_ms > 67) { // read ~@15Hz - recommended 60ms delay from datasheet
+        last_ping_ms = now;
+        // gcs().send_text(MAV_SEVERITY_INFO, "Ping!");
+        // raise stop pin for n-microseconds
+        hal.gpio->pinMode(trigger_pin, HAL_GPIO_OUTPUT);
+        hal.gpio->write(trigger_pin, 1);
+        hal.scheduler->delay_microseconds(10);
+        hal.gpio->write(trigger_pin, 0);
+    }
+}
+

libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend.h"
+#include "AP_RangeFinder_Params.h"
+
+class AP_RangeFinder_HC_SR04 : public AP_RangeFinder_Backend
+{
+public:
+    // constructor
+    AP_RangeFinder_HC_SR04(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
+
+    // static detection function
+    static bool detect(AP_RangeFinder_Params &_params);
+
+    // update state
+    void update(void) override;
+
+protected:
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_ULTRASOUND;
+    }
+
+private:
+
+    void check_pins();
+    void check_echo_pin();
+    void check_trigger_pin();
+    void irq_handler(uint8_t pin, bool pin_high, uint32_t timestamp_us);
+
+    int8_t echo_pin;
+    int8_t trigger_pin;
+    uint32_t last_reading_ms;      // system time of last read (used for health reporting)
+    uint32_t last_distance_cm;     // last distance reported (used to prevent glitches in measurement)
+    uint8_t glitch_count;           // glitch counter
+
+    // follow are modified by the IRQ handler:
+    uint32_t pulse_start_us;      // system time of start of timing pulse
+    uint32_t irq_value_us;         // last calculated pwm value (irq copy)
+
+    uint32_t last_ping_ms;
+};

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04
     // @User: Standard
     AP_GROUPINFO("TYPE",    1, AP_RangeFinder_Params, type, 0),