AP_RangeFinder: added PX4-PWM rangefinder

uses PWM input driver to read a rangefinder

libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp

@@ -0,0 +1,188 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "AP_RangeFinder_PX4_PWM.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <drivers/drv_pwm_input.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_sensor.h>
+#include <uORB/topics/pwm_input.h>
+#include <stdio.h>
+#include <errno.h>
+#include <math.h>
+
+extern const AP_HAL::HAL& hal;
+
+/* 
+   The constructor also initialises the rangefinder. Note that this
+   constructor is not called until detect() returns true, so we
+   already know that we should setup the rangefinder
+*/
+AP_RangeFinder_PX4_PWM::AP_RangeFinder_PX4_PWM(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
+	AP_RangeFinder_Backend(_ranger, instance, _state),
+    _last_timestamp(0),
+    _last_pulse_time_ms(0),
+    _disable_time_ms(0),
+    _good_sample_count(0),
+    _last_sample_distance_cm(0)
+{
+    state.healthy = false;
+
+    _fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
+    if (_fd == -1) {
+        hal.console->printf("Unable to open PX4 PWM rangefinder\n");
+        return;
+    }
+
+    // keep a queue of 20 samples
+    if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 20) != 0) {
+        hal.console->printf("Failed to setup range finder queue\n");
+        return;
+    }
+}
+
+/* 
+   close the file descriptor
+*/
+AP_RangeFinder_PX4_PWM::~AP_RangeFinder_PX4_PWM()
+{
+    if (_fd != -1) {
+        close(_fd);
+    }
+}
+
+/* 
+   see if the PX4 driver is available
+*/
+bool AP_RangeFinder_PX4_PWM::detect(RangeFinder &_ranger, uint8_t instance)
+{
+    int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
+    if (fd == -1) {
+        return false;
+    }
+    close(fd);
+    return true;
+}
+
+void AP_RangeFinder_PX4_PWM::update(void)
+{
+    if (_fd == -1) {
+        return;
+    }
+
+    struct pwm_input_s pwm;
+    float sum_cm = 0;
+    uint16_t count = 0;
+    const float scaling = ranger._scaling[state.instance];
+    uint32_t now = hal.scheduler->millis();
+
+    while (::read(_fd, &pwm, sizeof(pwm)) == sizeof(pwm)) {
+        // report the voltage as the pulse width, so we get the raw
+        // pulse widths in the log
+        state.voltage_mv = pwm.pulse_width;
+
+        _last_pulse_time_ms = now;
+
+        // setup for scaling in meters per millisecond
+        float distance_cm = pwm.pulse_width * 0.1f * scaling;
+
+        if (distance_cm > ranger._max_distance_cm[state.instance] ||
+            distance_cm < ranger._min_distance_cm[state.instance]) {
+            _good_sample_count = 0;
+            continue;
+        }
+        
+        float distance_delta_cm = fabsf(distance_cm - _last_sample_distance_cm);
+        _last_sample_distance_cm = distance_cm;
+
+        if (distance_delta_cm > 100) {
+            // varying by more than 1m in a single sample, which means
+            // between 50 and 100m/s vertically - discard
+            _good_sample_count = 0;
+            continue;
+        }
+
+        if (_good_sample_count > 1) {
+            count++;
+            sum_cm += distance_cm;
+            _last_timestamp = pwm.timestamp;
+        } else {
+            _good_sample_count++;
+        }
+    }
+
+    // if we haven't received a pulse for 1 second then we may need to
+    // reset the timer
+    int8_t stop_pin = ranger._stop_pin[state.instance];
+    uint16_t settle_time_ms = (uint16_t)ranger._settle_time_ms[state.instance];
+
+    if (stop_pin != -1 && out_of_range()) {
+        // we are above the power saving range. Disable the sensor
+        hal.gpio->pinMode(stop_pin, HAL_GPIO_OUTPUT);
+        hal.gpio->write(stop_pin, false);
+        state.healthy = false;
+        state.distance_cm = 0;
+        state.voltage_mv = 0;
+        return;
+    }
+
+    // consider the range finder healthy if we got a reading in the last 0.2s
+    state.healthy = (hal.scheduler->micros64() - _last_timestamp < 200000);
+
+    /* if we haven't seen any pulses for 0.5s then the sensor is
+       probably dead. Try resetting it. Tests show the sensor takes
+       about 0.2s to boot, so 500ms offers some safety margin
+    */
+    if (now - _last_pulse_time_ms > 500U && _disable_time_ms == 0) {
+        ioctl(_fd, SENSORIOCRESET, 0);
+        _last_pulse_time_ms = now;
+
+        // if a stop pin is configured then disable the sensor for the
+        // settle time
+        if (stop_pin != -1) {
+            hal.gpio->pinMode(stop_pin, HAL_GPIO_OUTPUT);
+            hal.gpio->write(stop_pin, false);            
+            _disable_time_ms = now;
+        }
+    }
+
+    /* the user can configure a settle time. This controls how 
+       long the sensor is disabled for using the stop pin when it is
+       reset. This is used both to make sure the sensor is properly
+       reset, and also to allow for power management by running a low
+       duty cycle when it has no signal 
+    */
+    if (stop_pin != -1 && _disable_time_ms != 0 && 
+        (now - _disable_time_ms > settle_time_ms)) {
+        hal.gpio->write(stop_pin, true);        
+        _disable_time_ms = 0;
+        _last_pulse_time_ms = now;
+    }
+
+    if (count != 0) {
+        state.distance_cm = sum_cm / count;
+    }
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h

@@ -0,0 +1,47 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+ 
+#ifndef AP_RangeFinder_PX4_PWM_H
+#define AP_RangeFinder_PX4_PWM_H
+
+#include "RangeFinder.h"
+#include "RangeFinder_Backend.h"
+
+class AP_RangeFinder_PX4_PWM : public AP_RangeFinder_Backend
+{
+public:
+    // constructor
+    AP_RangeFinder_PX4_PWM(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
+
+    // destructor
+    ~AP_RangeFinder_PX4_PWM(void);
+    
+    // static detection function
+    static bool detect(RangeFinder &ranger, uint8_t instance);
+
+    // update state
+    void update(void);
+
+private:
+    int _fd;
+    uint64_t _last_timestamp;
+    uint64_t _last_pulse_time_ms;
+    uint32_t _disable_time_ms;
+    uint32_t _good_sample_count;
+    float _last_sample_distance_cm;
+};
+
+#endif // AP_RangeFinder_PX4_PWM_H

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -19,13 +19,14 @@
 #include "AP_RangeFinder_PulsedLightLRF.h"
 #include "AP_RangeFinder_MaxsonarI2CXL.h"
 #include "AP_RangeFinder_PX4.h"
+#include "AP_RangeFinder_PX4_PWM.h"
 
 // table of user settable parameters
 const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = {
     // @Param: _TYPE
     // @DisplayName: Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C
+    // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM
     AP_GROUPINFO("_TYPE",    0, RangeFinder, _type[0], 0),
 
     // @Param: _PIN
@@ -87,13 +88,20 @@ const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = {
     // @Values: 0:No,1:Yes
     AP_GROUPINFO("_RMETRIC", 9, RangeFinder, _ratiometric[0], 1),
 
+    // @Param: RNGFND_PWRRNG
+    // @DisplayName: Powersave range
+    // @Description: This parameter sets the estimated terrain distance in meters above which the sensor will be put into a power saving mode (if available). A value of zero means power saving is not enabled
+    // @Units: meters
+    // @Range: 0 32767
+    AP_GROUPINFO("_PWRRNG", 10, RangeFinder, _powersave_range, 0),
+
     // 10..12 left for future expansion
 
 #if RANGEFINDER_MAX_INSTANCES > 1
     // @Param: 2_TYPE
     // @DisplayName: Second Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C
+    // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM
     AP_GROUPINFO("2_TYPE",    12, RangeFinder, _type[1], 0),
 
     // @Param: 2_PIN
@@ -240,6 +248,13 @@ void RangeFinder::detect_instance(uint8_t instance)
             return;
         }
     }
+    if (type == RangeFinder_TYPE_PX4_PWM) {
+        if (AP_RangeFinder_PX4_PWM::detect(*this, instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_RangeFinder_PX4_PWM(*this, instance, state[instance]);
+            return;
+        }
+    }
 #endif
     if (type == RangeFinder_TYPE_ANALOG) {
         // note that analog must be the last to be checked, as it will

libraries/AP_RangeFinder/RangeFinder.h

@@ -29,9 +29,11 @@ class AP_RangeFinder_Backend;
 class RangeFinder
 {
 public:
+    friend class AP_RangeFinder_Backend;
     RangeFinder(void) :
     primary_instance(0),
-    num_instances(0)
+    num_instances(0),
+    estimated_terrain_height(0)
     {
         AP_Param::setup_object_defaults(this, var_info);
     }
@@ -42,7 +44,8 @@ public:
         RangeFinder_TYPE_ANALOG = 1,
         RangeFinder_TYPE_MBI2C  = 2,
         RangeFinder_TYPE_PLI2C  = 3,
-        RangeFinder_TYPE_PX4    = 4
+        RangeFinder_TYPE_PX4    = 4,
+        RangeFinder_TYPE_PX4_PWM= 5
     };
 
     enum RangeFinder_Function {
@@ -72,6 +75,7 @@ public:
     AP_Int8  _function[RANGEFINDER_MAX_INSTANCES];
     AP_Int16 _min_distance_cm[RANGEFINDER_MAX_INSTANCES];
     AP_Int16 _max_distance_cm[RANGEFINDER_MAX_INSTANCES];
+    AP_Int16 _powersave_range;
 
     static const struct AP_Param::GroupInfo var_info[];
     
@@ -123,12 +127,21 @@ public:
     bool healthy() const {
         return healthy(primary_instance);
     }
+
+    /*
+      set an externally estimated terrain height. Used to enable power
+      saving (where available) at high altitudes.
+     */
+    void set_estimated_terrain_height(float height) {
+        estimated_terrain_height = height;
+    }
     
 private:
     RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
     AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
     uint8_t primary_instance:2;
     uint8_t num_instances:2;
+    float estimated_terrain_height;
 
     void detect_instance(uint8_t instance);
     void update_instance(uint8_t instance);  

libraries/AP_RangeFinder/RangeFinder_Backend.h

@@ -33,6 +33,11 @@ public:
 
     // update the state structure
     virtual void update() = 0;
+
+    // return true if we are beyond the power saving range
+    bool out_of_range(void) const {
+        return ranger._powersave_range > 0 && ranger.estimated_terrain_height > ranger._powersave_range;
+    }
     
 protected:
     RangeFinder &ranger;