Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: move multican to AP_CANSensor

This commit is contained in:
rishabsingh3003 2024-02-22 21:01:25 -05:00 committed by Andrew Tridgell
parent 8a24699bfa
commit 89d8a1351c
10 changed files with 25 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp
View File

@ -39,11 +39,16 @@ const AP_Param::GroupInfo AP_RangeFinder_Backend_CAN::var_info[] = {
// constructor // constructor
AP_RangeFinder_Backend_CAN::AP_RangeFinder_Backend_CAN( AP_RangeFinder_Backend_CAN::AP_RangeFinder_Backend_CAN(
RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_CAN::Protocol can_type,
const char *driver_name) :
AP_RangeFinder_Backend(_state, _params) AP_RangeFinder_Backend(_state, _params)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
state.var_info = var_info; state.var_info = var_info;
multican_rangefinder = new MultiCAN{FUNCTOR_BIND_MEMBER(&AP_RangeFinder_Backend_CAN::handle_frame, bool, AP_HAL::CANFrame &), can_type, driver_name};
if (multican_rangefinder == nullptr) {
AP_BoardConfig::allocation_error("Failed to create rangefinder multican");
}
} }
// update the state of the sensor // update the state of the sensor
@ -82,15 +87,4 @@ bool AP_RangeFinder_Backend_CAN::is_correct_id(uint32_t id) const
return true; 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 #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS

20
libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h
View File

@ -14,7 +14,8 @@ class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend
public: public:
// constructor // constructor
AP_RangeFinder_Backend_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Backend_CAN(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params); AP_RangeFinder_Params &_params, AP_CAN::Protocol can_type,
const char *driver_name);
friend class RangeFinder_MultiCAN; friend class RangeFinder_MultiCAN;
@ -53,26 +54,11 @@ protected:
AP_Int32 receive_id; // CAN ID to receive for this backend AP_Int32 receive_id; // CAN ID to receive for this backend
AP_Int32 snr_min; // minimum signal strength to accept packet AP_Int32 snr_min; // minimum signal strength to accept packet
MultiCAN* multican_rangefinder; // Allows for multiple CAN rangefinders on a single bus
private: private:
float _distance_sum; // meters float _distance_sum; // meters
uint32_t _distance_count; 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;
AP_RangeFinder_Backend_CAN *drivers;
};
#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS

24
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp
View File

@ -5,30 +5,6 @@
#if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED
RangeFinder_MultiCAN *AP_RangeFinder_Benewake_CAN::multican;
/*
constructor
*/
AP_RangeFinder_Benewake_CAN::AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend_CAN(_state, _params)
{
if (multican == nullptr) {
multican = new RangeFinder_MultiCAN(AP_CAN::Protocol::Benewake, "Benewake MultiCAN");
if (multican == nullptr) {
AP_BoardConfig::allocation_error("Benewake_CAN");
}
}
{
// add to linked list of drivers
WITH_SEMAPHORE(multican->sem);
auto *prev = multican->drivers;
next = prev;
multican->drivers = this;
}
}
// handler for incoming frames for H30 radar // handler for incoming frames for H30 radar
bool AP_RangeFinder_Benewake_CAN::handle_frame_H30(AP_HAL::CANFrame &frame) bool AP_RangeFinder_Benewake_CAN::handle_frame_H30(AP_HAL::CANFrame &frame)
{ {

8
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h
View File

@ -7,14 +7,14 @@
class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend_CAN { class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend_CAN {
public: public:
AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::Benewake, "benewake")
{
}
// handler for incoming frames. Return true if consumed // handler for incoming frames. Return true if consumed
bool handle_frame(AP_HAL::CANFrame &frame) override; bool handle_frame(AP_HAL::CANFrame &frame) override;
bool handle_frame_H30(AP_HAL::CANFrame &frame); bool handle_frame_H30(AP_HAL::CANFrame &frame);
private:
static RangeFinder_MultiCAN *multican;
}; };
#endif // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED #endif // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED

22
libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp
View File

@ -6,28 +6,6 @@
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
RangeFinder_MultiCAN *AP_RangeFinder_NRA24_CAN::multican_NRA24;
// constructor
AP_RangeFinder_NRA24_CAN::AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend_CAN(_state, _params)
{
if (multican_NRA24 == nullptr) {
multican_NRA24 = new RangeFinder_MultiCAN(AP_CAN::Protocol::NanoRadar_NRA24, "NRA24 MultiCAN");
if (multican_NRA24 == nullptr) {
AP_BoardConfig::allocation_error("Rangefinder_MultiCAN");
}
}
{
// add to linked list of drivers
WITH_SEMAPHORE(multican_NRA24->sem);
auto *prev = multican_NRA24->drivers;
next = prev;
multican_NRA24->drivers = this;
}
}
// update the state of the sensor // update the state of the sensor
void AP_RangeFinder_NRA24_CAN::update(void) void AP_RangeFinder_NRA24_CAN::update(void)
{ {

8
libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h
View File

@ -6,7 +6,10 @@
class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN { class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN {
public: public:
AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::NanoRadar, "nra24")
{
}
void update(void) override; void update(void) override;
@ -18,9 +21,6 @@ public:
private: private:
uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U) >> 4U); } uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U) >> 4U); }
static RangeFinder_MultiCAN *multican_NRA24;
uint32_t last_heartbeat_ms; // last status message received from the sensor uint32_t last_heartbeat_ms; // last status message received from the sensor
}; };

25
libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp
View File

@ -7,31 +7,6 @@
#include <AP_HAL/utility/sparse-endian.h> #include <AP_HAL/utility/sparse-endian.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
RangeFinder_MultiCAN *AP_RangeFinder_TOFSenseP_CAN::multican_TOFSenseP;
/*
constructor
*/
AP_RangeFinder_TOFSenseP_CAN::AP_RangeFinder_TOFSenseP_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend_CAN(_state, _params)
{
if (multican_TOFSenseP == nullptr) {
multican_TOFSenseP = new RangeFinder_MultiCAN(AP_CAN::Protocol::TOFSenseP, "TOFSenseP MultiCAN");
if (multican_TOFSenseP == nullptr) {
AP_BoardConfig::allocation_error("Rangefinder_MultiCAN");
}
}
{
// add to linked list of drivers
WITH_SEMAPHORE(multican_TOFSenseP->sem);
auto *prev = multican_TOFSenseP->drivers;
next = prev;
multican_TOFSenseP->drivers = this;
}
}
// handler for incoming frames. These come in at 10-30Hz // handler for incoming frames. These come in at 10-30Hz
bool AP_RangeFinder_TOFSenseP_CAN::handle_frame(AP_HAL::CANFrame &frame) bool AP_RangeFinder_TOFSenseP_CAN::handle_frame(AP_HAL::CANFrame &frame)
{ {

9
libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h
View File

@ -7,16 +7,15 @@
class AP_RangeFinder_TOFSenseP_CAN : public AP_RangeFinder_Backend_CAN { class AP_RangeFinder_TOFSenseP_CAN : public AP_RangeFinder_Backend_CAN {
public: public:
AP_RangeFinder_TOFSenseP_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); AP_RangeFinder_TOFSenseP_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::TOFSenseP, "tofsensep")
{
}
// handler for incoming frames // handler for incoming frames
bool handle_frame(AP_HAL::CANFrame &frame) override; bool handle_frame(AP_HAL::CANFrame &frame) override;
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private:
static RangeFinder_MultiCAN *multican_TOFSenseP;
}; };
#endif // AP_RANGEFINDER_USD1_CAN_ENABLED #endif // AP_RANGEFINDER_USD1_CAN_ENABLED

24
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp
View File

@ -4,30 +4,6 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
RangeFinder_MultiCAN *AP_RangeFinder_USD1_CAN::multican;
/*
constructor
*/
AP_RangeFinder_USD1_CAN::AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend_CAN(_state, _params)
{
if (multican == nullptr) {
multican = new RangeFinder_MultiCAN(AP_CAN::Protocol::USD1, "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;
}
}
// handler for incoming frames. These come in at 100Hz // handler for incoming frames. These come in at 100Hz
bool AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame) bool AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame)
{ {

11
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h
View File

@ -7,18 +7,15 @@
class AP_RangeFinder_USD1_CAN : public AP_RangeFinder_Backend_CAN { class AP_RangeFinder_USD1_CAN : public AP_RangeFinder_Backend_CAN {
public: public:
AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::USD1, "usd1")
{
}
// handler for incoming frames // handler for incoming frames
bool handle_frame(AP_HAL::CANFrame &frame) override; bool handle_frame(AP_HAL::CANFrame &frame) override;
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private:
static RangeFinder_MultiCAN *multican;
}; };
#endif // AP_RANGEFINDER_USD1_CAN_ENABLED #endif // AP_RANGEFINDER_USD1_CAN_ENABLED