AP_DAL: added ekf_low_time_remaining()

used by EKF for inter-lane scheduling

libraries/AP_DAL/AP_DAL.cpp

@@ -230,6 +230,30 @@ void AP_DAL::WriteLogMessage(enum LogMessages msg_type, void *msg, const void *o
     }
 }
 
+/*
+  check if we are low on CPU for this core. This needs to capture the
+  timing of running the cores
+*/
+bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core)
+{
+    static_assert(INS_MAX_INSTANCES <= 4, "max 4 IMUs");
+    const uint8_t mask = (1U<<(core+(uint8_t(etype)*4)));
+#if !APM_BUILD_TYPE(APM_BUILD_Replay)
+    /*
+      if we have used more than 1/3 of the time for a loop then we
+      return true, indicating that we are low on CPU. This changes the
+      scheduling of fusion between lanes
+     */
+    const auto &ins = AP::ins();
+    if ((AP_HAL::micros() - ins.get_last_update_usec())*1.0e-6 > ins.get_loop_delta_t()*0.33) {
+        _RFRF.core_slow |= mask;
+    } else {
+        _RFRF.core_slow &= ~mask;
+    }
+#endif
+    return (_RFRF.core_slow & mask) != 0;
+}
+
 #include <stdio.h>
 
 namespace AP {

libraries/AP_DAL/AP_DAL.h

@@ -98,6 +98,15 @@ public:
         ARMED = (1U<<0),
     };
 
+    // EKF ID for timing checks
+    enum class EKFType : uint8_t {
+        EKF2 = 0,
+        EKF3 = 1,
+    };
+
+    // check if we are low on CPU for this core
+    bool ekf_low_time_remaining(EKFType etype, uint8_t core);
+    
     // returns armed state for the current frame
     bool get_armed() { return _RFRN.state_bitmask & uint8_t(StateMask::ARMED); }
 
@@ -210,6 +219,9 @@ public:
         _home.alt = msg.alt;
     }
 
+    void handle_message(const log_RFRF &msg) {
+        _RFRF.core_slow = msg.core_slow;
+    }
     void handle_message(const log_RISH &msg) {
         _ins.handle_message(msg);
     }

libraries/AP_DAL/LogStructure.h

@@ -40,6 +40,7 @@ struct log_RFRH {
 
 struct log_RFRF {
     uint8_t frame_types;
+    uint8_t core_slow;
     uint8_t _end;
 };
 
@@ -307,7 +308,7 @@ struct log_RVOH {
     { LOG_RFRH_MSG, RLOG_SIZE(RFRH),                          \
       "RFRH", "QI", "TimeUS,TF", "s-", "F-" }, \
     { LOG_RFRF_MSG, RLOG_SIZE(RFRF),                          \
-      "RFRF", "B", "FTypes", "-", "-" }, \
+      "RFRF", "BB", "FTypes,Slow", "--", "--" }, \
     { LOG_RFRN_MSG, RLOG_SIZE(RFRN),                            \
       "RFRN", "IIIfIBBBBBBBB", "HLat,HLon,HAlt,E2T,AM,State,NlRF,NlCRP,NlAS,FF,VC,ASE,EKT", "DUm??????????", "GGB----------" }, \
     { LOG_REV2_MSG, RLOG_SIZE(REV2),                                   \