Browse Source

AP_NavEKF_Source: optimise configured_in_storage

This small CPU optimisation takes advantage of the fact that once configured in storage is true it will always remain true
c415-sdk
Randy Mackay 4 years ago committed by Andrew Tridgell
parent
commit
6c3948f417
  1. 10
      libraries/AP_NavEKF/AP_NavEKF_Source.cpp
  2. 3
      libraries/AP_NavEKF/AP_NavEKF_Source.h

10
libraries/AP_NavEKF/AP_NavEKF_Source.cpp

@ -293,10 +293,16 @@ bool AP_NavEKF_Source::usingGPS() const @@ -293,10 +293,16 @@ bool AP_NavEKF_Source::usingGPS() const
}
// true if some parameters have been configured (used during parameter conversion)
bool AP_NavEKF_Source::configured_in_storage() const
bool AP_NavEKF_Source::configured_in_storage()
{
if (config_in_storage) {
return true;
}
// first source parameter is used to determine if configured or not
return _source_set[0].posxy.configured_in_storage();
config_in_storage = _source_set[0].posxy.configured_in_storage();
return config_in_storage;
}
// mark parameters as configured in storage (used to ensure parameter conversion is only done once)

3
libraries/AP_NavEKF/AP_NavEKF_Source.h

@ -83,7 +83,7 @@ public: @@ -83,7 +83,7 @@ public:
bool usingGPS() const;
// true if source parameters have been configured (used for parameter conversion)
bool configured_in_storage() const;
bool configured_in_storage();
// mark parameters as configured in storage (used to ensure parameter conversion is only done once)
void mark_configured_in_storage();
@ -115,4 +115,5 @@ private: @@ -115,4 +115,5 @@ private:
SourceYaw yaw; // current yaw source
} _active_source_set;
bool initialised; // true once init has been run
bool config_in_storage; // true once configured in storage has returned true
};

Loading…
Cancel
Save