AP_Proximity: add rangefinder driver

This allows avoidance using upward or forward facing range finders

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;

libraries/AP_Proximity/AP_Proximity.h

@@ -19,7 +19,7 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_SerialManager/AP_SerialManager.h>
-
+#include <AP_RangeFinder/AP_RangeFinder.h>
 
 #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;

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_Proximity_RangeFinder.h"
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <ctype.h>
+#include <stdio.h>
+
+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; i<rngfnd->num_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;
+}

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
+};