HAL_SITL: reimplement CAN with BinarySemaphore

libraries/AP_HAL_SITL/CANSocketIface.cpp

@@ -52,8 +52,6 @@ using namespace HALSITL;
 #define Debug(fmt, args...)
 #endif
 
-CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES];
-
 uint8_t CANIface::_num_interfaces;
 
 bool CANIface::is_initialized() const
@@ -233,6 +231,9 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
         transport = nullptr;
         return false;
     }
+    if (sem_handle != nullptr) {
+        transport->set_event_handle(sem_handle);
+    }
     return true;
 }
 
@@ -256,8 +257,9 @@ bool CANIface::select(bool &read_select, bool &write_select,
         _pollfd.fd = transport->get_read_fd();
         _pollfd.events |= POLLIN;
     }
-    if (_evt_handle != nullptr && blocking_deadline > AP_HAL::micros64()) {
-        _evt_handle->wait(blocking_deadline - AP_HAL::micros64());
+    const uint64_t now_us = AP_HAL::micros64();
+    if (sem_handle != nullptr && blocking_deadline > now_us) {
+        IGNORE_RETURN(sem_handle->wait(blocking_deadline - now_us));
     }
 
     // Writing the output masks
@@ -267,70 +269,16 @@ bool CANIface::select(bool &read_select, bool &write_select,
     return true;
 }
 
-bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
+bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle)
 {
-    _evt_handle = handle;
-    evt_can_socket[_self_index]._ifaces[_self_index] = this;
-    _evt_handle->set_source(&evt_can_socket[_self_index]);
-    return true;
-}
-
-
-bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle)
-{
-    if (evt_handle == nullptr) {
-        return false;
-    }
-    pollfd pollfds[HAL_NUM_CAN_IFACES] {};
-    uint8_t pollfd_iface_map[HAL_NUM_CAN_IFACES] {};
-    unsigned long int num_pollfds = 0;
-    
-    // Poll FD set setup
-    for (unsigned i = 0; i < HAL_NUM_CAN_IFACES; i++) {
-        if (_ifaces[i] == nullptr) {
-            continue;
-        }
-        pollfds[num_pollfds] = _ifaces[i]->_pollfd;
-        pollfd_iface_map[num_pollfds] = i;
-        num_pollfds++;
-    }
-
-    if (num_pollfds == 0) {
-        return true;
-    }
-
-    const uint32_t start_us = AP_HAL::micros();
-    do {
-        uint16_t wait_us = MIN(100, duration_us);
-
-        // check FD for input
-        const int res = poll(pollfds, num_pollfds, wait_us/1000U);
-
-        if (res < 0) {
-            return false;
-        }
-        if (res > 0) {
-            break;
-        }
-        
-        // ensure simulator runs
-        hal.scheduler->delay_microseconds(wait_us);
-    } while (AP_HAL::micros() - start_us < duration_us);
-
-
-    // Handling poll output
-    for (unsigned i = 0; i < num_pollfds; i++) {
-        if (_ifaces[pollfd_iface_map[i]] == nullptr) {
-            continue;
-        }
-
-        const bool poll_read  = pollfds[i].revents & POLLIN;
-        const bool poll_write = pollfds[i].revents & POLLOUT;
-        _ifaces[pollfd_iface_map[i]]->_poll(poll_read, poll_write);
+    sem_handle = handle;
+    if (transport != nullptr) {
+        transport->set_event_handle(handle);
     }
     return true;
 }
 
+
 void CANIface::get_stats(ExpandingString &str)
 {
     str.printf("tx_requests:    %u\n"

libraries/AP_HAL_SITL/CANSocketIface.h

@@ -88,7 +88,7 @@ public:
                 uint64_t blocking_deadline) override;
     
     // setup event handle for waiting on events
-    bool set_event_handle(AP_HAL::EventHandle* handle) override;
+    bool set_event_handle(AP_HAL::BinarySemaphore *handle) override;
 
     // fetch stats text and return the size of the same,
     // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt 
@@ -101,16 +101,6 @@ public:
         return &stats;
     }
     
-    class CANSocketEventSource : public AP_HAL::EventSource {
-        friend class CANIface;
-        CANIface *_ifaces[HAL_NUM_CAN_IFACES];
-        
-    public:
-        // we just poll fd, no signaling is done
-        void signal(uint32_t evt_mask) override { return; }
-        bool wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) override;
-    };
-
 private:
     void _pollWrite();
 
@@ -134,8 +124,7 @@ private:
 
     unsigned _frames_in_socket_tx_queue;
     uint32_t _tx_frame_counter;
-    AP_HAL::EventHandle *_evt_handle;
-    static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES];
+    AP_HAL::BinarySemaphore *sem_handle;
 
     pollfd _pollfd;
     std::priority_queue<CanTxItem> _tx_queue;

libraries/AP_HAL_SITL/CAN_Multicast.cpp

@@ -84,6 +84,10 @@ bool CAN_Multicast::receive(AP_HAL::CANFrame &frame)
     // run constructor to initialise
     new(&frame) AP_HAL::CANFrame(pkt.message_id, pkt.data, ret-10, (pkt.flags & MCAST_FLAG_CANFD) != 0);
 
+    if (sem_handle != nullptr) {
+        sem_handle->signal();
+    }
+
     return true;
 }
 

libraries/AP_HAL_SITL/CAN_Multicast.h

@@ -9,6 +9,7 @@
 
 class CAN_Multicast : public CAN_Transport {
 public:
+
     bool init(uint8_t instance) override;
     bool send(const AP_HAL::CANFrame &frame) override;
     bool receive(AP_HAL::CANFrame &frame) override;

libraries/AP_HAL_SITL/CAN_SocketCAN.cpp

@@ -85,6 +85,10 @@ bool CAN_SocketCAN::receive(AP_HAL::CANFrame &frame)
 
     // run constructor to initialise
     new(&frame) AP_HAL::CANFrame(receive_frame.can_id, receive_frame.data, receive_frame.can_dlc, false);
+
+    if (sem_handle != nullptr) {
+        sem_handle->signal();
+    }
     return true;
 }
 

libraries/AP_HAL_SITL/CAN_Transport.h

@@ -16,6 +16,13 @@ public:
     virtual bool send(const AP_HAL::CANFrame &frame) = 0;
     virtual bool receive(AP_HAL::CANFrame &frame) = 0;
     virtual int get_read_fd(void) const = 0;
+
+    void set_event_handle(AP_HAL::BinarySemaphore *handle) {
+        sem_handle = handle;
+    }
+
+protected:
+    AP_HAL::BinarySemaphore *sem_handle;
 };
 
 #endif // HAL_NUM_CAN_IFACES

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -336,7 +336,9 @@ void SITL_State::_fdm_input_local(void)
     _check_rc_input();
 
     // construct servos structure for FDM
+    if (_sitl != nullptr) {
         _simulator_servos(input);
+    }
 
 #if HAL_SIM_JSON_MASTER_ENABLED
     // read servo inputs from ride along flight controllers
@@ -344,7 +346,9 @@ void SITL_State::_fdm_input_local(void)
 #endif
 
     // replace outputs from multicast
+    if (_sitl != nullptr) {
         multicast_servo_update(input);
+    }
 
     // update the model
     sitl_model->update_home();