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;