diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp index a62a3ecb4d..fa0c1032b5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp @@ -1,4 +1,5 @@ #include +#include #include "AP_RangeFinder_Benewake_CAN.h" #if HAL_MAX_CAN_PROTOCOL_DRIVERS @@ -22,16 +23,30 @@ const AP_Param::GroupInfo AP_RangeFinder_Benewake_CAN::var_info[] = { AP_GROUPEND }; +Benewake_MultiCAN *AP_RangeFinder_Benewake_CAN::multican; /* constructor */ AP_RangeFinder_Benewake_CAN::AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : - CANSensor("Benewake"), AP_RangeFinder_Backend(_state, _params) { + if (multican == nullptr) { + multican = new 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; + } + AP_Param::setup_object_defaults(this, var_info); - register_driver(AP_CANManager::Driver_Type_Benewake); state.var_info = var_info; } @@ -53,27 +68,40 @@ void AP_RangeFinder_Benewake_CAN::update(void) } // handler for incoming frames. These come in at 100Hz -void AP_RangeFinder_Benewake_CAN::handle_frame(AP_HAL::CANFrame &frame) +bool AP_RangeFinder_Benewake_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; + return false; } if (last_recv_id != -1 && id != last_recv_id) { // changing ID - return; + return false; } last_recv_id = id; + const uint16_t dist_cm = (frame.data[1]<<8) | frame.data[0]; const uint16_t snr = (frame.data[3]<<8) | frame.data[2]; if (snr_min != 0 && snr < uint16_t(snr_min.get())) { // too low signal strength - return; + return true; } _distance_sum_cm += dist_cm; _distance_count++; + return true; +} + +// handle frames from CANSensor, passing to the drivers +void Benewake_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_MAX_CAN_PROTOCOL_DRIVERS diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h index 40bcacecfc..b72aa30d55 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h @@ -5,14 +5,18 @@ #if HAL_MAX_CAN_PROTOCOL_DRIVERS -class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend, public CANSensor { +class Benewake_MultiCAN; + +class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend { public: + friend class Benewake_MultiCAN; + AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); void update() override; - // handler for incoming frames - void handle_frame(AP_HAL::CANFrame &frame) override; + // handler for incoming frames. Return true if consumed + bool handle_frame(AP_HAL::CANFrame &frame); static const struct AP_Param::GroupInfo var_info[]; @@ -20,6 +24,7 @@ protected: virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { return MAV_DISTANCE_SENSOR_RADAR; } + private: float _distance_sum_cm; uint32_t _distance_count; @@ -27,6 +32,26 @@ private: AP_Int32 snr_min; AP_Int32 receive_id; + + static Benewake_MultiCAN *multican; + AP_RangeFinder_Benewake_CAN *next; }; + +// a class to allow for multiple Benewake_CAN backends with one +// CANSensor driver +class Benewake_MultiCAN : public CANSensor { +public: + Benewake_MultiCAN() : CANSensor("Benewake") { + register_driver(AP_CANManager::Driver_Type_Benewake); + } + + // handler for incoming frames + void handle_frame(AP_HAL::CANFrame &frame) override; + + HAL_Semaphore sem; + AP_RangeFinder_Benewake_CAN *drivers; +}; + #endif //HAL_MAX_CAN_PROTOCOL_DRIVERS +