New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
AP_RangeFinder: Add new driver for TOFSense F I2C #25268
Merged
peterbarker
merged 4 commits into
ArduPilot:master
from
rishabsingh3003:nooploop_tof_p_iic
Oct 24, 2023
Merged
Changes from all commits
Commits
Show all changes
4 commits
Select commit
Hold shift + click to select a range
c436d07
AP_RangeFinder: Add NoopLoop TOFSenseF I2c driver
rishabsingh3003 27dde51
AP_RangeFinder: Parse signal strength and status for TOFSenseFI2c
rishabsingh3003 7739361
AP_RangeFinder: Allow new Maxsonar I2C reading even if reading fails
rishabsingh3003 35cda1d
Tools: Update new rangefinders in build_options.py
rishabsingh3003 File filter
Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
159 changes: 159 additions & 0 deletions
159
libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseF_I2C.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,159 @@ | ||
| /* | ||
| 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 | ||
|
|
||
| #define TOFSENSEP_I2C_COMMAND_TAKE_RANGE_READING 0x24 | ||
| #define TOFSENSEP_I2C_COMMAND_SIGNAL_STATUS 0x28 | ||
|
|
||
| #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); | ||
|
|
||
| uint32_t reading_mm; | ||
| uint16_t status; | ||
| uint16_t signal_strength; | ||
|
|
||
| if (!get_reading(reading_mm, signal_strength, status)) { | ||
| _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, TOFSENSEP_I2C_COMMAND_SIGNAL_STATUS}; | ||
|
|
||
| // 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(uint32_t &reading_mm, uint16_t &signal_strength, uint16_t &status) | ||
| { | ||
|
|
||
| struct PACKED { | ||
| uint32_t distance_mm; | ||
| uint32_t signal_strength_and_status; | ||
| } packet; | ||
|
|
||
| // take range reading and read back results | ||
| const bool ret = _dev->transfer(nullptr, 0, (uint8_t *) &packet, sizeof(packet)); | ||
|
|
||
| if (ret) { | ||
| // combine results into distance | ||
| reading_mm = packet.distance_mm; | ||
| signal_strength = (uint16_t)(packet.signal_strength_and_status >> 16); | ||
| status = (uint16_t)(packet.signal_strength_and_status); | ||
| } | ||
|
|
||
| // trigger a new reading | ||
| start_reading(); | ||
|
|
||
| return ret; | ||
| } | ||
|
|
||
| // timer called at 10Hz | ||
| void AP_RangeFinder_TOFSenseF_I2C::timer(void) | ||
| { | ||
| uint32_t dist_mm; | ||
| uint16_t status; | ||
| uint16_t signal_strength; | ||
|
|
||
| if (get_reading(dist_mm, signal_strength, status)) { | ||
| WITH_SEMAPHORE(_sem); | ||
| if (status == 1) { | ||
| // healthy data | ||
| distance_mm = dist_mm; | ||
| 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_mm * 0.001f; | ||
| 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 | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,49 @@ | ||
| #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 | ||
|
|
||
| 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); | ||
|
|
||
| uint32_t distance_mm; | ||
| bool new_distance; // true if we have a new distance | ||
|
|
||
| // get a reading | ||
| bool start_reading(void); | ||
| bool get_reading(uint32_t &reading_mm, uint16_t &signal_strength, uint16_t &status); | ||
| AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev; | ||
| }; | ||
|
|
||
| #endif // AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is a bad pattern, should be using
WITH_SEMAPHORE