Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   generic CAN sensor class, for easy creation of CAN sensors using proprietary protocols
      17             :  */
      18             : #include <AP_HAL/AP_HAL.h>
      19             : 
      20             : #if HAL_MAX_CAN_PROTOCOL_DRIVERS
      21             : 
      22             : #include <AP_Scheduler/AP_Scheduler.h>
      23             : #include "AP_CANSensor.h"
      24             : #include <AP_BoardConfig/AP_BoardConfig.h>
      25             : 
      26             : extern const AP_HAL::HAL& hal;
      27             : 
      28           0 : CANSensor::CANSensor(const char *driver_name, uint16_t stack_size) :
      29           0 :     _driver_name(driver_name),
      30           0 :     _stack_size(stack_size)
      31           0 : {}
      32             : 
      33             : 
      34           0 : void CANSensor::register_driver(AP_CAN::Protocol dtype)
      35             : {
      36             : #if HAL_CANMANAGER_ENABLED
      37           0 :     if (!AP::can().register_driver(dtype, this)) {
      38           0 :         if (AP::can().register_11bit_driver(dtype, this, _driver_index)) {
      39           0 :             is_aux_11bit_driver = true;
      40           0 :             _can_driver = AP::can().get_driver(_driver_index);
      41           0 :             _initialized = true;
      42             :         }
      43             :     }
      44             : #elif defined(HAL_BUILD_AP_PERIPH)
      45             :     register_driver_periph(dtype);
      46             : #endif
      47           0 : }
      48             : 
      49             : 
      50             : #ifdef HAL_BUILD_AP_PERIPH
      51             : CANSensor::CANSensor_Periph CANSensor::_periph[HAL_NUM_CAN_IFACES];
      52             : 
      53             : void CANSensor::register_driver_periph(const AP_CAN::Protocol dtype)
      54             : {
      55             :     for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
      56             :         if (_periph[i].protocol != dtype) {
      57             :             continue;
      58             :         }
      59             : 
      60             :         if (!add_interface(_periph[i].iface)) {
      61             :             continue;
      62             :         }
      63             : 
      64             :         init(0); // TODO: allow multiple drivers
      65             :         return;
      66             :     }
      67             : }
      68             : #endif
      69             : 
      70           0 : void CANSensor::init(uint8_t driver_index)
      71             : {
      72           0 :     _driver_index = driver_index;
      73             : 
      74           0 :     if (_initialized) {
      75           0 :         return;
      76             :     }
      77             : 
      78             : #ifndef HAL_BUILD_AP_PERIPH
      79             :     // get CAN manager instance
      80           0 :     _can_driver = AP::can().get_driver(driver_index);
      81             : 
      82           0 :     if (_can_driver == nullptr) {
      83           0 :         return;
      84             :     }
      85             : #endif
      86             : 
      87             :     // start thread for receiving and sending CAN frames
      88           0 :     if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&CANSensor::loop, void), _driver_name, _stack_size, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
      89           0 :         return;
      90             :     }
      91             : 
      92           0 :     _initialized = true;
      93             : }
      94             : 
      95           0 : bool CANSensor::add_interface(AP_HAL::CANIface* can_iface)
      96             : {
      97           0 :     if (_can_iface != nullptr) {
      98           0 :         return false;
      99             :     }
     100             : 
     101           0 :     _can_iface = can_iface;
     102             : 
     103           0 :     if (_can_iface == nullptr) {
     104           0 :         return false;
     105             :     }
     106             : 
     107           0 :     if (!_can_iface->is_initialized()) {
     108           0 :         return false;
     109             :     }
     110             : 
     111           0 :     if (!_can_iface->set_event_handle(&sem_handle)) {
     112           0 :         return false;
     113             :     }
     114           0 :     return true;
     115             : }
     116             : 
     117           0 : bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us)
     118             : {
     119           0 :     if (!_initialized) {
     120           0 :         return false;
     121             :     }
     122             : 
     123           0 :     if (is_aux_11bit_driver && _can_driver != nullptr) {
     124           0 :         return _can_driver->write_aux_frame(out_frame, timeout_us);
     125             :     }
     126             : 
     127           0 :     bool read_select = false;
     128           0 :     bool write_select = true;
     129           0 :     bool ret = _can_iface->select(read_select, write_select, &out_frame, AP_HAL::micros64() + timeout_us);
     130           0 :     if (!ret || !write_select) {
     131           0 :         return false;
     132             :     }
     133             : 
     134           0 :     uint64_t deadline = AP_HAL::micros64() + 2000000;
     135           0 :     return (_can_iface->send(out_frame, deadline, AP_HAL::CANIface::AbortOnError) == 1);
     136             : }
     137             : 
     138           0 : void CANSensor::loop()
     139             : {
     140           0 :     while (!hal.scheduler->is_system_initialized()) {
     141             :         // don't process packets till startup complete
     142           0 :         hal.scheduler->delay(1);
     143             :     }
     144             : 
     145             : #ifdef HAL_BUILD_AP_PERIPH
     146             :     const uint32_t LOOP_INTERVAL_US = 1000;
     147             : #else
     148           0 :     const uint32_t LOOP_INTERVAL_US = AP::scheduler().get_loop_period_us();
     149             : #endif
     150             : 
     151             :     while (true) {
     152           0 :         uint64_t deadline_us = AP_HAL::micros64() + LOOP_INTERVAL_US;
     153             : 
     154             :         // wait to receive frame
     155           0 :         bool read_select = true;
     156           0 :         bool write_select = false;
     157           0 :         bool ret = _can_iface->select(read_select, write_select, nullptr, deadline_us);
     158           0 :         if (ret && read_select) {
     159             :             uint64_t time;
     160           0 :             AP_HAL::CANIface::CanIOFlags flags {};
     161             : 
     162           0 :             AP_HAL::CANFrame frame;
     163           0 :             int16_t res = _can_iface->receive(frame, time, flags);
     164             : 
     165           0 :             if (res == 1) {
     166           0 :                 handle_frame(frame);
     167             :             }
     168             :         }
     169           0 :     }
     170             : }
     171             : 
     172             : MultiCAN::MultiCANLinkedList* MultiCAN::callbacks;
     173             : 
     174           0 : MultiCAN::MultiCAN(ForwardCanFrame cf, AP_CAN::Protocol can_type, const char *driver_name) :
     175           0 :         CANSensor(driver_name)
     176             : {
     177           0 :     if (callbacks == nullptr) {
     178           0 :         callbacks = NEW_NOTHROW MultiCANLinkedList();
     179             :     }
     180           0 :     if (callbacks == nullptr) {
     181           0 :         AP_BoardConfig::allocation_error("Failed to create multican callback");
     182             :     }
     183             : 
     184             :     // Register new driver
     185           0 :     register_driver(can_type);
     186           0 :     callbacks->register_callback(cf);
     187           0 : }
     188             : 
     189             : // handle a received frame from the CAN bus
     190           0 : void MultiCAN::handle_frame(AP_HAL::CANFrame &frame)
     191             : {
     192           0 :     if (callbacks != nullptr) {
     193           0 :         callbacks->handle_frame(frame);
     194             :     }
     195             : 
     196           0 : }
     197             : 
     198             : // register a callback for a CAN frame by adding it to the linked list
     199           0 : void MultiCAN::MultiCANLinkedList::register_callback(ForwardCanFrame callback)
     200             : {
     201           0 :     CANSensor_Multi* newNode = NEW_NOTHROW CANSensor_Multi();
     202           0 :     if (newNode == nullptr) {
     203           0 :         AP_BoardConfig::allocation_error("Failed to create multican node");
     204             :     }
     205           0 :     WITH_SEMAPHORE(sem);
     206             :     {
     207           0 :         newNode->_callback = callback;
     208           0 :         newNode->next = head;
     209           0 :         head = newNode;
     210             :     }
     211           0 : }
     212             : 
     213             : // distribute the CAN frame to the registered callbacks
     214           0 : void MultiCAN::MultiCANLinkedList::handle_frame(AP_HAL::CANFrame &frame)
     215             : {
     216           0 :     WITH_SEMAPHORE(sem);
     217           0 :     for (CANSensor_Multi* current = head; current != nullptr; current = current->next) {
     218           0 :         if (current->_callback(frame)) {
     219           0 :             return;
     220             :         }
     221             :     }
     222           0 : }
     223             : 
     224             : #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS
     225             :