From d71244dd2331cc2efef2b7d3d27204789e412a56 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 17 Jan 2019 09:48:22 +1100 Subject: [PATCH] APM_RangeFinder: remove PX4 backend type --- .../AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp | 199 ------------------ .../AP_RangeFinder/AP_RangeFinder_PX4_PWM.h | 51 ----- libraries/AP_RangeFinder/RangeFinder.cpp | 9 +- 3 files changed, 1 insertion(+), 258 deletions(-) delete mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp delete mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp deleted file mode 100644 index 89c2110f74..0000000000 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/* - 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 -#include -#include "AP_RangeFinder_PX4_PWM.h" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -extern const AP_HAL::HAL& hal; - -extern "C" { - int pwm_input_main(int, char **); -}; - -/* - 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::RangeFinder_State &_state, AP_Int16 &powersave_range, float &_estimated_terrain_height) : - AP_RangeFinder_Backend(_state), - _powersave_range(powersave_range), - estimated_terrain_height(_estimated_terrain_height) -{ - _fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); - if (_fd == -1) { - hal.console->printf("Unable to open PX4 PWM rangefinder\n"); - set_status(RangeFinder::RangeFinder_NotConnected); - return; - } - - // keep a queue of 20 samples - if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 20) != 0) { - hal.console->printf("Failed to setup range finder queue\n"); - set_status(RangeFinder::RangeFinder_NotConnected); - return; - } - - // initialise to connected but no data - set_status(RangeFinder::RangeFinder_NoData); -} - -/* - close the file descriptor -*/ -AP_RangeFinder_PX4_PWM::~AP_RangeFinder_PX4_PWM() -{ - if (_fd != -1) { - close(_fd); - } - set_status(RangeFinder::RangeFinder_NotConnected); -} - -/* - see if the PX4 driver is available -*/ -bool AP_RangeFinder_PX4_PWM::detect() -{ -#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && \ - !defined(CONFIG_ARCH_BOARD_AEROFC_V1) - if (AP_BoardConfig::px4_start_driver(pwm_input_main, "pwm_input", "start")) { - hal.console->printf("started pwm_input driver\n"); - } -#endif - 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) { - set_status(RangeFinder::RangeFinder_NotConnected); - return; - } - - struct pwm_input_s pwm; - float sum_cm = 0; - uint16_t count = 0; - const float scaling = state.scaling; - uint32_t now = AP_HAL::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; - - state.last_reading_ms = now; - - // setup for scaling in meters per millisecond - float _distance_cm = pwm.pulse_width * 0.1f * scaling + state.offset; - - 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 = state.stop_pin; - uint16_t settle_time_ms = (uint16_t)state.settle_time_ms; - - 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); - set_status(RangeFinder::RangeFinder_NoData); - state.distance_cm = 0; - state.voltage_mv = 0; - return; - } - - // if we have not taken a reading in the last 0.2s set status to No Data - if (AP_HAL::micros64() - _last_timestamp >= 200000) { - set_status(RangeFinder::RangeFinder_NoData); - } - - /* 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 - state.last_reading_ms > 500U && _disable_time_ms == 0) { - ioctl(_fd, SENSORIOCRESET, 0); - state.last_reading_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; - state.last_reading_ms = now; - } - - if (count != 0) { - state.distance_cm = sum_cm / count; - - // update range_valid state based on distance measured - update_status(); - } -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h deleted file mode 100644 index e8c0c9fde0..0000000000 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - 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 . - */ -#pragma once - -#include "RangeFinder.h" -#include "RangeFinder_Backend.h" - -class AP_RangeFinder_PX4_PWM : public AP_RangeFinder_Backend -{ -public: - // constructor - AP_RangeFinder_PX4_PWM(RangeFinder::RangeFinder_State &_state, AP_Int16 &powersave_range, float &_estimated_terrain_height); - - // destructor - ~AP_RangeFinder_PX4_PWM(void); - - // static detection function - static bool detect(); - - // update state - void update(void) override; - -protected: - - MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { - return MAV_DISTANCE_SENSOR_UNKNOWN; - } - -private: - int _fd; - uint64_t _last_timestamp; - uint32_t _disable_time_ms; - uint32_t _good_sample_count; - float _last_sample_distance_cm; - - AP_Int16 &_powersave_range; - float &estimated_terrain_height; - -}; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 69de403331..c7734e543d 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -18,7 +18,6 @@ #include "AP_RangeFinder_PulsedLightLRF.h" #include "AP_RangeFinder_MaxsonarI2CXL.h" #include "AP_RangeFinder_MaxsonarSerialLV.h" -#include "AP_RangeFinder_PX4_PWM.h" #include "AP_RangeFinder_BBB_PRU.h" #include "AP_RangeFinder_LightWareI2C.h" #include "AP_RangeFinder_LightWareSerial.h" @@ -703,13 +702,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - case RangeFinder_TYPE_PX4_PWM: - if (AP_RangeFinder_PX4_PWM::detect()) { - drivers[instance] = new AP_RangeFinder_PX4_PWM(state[instance], _powersave_range, estimated_terrain_height); - } - break; -#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS case RangeFinder_TYPE_PX4_PWM: // to ease moving from PX4 to ChibiOS we'll lie a little about // the backend driver...