fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 08:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: do not inline getPosition

Browse Source
This commit is contained in:
bresch
2024-11-22 15:13:47 +01:00
committed by Mathieu Bresciani
parent a224d38e52
commit 93c690f133
2 changed files with 21 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/estimator_interface.cpp
+20
View File
@@ -568,6 +568,26 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
return true;
}
Vector3f EstimatorInterface::getPosition() const
{
LatLonAlt lla = _output_predictor.getLatLonAlt();
float x;
float y;
if (_local_origin_lat_lon.isInitialized()) {
_local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
} else {
MapProjection zero_ref;
zero_ref.initReference(0.0, 0.0);
zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
}
const float z = -(lla.altitude() - getEkfGlobalOriginAltitude());
return Vector3f(x, y, z);
}
bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
{
return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag);
src/modules/ekf2/EKF/estimator_interface.h
+1 -19
View File
@@ -242,25 +242,7 @@ public:
Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
Vector3f getPosition() const
{
LatLonAlt lla = _output_predictor.getLatLonAlt();
float x;
float y;
if (_local_origin_lat_lon.isInitialized()) {
_local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
} else {
MapProjection zero_ref;
zero_ref.initReference(0.0, 0.0);
zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
}
const float z = -(lla.altitude() - getEkfGlobalOriginAltitude());
return Vector3f(x, y, z);
}
Vector3f getPosition() const;
LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); }
const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }