diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp index 0db71329e8..0976f68a53 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp @@ -31,107 +31,92 @@ extern const AP_HAL::HAL& hal; -// Constructor //////////////////////////////////////////////////////// -AP_RangeFinder_PX4::AP_RangeFinder_PX4(FilterInt16 *filter) : - RangeFinder(NULL, filter), - _num_instances(0){ } +uint8_t AP_RangeFinder_PX4::num_px4_instances = 0; -bool AP_RangeFinder_PX4::init(void) +/* + 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::AP_RangeFinder_PX4(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : + AP_RangeFinder_Backend(_ranger, instance, _state) { - _range_fd[0] = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - if (_range_fd[0] < 0) { - hal.console->printf("Unable to open " RANGE_FINDER_DEVICE_PATH "\n"); - return false; + _fd = open_driver(); + + // consider this path used up + num_px4_instances++; + + if (_fd == -1) { + hal.console->printf("Unable to open PX4 rangefinder %u\n", num_px4_instances); + state.healthy = false; + return; } - _range_fd[1] = open(RANGE_FINDER_DEVICE_PATH "1", O_RDONLY); - if (_range_fd[1] >= 0) { - _num_instances = 2; - } else { - _num_instances = 1; + // average over up to 20 samples + if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 20) != 0) { + hal.console->printf("Failed to setup range finder queue\n"); + state.healthy = false; + return; } - for (uint8_t i=0; i<_num_instances; i++) { - // average over up to 20 samples - if (ioctl(_range_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) { - hal.console->printf("Failed to setup range finder queue\n"); - return false; - } + state.healthy = true; +} - _count[0] = 0; - _sum[i] = 0; - _healthy[i] = false; +/* + open the PX4 driver, returning the file descriptor +*/ +int AP_RangeFinder_PX4::open_driver(void) +{ + // work out the device path based on how many PX4 drivers we have loaded + char path[] = RANGE_FINDER_DEVICE_PATH "n"; + if (num_px4_instances == 0) { + path[strlen(path)-1] = 0; + } else { + path[strlen(path)-1] = '1' + (num_px4_instances-1); } + return open(path, O_RDONLY); +} - // give the driver a chance to run, and gather one sample - hal.scheduler->delay(40); - accumulate(); - if (_count[0] == 0) { - hal.console->printf("Failed initial range finder accumulate\n"); +/* + see if the PX4 driver is available +*/ +bool AP_RangeFinder_PX4::detect(RangeFinder &_ranger, uint8_t instance) +{ + int fd = open_driver(); + if (fd == -1) { + return false; } + close(fd); return true; } -bool AP_RangeFinder_PX4::take_reading(void) +void AP_RangeFinder_PX4::update(void) { - // try to accumulate one more sample, so we have the latest data - accumulate(); - - // consider the range finder healthy if we got a reading in the last 0.2s - for (uint8_t i=0; i<_num_instances; i++) { - _healthy[i] = (hrt_absolute_time() - _last_timestamp[i] < 200000); + if (_fd == -1) { + state.healthy = false; + return; } - for (uint8_t i=0; i<_num_instances; i++) { - // avoid division by zero if we haven't received any range reports - if (_count[i] == 0) continue; - - _sum[i] /= _count[i]; - _sum[i] *= 100.00f; - - if (_mode_filter) { - _distance[i] = _mode_filter->apply(_sum[i]); - } - else { - _distance[i] = _sum[i]; - } - - _sum[i] = 0; - _count[i] = 0; - } - - return _healthy[_get_primary()]; -} - -void AP_RangeFinder_PX4::accumulate(void) -{ struct range_finder_report range_report; - for (uint8_t i=0; i<_num_instances; i++) { - while (::read(_range_fd[i], &range_report, sizeof(range_report)) == sizeof(range_report) && - range_report.timestamp != _last_timestamp[i]) { - + float sum = 0; + uint16_t count = 0; + + while (::read(_fd, &range_report, sizeof(range_report)) == sizeof(range_report) && + range_report.timestamp != _last_timestamp) { // Only take valid readings if (range_report.valid == 1) { - _sum[i] += range_report.distance; - _count[i]++; - _last_timestamp[i] = range_report.timestamp; + sum += range_report.distance; + count++; + _last_timestamp = range_report.timestamp; } - } } -} -uint8_t AP_RangeFinder_PX4::_get_primary(void) const -{ - for (uint8_t i=0; i<_num_instances; i++) { - if (_healthy[i]) return i; - } - return 0; -} + // consider the range finder healthy if we got a reading in the last 0.2s + state.healthy = (hrt_absolute_time() - _last_timestamp < 200000); -int16_t AP_RangeFinder_PX4::read() -{ - take_reading(); - return _distance[_get_primary()]; + if (count != 0) { + state.distance_cm = sum / count * 100.0f; + } } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h index a7a1a9114a..4e953b4935 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h @@ -18,32 +18,30 @@ #define AP_RangeFinder_PX4_H #include "RangeFinder.h" +#include "RangeFinder_Backend.h" -class AP_RangeFinder_PX4 : public RangeFinder +class AP_RangeFinder_PX4 : public AP_RangeFinder_Backend { public: // constructor - AP_RangeFinder_PX4(FilterInt16 *); - - // initialize all the range finder devices - bool init(void); - - bool take_reading(void); - - void accumulate(void); - - // read value from primary sensor and return distance in cm - int16_t read(); - - // return the number of compass instances - uint8_t get_count(void) const { return _num_instances; } + AP_RangeFinder_PX4(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state); + + // static detection function + static bool detect(RangeFinder &ranger, uint8_t instance); + + // update state + void update(void); + private: - uint8_t _get_primary(void) const; - uint8_t _num_instances; - int _range_fd[RANGEFINDER_MAX_INSTANCES]; - float _sum[RANGEFINDER_MAX_INSTANCES]; - uint32_t _count[RANGEFINDER_MAX_INSTANCES]; - uint64_t _last_timestamp[RANGEFINDER_MAX_INSTANCES]; + int _fd; + uint64_t _last_timestamp; + + // we need to keep track of how many PX4 drivers have been loaded + // so we can open the right device filename + static uint8_t num_px4_instances; + + // try to open the PX4 driver and return its fd + static int open_driver(void); }; #endif // AP_RangeFinder_PX4_H diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index f5313b806e..44d5ffe8d5 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -18,6 +18,7 @@ #include "AP_RangeFinder_analog.h" #include "AP_RangeFinder_PulsedLightLRF.h" #include "AP_RangeFinder_MaxsonarI2CXL.h" +#include "AP_RangeFinder_PX4.h" // table of user settable parameters const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = { @@ -209,6 +210,13 @@ void RangeFinder::detect_instance(uint8_t instance) state[instance].instance = instance; drivers[instance] = new AP_RangeFinder_MaxsonarI2CXL(*this, instance, state[instance]); } +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + } else if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_PX4) { + if (AP_RangeFinder_PX4::detect(*this, instance)) { + state[instance].instance = instance; + drivers[instance] = new AP_RangeFinder_PX4(*this, instance, state[instance]); + } +#endif } else if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_ANALOG) { // note that analog must be the last to be checked, as it will // always come back as present if the pin is valid