diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp
new file mode 100644
index 0000000000..7108b221c5
--- /dev/null
+++ b/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 .
+ */
+
+#include
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "AP_RangeFinder_PX4_PWM.h"
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+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
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h
new file mode 100644
index 0000000000..c19d0b425f
--- /dev/null
+++ b/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 .
+ */
+
+#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
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index fadd0b060f..ea29181a24 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/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
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index d37346eaa6..002ae85df3 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/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);
diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.h b/libraries/AP_RangeFinder/RangeFinder_Backend.h
index fc1caaf48e..fc76aed060 100644
--- a/libraries/AP_RangeFinder/RangeFinder_Backend.h
+++ b/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;