Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: Allow multiple USD-D1-CAN

Browse Source
This commit is contained in:
rishabsingh3003 2023-02-28 01:50:51 +05:30 committed by Peter Barker
parent a6041f77e8
commit d084ae0153
3 changed files with 97 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -68,7 +68,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[0], "1_", 25, RangeFinder, AP_RangeFinder_Params),
// @Group: 1_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[0], "1_", 57, RangeFinder, backend_var_info[0]),
#if RANGEFINDER_MAX_INSTANCES > 1
@ -77,7 +77,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[1], "2_", 27, RangeFinder, AP_RangeFinder_Params),
// @Group: 2_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]),
#endif
@ -87,7 +87,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[2], "3_", 29, RangeFinder, AP_RangeFinder_Params),
// @Group: 3_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]),
#endif
@ -97,7 +97,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[3], "4_", 31, RangeFinder, AP_RangeFinder_Params),
// @Group: 4_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]),
#endif
@ -107,7 +107,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[4], "5_", 33, RangeFinder, AP_RangeFinder_Params),
// @Group: 5_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[4], "5_", 34, RangeFinder, backend_var_info[4]),
#endif
@ -117,7 +117,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[5], "6_", 35, RangeFinder, AP_RangeFinder_Params),
// @Group: 6_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[5], "6_", 36, RangeFinder, backend_var_info[5]),
#endif
@ -127,7 +127,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[6], "7_", 37, RangeFinder, AP_RangeFinder_Params),
// @Group: 7_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[6], "7_", 38, RangeFinder, backend_var_info[6]),
#endif
@ -137,7 +137,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[7], "8_", 39, RangeFinder, AP_RangeFinder_Params),
// @Group: 8_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[7], "8_", 40, RangeFinder, backend_var_info[7]),
#endif
@ -147,7 +147,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[8], "9_", 41, RangeFinder, AP_RangeFinder_Params),
// @Group: 9_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[8], "9_", 42, RangeFinder, backend_var_info[8]),
#endif
@ -157,7 +157,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[9], "A_", 43, RangeFinder, AP_RangeFinder_Params),
// @Group: A_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp
AP_SUBGROUPVARPTR(drivers[9], "A_", 44, RangeFinder, backend_var_info[9]),
#endif

59
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp
View File

@ -1,17 +1,47 @@
#include "AP_RangeFinder_USD1_CAN.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#if AP_RANGEFINDER_USD1_CAN_ENABLED
#include <AP_HAL/AP_HAL.h>
const AP_Param::GroupInfo AP_RangeFinder_USD1_CAN::var_info[] = {
// @Param: RECV_ID
// @DisplayName: 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", 12, AP_RangeFinder_USD1_CAN, receive_id, 0),
AP_GROUPEND
};
USD1_MultiCAN *AP_RangeFinder_USD1_CAN::multican;
/*
constructor
*/
AP_RangeFinder_USD1_CAN::AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
CANSensor("USD1"),
AP_RangeFinder_Backend(_state, _params)
{
register_driver(AP_CANManager::Driver_Type_USD1);
if (multican == nullptr) {
multican = new USD1_MultiCAN();
if (multican == nullptr) {
AP_BoardConfig::allocation_error("USD1_CAN");
}
}
{
// add to linked list of drivers
WITH_SEMAPHORE(multican->sem);
auto *prev = multican->drivers;
next = prev;
multican->drivers = this;
}
AP_Param::setup_object_defaults(this, var_info);
state.var_info = var_info;
}
// update state
@ -32,12 +62,35 @@ void AP_RangeFinder_USD1_CAN::update(void)
}
// handler for incoming frames. These come in at 100Hz
void AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame)
bool AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame)
{
WITH_SEMAPHORE(_sem);
const uint16_t id = frame.id & AP_HAL::CANFrame::MaskStdID;
if (receive_id != 0 && id != uint16_t(receive_id.get())) {
// incorrect receive ID
return false;
}
if (last_recv_id != -1 && id != last_recv_id) {
// changing ID
return false;
}
last_recv_id = id;
const uint16_t dist_cm = (frame.data[0]<<8) | frame.data[1];
_distance_sum += dist_cm * 0.01;
_distance_count++;
return true;
}
// handle frames from CANSensor, passing to the drivers
void USD1_MultiCAN::handle_frame(AP_HAL::CANFrame &frame)
{
WITH_SEMAPHORE(sem);
for (auto *d = drivers; d; d=d->next) {
if (d->handle_frame(frame)) {
break;
}
}
}
#endif // AP_RANGEFINDER_USD1_CAN_ENABLED

34
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h
View File

@ -9,15 +9,22 @@
#if AP_RANGEFINDER_USD1_CAN_ENABLED
class AP_RangeFinder_USD1_CAN : public CANSensor, public AP_RangeFinder_Backend {
class USD1_MultiCAN;
class AP_RangeFinder_USD1_CAN : public AP_RangeFinder_Backend {
public:
friend class USD1_MultiCAN;
AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
void update() override;
// handler for incoming frames
void handle_frame(AP_HAL::CANFrame &frame) override;
bool handle_frame(AP_HAL::CANFrame &frame);
static const struct AP_Param::GroupInfo var_info[];
protected:
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_RADAR;
@ -25,6 +32,27 @@ protected:
private:
float _distance_sum;
uint32_t _distance_count;
int32_t last_recv_id = -1;
AP_Int32 receive_id;
static USD1_MultiCAN *multican;
AP_RangeFinder_USD1_CAN *next;
};
// a class to allow for multiple USD1_CAN backends with one
// CANSensor driver
class USD1_MultiCAN : public CANSensor {
public:
USD1_MultiCAN() : CANSensor("USD1") {
register_driver(AP_CANManager::Driver_Type_USD1);
}
// handler for incoming frames
void handle_frame(AP_HAL::CANFrame &frame) override;
HAL_Semaphore sem;
AP_RangeFinder_USD1_CAN *drivers;
};
#endif // AP_RANGEFINDER_USD1_CAN_ENABLED