diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp index 96c45d5238..659a4af199 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp @@ -39,11 +39,16 @@ const AP_Param::GroupInfo AP_RangeFinder_Backend_CAN::var_info[] = { // constructor 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_Param::setup_object_defaults(this, 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 @@ -82,15 +87,4 @@ bool AP_RangeFinder_Backend_CAN::is_correct_id(uint32_t id) const 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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h index cc93613035..96d62aa94d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h @@ -14,7 +14,8 @@ class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend public: // constructor 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; @@ -53,26 +54,11 @@ protected: AP_Int32 receive_id; // CAN ID to receive for this backend AP_Int32 snr_min; // minimum signal strength to accept packet + MultiCAN* multican_rangefinder; // Allows for multiple CAN rangefinders on a single bus 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; - AP_RangeFinder_Backend_CAN *drivers; -}; - #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp index 180c114f2a..7f97dab592 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp @@ -5,30 +5,6 @@ #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 bool AP_RangeFinder_Benewake_CAN::handle_frame_H30(AP_HAL::CANFrame &frame) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h index 6bc7057bed..a5c6441258 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h @@ -7,14 +7,14 @@ class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend_CAN { 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 bool handle_frame(AP_HAL::CANFrame &frame) override; bool handle_frame_H30(AP_HAL::CANFrame &frame); - -private: - static RangeFinder_MultiCAN *multican; }; #endif // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp index 1c922e121b..30f170addd 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp @@ -6,28 +6,6 @@ #include #include -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 void AP_RangeFinder_NRA24_CAN::update(void) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h index 4a114be187..eb0d472f31 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h @@ -6,7 +6,10 @@ class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN { 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; @@ -18,9 +21,6 @@ public: private: 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 }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp index 5b9465a1dd..16497bb8d8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp @@ -7,31 +7,6 @@ #include #include -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 bool AP_RangeFinder_TOFSenseP_CAN::handle_frame(AP_HAL::CANFrame &frame) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h index a3100eef09..ff88245ef5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h @@ -7,16 +7,15 @@ class AP_RangeFinder_TOFSenseP_CAN : public AP_RangeFinder_Backend_CAN { 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 bool handle_frame(AP_HAL::CANFrame &frame) override; static const struct AP_Param::GroupInfo var_info[]; - -private: - static RangeFinder_MultiCAN *multican_TOFSenseP; - }; #endif // AP_RANGEFINDER_USD1_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp index 780308aabd..19964746c7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp @@ -4,30 +4,6 @@ #include -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 bool AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h index 5a3827585a..445f7d3f50 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h @@ -7,18 +7,15 @@ class AP_RangeFinder_USD1_CAN : public AP_RangeFinder_Backend_CAN { 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 bool handle_frame(AP_HAL::CANFrame &frame) override; static const struct AP_Param::GroupInfo var_info[]; - -private: - - static RangeFinder_MultiCAN *multican; - }; #endif // AP_RANGEFINDER_USD1_CAN_ENABLED