AP_RangeFinder: add pwm backend

libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp

@@ -0,0 +1,134 @@
+/*
+   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/>.
+ */
+
+#include "AP_RangeFinder_PWM.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+/*
+   The constructor also initialises the rangefinder.
+*/
+AP_RangeFinder_PWM::AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state) :
+    AP_RangeFinder_Backend(_state)
+{
+}
+
+/*
+   There's no sensible way of detecting a PWM rangefinder as the pins are configurable
+*/
+bool AP_RangeFinder_PWM::detect()
+{
+    return true;
+}
+
+// interrupt handler for reading pwm value
+void AP_RangeFinder_PWM::irq_handler(uint8_t pin, bool pin_high, uint32_t timestamp_us)
+{
+    if (pin_high) {
+        irq_pulse_start_us = timestamp_us;
+    } else {
+        if (irq_pulse_start_us != 0) {
+            irq_value_us = timestamp_us - irq_pulse_start_us;
+            irq_pulse_start_us = 0;
+        }
+    }
+}
+
+// read - return last value measured by sensor
+bool AP_RangeFinder_PWM::get_reading(uint16_t &reading_cm)
+{
+    // 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);
+
+    if (value_us == 0) {
+        return false;
+    }
+    reading_cm = value_us * 1e-1f; // correct for LidarLite.  Parameter needed?
+    return true;
+}
+
+void AP_RangeFinder_PWM::check_pin()
+{
+    if (state.pin == last_pin) {
+        return;
+    }
+
+    // detach last one
+    if (last_pin > 0) {
+        if (!hal.gpio->detach_interrupt(last_pin)) {
+            gcs().send_text(MAV_SEVERITY_WARNING,
+                            "RangeFinder_PWM: Failed to detach from pin %u",
+                            last_pin);
+            // ignore this failure or the user may be stuck
+        }
+    }
+
+    // set last pin to state.pin so we don't continually try to attach
+    // to it if the attach is failing
+    last_pin = state.pin;
+
+    if (state.pin <= 0) {
+        // don't need to install handler
+        return;
+    }
+
+    // install interrupt handler on rising and falling edge
+    if (!hal.gpio->attach_interrupt(
+            state.pin,
+            FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PWM::irq_handler,
+                                void,
+                                uint8_t,
+                                bool,
+                                uint32_t),
+            AP_HAL::GPIO::INTERRUPT_BOTH)) {
+        // failed to attach interrupt
+        gcs().send_text(MAV_SEVERITY_WARNING,
+                        "RangeFinder_PWM: Failed to attach to pin %u",
+                        state.pin);
+        return;
+    }
+}
+
+/*
+   update the state of the sensor
+*/
+void AP_RangeFinder_PWM::update(void)
+{
+    // check if pin has changed and configure interrupt handlers if required:
+    check_pin();
+
+    if (last_pin <= 0) {
+        // disabled (by configuration)
+        return;
+    }
+
+    if (!get_reading(state.distance_cm)) {
+        // failure; consider changing our state
+        if (AP_HAL::millis() - state.last_reading_ms > 200) {
+            set_status(RangeFinder::RangeFinder_NoData);
+        }
+        return;
+    }
+
+    // update range_valid state based on distance measured
+    state.last_reading_ms = AP_HAL::millis();
+    update_status();
+}

libraries/AP_RangeFinder/AP_RangeFinder_PWM.h

@@ -0,0 +1,56 @@
+/*
+   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/>.
+ */
+#pragma once
+
+#include "RangeFinder.h"
+#include "RangeFinder_Backend.h"
+
+class AP_RangeFinder_PWM : public AP_RangeFinder_Backend
+{
+public:
+    // constructor
+    AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state);
+
+    // destructor
+    ~AP_RangeFinder_PWM(void) {};
+
+    // static detection function
+    static bool detect();
+
+    // update state
+    void update(void);
+
+protected:
+
+    bool get_reading(uint16_t &reading_cm);
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_UNKNOWN;
+    }
+
+private:
+
+    int8_t last_pin; // last pin used for reading pwm (used to recognise change in pin assignment)
+    uint32_t last_reading_ms;      // system time of last read (used for health reporting)
+
+    // the following two members are updated by the interrupt handler
+    uint32_t irq_value_us;         // last calculated pwm value (irq copy)
+    uint32_t irq_pulse_start_us;   // system time of start of pulse
+
+    void irq_handler(uint8_t pin, bool pin_high, uint32_t timestamp_us);
+
+    void check_pin();
+
+};

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -35,6 +35,7 @@
 #include "AP_RangeFinder_NMEA.h"
 #include "AP_RangeFinder_Wasp.h"
 #include "AP_RangeFinder_Benewake.h"
+#include "AP_RangeFinder_PWM.h"
 #include <AP_BoardConfig/AP_BoardConfig.h>
 
 extern const AP_HAL::HAL &hal;
@@ -44,14 +45,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM
     // @User: Standard
     AP_GROUPINFO("_TYPE",    0, RangeFinder, state[0].type, 0),
 
     // @Param: _PIN
     // @DisplayName: Rangefinder pin
-    // @Description: Analog pin that rangefinder is connected to. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
+    // @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
-    // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port
+    // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port, 50:GPIO1,51:GPIO2,52:GPIO3,53:GPIO4,54:GPIO5,55:GPIO6
     // @User: Standard
     AP_GROUPINFO("_PIN",     1, RangeFinder, state[0].pin, -1),
 
@@ -175,13 +176,13 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @Param: 2_TYPE
     // @DisplayName: Second Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM
     // @User: Advanced
     AP_GROUPINFO("2_TYPE",    12, RangeFinder, state[1].type, 0),
 
     // @Param: 2_PIN
     // @DisplayName: Rangefinder pin
-    // @Description: Analog pin that rangefinder is connected to. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
+    // @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
     // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port
     // @User: Advanced
     AP_GROUPINFO("2_PIN",     13, RangeFinder, state[1].pin, -1),
@@ -300,13 +301,13 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @Param: 3_TYPE
     // @DisplayName: Third Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM
     // @User: Advanced
     AP_GROUPINFO("3_TYPE",    25, RangeFinder, state[2].type, 0),
 
     // @Param: 3_PIN
     // @DisplayName: Rangefinder pin
-    // @Description: Analog pin that rangefinder is connected to. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
+    // @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
     // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port
     // @User: Advanced
     AP_GROUPINFO("3_PIN",     26, RangeFinder, state[2].pin, -1),
@@ -425,13 +426,13 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @Param: 4_TYPE
     // @DisplayName: Fourth Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM
     // @User: Advanced
     AP_GROUPINFO("4_TYPE",    37, RangeFinder, state[3].type, 0),
 
     // @Param: 4_PIN
     // @DisplayName: Rangefinder pin
-    // @Description: Analog pin that rangefinder is connected to. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
+    // @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
     // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port
     // @User: Advanced
     AP_GROUPINFO("4_PIN",     38, RangeFinder, state[3].pin, -1),
@@ -551,8 +552,6 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
 const AP_Param::GroupInfo *RangeFinder::backend_var_info[RANGEFINDER_MAX_INSTANCES];
 
 RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orientation_default) :
-    num_instances(0),
-    estimated_terrain_height(0),
     serial_manager(_serial_manager)
 {
     AP_Param::setup_object_defaults(this, var_info);
@@ -758,6 +757,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
             drivers[instance] = new AP_RangeFinder_Benewake(state[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini);
         }
         break;
+    case RangeFinder_TYPE_PWM:
+        if (AP_RangeFinder_PWM::detect()) {
+            drivers[instance] = new AP_RangeFinder_PWM(state[instance]);
+        }
+        break;
     default:
         break;
     }

libraries/AP_RangeFinder/RangeFinder.h

@@ -67,6 +67,7 @@ public:
         RangeFinder_TYPE_BenewakeTF02 = 19,
         RangeFinder_TYPE_BenewakeTFmini = 20,
         RangeFinder_TYPE_PLI2CV3HP = 21,
+        RangeFinder_TYPE_PWM = 22,
     };
 
     enum RangeFinder_Function {