AP_RangeFinder: move multican to AP_CANSensor

This commit is contained in:
rishabsingh3003 2024-02-22 21:01:25 -05:00 committed by Peter Barker
parent f32e6b733b
commit 22ace12538
10 changed files with 25 additions and 144 deletions

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

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

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)
{ {

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

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)
{ {

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
}; };

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)
{ {

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

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)
{ {

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