mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
AP_NavEKF: remove unnecessary memcopy
This commit is contained in:
parent
4a743a3827
commit
709a42cb7f
@ -134,7 +134,7 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void EKFGSF_yaw::fuseVelData(Vector2f vel, float velAcc)
|
void EKFGSF_yaw::fuseVelData(const Vector2f &vel, const float velAcc)
|
||||||
{
|
{
|
||||||
// convert reported accuracy to a variance, but limit lower value to protect algorithm stability
|
// convert reported accuracy to a variance, but limit lower value to protect algorithm stability
|
||||||
const float velObsVar = sq(fmaxf(velAcc, 0.5f));
|
const float velObsVar = sq(fmaxf(velAcc, 0.5f));
|
||||||
|
@ -24,8 +24,8 @@ public:
|
|||||||
|
|
||||||
// Fuse NE velocty mesurements and update the EKF's and GSF state and covariance estimates
|
// Fuse NE velocty mesurements and update the EKF's and GSF state and covariance estimates
|
||||||
// Should be called after update(...) whenever new velocity data is available
|
// Should be called after update(...) whenever new velocity data is available
|
||||||
void fuseVelData(Vector2f vel, // NE velocity measurement (m/s)
|
void fuseVelData(const Vector2f &vel, // NE velocity measurement (m/s)
|
||||||
float velAcc); // 1-sigma accuracy of velocity measurement (m/s)
|
const float velAcc); // 1-sigma accuracy of velocity measurement (m/s)
|
||||||
|
|
||||||
// get solution data for logging
|
// get solution data for logging
|
||||||
// return false if yaw estimation is inactive
|
// return false if yaw estimation is inactive
|
||||||
|
Loading…
Reference in New Issue
Block a user