AP_Rangefinder: Refactor CAN based drivers. Add TOFSenseP driver & NRA24 #24097
There are no files selected for viewing
This file contains hidden or 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 hidden or 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 |
|---|---|---|
|
|
@@ -27,5 +27,7 @@ class AP_CAN { | |
| Scripting = 10, | ||
| Benewake = 11, | ||
| Scripting2 = 12, | ||
| TOFSenseP = 13, | ||
| NanoRadar_NRA24 = 14, | ||
| }; | ||
| }; | ||
This file contains hidden or 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 hidden or 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 hidden or 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 hidden or 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,96 @@ | ||
| /* | ||
| 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_RangeFinder_Backend_CAN.h" | ||
|
|
||
| #if HAL_MAX_CAN_PROTOCOL_DRIVERS | ||
|
|
||
| const AP_Param::GroupInfo AP_RangeFinder_Backend_CAN::var_info[] = { | ||
|
|
||
| // @Param: RECV_ID | ||
| // @DisplayName: RangeFinder CAN receive ID | ||
| // @Description: The receive ID of the CAN frames. A value of zero means all IDs are accepted. | ||
| // @Range: 0 65535 | ||
| // @User: Advanced | ||
| AP_GROUPINFO("RECV_ID", 10, AP_RangeFinder_Backend_CAN, receive_id, 0), | ||
|
|
||
| // @Param: SNR_MIN | ||
| // @DisplayName: RangeFinder Minimum signal strength | ||
| // @Description: RangeFinder Minimum signal strength (SNR) to accept distance | ||
| // @Range: 0 65535 | ||
|
Contributor
Author
There was a problem hiding this comment. Will do in a follow up PR |
||
| // @User: Advanced | ||
| AP_GROUPINFO("SNR_MIN", 11, AP_RangeFinder_Backend_CAN, snr_min, 0), | ||
|
|
||
| AP_GROUPEND | ||
| }; | ||
|
|
||
| // constructor | ||
| AP_RangeFinder_Backend_CAN::AP_RangeFinder_Backend_CAN( | ||
| RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : | ||
| AP_RangeFinder_Backend(_state, _params) | ||
| { | ||
| AP_Param::setup_object_defaults(this, var_info); | ||
| state.var_info = var_info; | ||
| } | ||
|
|
||
| // update the state of the sensor | ||
| void AP_RangeFinder_Backend_CAN::update(void) | ||
| { | ||
| if (get_reading(state.distance_m)) { | ||
| // update range_valid state based on distance measured | ||
| state.last_reading_ms = AP_HAL::millis(); | ||
| update_status(); | ||
| } else if (AP_HAL::millis() - state.last_reading_ms >= read_timeout_ms()) { | ||
| set_status(RangeFinder::Status::NoData); | ||
| } | ||
| } | ||
|
|
||
| // get distance measurement | ||
| bool AP_RangeFinder_Backend_CAN::get_reading(float &reading_m) | ||
| { | ||
| WITH_SEMAPHORE(_sem); | ||
| if (_distance_count != 0) { | ||
| reading_m = _distance_sum / _distance_count; | ||
| _distance_sum = 0; | ||
| _distance_count = 0; | ||
| return true; | ||
| } | ||
|
|
||
| return false; | ||
| } | ||
|
|
||
| // return true if the CAN ID is correct | ||
| bool AP_RangeFinder_Backend_CAN::is_correct_id(uint32_t id) const | ||
| { | ||
| if (receive_id != 0 && id != uint32_t(receive_id.get())) { | ||
| // incorrect receive ID | ||
| return false; | ||
| } | ||
| return true; | ||
| } | ||
|
|
||
| // handle frames from CANSensor, passing to the drivers | ||
| void RangeFinder_MultiCAN::handle_frame(AP_HAL::CANFrame &frame) | ||
| { | ||
| WITH_SEMAPHORE(sem); | ||
| for (auto *d = drivers; d != nullptr; d=d->next) { | ||
| if (d->handle_frame(frame)) { | ||
| break; | ||
| } | ||
| } | ||
| } | ||
|
|
||
| #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS | ||
This file contains hidden or 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,78 @@ | ||
| #pragma once | ||
|
|
||
| #include "AP_RangeFinder_Backend.h" | ||
|
|
||
| #if HAL_MAX_CAN_PROTOCOL_DRIVERS | ||
|
|
||
| #include <AP_CANManager/AP_CANSensor.h> | ||
| #include <AP_BoardConfig/AP_BoardConfig.h> | ||
|
|
||
| class RangeFinder_MultiCAN; | ||
|
|
||
| class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend | ||
| { | ||
| public: | ||
| // constructor | ||
| AP_RangeFinder_Backend_CAN(RangeFinder::RangeFinder_State &_state, | ||
| AP_RangeFinder_Params &_params); | ||
|
|
||
| friend class RangeFinder_MultiCAN; | ||
|
|
||
| static const struct AP_Param::GroupInfo var_info[]; | ||
|
|
||
| protected: | ||
|
|
||
| // update state | ||
| virtual void update(void) override; | ||
|
|
||
| // get distance measurement | ||
| bool get_reading(float &reading_m); | ||
|
|
||
| // it is essential that anyone relying on the base-class update to implement this | ||
| virtual bool handle_frame(AP_HAL::CANFrame &frame) = 0; | ||
|
|
||
| // maximum time between readings before we change state to NoData: | ||
| virtual uint32_t read_timeout_ms() const { return 200; } | ||
|
|
||
| virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { | ||
| return MAV_DISTANCE_SENSOR_RADAR; | ||
| } | ||
|
|
||
| // return true if the CAN ID is correct | ||
| bool is_correct_id(uint32_t can_id) const; | ||
|
|
||
| // set distance and count | ||
| void accumulate_distance_m(float distance_m) { | ||
| _distance_sum += distance_m; | ||
| _distance_count++; | ||
| }; | ||
|
|
||
| // linked list | ||
| AP_RangeFinder_Backend_CAN *next; | ||
|
|
||
| AP_Int32 receive_id; // CAN ID to receive for this backend | ||
| AP_Int32 snr_min; // minimum signal strength to accept packet | ||
|
|
||
| private: | ||
|
|
||
| float _distance_sum; // meters | ||
| uint32_t _distance_count; | ||
| }; | ||
|
|
||
| // a class to allow for multiple CAN backends with one | ||
| // CANSensor driver | ||
| class RangeFinder_MultiCAN : public CANSensor { | ||
| public: | ||
| RangeFinder_MultiCAN(AP_CAN::Protocol can_type, const char *driver_name) : CANSensor(driver_name) { | ||
| register_driver(can_type); | ||
| } | ||
|
|
||
| // handler for incoming frames | ||
| void handle_frame(AP_HAL::CANFrame &frame) override; | ||
|
|
||
| // Semaphore for access to shared backend data | ||
| HAL_Semaphore sem; | ||
|
rishabsingh3003 marked this conversation as resolved.
|
||
| AP_RangeFinder_Backend_CAN *drivers; | ||
| }; | ||
|
|
||
| #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS | ||
Oops, something went wrong.
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.
Uh oh!
There was an error while loading. Please reload this page.