From a51614f3605d07a830d09d73c244c1820e77e1fe Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 30 Aug 2024 12:26:19 +1000 Subject: [PATCH] AP_NavEKF3: use reference for dal in frontend in place of method calls --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 69 ++++++++++--------- libraries/AP_NavEKF3/AP_NavEKF3.h | 2 + .../AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 1 + .../AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 4 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 4 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 12 +++- 6 files changed, 52 insertions(+), 40 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 97eb479a73..d62d52f2b4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -744,7 +744,8 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { AP_GROUPEND }; -NavEKF3::NavEKF3() +NavEKF3::NavEKF3() : + dal{AP::dal()} { AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info2); @@ -757,11 +758,11 @@ bool NavEKF3::InitialiseFilter(void) if (_enable == 0 || _imuMask == 0) { return false; } - const auto &ins = AP::dal().ins(); + const auto &ins = dal.ins(); - AP::dal().start_frame(AP_DAL::FrameType::InitialiseFilterEKF3); + dal.start_frame(AP_DAL::FrameType::InitialiseFilterEKF3); - imuSampleTime_us = AP::dal().micros64(); + imuSampleTime_us = dal.micros64(); // remember expected frame time const float loop_rate = ins.get_loop_rate_hz(); @@ -807,7 +808,7 @@ bool NavEKF3::InitialiseFilter(void) } // check if there is enough memory to create the EKF cores - if (AP::dal().available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) { + if (dal.available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 not enough memory"); _enable.set(0); num_cores = 0; @@ -815,7 +816,7 @@ bool NavEKF3::InitialiseFilter(void) } //try to allocate from CCM RAM, fallback to Normal RAM if not available or full - core = (NavEKF3_core*)AP::dal().malloc_type(sizeof(NavEKF3_core)*num_cores, AP::dal().MEM_FAST); + core = (NavEKF3_core*)dal.malloc_type(sizeof(NavEKF3_core)*num_cores, dal.MEM_FAST); if (core == nullptr) { _enable.set(0); num_cores = 0; @@ -825,7 +826,7 @@ bool NavEKF3::InitialiseFilter(void) // Call constructors on all cores for (uint8_t i = 0; i < num_cores; i++) { - new (&core[i]) NavEKF3_core(this); + new (&core[i]) NavEKF3_core(this, dal); } } @@ -905,13 +906,13 @@ bool NavEKF3::coreBetterScore(uint8_t new_core, uint8_t current_core) const */ void NavEKF3::UpdateFilter(void) { - AP::dal().start_frame(AP_DAL::FrameType::UpdateFilterEKF3); + dal.start_frame(AP_DAL::FrameType::UpdateFilterEKF3); if (!core) { return; } - imuSampleTime_us = AP::dal().micros64(); + imuSampleTime_us = dal.micros64(); for (uint8_t i=0; i 1E7; } - const bool armed = AP::dal().get_armed(); + const bool armed = dal.get_armed(); // core selection is only available after the vehicle is armed, else forced to lane 0 if its healthy if (runCoreSelection && armed) { @@ -990,7 +991,7 @@ void NavEKF3::UpdateFilter(void) resetCoreErrors(); coreLastTimePrimary_us[primary] = imuSampleTime_us; primary = newPrimaryIndex; - lastLaneSwitch_ms = AP::dal().millis(); + lastLaneSwitch_ms = dal.millis(); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 lane switch %u", primary); } } @@ -1020,9 +1021,9 @@ void NavEKF3::UpdateFilter(void) */ void NavEKF3::checkLaneSwitch(void) { - AP::dal().log_event3(AP_DAL::Event::checkLaneSwitch); + dal.log_event3(AP_DAL::Event::checkLaneSwitch); - uint32_t now = AP::dal().millis(); + uint32_t now = dal.millis(); if (lastLaneSwitch_ms != 0 && now - lastLaneSwitch_ms < 5000) { // don't switch twice in 5 seconds return; @@ -1057,7 +1058,7 @@ void NavEKF3::checkLaneSwitch(void) void NavEKF3::requestYawReset(void) { - AP::dal().log_event3(AP_DAL::Event::requestYawReset); + dal.log_event3(AP_DAL::Event::requestYawReset); for (uint8_t i = 0; i < num_cores; i++) { core[i].EKFGSF_requestYawReset(); @@ -1107,7 +1108,7 @@ void NavEKF3::resetCoreErrors(void) void NavEKF3::setPosVelYawSourceSet(uint8_t source_set_idx) { if (source_set_idx < AP_NAKEKF_SOURCE_SET_MAX) { - AP::dal().log_event3(AP_DAL::Event(uint8_t(AP_DAL::Event::setSourceSet0)+source_set_idx)); + dal.log_event3(AP_DAL::Event(uint8_t(AP_DAL::Event::setSourceSet0)+source_set_idx)); } sources.setPosVelYawSourceSet(source_set_idx); } @@ -1135,21 +1136,21 @@ bool NavEKF3::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f const AP_NavEKF_Source::SourceYaw yaw_source = sources.getYawSource(); if (((magCalParamVal == 5) || (magCalParamVal == 6)) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS)) { // yaw source is configured to use compass but MAG_CAL valid is deprecated - AP::dal().snprintf(failure_msg, failure_msg_len, "EK3_MAG_CAL and EK3_SRC1_YAW inconsistent"); + dal.snprintf(failure_msg, failure_msg_len, "EK3_MAG_CAL and EK3_SRC1_YAW inconsistent"); return false; } if (!core) { - AP::dal().snprintf(failure_msg, failure_msg_len, "no EKF3 cores"); + dal.snprintf(failure_msg, failure_msg_len, "no EKF3 cores"); return false; } for (uint8_t i = 0; i < num_cores; i++) { if (!core[i].healthy()) { const char *failure = core[i].prearm_failure_reason(); if (failure != nullptr) { - AP::dal().snprintf(failure_msg, failure_msg_len, failure); + dal.snprintf(failure_msg, failure_msg_len, failure); } else { - AP::dal().snprintf(failure_msg, failure_msg_len, "EKF3 core %d unhealthy", (int)i); + dal.snprintf(failure_msg, failure_msg_len, "EKF3 core %d unhealthy", (int)i); } return false; } @@ -1263,7 +1264,7 @@ uint8_t NavEKF3::get_active_source_set() const // reset body axis gyro bias estimates void NavEKF3::resetGyroBias(void) { - AP::dal().log_event3(AP_DAL::Event::resetGyroBias); + dal.log_event3(AP_DAL::Event::resetGyroBias); if (core) { for (uint8_t i=0; i +#include /******************************************************** * RESET FUNCTIONS * diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index f3b332dbf1..0d3125d8dd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -3,6 +3,8 @@ #if EK3_FEATURE_BEACON_FUSION +#include + // initialise state: void NavEKF3_core::BeaconFusion::InitialiseVariables() { @@ -40,7 +42,7 @@ void NavEKF3_core::BeaconFusion::InitialiseVariables() delete[] fusionReport; fusionReport = nullptr; numFusionReports = 0; - auto *beacon = AP::dal().beacon(); + auto *beacon = dal.beacon(); if (beacon != nullptr) { const uint8_t count = beacon->count(); fusionReport = NEW_NOTHROW BeaconFusion::FusionReport[count]; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index ae850c23b1..4e3f1b08d0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -8,9 +8,9 @@ #include // constructor -NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend) : +NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend, AP_DAL &_dal) : frontend(_frontend), - dal(AP::dal()), + dal(_dal), public_origin(frontend->common_EKF_origin) { firstInitTime_ms = 0; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index a6ee193702..521b76443c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -32,7 +32,7 @@ #include #include #include -#include +#include #include "AP_NavEKF/EKFGSF_yaw.h" @@ -128,7 +128,7 @@ class NavEKF3_core : public NavEKF_core_common { public: // Constructor - NavEKF3_core(class NavEKF3 *_frontend); + NavEKF3_core(class NavEKF3 *_frontend, AP_DAL &dal); // setup this core backend bool setup_core(uint8_t _imu_index, uint8_t _core_index); @@ -1356,6 +1356,10 @@ private: #if EK3_FEATURE_BEACON_FUSION class BeaconFusion { public: + BeaconFusion(AP_DAL &_dal) : + dal{_dal} + {} + void InitialiseVariables(); EKF_obs_buffer_t storedRange; // Beacon range buffer @@ -1406,7 +1410,9 @@ private: Vector3F beaconPosNED; // beacon NED position } *fusionReport; uint8_t numFusionReports; - } rngBcn; + + AP_DAL &dal; + } rngBcn{dal}; #endif // if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_DRAG_FUSION