aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: use reference for dal in frontend

Browse Source 
in place of method calls
This commit is contained in:
Peter Barker 2024-08-30 12:26:19 +10:00 committed by Andrew Tridgell
parent 4bcb3952c3
commit a51614f360
6 changed files with 52 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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<num_cores; i++) {
// if we have not overrun by more than 3 IMU frames, and we
@ -920,7 +921,7 @@ void NavEKF3::UpdateFilter(void)
// multiple EKF instances to cooperate on scheduling
bool allow_state_prediction = true;
if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
AP::dal().ekf_low_time_remaining(AP_DAL::EKFType::EKF3, i)) {
dal.ekf_low_time_remaining(AP_DAL::EKFType::EKF3, i)) {
allow_state_prediction = false;
}
core[i].UpdateFilter(allow_state_prediction);
@ -937,7 +938,7 @@ void NavEKF3::UpdateFilter(void)
runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 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<num_cores; i++) {
@ -1279,7 +1280,7 @@ void NavEKF3::resetGyroBias(void)
// If using a range finder for height no reset is performed and it returns false
bool NavEKF3::resetHeightDatum(void)
{
AP::dal().log_event3(AP_DAL::Event::resetHeightDatum);
dal.log_event3(AP_DAL::Event::resetHeightDatum);
bool status = true;
if (core) {
@ -1394,7 +1395,7 @@ bool NavEKF3::getOriginLLH(Location &loc) const
// Returns false if the filter has rejected the attempt to set the origin
bool NavEKF3::setOriginLLH(const Location &loc)
{
AP::dal().log_SetOriginLLH3(loc);
dal.log_SetOriginLLH3(loc);
if (!core) {
return false;
@ -1416,7 +1417,7 @@ bool NavEKF3::setOriginLLH(const Location &loc)
bool NavEKF3::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
{
#if EK3_FEATURE_POSITION_RESET
AP::dal().log_SetLatLng(loc, posAccuracy, timestamp_ms);
dal.log_SetLatLng(loc, posAccuracy, timestamp_ms);
if (!core) {
return false;
@ -1554,7 +1555,7 @@ bool NavEKF3::configuredToUseGPSForPosXY(void) const
#if EK3_FEATURE_OPTFLOW_FUSION
void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
{
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
@ -1577,7 +1578,7 @@ bool NavEKF3::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vecto
// write yaw angle sensor measurements
void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
{
AP::dal().log_writeEulerYawAngle(yawAngle, yawAngleErr, timeStamp_ms, type);
dal.log_writeEulerYawAngle(yawAngle, yawAngleErr, timeStamp_ms, type);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
@ -1600,7 +1601,7 @@ void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t tim
*/
void NavEKF3::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
AP::dal().writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
dal.writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
@ -1617,7 +1618,7 @@ void NavEKF3::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float
*/
void NavEKF3::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
AP::dal().writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
dal.writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
@ -1640,7 +1641,7 @@ void NavEKF3::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt
*/
void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{
AP::dal().writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset);
dal.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
@ -1659,7 +1660,7 @@ void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Ve
*/
void NavEKF3::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
{
AP::dal().writeWheelOdom(delAng, delTime, timeStamp_ms, posOffset, radius);
dal.writeWheelOdom(delAng, delTime, timeStamp_ms, posOffset, radius);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
@ -1764,7 +1765,7 @@ void NavEKF3::convert_parameters()
// if GPS and optical flow enabled set EK3_SRC2_VELXY to optical flow
// EK3_SRC_OPTIONS should default to 1 meaning both GPS and optical flow velocities will be fused
if (AP::dal().opticalflow_enabled() && (!found_gps_type || (gps_type_old.get() <= 2))) {
if (dal.opticalflow_enabled() && (!found_gps_type || (gps_type_old.get() <= 2))) {
AP_Param::set_and_save_by_name("EK3_SRC2_VELXY", (int8_t)AP_NavEKF_Source::SourceXY::OPTFLOW);
}
}
@ -1776,9 +1777,9 @@ void NavEKF3::convert_parameters()
void NavEKF3::setTerrainHgtStable(bool val)
{
if (val) {
AP::dal().log_event3(AP_DAL::Event::setTerrainHgtStable);
dal.log_event3(AP_DAL::Event::setTerrainHgtStable);
} else {
AP::dal().log_event3(AP_DAL::Event::unsetTerrainHgtStable);
dal.log_event3(AP_DAL::Event::unsetTerrainHgtStable);
}
if (core) {
@ -2050,7 +2051,7 @@ void NavEKF3::writeDefaultAirSpeed(float airspeed, float uncertainty)
return;
}
AP::dal().log_writeDefaultAirSpeed3(airspeed, uncertainty);
dal.log_writeDefaultAirSpeed3(airspeed, uncertainty);
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeDefaultAirSpeed(airspeed, uncertainty);

2
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -366,6 +366,8 @@ public:
const EKFGSF_yaw *get_yawEstimator(void) const;
private:
class AP_DAL &dal;
uint8_t num_cores; // number of allocated cores
uint8_t primary; // current primary core
NavEKF3_core *core = nullptr;

1
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -7,6 +7,7 @@
#if EK3_FEATURE_OPTFLOW_FUSION
#include <GCS_MAVLink/GCS.h>
#include <AP_DAL/AP_DAL.h>
/********************************************************
* RESET FUNCTIONS *

4
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -3,6 +3,8 @@
#if EK3_FEATURE_BEACON_FUSION
#include <AP_DAL/AP_DAL.h>
// 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];

4
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -8,9 +8,9 @@
#include <AP_DAL/AP_DAL.h>
// 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;

12
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -32,7 +32,7 @@
#include <AP_NavEKF/AP_NavEKF_Source.h>
#include <AP_NavEKF/EKF_Buffer.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_DAL/AP_DAL.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#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<rng_bcn_elements> 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