mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 01:51:36 +08:00
Interface: output vector quantities by "return-by-value"
This commit is contained in:
kamilritz
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
72d8f91b4d
commit
2fa43419d2
@@ -225,16 +225,14 @@ void Ekf::fuseAirspeed()
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::get_wind_velocity(float *wind)
|
||||
Vector2f Ekf::getWindVelocity() const
|
||||
{
|
||||
wind[0] = _state.wind_vel(0);
|
||||
wind[1] = _state.wind_vel(1);
|
||||
return _state.wind_vel;
|
||||
}
|
||||
|
||||
void Ekf::get_wind_velocity_var(float *wind_var)
|
||||
Vector2f Ekf::getWindVelocityVariance() const
|
||||
{
|
||||
wind_var[0] = P(22,22);
|
||||
wind_var[1] = P(23,23);
|
||||
return P.slice<2, 2>(22,22).diag();
|
||||
}
|
||||
|
||||
void Ekf::get_true_airspeed(float *tas)
|
||||
@@ -248,8 +246,7 @@ void Ekf::get_true_airspeed(float *tas)
|
||||
*/
|
||||
void Ekf::resetWindStates()
|
||||
{
|
||||
// get euler yaw angle
|
||||
Eulerf euler321(_state.quat_nominal);
|
||||
const Eulerf euler321(_state.quat_nominal);
|
||||
const float euler_yaw = euler321(2);
|
||||
|
||||
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
|
||||
|
||||
+4
-5
@@ -101,15 +101,14 @@ void Ekf::initialiseCovariance()
|
||||
|
||||
}
|
||||
|
||||
void Ekf::get_pos_var(Vector3f &pos_var)
|
||||
Vector3f Ekf::getPositionVariance() const
|
||||
{
|
||||
pos_var = P.slice<3,3>(7,7).diag();
|
||||
return P.slice<3,3>(7,7).diag();
|
||||
}
|
||||
|
||||
void Ekf::get_vel_var(Vector3f &vel_var)
|
||||
Vector3f Ekf::getVelocityVariance() const
|
||||
{
|
||||
vel_var = P.slice<3,3>(4,4).diag();
|
||||
|
||||
return P.slice<3,3>(4,4).diag();
|
||||
}
|
||||
|
||||
void Ekf::predictCovariance()
|
||||
|
||||
+11
-11
@@ -350,17 +350,17 @@ void Ekf::calculateOutputStates()
|
||||
_R_to_earth_now = Dcmf(_output_new.quat_nominal);
|
||||
|
||||
// correct delta velocity for bias offsets
|
||||
const Vector3f delta_vel{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction};
|
||||
const Vector3f delta_vel_body{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction};
|
||||
|
||||
// rotate the delta velocity to earth frame
|
||||
Vector3f delta_vel_NED{_R_to_earth_now * delta_vel};
|
||||
Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body};
|
||||
|
||||
// correct for measured acceleration due to gravity
|
||||
delta_vel_NED(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;
|
||||
delta_vel_earth(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;
|
||||
|
||||
// calculate the earth frame velocity derivatives
|
||||
if (imu.delta_vel_dt > 1e-4f) {
|
||||
_vel_deriv_ned = delta_vel_NED * (1.0f / imu.delta_vel_dt);
|
||||
_vel_deriv = delta_vel_earth * (1.0f / imu.delta_vel_dt);
|
||||
}
|
||||
|
||||
// save the previous velocity so we can use trapezoidal integration
|
||||
@@ -368,8 +368,8 @@ void Ekf::calculateOutputStates()
|
||||
|
||||
// increment the INS velocity states by the measurement plus corrections
|
||||
// do the same for vertical state used by alternative correction algorithm
|
||||
_output_new.vel += delta_vel_NED;
|
||||
_output_vert_new.vel_d += delta_vel_NED(2);
|
||||
_output_new.vel += delta_vel_earth;
|
||||
_output_vert_new.vel_d += delta_vel_earth(2);
|
||||
|
||||
// use trapezoidal integration to calculate the INS position states
|
||||
// do the same for vertical state used by alternative correction algorithm
|
||||
@@ -427,13 +427,13 @@ void Ekf::calculateOutputStates()
|
||||
_delta_angle_corr = delta_ang_error * att_gain;
|
||||
|
||||
// calculate velocity and position tracking errors
|
||||
const Vector3f vel_err{_state.vel - _output_sample_delayed.vel};
|
||||
const Vector3f pos_err{_state.pos - _output_sample_delayed.pos};
|
||||
const Vector3f vel_err(_state.vel - _output_sample_delayed.vel);
|
||||
const Vector3f pos_err(_state.pos - _output_sample_delayed.pos);
|
||||
|
||||
// collect magnitude tracking error for diagnostics
|
||||
_output_tracking_error[0] = delta_ang_error.norm();
|
||||
_output_tracking_error[1] = vel_err.norm();
|
||||
_output_tracking_error[2] = pos_err.norm();
|
||||
_output_tracking_error(0) = delta_ang_error.norm();
|
||||
_output_tracking_error(1) = vel_err.norm();
|
||||
_output_tracking_error(2) = pos_err.norm();
|
||||
|
||||
/*
|
||||
* Loop through the output filter state history and apply the corrections to the velocity and position states.
|
||||
|
||||
@@ -135,13 +135,13 @@ public:
|
||||
void getHaglInnovRatio(float &hagl_innov_ratio) const override;
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
void get_state_delayed(float *state) override;
|
||||
matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const override;
|
||||
|
||||
// get the wind velocity in m/s
|
||||
void get_wind_velocity(float *wind) override;
|
||||
Vector2f getWindVelocity() const override;
|
||||
|
||||
// get the wind velocity var
|
||||
void get_wind_velocity_var(float *wind_var) override;
|
||||
Vector2f getWindVelocityVariance() const override;
|
||||
|
||||
// get the true airspeed in m/s
|
||||
void get_true_airspeed(float *tas) override;
|
||||
@@ -187,13 +187,13 @@ public:
|
||||
*/
|
||||
bool reset_imu_bias() override;
|
||||
|
||||
void get_vel_var(Vector3f &vel_var) override;
|
||||
Vector3f getVelocityVariance() const override;
|
||||
|
||||
void get_pos_var(Vector3f &pos_var) override;
|
||||
Vector3f getPositionVariance() const override;
|
||||
|
||||
// return an array containing the output predictor angular, velocity and position tracking
|
||||
// error magnitudes (rad), (m/sec), (m)
|
||||
void get_output_tracking_error(float error[3]) override;
|
||||
Vector3f getOutputTrackingError() const override;
|
||||
|
||||
/*
|
||||
Returns following IMU vibration metrics in the following array locations
|
||||
@@ -201,7 +201,7 @@ public:
|
||||
1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
|
||||
2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
|
||||
*/
|
||||
void get_imu_vibe_metrics(float vibe[3]) override;
|
||||
Vector3f getImuVibrationMetrics() const override;
|
||||
|
||||
/*
|
||||
First argument returns GPS drift metrics in the following array locations
|
||||
@@ -224,16 +224,16 @@ public:
|
||||
void updateTerrainValidity();
|
||||
|
||||
// get the estimated terrain vertical position relative to the NED origin
|
||||
void getTerrainVertPos(float *ret) override;
|
||||
float getTerrainVertPos() const override;
|
||||
|
||||
// get the terrain variance
|
||||
float get_terrain_var() const { return _terrain_var; }
|
||||
|
||||
// get the accelerometer bias in m/s/s
|
||||
void get_accel_bias(float bias[3]) override;
|
||||
// get the accelerometer bias in m/s**2
|
||||
Vector3f getAccelBias() const override;
|
||||
|
||||
// get the gyroscope bias in rad/s
|
||||
void get_gyro_bias(float bias[3]) override;
|
||||
Vector3f getGyroBias() const override;
|
||||
|
||||
// get GPS check status
|
||||
void get_gps_check_status(uint16_t *val) override;
|
||||
@@ -276,7 +276,7 @@ public:
|
||||
void get_ekf_soln_status(uint16_t *status) override;
|
||||
|
||||
// return the quaternion defining the rotation from the External Vision to the EKF reference frame
|
||||
void get_ev2ekf_quaternion(float *quat) override;
|
||||
matrix::Quatf getVisionAlignmentQuaternion() const override;
|
||||
|
||||
// use the latest IMU data at the current time horizon.
|
||||
Quatf calculate_quaternion() const;
|
||||
@@ -441,7 +441,7 @@ private:
|
||||
Vector3f _delta_angle_corr; ///< delta angle correction vector (rad)
|
||||
Vector3f _vel_err_integ; ///< integral of velocity tracking error (m)
|
||||
Vector3f _pos_err_integ; ///< integral of position tracking error (m.s)
|
||||
float _output_tracking_error[3] {}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
|
||||
Vector3f _output_tracking_error; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
|
||||
|
||||
// variables used for the GPS quality checks
|
||||
Vector3f _gps_pos_deriv_filt; ///< GPS NED position derivative (m/sec)
|
||||
|
||||
+22
-34
@@ -876,59 +876,51 @@ void Ekf::get_gps_check_status(uint16_t *val)
|
||||
}
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
void Ekf::get_state_delayed(float *state)
|
||||
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
||||
{
|
||||
matrix::Vector<float, 24> state;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
state[i] = _state.quat_nominal(i);
|
||||
state(i) = _state.quat_nominal(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 4] = _state.vel(i);
|
||||
state(i + 4) = _state.vel(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 7] = _state.pos(i);
|
||||
state(i + 7) = _state.pos(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 10] = _state.delta_ang_bias(i);
|
||||
state(i + 10) = _state.delta_ang_bias(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 13] = _state.delta_vel_bias(i);
|
||||
state(i + 13) = _state.delta_vel_bias(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 16] = _state.mag_I(i);
|
||||
state(i + 16) = _state.mag_I(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 19] = _state.mag_B(i);
|
||||
state(i + 19) = _state.mag_B(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
state[i + 22] = _state.wind_vel(i);
|
||||
state(i + 22) = _state.wind_vel(i);
|
||||
}
|
||||
return state;
|
||||
}
|
||||
|
||||
// get the accelerometer bias
|
||||
void Ekf::get_accel_bias(float bias[3])
|
||||
Vector3f Ekf::getAccelBias() const
|
||||
{
|
||||
float temp[3];
|
||||
temp[0] = _state.delta_vel_bias(0) / _dt_ekf_avg;
|
||||
temp[1] = _state.delta_vel_bias(1) / _dt_ekf_avg;
|
||||
temp[2] = _state.delta_vel_bias(2) / _dt_ekf_avg;
|
||||
memcpy(bias, temp, 3 * sizeof(float));
|
||||
return _state.delta_vel_bias / _dt_ekf_avg;
|
||||
}
|
||||
|
||||
// get the gyroscope bias in rad/s
|
||||
void Ekf::get_gyro_bias(float bias[3])
|
||||
Vector3f Ekf::getGyroBias() const
|
||||
{
|
||||
float temp[3];
|
||||
temp[0] = _state.delta_ang_bias(0) / _dt_ekf_avg;
|
||||
temp[1] = _state.delta_ang_bias(1) / _dt_ekf_avg;
|
||||
temp[2] = _state.delta_ang_bias(2) / _dt_ekf_avg;
|
||||
memcpy(bias, temp, 3 * sizeof(float));
|
||||
return _state.delta_ang_bias / _dt_ekf_avg;
|
||||
}
|
||||
|
||||
// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
|
||||
@@ -941,11 +933,11 @@ bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig
|
||||
return _NED_origin_initialised;
|
||||
}
|
||||
|
||||
// return an array containing the output predictor angular, velocity and position tracking
|
||||
// return a vector containing the output predictor angular, velocity and position tracking
|
||||
// error magnitudes (rad), (m/s), (m)
|
||||
void Ekf::get_output_tracking_error(float error[3])
|
||||
Vector3f Ekf::getOutputTrackingError() const
|
||||
{
|
||||
memcpy(error, _output_tracking_error, 3 * sizeof(float));
|
||||
return _output_tracking_error;
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -954,9 +946,9 @@ Returns following IMU vibration metrics in the following array locations
|
||||
1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
|
||||
2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
|
||||
*/
|
||||
void Ekf::get_imu_vibe_metrics(float vibe[3])
|
||||
Vector3f Ekf::getImuVibrationMetrics() const
|
||||
{
|
||||
memcpy(vibe, _vibe_metrics, 3 * sizeof(float));
|
||||
return _vibe_metrics;
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -1503,13 +1495,9 @@ void Ekf::calcExtVisRotMat()
|
||||
}
|
||||
|
||||
// return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame
|
||||
void Ekf::get_ev2ekf_quaternion(float *quat)
|
||||
matrix::Quatf Ekf::getVisionAlignmentQuaternion() const
|
||||
{
|
||||
const Quatf quat_ev2ekf(_R_ev_to_ekf);
|
||||
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
quat[i] = quat_ev2ekf(i);
|
||||
}
|
||||
return Quatf(_R_ev_to_ekf);
|
||||
}
|
||||
|
||||
// Increase the yaw error variance of the quaternions
|
||||
|
||||
@@ -91,25 +91,25 @@ void EstimatorInterface::computeVibrationMetric()
|
||||
{
|
||||
// calculate a metric which indicates the amount of coning vibration
|
||||
Vector3f temp = _newest_high_rate_imu_sample.delta_ang % _delta_ang_prev;
|
||||
_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();
|
||||
_vibe_metrics(0) = 0.99f * _vibe_metrics(0) + 0.01f * temp.norm();
|
||||
|
||||
// calculate a metric which indicates the amount of high frequency gyro vibration
|
||||
temp = _newest_high_rate_imu_sample.delta_ang - _delta_ang_prev;
|
||||
_delta_ang_prev = _newest_high_rate_imu_sample.delta_ang;
|
||||
_vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm();
|
||||
_vibe_metrics(1) = 0.99f * _vibe_metrics(1) + 0.01f * temp.norm();
|
||||
|
||||
// calculate a metric which indicates the amount of high frequency accelerometer vibration
|
||||
temp = _newest_high_rate_imu_sample.delta_vel - _delta_vel_prev;
|
||||
_delta_vel_prev = _newest_high_rate_imu_sample.delta_vel;
|
||||
_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm();
|
||||
_vibe_metrics(2) = 0.99f * _vibe_metrics(2) + 0.01f * temp.norm();
|
||||
}
|
||||
|
||||
bool EstimatorInterface::checkIfVehicleAtRest(float dt)
|
||||
{
|
||||
// detect if the vehicle is not moving when on ground
|
||||
if (!_control_status.flags.in_air) {
|
||||
if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler)
|
||||
|| (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler)
|
||||
if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler)
|
||||
|| (_vibe_metrics(2) * 2.1E2f > _params.is_moving_scaler)
|
||||
|| ((_newest_high_rate_imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
|
||||
|
||||
_time_last_move_detect_us = _newest_high_rate_imu_sample.time_us;
|
||||
|
||||
+28
-46
@@ -116,24 +116,17 @@ public:
|
||||
virtual void getHaglInnovVar(float &hagl_innov_var) const = 0;
|
||||
virtual void getHaglInnovRatio(float &hagl_innov_ratio) const = 0;
|
||||
|
||||
virtual void get_state_delayed(float *state) = 0;
|
||||
virtual matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const = 0;
|
||||
|
||||
virtual void get_wind_velocity(float *wind) = 0;
|
||||
virtual Vector2f getWindVelocity() const = 0;
|
||||
|
||||
virtual void get_wind_velocity_var(float *wind_var) = 0;
|
||||
virtual Vector2f getWindVelocityVariance() const = 0;
|
||||
|
||||
virtual void get_true_airspeed(float *tas) = 0;
|
||||
|
||||
// gets the variances for the NED velocity states
|
||||
virtual void get_vel_var(Vector3f &vel_var) = 0;
|
||||
|
||||
// gets the variances for the NED position states
|
||||
virtual void get_pos_var(Vector3f &pos_var) = 0;
|
||||
|
||||
|
||||
// return an array containing the output predictor angular, velocity and position tracking
|
||||
// error magnitudes (rad), (m/s), (m)
|
||||
virtual void get_output_tracking_error(float error[3]) = 0;
|
||||
virtual Vector3f getOutputTrackingError() const = 0;
|
||||
|
||||
/*
|
||||
Returns following IMU vibration metrics in the following array locations
|
||||
@@ -141,7 +134,7 @@ public:
|
||||
1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
|
||||
2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
|
||||
*/
|
||||
virtual void get_imu_vibe_metrics(float vibe[3]) = 0;
|
||||
virtual Vector3f getImuVibrationMetrics() const = 0;
|
||||
|
||||
/*
|
||||
First argument returns GPS drift metrics in the following array locations
|
||||
@@ -281,59 +274,48 @@ public:
|
||||
bool get_terrain_valid() { return isTerrainEstimateValid(); }
|
||||
|
||||
// get the estimated terrain vertical position relative to the NED origin
|
||||
virtual void getTerrainVertPos(float *ret) = 0;
|
||||
//[[deprecated("Replaced by getTerrainVertPos")]]
|
||||
void get_terrain_vert_pos(float *ret) { getTerrainVertPos(ret); }
|
||||
virtual float getTerrainVertPos() const = 0;
|
||||
|
||||
// return true if the local position estimate is valid
|
||||
bool local_position_is_valid();
|
||||
|
||||
const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; }
|
||||
const matrix::Quatf getQuaternion() const { return _output_new.quat_nominal; }
|
||||
|
||||
// return the quaternion defining the rotation from the EKF to the External Vision reference frame
|
||||
virtual void get_ev2ekf_quaternion(float *quat) = 0;
|
||||
virtual matrix::Quatf getVisionAlignmentQuaternion() const = 0;
|
||||
|
||||
// get the velocity of the body frame origin in local NED earth frame
|
||||
void get_velocity(float *vel)
|
||||
Vector3f getVelocity() const
|
||||
{
|
||||
Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
vel[i] = vel_earth(i);
|
||||
}
|
||||
const Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
|
||||
return vel_earth;
|
||||
}
|
||||
|
||||
// get the NED velocity derivative in earth frame
|
||||
void get_vel_deriv_ned(float *vel_deriv)
|
||||
virtual Vector3f getVelocityVariance() const = 0;
|
||||
|
||||
// get the velocity derivative in earth frame
|
||||
Vector3f getVelocityDerivative() const
|
||||
{
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
vel_deriv[i] = _vel_deriv_ned(i);
|
||||
}
|
||||
return _vel_deriv;
|
||||
}
|
||||
|
||||
// get the derivative of the vertical position of the body frame origin in local NED earth frame
|
||||
void get_pos_d_deriv(float *pos_d_deriv)
|
||||
float getVerticalPositionDerivative() const
|
||||
{
|
||||
float var = _output_vert_new.vel_d - _vel_imu_rel_body_ned(2);
|
||||
*pos_d_deriv = var;
|
||||
return _output_vert_new.vel_d - _vel_imu_rel_body_ned(2);
|
||||
}
|
||||
|
||||
// get the position of the body frame origin in local NED earth frame
|
||||
void get_position(float *pos)
|
||||
// get the position of the body frame origin in local earth frame
|
||||
Vector3f getPosition() const
|
||||
{
|
||||
// rotate the position of the IMU relative to the boy origin into earth frame
|
||||
Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
|
||||
|
||||
const Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
|
||||
// subtract from the EKF position (which is at the IMU) to get position at the body origin
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
pos[i] = _output_new.pos(i) - pos_offset_earth(i);
|
||||
}
|
||||
}
|
||||
void copy_timestamp(uint64_t *time_us)
|
||||
{
|
||||
*time_us = _time_last_imu;
|
||||
return _output_new.pos - pos_offset_earth;
|
||||
}
|
||||
|
||||
virtual Vector3f getPositionVariance() const = 0;
|
||||
|
||||
// Get the value of magnetic declination in degrees to be saved for use at the next startup
|
||||
// Returns true when the declination can be saved
|
||||
// At the next startup, set param.mag_declination_deg to the value saved
|
||||
@@ -349,8 +331,8 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
virtual void get_accel_bias(float bias[3]) = 0;
|
||||
virtual void get_gyro_bias(float bias[3]) = 0;
|
||||
virtual Vector3f getAccelBias() const = 0;
|
||||
virtual Vector3f getGyroBias() const = 0;
|
||||
|
||||
// get EKF mode status
|
||||
void get_control_mode(uint32_t *val)
|
||||
@@ -482,7 +464,7 @@ protected:
|
||||
imuSample _newest_high_rate_imu_sample{}; // imu sample capturing the newest imu data
|
||||
Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time
|
||||
Vector3f _vel_imu_rel_body_ned; // velocity of IMU relative to body origin in NED earth frame
|
||||
Vector3f _vel_deriv_ned; // velocity derivative at the IMU in NED earth frame (m/s/s)
|
||||
Vector3f _vel_deriv; // velocity derivative at the IMU in NED earth frame (m/s/s)
|
||||
|
||||
bool _imu_updated{false}; // true if the ekf should update (completed downsampling process)
|
||||
bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized
|
||||
@@ -520,7 +502,7 @@ protected:
|
||||
// IMU vibration and movement monitoring
|
||||
Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement
|
||||
Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement
|
||||
float _vibe_metrics[3] {}; // IMU vibration metrics
|
||||
Vector3f _vibe_metrics; // IMU vibration metrics
|
||||
// [0] Level of coning vibration in the IMU delta angles (rad^2)
|
||||
// [1] high frequency vibration level in the IMU delta angle data (rad)
|
||||
// [2] high frequency vibration level in the IMU delta velocity data (m/s)
|
||||
|
||||
@@ -307,7 +307,7 @@ void Ekf::updateTerrainValidity()
|
||||
}
|
||||
|
||||
// get the estimated vertical position of the terrain relative to the NED origin
|
||||
void Ekf::getTerrainVertPos(float *ret)
|
||||
float Ekf::getTerrainVertPos() const
|
||||
{
|
||||
memcpy(ret, &_terrain_vpos, sizeof(float));
|
||||
return _terrain_vpos;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user