diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 1357ab295e..e81a8e7bce 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/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 namespace AP { diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index b4093d30d9..76419b969b 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/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); } diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index 6e86a7d1b3..a0fdac9a41 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/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), \