From 9a02967e3d36b69b720564bdb56e79c48a28696a Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Sat, 14 Oct 2023 19:44:26 -0400 Subject: [PATCH] AP_RangeFinder: Add NoopLoop TOFSenseF I2c driver --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 17 +++ libraries/AP_RangeFinder/AP_RangeFinder.h | 1 + .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 2 +- .../AP_RangeFinder_TOFSenseF_I2C.cpp | 141 ++++++++++++++++++ .../AP_RangeFinder_TOFSenseF_I2C.h | 50 +++++++ .../AP_RangeFinder/AP_RangeFinder_config.h | 5 +- 6 files changed, 214 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 9651b919a4..646011b373 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/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 #include @@ -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; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 7365ed3912..6ffda40ff9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/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, }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 3f3cd9511d..8d6a2f4435 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/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), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.cpp new file mode 100644 index 0000000000..c41817da4f --- /dev/null +++ b/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 . + */ + +#include "AP_RangeFinder_TOFSenseF_I2C.h" + +#if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED + +#include + +#include +#include + +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 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 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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.h new file mode 100644 index 0000000000..d7d62e5ace --- /dev/null +++ b/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 + +#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 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 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 _dev; +}; + +#endif // AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h index 36c5dcf89c..d3dee401d7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/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