From a093bcb05fa1184e4a9e2aadae600e19e1d7942b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 9 Feb 2017 20:30:04 +0900 Subject: [PATCH] AP_Proximity: add rangefinder driver This allows avoidance using upward or forward facing range finders --- libraries/AP_Proximity/AP_Proximity.cpp | 10 ++- libraries/AP_Proximity/AP_Proximity.h | 8 +- .../AP_Proximity/AP_Proximity_RangeFinder.cpp | 78 +++++++++++++++++++ .../AP_Proximity/AP_Proximity_RangeFinder.h | 35 +++++++++ 4 files changed, 128 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp create mode 100644 libraries/AP_Proximity/AP_Proximity_RangeFinder.h diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index b9dcf1f51a..35912af3b8 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -16,6 +16,7 @@ #include "AP_Proximity.h" #include "AP_Proximity_LightWareSF40C.h" #include "AP_Proximity_TeraRangerTower.h" +#include "AP_Proximity_RangeFinder.h" #include "AP_Proximity_MAV.h" #include "AP_Proximity_SITL.h" @@ -28,7 +29,7 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { // @Param: _TYPE // @DisplayName: Proximity type // @Description: What type of proximity sensor is connected - // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower + // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder // @User: Standard AP_GROUPINFO("_TYPE", 1, AP_Proximity, _type[0], 0), @@ -147,7 +148,7 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { // @Param: 2_TYPE // @DisplayName: Second Proximity type // @Description: What type of proximity sensor is connected - // @Values: 0:None,1:LightWareSF40C,2:MAVLink + // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder // @User: Advanced AP_GROUPINFO("2_TYPE", 16, AP_Proximity, _type[1], 0), @@ -290,6 +291,11 @@ void AP_Proximity::detect_instance(uint8_t instance) return; } } + if (type == Proximity_Type_RangeFinder) { + state[instance].instance = instance; + drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]); + return; + } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (type == Proximity_Type_SITL) { state[instance].instance = instance; diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 232c29f999..215085d99b 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -19,7 +19,7 @@ #include #include #include - +#include #define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform #define PROXIMITY_YAW_CORRECTION_DEFAULT 22 // default correction for sensor error in yaw @@ -40,6 +40,7 @@ public: Proximity_Type_SF40C = 1, Proximity_Type_MAV = 2, Proximity_Type_TRTOWER = 3, + Proximity_Type_RangeFinder = 4, Proximity_Type_SITL = 10, }; @@ -55,6 +56,10 @@ public: // update state of all proximity sensors. Should be called at high rate from main loop void update(void); + // set pointer to rangefinder object + void set_rangefinder(const RangeFinder *rangefinder) { _rangefinder = rangefinder; } + const RangeFinder *get_rangefinder() const { return _rangefinder; } + // return sensor orientation and yaw correction uint8_t get_orientation(uint8_t instance) const; int16_t get_yaw_correction(uint8_t instance) const; @@ -122,6 +127,7 @@ public: private: Proximity_State state[PROXIMITY_MAX_INSTANCES]; AP_Proximity_Backend *drivers[PROXIMITY_MAX_INSTANCES]; + const RangeFinder *_rangefinder; uint8_t primary_instance:3; uint8_t num_instances:3; AP_SerialManager &serial_manager; diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp new file mode 100644 index 0000000000..50cb84a65a --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp @@ -0,0 +1,78 @@ +/* + 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 +#include "AP_Proximity_RangeFinder.h" +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +AP_Proximity_RangeFinder::AP_Proximity_RangeFinder(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state) : + AP_Proximity_Backend(_frontend, _state) +{ +} + +// update the state of the sensor +void AP_Proximity_RangeFinder::update(void) +{ + // exit immediately if no rangefinder object + const RangeFinder *rngfnd = frontend.get_rangefinder(); + if (rngfnd == nullptr) { + set_status(AP_Proximity::Proximity_NoData); + return; + } + + // look through all rangefinders + for (uint8_t i=0; inum_sensors(); i++) { + if (rngfnd->has_data(i)) { + // check for horizontal range finders + if (rngfnd->get_orientation(i) <= ROTATION_YAW_315) { + uint8_t sector = (uint8_t)rngfnd->get_orientation(i); + _angle[sector] = sector * 45; + _distance[sector] = rngfnd->distance_cm(i) / 100.0f; + _distance_valid[sector] = true; + _distance_min = rngfnd->min_distance_cm(i) / 100.0f; + _distance_max = rngfnd->max_distance_cm(i) / 100.0f; + _last_update_ms = AP_HAL::millis(); + update_boundary_for_sector(sector); + } + // check upward facing range finder + if (rngfnd->get_orientation(i) == ROTATION_PITCH_90) { + _distance_upward = rngfnd->distance_cm(i) / 100.0f; + _last_upward_update_ms = AP_HAL::millis(); + } + } + } + + // check for timeout and set health status + if ((_last_update_ms == 0) || (AP_HAL::millis() - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS)) { + set_status(AP_Proximity::Proximity_NoData); + } else { + set_status(AP_Proximity::Proximity_Good); + } +} + +// get distance upwards in meters. returns true on success +bool AP_Proximity_RangeFinder::get_upward_distance(float &distance) const +{ + if ((_last_upward_update_ms != 0) && (AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_RANGEFIDER_TIMEOUT_MS)) { + distance = _distance_upward; + return true; + } + return false; +} diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.h b/libraries/AP_Proximity/AP_Proximity_RangeFinder.h new file mode 100644 index 0000000000..ab863107dd --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.h @@ -0,0 +1,35 @@ +#pragma once + +#include "AP_Proximity.h" +#include "AP_Proximity_Backend.h" + +#define PROXIMITY_RANGEFIDER_TIMEOUT_MS 200 // requests timeout after 0.5 seconds + +class AP_Proximity_RangeFinder : public AP_Proximity_Backend +{ + +public: + // constructor + AP_Proximity_RangeFinder(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); + + // update state + void update(void); + + // get maximum and minimum distances (in meters) of sensor + float distance_max() const { return _distance_max; } + float distance_min() const { return _distance_min; }; + + // get distance upwards in meters. returns true on success + bool get_upward_distance(float &distance) const; + +private: + + // horizontal distance support + uint32_t _last_update_ms; // system time of last RangeFinder reading + float _distance_max; // max range of sensor in meters + float _distance_min; // min range of sensor in meters + + // upward distance support + uint32_t _last_upward_update_ms; // system time of last update distance + float _distance_upward; // upward distance in meters +};