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

Interface: output vector quantities by "return-by-value"

Browse Source
This commit is contained in:
kamilritz
2020-03-30 13:07:00 +02:00
committed by Mathieu Bresciani
parent 72d8f91b4d
commit 2fa43419d2
16 changed files with 126 additions and 233 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/ekf_logger.cpp
+1 -1
View File
@@ -58,7 +58,7 @@ void EkfLogger::writeState()
_file << time;
if(_state_logging_enabled)
{
matrix::Vector<float, 24> state = _ekf_wrapper.getState();
matrix::Vector<float, 24> state = _ekf->getStateAtFusionHorizonAsVector();
for(int i = 0; i < 24; i++)
{
_file << "," << state(i);
test/sensor_simulator/ekf_wrapper.cpp
+1 -63
View File
@@ -160,74 +160,12 @@ bool EkfWrapper::isWindVelocityEstimated() const
return control_status.flags.wind;
}
Vector3f EkfWrapper::getPosition() const
{
float temp[3];
_ekf->get_position(temp);
return Vector3f(temp);
}
Vector3f EkfWrapper::getVelocity() const
{
float temp[3];
_ekf->get_velocity(temp);
return Vector3f(temp);
}
Vector3f EkfWrapper::getAccelBias() const
{
float temp[3];
_ekf->get_accel_bias(temp);
return Vector3f(temp);
}
Vector3f EkfWrapper::getGyroBias() const
{
float temp[3];
_ekf->get_gyro_bias(temp);
return Vector3f(temp);
}
Quatf EkfWrapper::getQuaternion() const
{
return _ekf->get_quaternion();
}
Eulerf EkfWrapper::getEulerAngles() const
{
return Eulerf(getQuaternion());
}
matrix::Vector<float, 24> EkfWrapper::getState() const
{
float state[24];
_ekf->get_state_delayed(state);
return matrix::Vector<float, 24>{state};
return Eulerf(_ekf->getQuaternion());
}
matrix::Vector<float, 4> EkfWrapper::getQuaternionVariance() const
{
return matrix::Vector<float, 4>(_ekf->orientation_covariances().diag());
}
Vector3f EkfWrapper::getPositionVariance() const
{
return Vector3f(_ekf->position_covariances().diag());
}
Vector3f EkfWrapper::getVelocityVariance() const
{
return Vector3f(_ekf->velocity_covariances().diag());
}
Vector2f EkfWrapper::getWindVelocity() const
{
float temp[2];
_ekf->get_wind_velocity(temp);
return Vector2f(temp);
}
Quatf EkfWrapper::getVisionAlignmentQuaternion() const
{
float temp[4];
_ekf->get_ev2ekf_quaternion(temp);
return Quatf(temp);
}
test/sensor_simulator/ekf_wrapper.h
-11
View File
@@ -85,19 +85,8 @@ public:
bool isWindVelocityEstimated() const;
Vector3f getPosition() const;
Vector3f getVelocity() const;
Vector3f getAccelBias() const;
Vector3f getGyroBias() const;
Quatf getQuaternion() const;
Eulerf getEulerAngles() const;
matrix::Vector<float, 24> getState() const;
matrix::Vector<float, 4> getQuaternionVariance() const;
Vector3f getPositionVariance() const;
Vector3f getVelocityVariance() const;
Vector2f getWindVelocity() const;
Quatf getVisionAlignmentQuaternion() const;
private:
std::shared_ptr<Ekf> _ekf;