From 2142339c5f367abe9696ac652c305112e0574652 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 23 Nov 2023 14:15:34 +1100 Subject: [PATCH] AP_CANManager: support CAN_Dn_PROTOCOL2 for an aux 11 bit protocol --- libraries/AP_CANManager/AP_CANDriver.cpp | 8 +++++++ libraries/AP_CANManager/AP_CANDriver.h | 7 ++++++ libraries/AP_CANManager/AP_CANManager.cpp | 27 +++++++++++++++++++++++ libraries/AP_CANManager/AP_CANManager.h | 6 +++++ libraries/AP_CANManager/AP_CANSensor.cpp | 12 +++++++++- libraries/AP_CANManager/AP_CANSensor.h | 4 ++++ 6 files changed, 63 insertions(+), 1 deletion(-) diff --git a/libraries/AP_CANManager/AP_CANDriver.cpp b/libraries/AP_CANManager/AP_CANDriver.cpp index 1f279c12a690d..0ec7ebbceb2a0 100644 --- a/libraries/AP_CANManager/AP_CANDriver.cpp +++ b/libraries/AP_CANManager/AP_CANDriver.cpp @@ -51,6 +51,14 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = { AP_SUBGROUPPTR(_piccolocan, "PC_", 5, AP_CANManager::CANDriver_Params, AP_PiccoloCAN), #endif + // @Param: PROTOCOL2 + // @DisplayName: Secondary protocol with 11 bit CAN addressing + // @Description: Secondary protocol with 11 bit CAN addressing + // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("PROTOCOL2", 6, AP_CANManager::CANDriver_Params, _driver_type_11bit, float(AP_CAN::Protocol::None)), + AP_GROUPEND }; #endif diff --git a/libraries/AP_CANManager/AP_CANDriver.h b/libraries/AP_CANManager/AP_CANDriver.h index a950535019f16..f93e96cec0e7a 100644 --- a/libraries/AP_CANManager/AP_CANDriver.h +++ b/libraries/AP_CANManager/AP_CANDriver.h @@ -21,6 +21,8 @@ #include class AP_CANManager; +class CANSensor; + class AP_CANDriver { public: @@ -34,4 +36,9 @@ class AP_CANDriver // link protocol drivers with interfaces by adding reference to CANIface virtual bool add_interface(AP_HAL::CANIface* can_iface) = 0; + // add an 11 bit auxillary driver + virtual bool add_11bit_driver(CANSensor *sensor) { return false; } + + // handler for outgoing frames for auxillary drivers + virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) { return false; } }; diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 78be64a97c66c..af4b94e323fd6 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -277,6 +277,7 @@ void AP_CANManager::init() } } #endif + /* register a new CAN driver */ @@ -328,6 +329,32 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver return false; } +// register a new auxillary sensor driver for 11 bit address frames +bool AP_CANManager::register_11bit_driver(AP_CAN::Protocol dtype, CANSensor *sensor, uint8_t &driver_index) +{ + WITH_SEMAPHORE(_sem); + + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + uint8_t drv_num = _interfaces[i]._driver_number; + if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) { + continue; + } + // from 1 based to 0 based + drv_num--; + + if (dtype != (AP_CAN::Protocol)_drv_param[drv_num]._driver_type_11bit.get()) { + continue; + } + if (_drivers[drv_num] != nullptr && + _drivers[drv_num]->add_11bit_driver(sensor)) { + driver_index = drv_num; + return true; + } + } + return false; + +} + // Method used by CAN related library methods to report status and debug info // The result of this method can be accessed via ftp get @SYS/can_log.txt void AP_CANManager::log_text(AP_CANManager::LogLevel loglevel, const char *tag, const char *fmt, ...) diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index 85d4aa31ed31f..5c352ae5b3923 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -34,6 +34,8 @@ #include "AP_CAN.h" +class CANSensor; + class AP_CANManager { public: @@ -63,6 +65,9 @@ class AP_CANManager // register a new driver bool register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver); + // register a new auxillary sensor driver for 11 bit address frames + bool register_11bit_driver(AP_CAN::Protocol dtype, CANSensor *sensor, uint8_t &driver_index); + // returns number of active CAN Drivers uint8_t get_num_drivers(void) const { @@ -141,6 +146,7 @@ class AP_CANManager private: AP_Int8 _driver_type; + AP_Int8 _driver_type_11bit; AP_CANDriver* _uavcan; AP_CANDriver* _piccolocan; }; diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp index 881eb39ce93c8..025dfd8036e2f 100644 --- a/libraries/AP_CANManager/AP_CANSensor.cpp +++ b/libraries/AP_CANManager/AP_CANSensor.cpp @@ -40,7 +40,13 @@ void CANSensor::register_driver(AP_CAN::Protocol dtype) { #if HAL_CANMANAGER_ENABLED if (!AP::can().register_driver(dtype, this)) { - debug_can(AP_CANManager::LOG_ERROR, "Failed to register CANSensor %s", _driver_name); + if (AP::can().register_11bit_driver(dtype, this, _driver_index)) { + is_aux_11bit_driver = true; + _can_driver = AP::can().get_driver(_driver_index); + _initialized = true; + } else { + debug_can(AP_CANManager::LOG_ERROR, "Failed to register CANSensor %s", _driver_name); + } } else { debug_can(AP_CANManager::LOG_INFO, "%s: constructed", _driver_name); } @@ -135,6 +141,10 @@ bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_ return false; } + if (is_aux_11bit_driver && _can_driver != nullptr) { + return _can_driver->write_aux_frame(out_frame, timeout_us); + } + bool read_select = false; bool write_select = true; bool ret = _can_iface->select(read_select, write_select, &out_frame, AP_HAL::micros64() + timeout_us); diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h index a1374821e40e4..e340b71facdd0 100644 --- a/libraries/AP_CANManager/AP_CANSensor.h +++ b/libraries/AP_CANManager/AP_CANSensor.h @@ -72,6 +72,10 @@ class CANSensor : public AP_CANDriver { const uint16_t _stack_size; bool _initialized; uint8_t _driver_index; + + // this is true when we are setup as an auxillary driver using CAN_Dn_PROTOCOL2 + bool is_aux_11bit_driver; + AP_CANDriver *_can_driver; HAL_EventHandle _event_handle; AP_HAL::CANIface* _can_iface;