AP_ToshibaCAN: add get_present_mask for use in pre-arm checks

libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp

@@ -325,7 +325,7 @@ void AP_ToshibaCAN::loop()
                             _telemetry[esc_id].current_tot_mah += _telemetry[esc_id].current_ca * diff_ms * centiamp_ms_to_mah;
                         }
                         _telemetry[esc_id].last_update_ms = now_ms;
-                        _esc_present_bitmask |= ((uint32_t)1 << esc_id);
+                        _esc_present_bitmask_recent |= ((uint32_t)1 << esc_id);
                     }
                 }
 
@@ -347,10 +347,13 @@ void AP_ToshibaCAN::loop()
                     if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) {
                         WITH_SEMAPHORE(_telem_sem);
                         _telemetry[esc_id].temperature = temp_max < 20 ? 0 : temp_max / 5 - 20;
-                        _esc_present_bitmask |= ((uint32_t)1 << esc_id);
+                        _esc_present_bitmask_recent |= ((uint32_t)1 << esc_id);
                     }
                 }
             }
+
+            // update bitmask of escs that replied
+            update_esc_present_bitmask();
         }
 
         // success!
@@ -404,6 +407,23 @@ bool AP_ToshibaCAN::read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTi
     return (_can_driver->getIface(CAN_IFACE_INDEX)->receive(recv_frame, time, utc_time, flags) == 1);
 }
 
+// update esc_present_bitmask
+void AP_ToshibaCAN::update_esc_present_bitmask()
+{
+    // recently detected escs are immediately considered present
+    _esc_present_bitmask |= _esc_present_bitmask_recent;
+
+    // escs that don't respond disappear in 1 to 2 seconds
+    // set the _esc_present_bitmask to the "recent" bitmask and
+    // clear the "recent" bitmask every second
+    uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - _esc_present_update_ms > 1000) {
+        _esc_present_bitmask = _esc_present_bitmask_recent;
+        _esc_present_bitmask_recent = 0;
+        _esc_present_update_ms = now_ms;
+    }
+}
+
 // called from SRV_Channels
 void AP_ToshibaCAN::update()
 {

libraries/AP_ToshibaCAN/AP_ToshibaCAN.h

@@ -41,6 +41,9 @@ public:
     // send ESC telemetry messages over MAVLink
     void send_esc_telemetry_mavlink(uint8_t mav_chan);
 
+    // return a bitmask of escs that are "present" which means they are responding to requests.  Bitmask matches RC outputs
+    uint16_t get_present_mask() const { return _esc_present_bitmask; }
+
 private:
 
     // loop to send output to ESCs in background thread
@@ -52,6 +55,9 @@ private:
     // read frame on CAN bus, returns true on success
     bool read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTime timeout);
 
+    // update esc_present_bitmask
+    void update_esc_present_bitmask();
+
     bool _initialized;
     char _thread_name[9];
     uint8_t _driver_index;
@@ -82,8 +88,10 @@ private:
     uint8_t _telemetry_temp_req_counter;    // counter used to trigger temp data requests from ESCs (10x slower than other telem data)
     const float centiamp_ms_to_mah = 1.0f / 360000.0f;  // for converting centi-amps milliseconds to mAh
 
-    // bitmask of which escs seem to be present
-    uint16_t _esc_present_bitmask;
+    // variables for updating bitmask of responsive escs
+    uint16_t _esc_present_bitmask;      // bitmask of which escs seem to be present
+    uint16_t _esc_present_bitmask_recent;   // bitmask of escs that have responded in the last second
+    uint32_t _esc_present_update_ms;    // system time _esc_present_bitmask was last updated
 
     // structure for sending motor lock command to ESC
     union motor_lock_cmd_t {