diff --git a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp index 4f0225c65b..9d9618286c 100644 --- a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp @@ -32,26 +32,14 @@ const AP_Param::GroupInfo AP_Proximity_MR72_CAN::var_info[] = { AP_GROUPEND }; -MR72_MultiCAN *AP_Proximity_MR72_CAN::multican; - AP_Proximity_MR72_CAN::AP_Proximity_MR72_CAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params): AP_Proximity_Backend(_frontend, _state, _params) { - if (multican == nullptr) { - multican = new MR72_MultiCAN(); - if (multican == nullptr) { - AP_BoardConfig::allocation_error("MR72_CAN"); - } - } - - { - // add to linked list of drivers - WITH_SEMAPHORE(multican->sem); - auto *prev = multican->drivers; - next = prev; - multican->drivers = this; + multican_MR72 = new MultiCAN{FUNCTOR_BIND_MEMBER(&AP_Proximity_MR72_CAN::handle_frame, bool, AP_HAL::CANFrame &), AP_CAN::Protocol::NanoRadar, "MR72 MultiCAN"}; + if (multican_MR72 == nullptr) { + AP_BoardConfig::allocation_error("Failed to create proximity multican"); } AP_Param::setup_object_defaults(this, var_info); @@ -132,15 +120,4 @@ bool AP_Proximity_MR72_CAN::parse_distance_message(AP_HAL::CANFrame &frame) return true; } -// handle frames from CANSensor, passing to the drivers -void MR72_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 // HAL_PROXIMITY_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h index e2a3dcbb9d..210e8036b0 100644 --- a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h +++ b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h @@ -45,24 +45,7 @@ private: AP_Int32 receive_id; // ID of the sensor - static MR72_MultiCAN *multican; // linked list - AP_Proximity_MR72_CAN *next; -}; - -// a class to allow for multiple MR_72 backends with one -// CANSensor driver -class MR72_MultiCAN : public CANSensor { -public: - MR72_MultiCAN() : CANSensor("MR72") { - register_driver(AP_CAN::Protocol::MR72); - } - - // handler for incoming frames - void handle_frame(AP_HAL::CANFrame &frame) override; - - HAL_Semaphore sem; - AP_Proximity_MR72_CAN *drivers; - + MultiCAN* multican_MR72; // Allows for multiple CAN rangefinders on a single bus }; #endif // HAL_PROXIMITY_ENABLED