AP_RangeFinder: Add NoopLoop TOFSenseF I2c driver

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -55,6 +55,7 @@
 #include "AP_RangeFinder_NoopLoop.h"
 #include "AP_RangeFinder_TOFSenseP_CAN.h"
 #include "AP_RangeFinder_NRA24_CAN.h"
+#include "AP_RangeFinder_TOFSenseF_I2C.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -556,6 +557,22 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         _add_backend(new AP_RangeFinder_NRA24_CAN(state[instance], params[instance]), instance);
 #endif
         break;
+    case Type::TOFSenseF_I2C: {
+#if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED
+        uint8_t addr = TOFSENSEP_I2C_DEFAULT_ADDR;
+        if (params[instance].address != 0) {
+            addr = params[instance].address;
+        }
+        FOREACH_I2C(i) {
+            if (_add_backend(AP_RangeFinder_TOFSenseF_I2C::detect(state[instance], params[instance],
+                                                                  hal.i2c_mgr->get_device(i, addr)),
+                             instance)) {
+                break;
+            }
+        }
+        break;
+#endif
+    }
 
     case Type::NONE:
         break;

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -96,6 +96,7 @@ public:
         NoopLoop_P = 37,
         TOFSenseP_CAN = 38,
         NRA24_CAN = 39,
+        TOFSenseF_I2C = 40,
         SIM = 100,
     };
 

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -10,7 +10,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: Type of connected rangefinder
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense, 38:NoopLoop_TOFSense_CAN, 39: NRA24_CAN, 100:SITL
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense, 38:NoopLoop_TOFSense_CAN, 39: NRA24_CAN, 40: NoopLoop_TOFSenseF_I2C, 100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),
 

libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.cpp

@@ -0,0 +1,141 @@
+/*
+   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_RangeFinder_TOFSenseF_I2C.h"
+
+#if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED
+
+#include <utility>
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/sparse-endian.h>
+
+extern const AP_HAL::HAL& hal;
+
+AP_RangeFinder_TOFSenseF_I2C::AP_RangeFinder_TOFSenseF_I2C(RangeFinder::RangeFinder_State &_state,
+                                                           AP_RangeFinder_Params &_params,
+                                                           AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
+    : AP_RangeFinder_Backend(_state, _params)
+    , _dev(std::move(dev))
+{
+}
+
+// detect if a TOFSenseP 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_TOFSenseF_I2C::detect(RangeFinder::RangeFinder_State &_state,
+																AP_RangeFinder_Params &_params,
+                                                             AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
+{
+    if (!dev) {
+        return nullptr;
+    }
+
+    AP_RangeFinder_TOFSenseF_I2C *sensor
+        = new AP_RangeFinder_TOFSenseF_I2C(_state, _params, std::move(dev));
+    if (!sensor) {
+        return nullptr;
+    }
+
+    if (!sensor->_init()) {
+        delete sensor;
+        return nullptr;
+    }
+
+    return sensor;
+}
+
+// initialise sensor
+bool AP_RangeFinder_TOFSenseF_I2C::_init(void)
+{
+    _dev->get_semaphore()->take_blocking();
+
+    if (!start_reading()) {
+        _dev->get_semaphore()->give();
+        return false;
+    }
+
+    // give time for the sensor to process the request
+    hal.scheduler->delay(100);
+
+    uint16_t reading_cm;
+    if (!get_reading(reading_cm)) {
+        _dev->get_semaphore()->give();
+        return false;
+    }
+
+    _dev->get_semaphore()->give();
+
+    _dev->register_periodic_callback(100000,
+                                     FUNCTOR_BIND_MEMBER(&AP_RangeFinder_TOFSenseF_I2C::_timer, void));
+
+    return true;
+}
+
+// start_reading() - ask sensor to make a range reading
+bool AP_RangeFinder_TOFSenseF_I2C::start_reading()
+{
+    uint8_t cmd = TOFSENSEP_I2C_COMMAND_TAKE_RANGE_READING;
+
+    // send command to take reading
+    return _dev->transfer(&cmd, sizeof(cmd), nullptr, 0);
+}
+
+// read - return last value measured by sensor
+bool AP_RangeFinder_TOFSenseF_I2C::get_reading(uint16_t &reading_cm)
+{
+    le32_t val;
+
+    // take range reading and read back results
+    bool ret = _dev->transfer(nullptr, 0, (uint8_t *) &val, sizeof(val));
+
+    if (ret) {
+        // combine results into distance
+        reading_cm = le32toh(val);
+
+        // trigger a new reading
+        start_reading();
+    }
+
+    return ret;
+}
+
+//  timer called at 10Hz
+void AP_RangeFinder_TOFSenseF_I2C::_timer(void)
+{
+    uint16_t d;
+    if (get_reading(d)) {
+        WITH_SEMAPHORE(_sem);
+        distance = d;
+        new_distance = true;
+        state.last_reading_ms = AP_HAL::millis();
+    }
+}
+
+// update the state of the sensor
+void AP_RangeFinder_TOFSenseF_I2C::update(void)
+{
+    WITH_SEMAPHORE(_sem);
+    if (new_distance) {
+        state.distance_m = distance * 0.01f;
+        new_distance = false;
+        update_status();
+    } else if (AP_HAL::millis() - state.last_reading_ms > 300) {
+        // if no updates for 0.3 seconds set no-data
+        set_status(RangeFinder::Status::NoData);
+    }
+}
+
+#endif  // AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include "AP_RangeFinder_config.h"
+
+#if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend.h"
+
+#include <AP_HAL/I2CDevice.h>
+
+#define TOFSENSEP_I2C_DEFAULT_ADDR   0x08
+#define TOFSENSEP_I2C_COMMAND_TAKE_RANGE_READING 0x24
+
+class AP_RangeFinder_TOFSenseF_I2C : public AP_RangeFinder_Backend
+{
+public:
+    // static detection function
+    static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state,
+                                          AP_RangeFinder_Params &_params,
+                                          AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
+
+    // update state
+    void update(void) override;
+
+protected:
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_LASER;
+    }
+
+private:
+    // constructor
+    AP_RangeFinder_TOFSenseF_I2C(RangeFinder::RangeFinder_State &_state,
+    								AP_RangeFinder_Params &_params,
+                                 AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
+
+    bool _init(void);
+    void _timer(void);
+
+    uint16_t distance;
+    bool new_distance;
+
+    // start a reading
+    bool start_reading(void);
+    bool get_reading(uint16_t &reading_cm);
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
+};
+
+#endif  // AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_config.h

@@ -121,7 +121,6 @@
 #define AP_RANGEFINDER_NOOPLOOP_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
 #endif
 
-
 #ifndef AP_RANGEFINDER_NRA24_CAN_ENABLED
 #define AP_RANGEFINDER_NRA24_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
 #endif
@@ -146,6 +145,10 @@
 #define AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
 #endif
 
+#ifndef AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED
+#define AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
+#endif
+
 #ifndef AP_RANGEFINDER_TRI2C_ENABLED
 #define AP_RANGEFINDER_TRI2C_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
 #endif