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

ekf2: move estimator states publication to method

Browse Source
This commit is contained in:
Daniel Agar
2020-11-11 20:17:52 -05:00
parent 98334d1325
commit 03388f4656
2 changed files with 24 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+23 -13
View File
@@ -684,15 +684,6 @@ void EKF2::Run()
filter_control_status_u control_status; filter_control_status_u control_status;
_ekf.get_control_mode(&control_status.value); _ekf.get_control_mode(&control_status.value);
// publish estimator states
estimator_states_s states;
states.timestamp_sample = imu_sample_new.time_us;
states.n_states = 24;
_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
_ekf.covariances_diagonal().copyTo(states.covariances);
states.timestamp = _replay_mode ? now : hrt_absolute_time();
_estimator_states_pub.publish(states);
// publish estimator status // publish estimator status
estimator_status_s status{}; estimator_status_s status{};
status.timestamp_sample = imu_sample_new.time_us; status.timestamp_sample = imu_sample_new.time_us;
@@ -765,9 +756,15 @@ void EKF2::Run()
// Declare all bias estimates invalid if any variances are out of range // Declare all bias estimates invalid if any variances are out of range
bool all_estimates_invalid = false; bool all_estimates_invalid = false;
float states[24];
_ekf.getStateAtFusionHorizonAsVector().copyTo(states);
float covariances[24];
_ekf.covariances_diagonal().copyTo(covariances);
for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) { for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
if (states.covariances[axis_index + 19] < min_var_allowed if (covariances[axis_index + 19] < min_var_allowed
|| states.covariances[axis_index + 19] > max_var_allowed) { || covariances[axis_index + 19] > max_var_allowed) {
all_estimates_invalid = true; all_estimates_invalid = true;
} }
} }
@@ -775,9 +772,9 @@ void EKF2::Run()
// Store valid estimates and their associated variances // Store valid estimates and their associated variances
if (!all_estimates_invalid) { if (!all_estimates_invalid) {
for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) { for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
_last_valid_mag_cal[axis_index] = states.states[axis_index + 19]; _last_valid_mag_cal[axis_index] = states[axis_index + 19];
_valid_cal_available[axis_index] = true; _valid_cal_available[axis_index] = true;
_last_valid_variance[axis_index] = states.covariances[axis_index + 19]; _last_valid_variance[axis_index] = covariances[axis_index + 19];
} }
} }
} }
@@ -802,6 +799,7 @@ void EKF2::Run()
// publish status/logging messages // publish status/logging messages
PublishEkfDriftMetrics(now); PublishEkfDriftMetrics(now);
PublishStates(now);
if (!_mag_decl_saved && _standby) { if (!_mag_decl_saved && _standby) {
_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl); _mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
@@ -1355,6 +1353,18 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
} }
} }
void EKF2::PublishStates(const hrt_abstime &timestamp)
{
// publish estimator states
estimator_states_s states;
states.timestamp_sample = timestamp;
states.n_states = 24;
_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
_ekf.covariances_diagonal().copyTo(states.covariances);
states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_states_pub.publish(states);
}
void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp) void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
{ {
static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF, static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,
src/modules/ekf2/EKF2.hpp
+1
View File
@@ -141,6 +141,7 @@ private:
void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu); void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu);
void PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &optical_flow); void PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &optical_flow);
void PublishSensorBias(const hrt_abstime &timestamp); void PublishSensorBias(const hrt_abstime &timestamp);
void PublishStates(const hrt_abstime &timestamp);
void PublishWindEstimate(const hrt_abstime &timestamp); void PublishWindEstimate(const hrt_abstime &timestamp);
void PublishYawEstimatorStatus(const hrt_abstime &timestamp); void PublishYawEstimatorStatus(const hrt_abstime &timestamp);