AP_RangeFinder: added trone in-tree driver

libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp

@@ -0,0 +1,155 @@
+/*
+   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/>.
+ */
+/*
+  driver for trone rangefinder
+ */
+#include "AP_RangeFinder_trone.h"
+
+#include <utility>
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define TRONE_I2C_ADDR 0x30
+
+// registers
+#define TRONE_MEASURE 0x00
+#define TRONE_WHOAMI  0x01
+#define TRONE_WHOAMI_VALUE 0xA1
+
+/*
+   The constructor also initializes 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_trone::AP_RangeFinder_trone(uint8_t bus, RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state)
+    : AP_RangeFinder_Backend(_ranger, instance, _state)
+    , dev(hal.i2c_mgr->get_device(bus, TRONE_I2C_ADDR))
+{
+}
+
+/*
+   detect if a trone rangefinder is connected. We'll detect by
+   trying to take a reading on I2C. If we get a result the sensor is
+   there.
+*/
+AP_RangeFinder_Backend *AP_RangeFinder_trone::detect(uint8_t bus, RangeFinder &_ranger, uint8_t instance,
+                                                     RangeFinder::RangeFinder_State &_state)
+{
+    AP_RangeFinder_trone *sensor = new AP_RangeFinder_trone(bus, _ranger, instance, _state);
+    if (!sensor) {
+        return nullptr;
+    }
+
+    if (!sensor->init()) {
+        delete sensor;
+        return nullptr;
+    }
+
+    return sensor;
+}
+
+/*
+  initialise sensor
+ */
+bool AP_RangeFinder_trone::init(void)
+{
+    if (!dev->get_semaphore()->take(0)) {
+        return false;
+    }
+
+    dev->set_retries(10);
+
+    if (!measure()) {
+        dev->get_semaphore()->give();
+        return false;
+    }
+
+    // give time for the sensor to process the request
+    hal.scheduler->delay(70);
+
+    uint16_t distance_cm;
+    if (!collect(distance_cm)) {
+        dev->get_semaphore()->give();
+        return false;
+    }
+
+    dev->get_semaphore()->give();
+
+    dev->set_retries(1);
+
+    dev->register_periodic_callback(50000,
+                                    FUNCTOR_BIND_MEMBER(&AP_RangeFinder_trone::timer, void));
+
+    return true;
+}
+
+// measure() - ask sensor to make a range reading
+bool AP_RangeFinder_trone::measure()
+{
+    uint8_t cmd = TRONE_MEASURE;
+    return dev->transfer(&cmd, 1, nullptr, 0);
+}
+
+// collect - return last value measured by sensor
+bool AP_RangeFinder_trone::collect(uint16_t &distance_cm)
+{
+    uint8_t d[3];
+
+    // take range reading and read back results
+    if (!dev->transfer(nullptr, 0, d, sizeof(d))) {
+        return false;
+    }
+
+    distance_cm = ((uint16_t(d[0]) << 8) | d[1]) / 10;
+
+    return true;
+}
+
+/*
+  timer called at 20Hz
+*/
+void AP_RangeFinder_trone::timer(void)
+{
+    // take a reading
+    uint16_t distance_cm;
+    if (collect(distance_cm) && _sem->take(0)) {
+        accum.sum += distance_cm;
+        accum.count++;
+        _sem->give();
+    }
+
+    // and immediately ask for a new reading
+    measure();
+}
+
+
+/*
+   update the state of the sensor
+*/
+void AP_RangeFinder_trone::update(void)
+{
+    if (_sem->take(0)) {
+        if (accum.count > 0) {
+            state.distance_cm = accum.sum / accum.count;
+            accum.sum = 0;
+            accum.count = 0;
+            update_status();
+        } else {
+            set_status(RangeFinder::RangeFinder_NoData);
+        }
+         _sem->give();
+    }
+}

libraries/AP_RangeFinder/AP_RangeFinder_trone.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "RangeFinder.h"
+#include "RangeFinder_Backend.h"
+#include <AP_HAL/I2CDevice.h>
+
+class AP_RangeFinder_trone : public AP_RangeFinder_Backend
+{
+public:
+    // static detection function
+    static AP_RangeFinder_Backend *detect(uint8_t bus, RangeFinder &ranger, uint8_t instance,
+                                          RangeFinder::RangeFinder_State &_state);
+
+    // update state
+    void update(void);
+
+private:
+    // constructor
+    AP_RangeFinder_trone(uint8_t bus, RangeFinder &ranger, uint8_t instance,
+                         RangeFinder::RangeFinder_State &_state);
+
+    bool measure(void);
+    bool collect(uint16_t &distance_cm);
+    
+    bool init(void);    
+    void timer(void);
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
+
+    struct {
+        uint32_t sum;
+        uint32_t count;
+    } accum;
+};

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -27,6 +27,7 @@
 #include "AP_RangeFinder_MAVLink.h"
 #include "AP_RangeFinder_LeddarOne.h"
 #include "AP_RangeFinder_uLanding.h"
+#include "AP_RangeFinder_trone.h"
 #include <AP_BoardConfig/AP_BoardConfig.h>
 
 extern const AP_HAL::HAL &hal;
@@ -601,6 +602,11 @@ void RangeFinder::detect_instance(uint8_t instance)
                 hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, _address[instance])));
         }
         break;
+    case RangeFinder_TYPE_TRONE:
+        if (!_add_backend(AP_RangeFinder_trone::detect(0, *this, instance, state[instance]))) {
+            _add_backend(AP_RangeFinder_trone::detect(1, *this, instance, state[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)) {

libraries/AP_RangeFinder/RangeFinder.h

@@ -50,7 +50,8 @@ public:
         RangeFinder_TYPE_MAVLink = 10,
         RangeFinder_TYPE_ULANDING= 11,
         RangeFinder_TYPE_LEDDARONE = 12,
-        RangeFinder_TYPE_MBSER  = 13
+        RangeFinder_TYPE_MBSER  = 13,
+        RangeFinder_TYPE_TRONE  = 14
     };
 
     enum RangeFinder_Function {