From 848c03650b4e7ec607c212b708c8ab50ffa1bdd2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Jan 2017 16:13:13 +1100 Subject: [PATCH] AP_RangeFinder: removed old PX4 rangefinder drivers --- .../AP_RangeFinder/AP_RangeFinder_PX4.cpp | 171 ------------------ libraries/AP_RangeFinder/AP_RangeFinder_PX4.h | 48 ----- libraries/AP_RangeFinder/RangeFinder.cpp | 15 +- 3 files changed, 4 insertions(+), 230 deletions(-) delete mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp delete mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_PX4.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp deleted file mode 100644 index c68403ebb1..0000000000 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp +++ /dev/null @@ -1,171 +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 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include -#include "AP_RangeFinder_PX4.h" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -extern const AP_HAL::HAL& hal; - -uint8_t AP_RangeFinder_PX4::num_px4_instances = 0; - -/* - 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), - _last_max_distance_cm(-1), - _last_min_distance_cm(-1) -{ - _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); - set_status(RangeFinder::RangeFinder_NotConnected); - return; - } - - // average over up to 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::~AP_RangeFinder_PX4() -{ - if (_fd != -1) { - close(_fd); - } -} - -extern "C" { - int ll40ls_main(int, char **); - int trone_main(int, char **); - int mb12xx_main(int, char **); -}; - -/* - open the PX4 driver, returning the file descriptor -*/ -int AP_RangeFinder_PX4::open_driver(void) -{ - if (num_px4_instances == 0) { - /* - we start the px4 rangefinder drivers on demand - */ - if (AP_BoardConfig::px4_start_driver(ll40ls_main, "ll40ls", "-X start")) { - hal.console->printf("Found external ll40ls sensor\n"); - } - if (AP_BoardConfig::px4_start_driver(ll40ls_main, "ll40ls", "-I start")) { - hal.console->printf("Found internal ll40ls sensor\n"); - } - if (AP_BoardConfig::px4_start_driver(trone_main, "trone", "start")) { - hal.console->printf("Found trone sensor\n"); - } - if (AP_BoardConfig::px4_start_driver(mb12xx_main, "mb12xx", "start")) { - hal.console->printf("Found mb12xx sensor\n"); - } - } - // work out the device path based on how many PX4 drivers we have loaded - char path[] = RANGE_FINDER_BASE_DEVICE_PATH "n"; - path[strlen(path)-1] = '0' + num_px4_instances; - return open(path, O_RDONLY); -} - -/* - 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; -} - -void AP_RangeFinder_PX4::update(void) -{ - if (_fd == -1) { - set_status(RangeFinder::RangeFinder_NotConnected); - return; - } - - struct distance_sensor_s range_report; - float sum = 0; - uint16_t count = 0; - - if (_last_max_distance_cm != ranger._max_distance_cm[state.instance] || - _last_min_distance_cm != ranger._min_distance_cm[state.instance]) { - float max_distance = ranger._max_distance_cm[state.instance]*0.01f; - float min_distance = ranger._min_distance_cm[state.instance]*0.01f; - if (ioctl(_fd, RANGEFINDERIOCSETMAXIUMDISTANCE, (unsigned long)&max_distance) == 0 && - ioctl(_fd, RANGEFINDERIOCSETMINIUMDISTANCE, (unsigned long)&min_distance) == 0) { - _last_max_distance_cm = ranger._max_distance_cm[state.instance]; - _last_min_distance_cm = ranger._min_distance_cm[state.instance]; - } - } - - - while (::read(_fd, &range_report, sizeof(range_report)) == sizeof(range_report) && - range_report.timestamp != _last_timestamp) { - // take reading - sum += range_report.current_distance; - count++; - _last_timestamp = range_report.timestamp; - } - - // 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 (count != 0) { - state.distance_cm = sum / count * 100.0f; - state.distance_cm += ranger._offset[state.instance]; - - // update range_valid state based on distance measured - update_status(); - } -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h deleted file mode 100644 index 4a7b9a5948..0000000000 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h +++ /dev/null @@ -1,48 +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 : public AP_RangeFinder_Backend -{ -public: - // constructor - AP_RangeFinder_PX4(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state); - - // destructor - ~AP_RangeFinder_PX4(void); - - // static detection function - static bool detect(RangeFinder &ranger, uint8_t instance); - - // update state - void update(void); - -private: - int _fd; - uint64_t _last_timestamp; - - int16_t _last_max_distance_cm; - int16_t _last_min_distance_cm; - - // 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); -}; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index c6deb38f96..5a7981ab10 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.h" #include "AP_RangeFinder_PX4_PWM.h" #include "AP_RangeFinder_BBB_PRU.h" #include "AP_RangeFinder_LightWareI2C.h" @@ -37,7 +36,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Param: _TYPE // @DisplayName: Rangefinder type // @Description: What type of rangefinder device that is connected - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:PulsedLightI2C,4:PX4-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TrOneI2C + // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:PulsedLightI2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TrOneI2C // @User: Standard AP_GROUPINFO("_TYPE", 0, RangeFinder, _type[0], 0), @@ -157,7 +156,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Param: 2_TYPE // @DisplayName: Second Rangefinder type // @Description: What type of rangefinder device that is connected - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:PulsedLightI2C,4:PX4-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TrOneI2C + // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:PulsedLightI2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TrOneI2C // @User: Advanced AP_GROUPINFO("2_TYPE", 12, RangeFinder, _type[1], 0), @@ -272,7 +271,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Param: 3_TYPE // @DisplayName: Third 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,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial + // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial // @User: Advanced AP_GROUPINFO("3_TYPE", 25, RangeFinder, _type[2], 0), @@ -387,7 +386,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Param: 4_TYPE // @DisplayName: Fourth 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,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial + // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial // @User: Advanced AP_GROUPINFO("4_TYPE", 37, RangeFinder, _type[3], 0), @@ -608,12 +607,6 @@ void RangeFinder::detect_instance(uint8_t instance) } break; #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - case RangeFinder_TYPE_PX4: - if (AP_RangeFinder_PX4::detect(*this, instance)) { - state[instance].instance = instance; - drivers[instance] = new AP_RangeFinder_PX4(*this, instance, state[instance]); - } - break; case RangeFinder_TYPE_PX4_PWM: if (AP_RangeFinder_PX4_PWM::detect(*this, instance)) { state[instance].instance = instance;